From afc360d637d4ce9eca67a01c1faef15c16d4ebc6 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 9 Aug 2024 14:18:39 +0200 Subject: [PATCH 01/19] FW Position control: do not invalidate airspeed from negative readings Signed-off-by: Silvan Fuhrer --- src/modules/fw_pos_control/FixedwingPositionControl.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 4da7168255f9..9d126262fb17 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -212,8 +212,7 @@ FixedwingPositionControl::airspeed_poll() _eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) - && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) - && (airspeed_validated.calibrated_airspeed_m_s > FLT_EPSILON)) { + && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) { airspeed_valid = true; From acc0cd7e8ad915eaede3f7efaa58bbb831c12628 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 9 Aug 2024 14:22:33 +0200 Subject: [PATCH 02/19] FW Position Control: disable underspeed handling during auto takeoff Signed-off-by: Silvan Fuhrer --- src/modules/fw_pos_control/FixedwingPositionControl.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 9d126262fb17..5143a4da3899 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -1549,7 +1549,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_min.get(), _param_fw_thr_max.get(), _param_sinkrate_target.get(), - _performance_model.getMaximumClimbRate(_air_density)); + _performance_model.getMaximumClimbRate(_air_density), + true); // disable underspeed handling _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation @@ -1634,7 +1635,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_min.get(), max_takeoff_throttle, _param_sinkrate_target.get(), - _performance_model.getMaximumClimbRate(_air_density)); + _performance_model.getMaximumClimbRate(_air_density), + true); // disable underspeed handling if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) { // explicitly set idle throttle until motors are enabled From a0d22a4d212a87980a47524b5d5633cf6e918ace Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 12 Aug 2024 10:39:29 +0200 Subject: [PATCH 03/19] FW Position Control: make explicit when underspeed detection logic is en-/disabled Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 31 +++++++++++++------ 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 5143a4da3899..7858fe2c1c5f 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -973,6 +973,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) // Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover, // but not letting it drift too far away. const float descend_rate = -0.5f; + const bool disable_underspeed_handling = false; tecs_update_pitch_throttle(control_interval, _current_altitude, @@ -983,7 +984,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) _param_fw_thr_max.get(), _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, descend_rate); const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle @@ -1170,6 +1171,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co target_airspeed = _npfg.getAirspeedRef() / _eas2tas; float yaw_body = _yaw; + const bool disable_underspeed_handling = false; tecs_update_pitch_throttle(control_interval, position_sp_alt, @@ -1180,7 +1182,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, pos_sp_curr.vz); const float pitch_body = get_tecs_pitch(); @@ -1541,6 +1543,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _tecs.resetIntegrals(); } + const bool disable_underspeed_handling = true; + tecs_update_pitch_throttle(control_interval, altitude_setpoint_amsl, target_airspeed, @@ -1550,7 +1554,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_max.get(), _param_sinkrate_target.get(), _performance_model.getMaximumClimbRate(_air_density), - true); // disable underspeed handling + disable_underspeed_handling); _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation @@ -1626,6 +1630,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, altitude_setpoint_amsl, @@ -1636,7 +1641,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo max_takeoff_throttle, _param_sinkrate_target.get(), _performance_model.getMaximumClimbRate(_air_density), - true); // disable underspeed handling + disable_underspeed_handling); if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) { // explicitly set idle throttle until motors are enabled @@ -1813,6 +1818,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, altitude_setpoint, @@ -1823,7 +1829,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - true, + disable_underspeed_handling, height_rate_setpoint); /* set the attitude and throttle commands */ @@ -2029,6 +2035,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, _current_altitude, // is not controlled, control descend rate @@ -2039,7 +2046,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - true, + disable_underspeed_handling, height_rate_setpoint); /* set the attitude and throttle commands */ @@ -2085,6 +2092,8 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float glide_slope_sink_rate = airspeed_land * glide_slope / sqrtf(glide_slope * glide_slope + 1.0f); const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()), _param_fw_t_sink_max.get()); + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, // is not controlled, control descend rate target_airspeed, @@ -2094,7 +2103,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _param_fw_thr_max.get(), desired_max_sinkrate, _param_climbrate_target.get(), - false, + disable_underspeed_handling, -glide_slope_sink_rate); // heightrate = -sinkrate /* set the attitude and throttle commands */ @@ -2151,6 +2160,8 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, throttle_max = 0.0f; } + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, calibrated_airspeed_sp, @@ -2160,7 +2171,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, height_rate_sp); float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); @@ -2251,6 +2262,8 @@ FixedwingPositionControl::control_manual_position(const float control_interval, } } + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude. calibrated_airspeed_sp, @@ -2260,7 +2273,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, height_rate_sp); if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.roll) >= HDG_HOLD_MAN_INPUT_THRESH || From aad607e0dd1b216ec3e156d2d00fff7b870a74f5 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 13 Aug 2024 14:37:52 +0200 Subject: [PATCH 04/19] ekf2: send airspeed data to ekf backend regardless of sign On ground the airspeed can sometimes be slightly negative but the ekf should still know that airspeed data is flowing regularly --- src/modules/ekf2/EKF2.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index bceb4d3dcc23..af2eb6615f40 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2076,14 +2076,19 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) if (_airspeed_validated_sub.update(&airspeed_validated)) { if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) - && PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) - && (airspeed_validated.calibrated_airspeed_m_s > 0.f) && (airspeed_validated.selected_airspeed_index > 0) ) { + float cas2tas = 1.f; + + if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) + && (airspeed_validated.calibrated_airspeed_m_s > FLT_EPSILON)) { + cas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s; + } + airspeedSample airspeed_sample { .time_us = airspeed_validated.timestamp, .true_airspeed = airspeed_validated.true_airspeed_m_s, - .eas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, + .eas2tas = cas2tas, }; _ekf.setAirspeedData(airspeed_sample); } From e2c0e5c88ab68cafbee9b4fe4562882858706a63 Mon Sep 17 00:00:00 2001 From: Stefano Colli <45536733+StefanoColli@users.noreply.github.com> Date: Tue, 13 Aug 2024 22:20:28 +0200 Subject: [PATCH 05/19] MissionBase: replay the gimbal and trigger cached items only upon reaching resume waypoint (#23484) * Fix: replay the mission cached items only upon reaching resume waypoint * Refactoring Split camera mode mission items from gimbal ones so to have a finer control over the relative replays * Chore: fix formatting --------- Co-authored-by: Silvan Fuhrer --- src/modules/navigator/mission_base.cpp | 42 ++++++++++++++++++-------- src/modules/navigator/mission_base.h | 21 ++++++++++--- 2 files changed, 47 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index d41456ccce66..6a64f46044e8 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -235,6 +235,7 @@ MissionBase::on_activation() checkClimbRequired(_mission.current_seq); set_mission_items(); + _mission_activation_index = _mission.current_seq; _inactivation_index = -1; // reset // reset cruise speed @@ -293,17 +294,27 @@ MissionBase::on_active() _align_heading_necessary = false; } - // replay gimbal and camera commands immediately after resuming mission - if (haveCachedGimbalOrCameraItems()) { - replayCachedGimbalCameraItems(); + // Replay camera mode commands immediately upon mission resume + if (haveCachedCameraModeItems()) { + replayCachedCameraModeItems(); } - // replay trigger commands upon raching the resume waypoint if the trigger relay flag is set - if (cameraWasTriggering() && is_mission_item_reached_or_completed()) { - replayCachedTriggerItems(); - } - replayCachedSpeedChangeItems(); + // Replay cached mission commands once the last mission waypoint is re-reached after the mission interruption. + // Each replay function also clears the cached items afterwards + if (_mission.current_seq > _mission_activation_index) { + // replay gimbal commands + if (haveCachedGimbalItems()) { + replayCachedGimbalItems(); + } + + // replay trigger commands + if (cameraWasTriggering()) { + replayCachedTriggerItems(); + } + + replayCachedSpeedChangeItems(); + } /* lets check if we reached the current mission item */ if (_mission_type != MissionType::MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { @@ -1255,7 +1266,7 @@ void MissionBase::cacheItem(const mission_item_s &mission_item) } } -void MissionBase::replayCachedGimbalCameraItems() +void MissionBase::replayCachedGimbalItems() { if (_last_gimbal_configure_item.nav_cmd > 0) { issue_command(_last_gimbal_configure_item); @@ -1266,7 +1277,10 @@ void MissionBase::replayCachedGimbalCameraItems() issue_command(_last_gimbal_control_item); _last_gimbal_control_item = {}; // delete cached item } +} +void MissionBase::replayCachedCameraModeItems() +{ if (_last_camera_mode_item.nav_cmd > 0) { issue_command(_last_camera_mode_item); _last_camera_mode_item = {}; // delete cached item @@ -1297,11 +1311,15 @@ void MissionBase::resetItemCache() _last_camera_trigger_item = {}; } -bool MissionBase::haveCachedGimbalOrCameraItems() +bool MissionBase::haveCachedGimbalItems() { return _last_gimbal_configure_item.nav_cmd > 0 || - _last_gimbal_control_item.nav_cmd > 0 || - _last_camera_mode_item.nav_cmd > 0; + _last_gimbal_control_item.nav_cmd > 0; +} + +bool MissionBase::haveCachedCameraModeItems() +{ + return _last_camera_mode_item.nav_cmd > 0; } bool MissionBase::cameraWasTriggering() diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 2f416f29e206..35d4d37030de 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -328,6 +328,7 @@ class MissionBase : public MissionBlock, public ModuleParams mission_s _mission; /**< Currently active mission*/ float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */ int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose images. + int _mission_activation_index{-1}; /**< Index of the mission item that will bring the vehicle back to a mission waypoint */ int32_t _load_mission_index{-1}; /**< Mission inted of loaded mission items in dataman cache*/ int32_t _dataman_cache_size_signed; /**< Size of the dataman cache. A negativ value indicates that previous mission items should be loaded, a positiv value the next mission items*/ @@ -398,9 +399,14 @@ class MissionBase : public MissionBlock, public ModuleParams void updateCachedItemsUpToIndex(int end_index); /** - * @brief Replay the cached gimbal and camera mode items + * @brief Replay the cached gimbal items */ - void replayCachedGimbalCameraItems(); + void replayCachedGimbalItems(); + + /** + * @brief Replay the cached camera mode items + */ + void replayCachedCameraModeItems(); /** * @brief Replay the cached trigger items @@ -415,11 +421,18 @@ class MissionBase : public MissionBlock, public ModuleParams void replayCachedSpeedChangeItems(); /** - * @brief Check if there are cached gimbal or camera mode items to be replayed + * @brief Check if there are cached gimbal items to be replayed + * + * @return true if there are cached items + */ + bool haveCachedGimbalItems(); + + /** + * @brief Check if there are cached camera mode items to be replayed * * @return true if there are cached items */ - bool haveCachedGimbalOrCameraItems(); + bool haveCachedCameraModeItems(); /** * @brief Check if the camera was triggering From fd062d00859fccce7f25de8dacae9d67c5440545 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Tue, 13 Aug 2024 13:30:33 +0300 Subject: [PATCH 06/19] icm40609d: Clear interrupt status at FIFO reset If DRDY signal is used, the interrupt status needs to be cleared at FIFO reset in order to make DRDY go back inactive. Otherwise there won't be a falling edge interrupt at the next sample. Signed-off-by: Jukka Laitinen --- src/drivers/imu/invensense/icm40609d/ICM40609D.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 8dc0f4b79512..55db372d5d39 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -599,6 +599,9 @@ void ICM40609D::FIFOReset() // SIGNAL_PATH_RESET: FIFO flush RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); + // Read INT_STATUS to clear + RegisterRead(Register::BANK_0::INT_STATUS); + // reset while FIFO is disabled _drdy_timestamp_sample.store(0); } From cc4d5bd2a66f5a68c8e1b5850545376449e41dd4 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Tue, 13 Aug 2024 13:40:49 +0300 Subject: [PATCH 07/19] icm40609d: Add INTF register definition and disable I2C interface Disable I2C to make sure that the sensor doesn't switch to that by accident Signed-off-by: Jukka Laitinen --- src/drivers/imu/invensense/icm40609d/ICM40609D.hpp | 3 ++- .../icm40609d/InvenSense_ICM40609D_registers.hpp | 12 ++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index ab6fcfa1f27b..882e11a91ee8 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -160,10 +160,11 @@ class ICM40609D : public device::SPI, public I2CSPIDriver int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; - static constexpr uint8_t size_register_bank0_cfg{10}; + static constexpr uint8_t size_register_bank0_cfg{11}; register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { // Register | Set bits, Clear bits { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, + { Register::BANK_0::INTF_CONFIG0, INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0}, { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 }, diff --git a/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp b/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp index 4d359526ba92..e9880ec8cdd0 100644 --- a/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp +++ b/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp @@ -82,6 +82,8 @@ enum class BANK_0 : uint8_t { SIGNAL_PATH_RESET = 0x4B, + INTF_CONFIG0 = 0x4C, + PWR_MGMT0 = 0x4E, GYRO_CONFIG0 = 0x4F, ACCEL_CONFIG0 = 0x50, @@ -132,6 +134,16 @@ enum SIGNAL_PATH_RESET_BIT : uint8_t { FIFO_FLUSH = Bit1, }; +// INTF_CONFIG0 +enum INTF_CONFIG0_BIT : uint8_t { + FIFO_HOLD_LAST_DATA_EN = Bit7, + FIFO_COUNT_REC = Bit6, + FIFO_COUNT_ENDIAN = Bit5, + SENSOR_DATA_ENDIAN = Bit4, + UI_SIFS_CFG_DISABLE_SPI = Bit1, + UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0 +}; + // PWR_MGMT0 enum PWR_MGMT0_BIT : uint8_t { GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode From 0459481cb4e4e22912fa9d29bea2a1385bf05ea6 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Mon, 12 Aug 2024 16:08:10 +0300 Subject: [PATCH 08/19] icm40609d: Change FIFO count to samples instead of bytes As the sensor can directly report the amount of samples in the fifo, use it to simplify the logic. Also combine the fifo empty/fifo overflow checks for interrupt and polling modes. Signed-off-by: Jukka Laitinen --- .../imu/invensense/icm40609d/ICM40609D.cpp | 89 ++++++------------- .../imu/invensense/icm40609d/ICM40609D.hpp | 2 +- 2 files changed, 29 insertions(+), 62 deletions(-) diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 55db372d5d39..28fce0691ce9 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -203,57 +203,45 @@ void ICM40609D::RunImpl() case STATE::FIFO_READ: { hrt_abstime timestamp_sample = now; - uint8_t samples = 0; + uint8_t samples = FIFOReadCount(); - if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_timestamp_sample was set as expected - const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - - if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { - timestamp_sample = drdy_timestamp_sample; - samples = _fifo_gyro_samples; + if (samples == 0) { + perf_count(_fifo_empty_perf); - } else { - perf_count(_drdy_missed_perf); + } else { + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; } - // push backup schedule back - ScheduleDelayed(_fifo_empty_interval_us * 2); - } - - if (samples == 0) { - // check current FIFO count - const uint16_t fifo_count = FIFOReadCount(); - - if (fifo_count >= FIFO::SIZE) { + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); + samples = 0; + } + } - } else if (fifo_count == 0) { - perf_count(_fifo_empty_perf); + bool success = false; - } else { - // FIFO count (size in bytes) - samples = (fifo_count / sizeof(FIFO::DATA)); + if (samples > 0) { + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - // tolerate minor jitter, leave sample to next iteration if behind by only 1 - if (samples == _fifo_gyro_samples + 1) { - timestamp_sample -= static_cast(FIFO_SAMPLE_DT); - samples--; - } + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + samples = _fifo_gyro_samples; - if (samples > FIFO_MAX_SAMPLES) { - // not technically an overflow, but more samples than we expected or can publish - FIFOReset(); - perf_count(_fifo_overflow_perf); - samples = 0; + } else { + perf_count(_drdy_missed_perf); } - } - } - bool success = false; + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } - if (samples >= 1) { if (FIFORead(timestamp_sample, samples)) { success = true; @@ -374,17 +362,11 @@ void ICM40609D::ConfigureSampleRate(int sample_rate) void ICM40609D::ConfigureFIFOWatermark(uint8_t samples) { - // FIFO watermark threshold in number of bytes - const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA); - for (auto &r : _register_bank0_cfg) { if (r.reg == Register::BANK_0::FIFO_CONFIG2) { // FIFO_WM[7:0] FIFO_CONFIG2 - r.set_bits = fifo_watermark_threshold & 0xFF; + r.set_bits = samples & 0xFF; - } else if (r.reg == Register::BANK_0::FIFO_CONFIG3) { - // FIFO_WM[11:8] FIFO_CONFIG3 - r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F; } } } @@ -537,25 +519,10 @@ bool ICM40609D::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) return false; } - const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL); - - if (fifo_count_bytes >= FIFO::SIZE) { - perf_count(_fifo_overflow_perf); - FIFOReset(); - return false; - } - - const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA); - - if (fifo_count_samples == 0) { - perf_count(_fifo_empty_perf); - return false; - } - // check FIFO header in every sample uint8_t valid_samples = 0; - for (int i = 0; i < math::min(samples, fifo_count_samples); i++) { + for (int i = 0; i < samples; i++) { bool valid = true; // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index 882e11a91ee8..8dee6c2fb793 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -164,7 +164,7 @@ class ICM40609D : public device::SPI, public I2CSPIDriver register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { // Register | Set bits, Clear bits { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, - { Register::BANK_0::INTF_CONFIG0, INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0}, + { Register::BANK_0::INTF_CONFIG0, INTF_CONFIG0_BIT::FIFO_COUNT_REC | INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0}, { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 }, From a327b14cefbbe1f61b56a2e1ec3b71894e920f19 Mon Sep 17 00:00:00 2001 From: "murata,katsutoshi" Date: Wed, 14 Aug 2024 10:33:36 +0900 Subject: [PATCH 09/19] navigator: always fully initialize geofence msg --- src/modules/navigator/geofence.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index ea2c30a18eee..2c317bb79d9b 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -104,7 +104,7 @@ void Geofence::run() _initiate_fence_updated = false; _dataman_state = DatamanState::Read; - geofence_status_s status; + geofence_status_s status{}; status.timestamp = hrt_absolute_time(); status.geofence_id = _opaque_id; status.status = geofence_status_s::GF_STATUS_LOADING; @@ -159,7 +159,7 @@ void Geofence::run() _dataman_state = DatamanState::UpdateRequestWait; _fence_updated = true; - geofence_status_s status; + geofence_status_s status{}; status.timestamp = hrt_absolute_time(); status.geofence_id = _opaque_id; status.status = geofence_status_s::GF_STATUS_READY; @@ -179,7 +179,7 @@ void Geofence::run() _updateFence(); _fence_updated = true; - geofence_status_s status; + geofence_status_s status{}; status.timestamp = hrt_absolute_time(); status.geofence_id = _opaque_id; status.status = geofence_status_s::GF_STATUS_READY; From 5121358e879d9a61bb9e4565206e2705be2e650e Mon Sep 17 00:00:00 2001 From: mirusu400 Date: Tue, 13 Aug 2024 21:34:39 -0400 Subject: [PATCH 10/19] Makefile: Fix error message when cannot find target board not $(MAKE) help|list_config_targets, we should use $(MAKE) list_config_targets for the desired results. --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index fe8580402dab..7d4283a95690 100644 --- a/Makefile +++ b/Makefile @@ -549,7 +549,7 @@ distclean: # All other targets are handled by PX4_MAKE. Add a rule here to avoid printing an error. %: $(if $(filter $(FIRST_ARG),$@), \ - $(error "Make target $@ not found. It either does not exist or $@ cannot be the first argument. Use '$(MAKE) help|list_config_targets' to get a list of all possible [configuration] targets."),@#) + $(error "Make target $@ not found. It either does not exist or $@ cannot be the first argument. Use '$(MAKE) list_config_targets' to get a list of all possible [configuration] targets."),@#) # Print a list of non-config targets (based on http://stackoverflow.com/a/26339924/1487069) help: From f3a8d05f8c2ad79c8ea22ea70328aebddc3371b7 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Wed, 14 Aug 2024 12:18:14 +1000 Subject: [PATCH 11/19] MPC_ACC_DECOUPLE - better description (#23518) --- .../mc_pos_control/multicopter_position_control_limits_params.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mc_pos_control/multicopter_position_control_limits_params.c b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c index 91039f04ea6d..8307b82d7259 100644 --- a/src/modules/mc_pos_control/multicopter_position_control_limits_params.c +++ b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c @@ -143,6 +143,8 @@ PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f); * Acceleration to tilt coupling * * Set to decouple tilt from vertical acceleration. + * This provides smoother flight but slightly worse tracking in position and auto modes. + * Unset if accurate position tracking during dynamic maneuvers is more important than a smooth flight. * * @boolean * @group Multicopter Position Control From dec550dcb92039d5fc377c989d06a7d3f2bd7f3b Mon Sep 17 00:00:00 2001 From: "murata,katsutoshi" Date: Wed, 14 Aug 2024 16:40:36 +0900 Subject: [PATCH 12/19] navigator: Change IF statement to SWITCH statement (#23534) --- src/modules/navigator/geofence.cpp | 62 +++++++++++++++++++++--------- 1 file changed, 44 insertions(+), 18 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 2c317bb79d9b..00a1e3f94011 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -406,17 +406,25 @@ bool Geofence::checkPointAgainstPolygonCircle(const PolygonInfo &polygon, double { bool checksPass = true; - if (polygon.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) { + switch (polygon.fence_type) { + case NAV_CMD_FENCE_CIRCLE_INCLUSION: checksPass &= insideCircle(polygon, lat, lon, altitude); + break; - } else if (polygon.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) { + case NAV_CMD_FENCE_CIRCLE_EXCLUSION: checksPass &= !insideCircle(polygon, lat, lon, altitude); + break; - } else if (polygon.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) { + case NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION: checksPass &= insidePolygon(polygon, lat, lon, altitude); + break; - } else if (polygon.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) { + case NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION: checksPass &= !insidePolygon(polygon, lat, lon, altitude); + break; + + default: // unknown fence type + break; } return checksPass; @@ -452,12 +460,18 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, break; } - if (temp_vertex_i.frame != NAV_FRAME_GLOBAL && temp_vertex_i.frame != NAV_FRAME_GLOBAL_INT - && temp_vertex_i.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT - && temp_vertex_i.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + switch (temp_vertex_i.frame) { + case NAV_FRAME_GLOBAL: + case NAV_FRAME_GLOBAL_INT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + break; + + default: // TODO: handle different frames PX4_ERR("Frame type %i not supported", (int)temp_vertex_i.frame); - break; + return c; + } if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && @@ -482,12 +496,18 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, return false; } - if (circle_point.frame != NAV_FRAME_GLOBAL && circle_point.frame != NAV_FRAME_GLOBAL_INT - && circle_point.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT - && circle_point.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + switch (circle_point.frame) { + case NAV_FRAME_GLOBAL: + case NAV_FRAME_GLOBAL_INT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + break; + + default: // TODO: handle different frames PX4_ERR("Frame type %i not supported", (int)circle_point.frame); return false; + } if (!_projection_reference.isInitialized()) { @@ -675,20 +695,26 @@ void Geofence::printStatus() for (int i = 0; i < _num_polygons; ++i) { total_num_vertices += _polygons[i].vertex_count; - if (_polygons[i].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) { + switch (_polygons[i].fence_type) { + case NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION: ++num_inclusion_polygons; - } + break; - if (_polygons[i].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) { + case NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION: ++num_exclusion_polygons; - } + break; - if (_polygons[i].fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) { + case NAV_CMD_FENCE_CIRCLE_INCLUSION: ++num_inclusion_circles; - } + break; - if (_polygons[i].fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) { + case NAV_CMD_FENCE_CIRCLE_EXCLUSION: ++num_exclusion_circles; + break; + + default: // unknown fence type + break; + } } From 1fa878ad88d80e084f0b332b76677321376ff0ff Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 9 Aug 2024 11:15:15 +0200 Subject: [PATCH 13/19] navigator: add navigation state ID to every mode class --- src/modules/navigator/land.cpp | 4 ++-- src/modules/navigator/loiter.cpp | 2 +- src/modules/navigator/mission.cpp | 2 +- src/modules/navigator/mission_base.cpp | 6 +++--- src/modules/navigator/mission_base.h | 2 +- src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/navigator/mission_block.h | 2 +- src/modules/navigator/navigator_mode.cpp | 5 +++-- src/modules/navigator/navigator_mode.h | 7 ++++++- src/modules/navigator/precland.cpp | 2 +- src/modules/navigator/rtl.cpp | 2 +- src/modules/navigator/rtl_base.h | 2 +- src/modules/navigator/rtl_direct.cpp | 4 ++-- src/modules/navigator/takeoff.cpp | 4 ++-- src/modules/navigator/vtol_takeoff.cpp | 4 ++-- 15 files changed, 29 insertions(+), 23 deletions(-) diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index b6a951d1b766..74f54fbb079a 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -42,7 +42,7 @@ #include "navigator.h" Land::Land(Navigator *navigator) : - MissionBlock(navigator) + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { } @@ -93,7 +93,7 @@ Land::on_active() if (_navigator->get_land_detected()->landed) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND); + _navigator->mode_completed(getNavigatorStateId()); set_idle_item(&_mission_item); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 15408baaff05..db61030528c1 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -43,7 +43,7 @@ #include "navigator.h" Loiter::Loiter(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER), ModuleParams(navigator) { } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 2949c88d62b3..ea7dae675217 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -65,7 +65,7 @@ using namespace time_literals; static constexpr int32_t DEFAULT_MISSION_CACHE_SIZE = 10; Mission::Mission(Navigator *navigator) : - MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE) + MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE, vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { } diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 6a64f46044e8..8963a0685d08 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -46,8 +46,8 @@ #include "mission_feasibility_checker.h" #include "navigator.h" -MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed) : - MissionBlock(navigator), +MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id) : + MissionBlock(navigator, navigator_state_id), ModuleParams(navigator), _dataman_cache_size_signed(dataman_cache_size_signed) { @@ -476,7 +476,7 @@ MissionBase::set_mission_items() if (set_end_of_mission) { setEndOfMissionItems(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); + _navigator->mode_completed(getNavigatorStateId()); } } diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 35d4d37030de..f819b5c0e51a 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -65,7 +65,7 @@ class Navigator; class MissionBase : public MissionBlock, public ModuleParams { public: - MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed); + MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id); ~MissionBase() override = default; virtual void on_inactive() override; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1f0c9f0dd021..f1f202231853 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -55,8 +55,8 @@ using matrix::wrap_pi; -MissionBlock::MissionBlock(Navigator *navigator) : - NavigatorMode(navigator) +MissionBlock::MissionBlock(Navigator *navigator, uint8_t navigator_state_id) : + NavigatorMode(navigator, navigator_state_id) { } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 13815b860211..edc1b0e843f5 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -64,7 +64,7 @@ class MissionBlock : public NavigatorMode /** * Constructor */ - MissionBlock(Navigator *navigator); + MissionBlock(Navigator *navigator, uint8_t navigator_state_id); virtual ~MissionBlock() = default; MissionBlock(const MissionBlock &) = delete; diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index b0ced9006c56..392ca6abb458 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -42,8 +42,9 @@ #include "navigator_mode.h" #include "navigator.h" -NavigatorMode::NavigatorMode(Navigator *navigator) : - _navigator(navigator) +NavigatorMode::NavigatorMode(Navigator *navigator, uint8_t navigator_state_id) : + _navigator(navigator), + _navigator_state_id(navigator_state_id) { /* set initial mission items */ on_inactivation(); diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 60abdcd165e0..ca418e5b2d9a 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -41,12 +41,14 @@ #pragma once +#include + class Navigator; class NavigatorMode { public: - NavigatorMode(Navigator *navigator); + NavigatorMode(Navigator *navigator, uint8_t navigator_state_id); virtual ~NavigatorMode() = default; NavigatorMode(const NavigatorMode &) = delete; NavigatorMode &operator=(const NavigatorMode &) = delete; @@ -56,6 +58,8 @@ class NavigatorMode bool isActive() {return _active;}; + uint8_t getNavigatorStateId() const { return _navigator_state_id; } + /** * This function is called while the mode is inactive */ @@ -81,4 +85,5 @@ class NavigatorMode private: bool _active{false}; + uint8_t _navigator_state_id{0}; }; diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 080491d37ca7..74c81569cf3d 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -62,7 +62,7 @@ static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing"; PrecLand::PrecLand(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND), ModuleParams(navigator) { _handle_param_acceleration_hor = param_find("MPC_ACC_HOR"); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 2b6bd55b3f91..6fef45f9df6e 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -55,7 +55,7 @@ static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES{10.0f}; // [m] We static constexpr float MIN_DIST_THRESHOLD = 2.f; RTL::RTL(Navigator *navigator) : - NavigatorMode(navigator), + NavigatorMode(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL), ModuleParams(navigator), _rtl_direct(navigator) { diff --git a/src/modules/navigator/rtl_base.h b/src/modules/navigator/rtl_base.h index dc698687fce6..a4c88e06b026 100644 --- a/src/modules/navigator/rtl_base.h +++ b/src/modules/navigator/rtl_base.h @@ -46,7 +46,7 @@ class RtlBase : public MissionBase { public: RtlBase(Navigator *navigator, int32_t dataman_cache_size_signed): - MissionBase(navigator, dataman_cache_size_signed) {}; + MissionBase(navigator, dataman_cache_size_signed, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {}; virtual ~RtlBase() = default; virtual rtl_time_estimate_s calc_rtl_time_estimate() = 0; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 62a940f6c0cf..145b99d0d1db 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -51,7 +51,7 @@ using namespace math; RtlDirect::RtlDirect(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL), ModuleParams(navigator) { _destination.lat = static_cast(NAN); @@ -318,7 +318,7 @@ void RtlDirect::set_rtl_item() case RTLState::IDLE: { set_idle_item(&_mission_item); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + _navigator->mode_completed(getNavigatorStateId()); break; } diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index c9e59fec1e20..d9b22720f34a 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -43,7 +43,7 @@ #include Takeoff::Takeoff(Navigator *navigator) : - MissionBlock(navigator) + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) { } @@ -68,7 +68,7 @@ Takeoff::on_active() } else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF); + _navigator->mode_completed(getNavigatorStateId()); position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index 7b36201856d3..f9a552ef58fc 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -43,7 +43,7 @@ using matrix::wrap_pi; VtolTakeoff::VtolTakeoff(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF), ModuleParams(navigator) { } @@ -151,7 +151,7 @@ VtolTakeoff::on_active() // the VTOL takeoff is done _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF); + _navigator->mode_completed(getNavigatorStateId()); break; } From 9f69e9ee6ca3aaf5091866b202671311885fc10e Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 9 Aug 2024 11:19:08 +0200 Subject: [PATCH 14/19] navigator: publish navigator_state feedback to commander --- msg/CMakeLists.txt | 1 + msg/NavigatorStatus.msg | 6 ++++ src/modules/logger/logged_topics.cpp | 1 + src/modules/navigator/navigator.h | 14 ++++++++- src/modules/navigator/navigator_main.cpp | 37 ++++++++++++++++++++++++ src/modules/navigator/rtl_direct.cpp | 10 +++++++ 6 files changed, 68 insertions(+), 1 deletion(-) create mode 100644 msg/NavigatorStatus.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index f0f6d520c701..3b31b7de554d 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -146,6 +146,7 @@ set(msg_files MountOrientation.msg ModeCompleted.msg NavigatorMissionItem.msg + NavigatorStatus.msg NormalizedUnsignedSetpoint.msg NpfgStatus.msg ObstacleDistance.msg diff --git a/msg/NavigatorStatus.msg b/msg/NavigatorStatus.msg new file mode 100644 index 000000000000..21517d61e7bb --- /dev/null +++ b/msg/NavigatorStatus.msg @@ -0,0 +1,6 @@ +# Current status of a Navigator mode +# The possible values of nav_state are defined in the VehicleStatus msg. +uint64 timestamp # time since system start (microseconds) + +uint8 nav_state # Source mode (values in VehicleStatus) +bool failure # True when the current mode cannot continue diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 24b5004ee9af..5be39234e34f 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -91,6 +91,7 @@ void LoggedTopics::add_default_topics() add_topic("manual_control_switches"); add_topic("mission_result"); add_topic("navigator_mission_item"); + add_topic("navigator_status"); add_topic("npfg_status", 100); add_topic("offboard_control_mode", 100); add_topic("onboard_computer_status", 10); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 45c4f158b7c4..70068a9c515b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -283,8 +284,12 @@ class Navigator : public ModuleBase, public ModuleParams void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS); + void set_failsafe_status(uint8_t nav_state, bool failsafe); + void sendWarningDescentStoppedDueToTerrain(); + void trigger_failsafe(uint8_t nav_state); + private: int _local_pos_sub{-1}; @@ -305,6 +310,7 @@ class Navigator : public ModuleBase, public ModuleParams uORB::Publication _geofence_result_pub{ORB_ID(geofence_result)}; uORB::Publication _mission_result_pub{ORB_ID(mission_result)}; + uORB::Publication _navigator_status_pub{ORB_ID(navigator_status)}; uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; uORB::Publication _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::Publication _vehicle_cmd_pub{ORB_ID(vehicle_command)}; @@ -324,6 +330,7 @@ class Navigator : public ModuleBase, public ModuleParams // Publications geofence_result_s _geofence_result{}; + navigator_status_s _navigator_status{}; position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */ position_setpoint_triplet_s _reposition_triplet{}; /**< triplet for non-mission direct position command */ position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */ @@ -333,7 +340,10 @@ class Navigator : public ModuleBase, public ModuleParams Geofence _geofence; /**< class that handles the geofence */ GeofenceBreachAvoidance _gf_breach_avoidance; - hrt_abstime _last_geofence_check = 0; + hrt_abstime _last_geofence_check{0}; + + bool _navigator_status_updated{false}; + hrt_abstime _last_navigator_status_publication{0}; hrt_abstime _wait_for_vehicle_status_timestamp{0}; /**< If non-zero, wait for vehicle_status update before processing next cmd */ @@ -386,6 +396,8 @@ class Navigator : public ModuleBase, public ModuleParams */ void publish_mission_result(); + void publish_navigator_status(); + void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result); bool geofence_allows_position(const vehicle_global_position_s &pos); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f2694b595ab7..a00ca0e79c49 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -41,6 +41,7 @@ * @author Julian Oes * @author Anton Babushkin * @author Thomas Gubler + * and many more... */ #include "navigator.h" @@ -897,6 +898,8 @@ void Navigator::run() publish_mission_result(); } + publish_navigator_status(); + _geofence.run(); perf_end(_loop_perf); @@ -1355,6 +1358,40 @@ void Navigator::set_mission_failure_heading_timeout() } } +void Navigator::trigger_failsafe(const uint8_t nav_state) +{ + if (!_navigator_status.failure || _navigator_status.nav_state != nav_state) { + _navigator_status.failure = true; + _navigator_status.nav_state = nav_state; + + _navigator_status_updated = true; + } +} + +void Navigator::publish_navigator_status() +{ + uint8_t current_nav_state = _vstatus.nav_state; + + if (_navigation_mode != nullptr) { + current_nav_state = _navigation_mode->getNavigatorStateId(); + } + + if (_navigator_status.nav_state != current_nav_state) { + _navigator_status.nav_state = current_nav_state; + _navigator_status.failure = false; + _navigator_status_updated = true; + } + + if (_navigator_status_updated + || (hrt_elapsed_time(&_last_navigator_status_publication) > 500_ms)) { + _navigator_status.timestamp = hrt_absolute_time(); + _navigator_status_pub.publish(_navigator_status); + + _navigator_status_updated = false; + _last_navigator_status_publication = hrt_absolute_time(); + } +} + void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) { vcmd->timestamp = hrt_absolute_time(); diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 145b99d0d1db..216a94c858b4 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -158,6 +158,16 @@ void RtlDirect::set_rtl_item() { position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + if (_global_pos_sub.get().terrain_alt_valid + && ((_rtl_alt - _global_pos_sub.get().terrain_alt) > _navigator->get_local_position()->hagl_max)) { + // Handle case where the RTL altidude is above the maximum HAGL and land in place instead of RTL + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL, landing\t"); + events::send(events::ID("rtl_fail_max_hagl"), events::Log::Warning, "RTL: return alt higher than max HAGL, landing"); + + _navigator->trigger_failsafe(getNavigatorStateId()); + _rtl_state = RTLState::IDLE; + } + const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon); const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt); From 25fcb3c91338d62c3c2cfeae2851c9f768c98ff2 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 9 Aug 2024 14:12:23 +0200 Subject: [PATCH 15/19] comander: trigger failsafe when navigator reports failure --- msg/FailsafeFlags.msg | 1 + .../HealthAndArmingChecks/CMakeLists.txt | 1 + .../HealthAndArmingChecks.hpp | 3 ++ .../checks/navigatorCheck.cpp | 47 +++++++++++++++++ .../checks/navigatorCheck.hpp | 51 +++++++++++++++++++ src/modules/commander/failsafe/failsafe.cpp | 10 ++++ src/modules/navigator/rtl_direct.cpp | 4 +- 7 files changed, 115 insertions(+), 2 deletions(-) create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 2cd31bf83598..de514fb2db4b 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -49,6 +49,7 @@ bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure fai bool wind_limit_exceeded # Wind limit exceeded bool flight_time_limit_exceeded # Maximum flight time exceeded bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid +bool navigator_failure # Navigator failed to execute a mode # Failure detector bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS) diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 7c78c47c8534..7265e042b4c0 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -55,6 +55,7 @@ px4_add_library(health_and_arming_checks checks/manualControlCheck.cpp checks/missionCheck.cpp checks/modeCheck.cpp + checks/navigatorCheck.cpp checks/offboardCheck.cpp checks/openDroneIDCheck.cpp checks/parachuteCheck.cpp diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index af0fd9aec5f0..fdf38f5d5d77 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -49,6 +49,7 @@ #include "checks/escCheck.hpp" #include "checks/estimatorCheck.hpp" #include "checks/failureDetectorCheck.hpp" +#include "checks/navigatorCheck.hpp" #include "checks/gyroCheck.hpp" #include "checks/imuConsistencyCheck.hpp" #include "checks/loggerCheck.hpp" @@ -129,6 +130,7 @@ class HealthAndArmingChecks : public ModuleParams EscChecks _esc_checks; EstimatorChecks _estimator_checks; FailureDetectorChecks _failure_detector_checks; + NavigatorChecks _navigator_checks; GyroChecks _gyro_checks; ImuConsistencyChecks _imu_consistency_checks; LoggerChecks _logger_checks; @@ -167,6 +169,7 @@ class HealthAndArmingChecks : public ModuleParams &_esc_checks, &_estimator_checks, &_failure_detector_checks, + &_navigator_checks, &_gyro_checks, &_imu_consistency_checks, &_logger_checks, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp new file mode 100644 index 000000000000..8c7d0e314af4 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "navigatorCheck.hpp" + +void NavigatorChecks::checkAndReport(const Context &context, Report &reporter) +{ + navigator_status_s status; + + if (!_navigator_status_sub.copy(&status)) { + status = {}; + } + + if (context.status().nav_state == status.nav_state) { + reporter.failsafeFlags().navigator_failure = status.failure; + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp new file mode 100644 index 000000000000..48c6965deaf8 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" +#include +#include + + +class NavigatorChecks : public HealthAndArmingCheckBase +{ +public: + NavigatorChecks() = default; + ~NavigatorChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _navigator_status_sub{ORB_ID(navigator_status)}; +}; diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index fef6e2136bf3..11285af40352 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -472,6 +472,16 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL)); } + if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + CHECK_FAILSAFE(status_flags, navigator_failure, + ActionOptions(Action::Land).clearOn(ClearCondition::OnModeChangeOrDisarm)); + + } else { + CHECK_FAILSAFE(status_flags, navigator_failure, + ActionOptions(Action::Hold).clearOn(ClearCondition::OnModeChangeOrDisarm)); + } + CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred()); // Battery flight time remaining failsafe diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 216a94c858b4..9be8ce24a937 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -161,8 +161,8 @@ void RtlDirect::set_rtl_item() if (_global_pos_sub.get().terrain_alt_valid && ((_rtl_alt - _global_pos_sub.get().terrain_alt) > _navigator->get_local_position()->hagl_max)) { // Handle case where the RTL altidude is above the maximum HAGL and land in place instead of RTL - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL, landing\t"); - events::send(events::ID("rtl_fail_max_hagl"), events::Log::Warning, "RTL: return alt higher than max HAGL, landing"); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL\t"); + events::send(events::ID("rtl_fail_max_hagl"), events::Log::Error, "RTL: return alt higher than max HAGL"); _navigator->trigger_failsafe(getNavigatorStateId()); _rtl_state = RTLState::IDLE; From 4ed3e9e2103e75c1e9d52fe240d4f0c2cf6266c8 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 13 Aug 2024 17:57:29 +0200 Subject: [PATCH 16/19] navigator: add failure enum --- msg/NavigatorStatus.msg | 5 ++++- .../HealthAndArmingChecks/checks/navigatorCheck.cpp | 13 ++++++++++++- src/modules/navigator/navigator.h | 2 +- src/modules/navigator/navigator_main.cpp | 8 ++++---- src/modules/navigator/rtl_direct.cpp | 2 +- 5 files changed, 22 insertions(+), 8 deletions(-) diff --git a/msg/NavigatorStatus.msg b/msg/NavigatorStatus.msg index 21517d61e7bb..da666533574f 100644 --- a/msg/NavigatorStatus.msg +++ b/msg/NavigatorStatus.msg @@ -3,4 +3,7 @@ uint64 timestamp # time since system start (microseconds) uint8 nav_state # Source mode (values in VehicleStatus) -bool failure # True when the current mode cannot continue +uint8 failure # Navigator failure enum + +uint8 FAILURE_NONE = 0 +uint8 FAILURE_HAGL = 1 # Target altitude exceeds maximum height above ground diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp index 8c7d0e314af4..2a0245bb1fbd 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp @@ -42,6 +42,17 @@ void NavigatorChecks::checkAndReport(const Context &context, Report &reporter) } if (context.status().nav_state == status.nav_state) { - reporter.failsafeFlags().navigator_failure = status.failure; + + reporter.failsafeFlags().navigator_failure = (status.failure != navigator_status_s::FAILURE_NONE); + + if (status.failure == navigator_status_s::FAILURE_HAGL) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, + health_component_t::system, + events::ID("check_navigator_failure_hagl"), + events::Log::Error, + "Waypoint above maximum height"); + } } } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 70068a9c515b..ec72881fc527 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -288,7 +288,7 @@ class Navigator : public ModuleBase, public ModuleParams void sendWarningDescentStoppedDueToTerrain(); - void trigger_failsafe(uint8_t nav_state); + void trigger_hagl_failsafe(uint8_t nav_state); private: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a00ca0e79c49..15db1abdc44d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1358,10 +1358,10 @@ void Navigator::set_mission_failure_heading_timeout() } } -void Navigator::trigger_failsafe(const uint8_t nav_state) +void Navigator::trigger_hagl_failsafe(const uint8_t nav_state) { - if (!_navigator_status.failure || _navigator_status.nav_state != nav_state) { - _navigator_status.failure = true; + if ((_navigator_status.failure != navigator_status_s::FAILURE_HAGL) || _navigator_status.nav_state != nav_state) { + _navigator_status.failure = navigator_status_s::FAILURE_HAGL; _navigator_status.nav_state = nav_state; _navigator_status_updated = true; @@ -1378,7 +1378,7 @@ void Navigator::publish_navigator_status() if (_navigator_status.nav_state != current_nav_state) { _navigator_status.nav_state = current_nav_state; - _navigator_status.failure = false; + _navigator_status.failure = navigator_status_s::FAILURE_NONE; _navigator_status_updated = true; } diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 9be8ce24a937..f26df56530d2 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -164,7 +164,7 @@ void RtlDirect::set_rtl_item() mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL\t"); events::send(events::ID("rtl_fail_max_hagl"), events::Log::Error, "RTL: return alt higher than max HAGL"); - _navigator->trigger_failsafe(getNavigatorStateId()); + _navigator->trigger_hagl_failsafe(getNavigatorStateId()); _rtl_state = RTLState::IDLE; } From c8501cc1d02021e85fcc020bab3287d9e0ae08e8 Mon Sep 17 00:00:00 2001 From: Alexis Guijarro Date: Wed, 14 Aug 2024 17:52:42 -0600 Subject: [PATCH 17/19] boards: Support for 3DR Control Zero H7 OEM Rev G board --- .ci/Jenkinsfile-compile | 1 + .github/workflows/compile_nuttx.yml | 1 + .vscode/cmake-variants.yaml | 5 + Makefile | 1 + .../ctrl-zero-h7-oem-revg/bootloader.px4board | 3 + .../ctrl-zero-h7-oem-revg/default.px4board | 96 ++++++ .../3dr_ctrl-zero-h7-oem-revg_bootloader.bin | Bin 0 -> 42864 bytes .../ctrl-zero-h7-oem-revg/firmware.prototype | 13 + .../init/rc.board_defaults | 9 + .../init/rc.board_sensors | 19 ++ .../nuttx-config/bootloader/defconfig | 91 ++++++ .../nuttx-config/include/board.h | 288 ++++++++++++++++++ .../nuttx-config/include/board_dma_map.h | 44 +++ .../nuttx-config/nsh/defconfig | 260 ++++++++++++++++ .../nuttx-config/scripts/bootloader_script.ld | 221 ++++++++++++++ .../nuttx-config/scripts/script.ld | 228 ++++++++++++++ .../ctrl-zero-h7-oem-revg/src/CMakeLists.txt | 65 ++++ .../ctrl-zero-h7-oem-revg/src/board_config.h | 187 ++++++++++++ .../src/bootloader_main.c | 75 +++++ .../3dr/ctrl-zero-h7-oem-revg/src/hw_config.h | 135 ++++++++ boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp | 40 +++ boards/3dr/ctrl-zero-h7-oem-revg/src/init.c | 204 +++++++++++++ boards/3dr/ctrl-zero-h7-oem-revg/src/led.c | 111 +++++++ boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp | 53 ++++ .../src/timer_config.cpp | 54 ++++ boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c | 82 +++++ 26 files changed, 2286 insertions(+) create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board create mode 100644 boards/3dr/ctrl-zero-h7-oem-revg/default.px4board create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/extras/3dr_ctrl-zero-h7-oem-revg_bootloader.bin create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/firmware.prototype create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_defaults create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_sensors create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/bootloader/defconfig create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board.h create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h create mode 100644 boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/init.c create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/led.c create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index c482b14d459d..fcf70a5a2fdc 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -35,6 +35,7 @@ pipeline { def nuttx_builds_archive = [ target: [ + "3dr_ctrl-zero-h7-oem-revg_default", "airmind_mindpx-v2_default", "ark_can-flow_canbootloader", "ark_can-flow_default", diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index 5468e51d2049..cc44c55ea252 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -19,6 +19,7 @@ jobs: fail-fast: false matrix: config: [ + 3dr_ctrl-zero-h7-oem-revg, airmind_mindpx-v2, ark_can-flow, ark_can-gps, diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 1311079ff988..d55b170485e2 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -111,6 +111,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: px4_fmu-v6xrt_bootloader + 3dr_ctrl-zero-h7-oem-revg_default: + short: 3dr_ctrl-zero-h7-oem-revg + buildType: MinSizeRel + settings: + CONFIG: 3dr_ctrl-zero-h7-oem-revg_default airmind_mindpx-v2_default: short: airmind_mindpx-v2 buildType: MinSizeRel diff --git a/Makefile b/Makefile index 7d4283a95690..37c61db1c0b3 100644 --- a/Makefile +++ b/Makefile @@ -324,6 +324,7 @@ px4io_update: git status bootloaders_update: \ + 3dr_ctrl-zero-h7-oem-revg_bootloader \ ark_fmu-v6x_bootloader \ ark_pi6x_bootloader \ cuav_nora_bootloader \ diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board b/boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board new file mode 100755 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board b/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board new file mode 100644 index 000000000000..549ba99b7331 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board @@ -0,0 +1,96 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/extras/3dr_ctrl-zero-h7-oem-revg_bootloader.bin b/boards/3dr/ctrl-zero-h7-oem-revg/extras/3dr_ctrl-zero-h7-oem-revg_bootloader.bin new file mode 100755 index 0000000000000000000000000000000000000000..86dc50de7b9c4140d7356a8214ecd9a4b48857a7 GIT binary patch literal 42864 zcmeFZd3;k<{y%)~lH|52O}YT-0%T$72Bs}&1sy|DZkK>%aU2#$14X^H3RTdl%9NC% zBB%qDae(S~kUEN@LJFP7!qis58J*GjCM-^&FjheVEy@jT$?clweG+hH)OlXN=fCHV zC$HDZz4zR6&+<8+^V!eo98Jbg;E3=R`~Ro^9|r&5!2xpp7rOk>)?fZw_3@Q%m3yqr z>sarV?x^Y)#fL;TAhB1v<7y^!Y_^j3JCQ0XJ^T1r=eXi=MVst59Vc>1-<9sf8i&ktHQk9DQi>C#-MwEtW|3`< zkCb<^6C_7Z1()C=@-O>Zx=Vi3(!KIG2`FQc8RRYqUl+J;ydknUNaW~#BF`OkD16E% z{`m^8#hvyO|6n2+-$BTDfMopUH;6#*r(Pi)n~@ed*X>W1%spX2=m_QrjWyHx9<{i{G38_Lvv ze_!izIP;6sX6#(%C{8YQ6z?o$dr406a(nUVWyz9V-ZMYhl`LUnSdi+ntICqf2 zlQVF2$ZFJes}tpVZ1!+De(>bYMbXk`T&Lk`msvL2H8p(4;0`t6O}5i)PIg)Gomtkf zc9~=8x2&AU8Z!EQnT1)!9L8W<;OHR>INI+X+bZ0zN^nhdCP}8Aj+rxi-1CxL;d>MA zvy)tLl1WCs?4B3r0ygy6;%4@cIELo;*s|kLzgf0nEbRV@f@En6cSr9p_c1$}>38`_ zKD$u4X$vvnnu?T$WVYEz3Cn55u?(|8Xdy9$>_P()kJN+rSv*-tjAXtZ_je&JM4F2P zd~z}-Vx*8`FsjtzI3zt%k`!%AEwamc{_4*Sq`MQ;&^AAq;({?;==ihrNyqD(oVeR@t)5*EuD4=&7_5nMW`n8Xulr3GTcc> zh{?iz8`2r14x|u6^cQihCh9ZPb_Zpm2Q2_V}D$Sl~L2J-#~G9q;phpiWIkj z>v2C`>0z?t-0>S8_Z;#N3w?XOVLi(3_3%dPpRf+zFY++Ae(NV0SG z9h1w{_WC2a%-Q96-(1hT7S21+yJ9WV9-VjgKT6)4$(&{KzWBc6yQ>&-gGQ*G1SDopcHEIA zw586r=B4Jxg!754cU|@0m&LakX3q`hcKg@jY2@sR@Y7}gZ?o7_C348BZ;qS2Ih^;k zU+?78v@Xt9(&I*7j`>w-Ebd9J;#Y_7+?H;?F72F8S6ZCJBeZV)`Il9O&JIPfdV-+toxppjFz>F-8 zjBdHZ4tiOOQgMJ!T#?q8j2VBepO|$;oRP?{^jkp3);b(=hr{BMEp$eg{ZS7b;L*05 z_6hv}Z52}K^*rgIxbfR4B0P$e(CD@>ZI~AWrRMEP` zq_LJX(z36cxJe^(kjT8k{E0M2p7{({JCV8ArQJ2LPdSfYjFoBrjTEOaGYHb|0K@*b z`^lH&acLQvcF#p++%BZufiyNPBY65t<(02WI=5>7c~L9RQ}`J+q>13R2C@sugu6W4 z<&jj}RpPFajK-aUJB7s4I|g^m2ztlijuYse26r0DvlSigbj=s#aV&TP$p8UvzR^Qw z`hS0ljQju1_|h__wbL=8&2dJBol(-6;8@;0&Qa2tn16rwxcrjNk+UA?9yhC`bCmdC z_c*bnb98~!J+7dn(@_!k<9{PF;$`Ai`rzjlbUM8&X#7(ro7g2>2)hj{r!^MX4W3mLy) zPHsg>&%wv$J5{}}SJpJnD=~>_;%%-qu2k1Ot~;Io`kxcNkNk7;{|s{$gSoiKChmQr z*!fQN$CvnAwPkM&Cx1O$;Jg)eJfpZ~eyl9dK5~hy=j7Xm>`vw_NgU&1Ei-55Rxt9k zeyl+TZ~A$Q#buG0%(-WcQY+!)VyyaGoh8>oYetI(mr+WRHrr>NF-v6p2v>?UvG5gl z`dV;4HQExN+o3L?)RP|4GWiF`;=;cFt#R*3wO!pQ!?LBB;> zLf-VxRB|ekB&O>lhRJzN;alGUU6gn-Ud9@J7}rXq7m&6gtw&mj^bTmkv=|~>L>V`p zmm{r4av_-#iBM)F!r>YsoJ^(k3bY`Y7E(D&dh(VnH<4{T#`O=yC{lVwT0Y%D1O@fc zaI8$pC2l=(wiMWafL5tG|Mvz`Ub`T-YCZ3+cf4w*&GY@zU4gH4%c zBV8X8yX#p{Sl~?aN_iw1!1&&ZCBiG{>mSmD#+bIK;`xfzy2QhFwX*Z2?FDMcDo9i^ zF{_-{{K+EyhL)$WpyQ?A|2Mjs3%VH!zA`MlMg)~p-rb?gT-Nb?YO&HHJKc>M4`^H*HX4DyCIad?qOpz8eecM1~0%FW-W@gkiptj6j z-xWV2#|#9}M_MnPvx>%N_wypwLPf0j`6WJwlSlQb#7{giJD#m!w`g}TTcUTIMLyYL zf($ZM;m4{@r-_{01&FKnS2U-IjeRjTBG>m_^RJY(e7ApGqxu!HH5#=sQ;8)Byyx=} zkNF$dsCk9mrhet~OJr(-7%j4%fL4+Ml$J95Fgz zU}#<+zP@sLZ#j1$J6yiLKc!=@R3z$)=^9ECwRPlWB9FxwN3_T6oQX0-{+mMoclYt4 z&7&!zBfocm+;qlEbAR7QF%wkpInvS>PXvnZptU8>0HPRuJXd=i_kJLIeyQYbL8|_% z*aW^vjBN_p&*T)8#5de7?Xiz&&yy&Huq*Z=l}OU&_^vfjti))Dt%J$9U3$NTF*CQ1 zw3T#z9HZ^}IChCLZ7!KS&G&bO*se98HtXA{_m%v26yCVIntsdHamH7bJfmePS4TO{ zs|v3#U0T1%^n^t|!YzFwT}*KBfqb^U25TOp`oj~9+}alk9vn6o88PNlpjP!ygo1~L z(?k^=@o$(M#v0%7hJU(50(=apR@Hnm-9^l!ML}9;*R?Mai7~!px+G+?UB@2V-?dDq zZC@y|^8bcDQOMkN{#l?USxQa^lXC%eFxKoo{xfF!JVm%Irjun=T_44u1kKH&qd4uS zbc4c|(hW*KraC8I3YL)c#Fh=>c2Oge@gv2VqQOjhU$5ZgltG*L94Ij-#}10Avkqs8 z?9@C_-L;-!s_73o(Cx52MOr49T3j=rM zpPH5IBgSHdkNJzTd?^o)z$|l-1;t z->L8=KUes1;6Gb9`I7;+$m}qRLVyuCxoIFj--k7`n4F|5haB@8K?n3V>TRt2?FY0EDo~AQD_D7Wm>2UrQ;n?LH@wT0)?~zr?+DQmsX7=lF>&%xA2Gh; z%4^==7h|lEpgX!5^WsM<3i2A)ODb`yr}RlClRw4GD1DSz_M#je+5LTdz(~~c#eRx^ z!=N^yU}NYzKkXNKWmU`nz#G~hBLDI5zj8nOK6me7Vo?{X3zI}9_l?Ytz;)Sxz$(5_ zZ~|llN~0-lu0VZr51?nH!-LgI^|c>pR5~Y_m?AMVINOLql{8DqbrFel9pQlU72B_C zZ}sH~9tyjs!u_lbfiB34hbbSq!SAddZbJ&soPaLlrZNfTFBJv)#!&ETh|<*Mc21TQ z9$G?Zs!`N8D~+SYB&Ql2Y>X5N%7Z#j`NK0@CukpQh&jy7);;z|s#C6mHp@*^OGCx zY9AL!WUNgBX8up|CtN@jwT?zAmL#qArT#6!x%&P}oyzNNHJu$MEC^yWl(4 zKrJM>o($Mu9)kU)!=EC_7{{ey#dB97EANATxB%C^A(D2?n<+V5er|xIIlZgUeuK2m)h*Sj`4j1tjAfH8!BOMNl#ZZZ+|r+Jkq^vfXq)NRp%V(; zIe?NIehlMea6iz$;rc2y=;;rkYYE-gzd_kRzr0ECGN-Yg)K<_Wxf;|@ecWGPrOM=d zyouJ)+gD$u$-LsrtQ0aY`)JOGeHXDNZs1(NKm<)AvxUeH_oaE59o+t3DtuQc*!#gs zh}Yq#0V-SY;xqeUNr(alMw7r@0{UIU{2sIU++%-y9zA&uv$=Z{^Sdt_;(N$ry!Sce zML0IN*;er1VQ!}yQpHv8Yn!;OupmDATFp#QtNP&+O8)*v$=Rt=-M^fEwMtS4Vi<0x z_37;ev=w$Im1pRRrR{{N&eoaY?oC$FjFxuSEPMFpwsoCacBJi@PA%%doDOq!5gHT< z{_l_#WB#K54cHMPvTi8&3}`ixQ~T%&3o#df?rO7Gh+Kkhv{FalDeNuLi|nL6H|$wmM)z2VG@ywV45Myo!P>@YKALt_Cq}rYoF0Bfrl6#~-g=xr-XP&-24;=Z+Ftq}b@d+KY&VTy+6pMcX*ID@w!~ddA zq=mMN`Eoo9{Y^ioL}sH>Z{CHuvjWy{50^`;JU3M`ZxQq7O3dawDlO(gFO@6%ZWU>X zwZqI*VVX9p6LLp*_G)N-`o@|~yqKaId188#TT1gdz&BZ68lbWpyqU<_F!#9&v^o@g zXQ+M?EN!LrHG0rWP%PPqw-J8%^7|M=^^eC~v+2`@1es^mUWcsbYHBEVp)jOZy;xB& z-b!T4Ax$F-4$J_TMh?Mlw39IzAdmO?9us3v>z`i%8 z9c@zz^aFh?+Nmu{k)pzUmgzdeAME38>+LZu^Dtw?7_pH;2Q&_1&D`Lrl&=2tY}8co zGq_AxJLLpOt{SXB%X$L&KQcpk%^DMt-yaAt$V)DkI9LdN^YPlM~;N;U;EgJqs!w8jQ4+ z2V1QI-8Rjcey7U_3qpRHlm1V-{{Ls((`Qzm+OHA|JtW}LWIdiH8m46fXFeL-XAk;X zK|Lq*Q`wCKT2<_%(RWUI+$b8R(h{D*KcIvSB@(Zfu&Kx-{hgCmLE;FP*cC3}MhRG4 z!*#e(hwyJDeiJUCG=By@c>?JS(hV9`(Kr`hUVyGd`!-j_-)R&R!}YEne8;Z%D!^mu zcT5p0m;WhpKlzg47%ARTJPw@hm_ppKVw)mt=1x)hsQIE&GqdSdu@Kn%YseS(H;p<> zjNN{c+2hl~auV={fC&>zpxve2G|C$dVWQS)NiU(^W5(yUt11 z{K(IL$JOqiCNY!ad>POi=~q{VRN^yDB+KhHicz3>tUNKihW3X3;)hKpT;}Z1xBeHJ zV!`>Q4DzBCGSP-mG@d5mX`zRHWfUi^X64Yp{jOC~z3YH<&b22j*Vlz*Z8~mZxutQP z#89p9nIGc+U1eF(}T3n@5j6ogL_C(iKMJntvSrX$)5vQtI}}0HtneO$N+cbf4e> z$9-Jid}CfAWBvNS$yltsap1-Z`9GDheD9N%8+HHpwJUoZ(m9XbNsK8{yQki+^I4=6 z&razzDLQ|rYoR9^*z?2yF}hqgRB6K zww~)c%B4KnuHjA+d5rST6M4<7ymX-7|32(RD(AW?ed7WpN6%xWJUjsT=?Q9cqVHJP z=CSU~)K@|4)HiA+w(5WmY&*9!#?Q%u%_j~$e-9Q+I`2B%?-_tTmry2_T#KoA7ux)b3TtD2 z%gNVbI}qnO63{Q;dWl8r zqW#~D@w9ex2|{Lcn;Ohe%F1V*M{p}T-`U>sz{ z-JRG|QF64KyDiac)Rf3N!P)XgK;Ux4@kXPuj?7h59YQ~+#PoCBQ#yrMM`tYOm{-y% z3Y;ZP;?lW(q1)KWk(HfXEQ5PTC*d+(E4_S=Ef@W`<6$p)cSlSM3mZ$9MEV6ewv*!w zj*G581&B`Xv(Pr9e$-}b6xz!|Nyogxu}qq?4fU0xrj>=L&$hC_Ti@$`_)LIsj;RgM9+b=h#ORKZP))(mf?>GI+hDm)$)po(l=P`j39J~jao9yt{+|=iz zym*9IQ_hQVqGs9zS)rzMvO-V095q{Lv``K9C;VV)d3m2vS`8^_c^@4wF>8y2MlI~a zZcB7AGlTm227FPly;Bd43VfQ#n+7Pf82JeJ;gt{G{U;quM4TOs8l`bmQLNAtRvlFa ztt;hGcz(r7EJrWR&6O?n59T#fI>1s%JI~NUWjxw$MWYtA zi<5d!SAB>)S{ktgEnY=2&U|3q1M60P^nh0xG_ns0#jVJ5o7X*vvX8YQkCvr)eWRb# zCK6#J>>R1m|FxfyJt38ZF*3{^*sF$b_*P3-7{!Rb^JDp#k$)Fbx$0Lid(5@xR10jhI@VE`V~x1+p1m4Ts(3N zEwDU&`Y9$Gx~Hv3Frv)P6?)gs6+Tz3a<|@6%%roOgw6HYds>&(D!(96X{=XBv-+Mq zfLNpXsMoc9MXj>XHIqiq%!~Cv6P`CSoVUnTP4gZd>$!I&&38Xq?aFY}tvKJ1?4r_= z-27p;oi)ii#)h_|_^6mTN2{Mr7g|vBvQPX6(7va)VT$?M<~*qRq7ZFe-)!mG{HO^q@9`fg zuUpYp*4k~;V@`3uV_D=%^IDNdWvp=BKcTPEx)*sIohdu5D$;q#VZb(!X+0D?A zg@R*Ym!dYiUzSmsWQYB7xytM5tzMh%T`3y~FgRnjcbjszb-=@9BAVjSn3fX6F+tjJ zY*QBM^&+*<+f2knpBL%r6L{E6`s|RcT&?S?M_Bn6z{^wqB=NjI=RmE*$yUaiZt{^# z=Je8)mplomb+k93hpbI(VIbA7M{Bi`UOdw6PFSxj#C^MNy|M^e@B*dd{xooue++-& zM=5<*m><3aE$?XgHpNATf=`5qncl^PHDSopO?izUm#d31#NQ%!ICvhqi=m0$4+U3; zrR(Q2sNQ{~JW;Gg8=0P(^13p!hnTh(SVRNtw2Y|_^(Ts3R~#>^YuK)SU72r9IC&83 z;(4|1B!y~P=pgcv)oPk+O;NMqTtBSC`sM^VR(Hi4Ye`sk8FU~I{Uj!pSid64Me${< zbEloBF)HRUg+oF0aK{R)>jC4+P%vk}wNe0Oo-(j|1%>khL=^pcL6ip?|AGbZWbIxN zN29512`>8n=X$J3M9k53>FAtUTI&i0XASROL5wuQjVy4;BM^_pSYB7=lJ&}6R?I45 zk%*1PP;noGa-ow>JX)?2@s3*ho^zXXp?Cj#G*z}pZ4Ny{p$2L>>O$W~pzkVQ4txY| zlNP;yb7=Pp#JPoAqT|>X9>+Tvhrt6W2lk&g4lDoG?%GCekpzqS#1OGW%}EvEMM=5U zb8o6OR}x=TqF^0eH<$|_jgS@a@$f@dKz3P!M6n)}sth(%kF$(h$~8Hb5{q+bX_c|+ zNO!Bs*wi7BHLmU>T)&1e;JV@kNF;cYX_Hfccpfr+owG+bFUsUcpV-Ukoso7jVe|aHoz539pOLl>|3_P-im|AQ zYiz_AiL>iyCQ-Eh+(BBu^U3HZKdrQPa%M*r%v?UaLsbyvrTlF_}NvA!2 zc8lPQaz3v~$fwbLB)%|AEr9kzO!sI*G$ONhRs7)rmU7H(%%6y~2K81ku&_`@|?$C1@HtV&c4 zYV95omVpJpf?Kn+dnU)NrlUb^uA>Y zjqx(W=ejWM%GB%nIh8p`3xz9%i`uCvMvQ8}Wk04|oX%dmXhsY&qCTmdrU%Zz4DON} zRaM54N0_cfoJ<@fBiH-q4R?*5ntyQMT8~(j;Go`rN9R!oGsSSH(BkOKU)42lRux~R zk~!6KhM8QyNaKs2Gg1u|*UzKtE`{R)$m?Y+>+DRIn`h6+ z&CK|}`^Uo@o$BhrJnHHCK%#wD8e_{e@W$QSq^-KCXbH1;pSwuGFm!4PEs*;q=I2#-f)t**HMzQ~wz8TYv8J8Dci* zS5kf`_+W^B$)gvRDofMTYgFcC-I|1_SLZz6y?x@ z1JYEC%i73^+S$otQj&Mr$r)?2MLN>0nZPRiZLGrGgZo)XrL4le30QmhUJtmW zt=q3Y3#(@lpBpW0w=-M&&(Jth`YsZAV=i0VWT{FnT*4$RoGFu~Nr2U|Fsv$?eH)U( ze&Y0@F8{d=@pmpB8<%gc@(gqb<}*&m=0%Rq4;?W@@S$PEweu`h5qR@<3hx^uC7Gvv zztj(C!Gfv;#F3BjC6+mt-qu8)c=bn@__!$8 z|G191Wiyu=#Cc02vQPzhw4RR>IcjShwu;^GNBt($3CTi7;NNQ@mj3IEfADA?R-mx5v5uOvg^w z1`_2T6!roVF;i_YPIOsA!Ia_BHGFt}N>@jGJmdrVHWYjSJ%~0&7ttPIg*qHPBw@ka zq2Swn>f*3e5^j~wwE(Q9b~su;g{i%Y6OXBx6QSVg7Pd-VIG>4INaNt+<`U!LD!VMO zi=UwIr+xcxRFLYlzrAjA2aOA!3@MP<0(jxa%WT7Dgkm8H`E~+AU&AGcEpDbTPBx$sT_5aiLf6;m&mD6 z=T=)Dj~Y<-=81@d*+pj!*WJv`pI7FVX;VwPvK ziz`CGfx+@8HB&6|Y_9y#(}*pm9%o`oEKG)m%)~EtX)V~N;ew^%w)slpZI+(lQai3T zvt?ss4+YOz*0C1LW1-*|12#~40TF1g!UiE1go4?CGlel7+j%4d?h}#r#KJEJc+!|G z(!2F>UR)`Qh^Rt*&>$ah5&FbzroOsu%I(UKx`-EA=?7WI+d3Xj11$hZ6Lp+zG|D2+ObU;-8!jiY1P+NvpY4DW_EI>ZOW+4 z_Rd(duKzhj+o0=TH{2A6(WSNNuJX}~m2`RL&9HgiLWE!7T6X=a%_G8`>*?pXu0r~B zS|r_va@!PrYby55sJv_j=Nl!vb;LXFzzrGCvZh@#>f|=1(wZi2jZl0@Jd5Yj@ld>^ zGXf-r!wxqL$o898drBaS1e+iXaxzb<3BK<2!qTu=~2miIM zLczNS-||2Bt2cio`;BY#7|s89NyaNa+B!RBl#krBwx?uYq>VeS74M_1Y$sYZ>bGJp zD~#YDI@pjyLG$o4ellfkPu@NT-%Q)biOahA1k36h_1@9**glf6wrAzONZs+M`)9sL z&C|oZkA;j_L;k(yv|*qBfy2yAoX;lfiE;H`{p?NSeQ|O~XYxfw*RNr2<a{(V zeG6cDUE4Em-`bn*xR$!FbPbKiXEX2U8MV&>d5x|GSYi_rTt=sPV}gy^fEn!NShvcu zuFBty8rXeJ7ffqPQ0LHlGM@Y2TL>%g+Ma9gF&KCEdv9MbubGFp-@QlM>v}KJp8vfY z_}kG(($@Bzd(XVlfDsB#4wN~u#nHoJZ;uQ9QpS0<7ZB4U@Gu}u}#CA)Jt)$ zC4w<$v7b!*QfYO6skH3^t=@k{bIVEi7?Kt#Ld9u#U&h%KP0C5eGD;4SB1N#)DRI_J z;JwVbGIhR5pr2n2)}b9p5sU`WPA~P-5=$Y0ZNjxl!=7X<^tKgzjqG_ z;dhS+>Hwqu&^H=UP84U)`RF*ve!47LA7Rr}PPRo4?pG*{ru>)k zUP_~H&~GE&6TO>I+D&PAiR{wC@*;evS#|OeMDZ6UKjOrwx1>m;5H-G7iK<}xg{wJs zveiIkW^SBqG$=Jwdz+WkA^C8`SY9zf@*_xUzdz+|D&;6awp> zE*2}BMQjYPoaZbl*P zTb`4?9}6Cw-tP{VctoI)mfAq7w#K*S0$O3&j1%ZjQoqknx!0@x{tJ1}4{w+u-JER5 zf6u)M`^?~S87nPe&({ry79 z9Koatj3lAp!v5d(jYJ%FbbH3yuBru{tVQK5Uqh*yCZT+F(}nUitXc8b^J{xm@99y) z?rS})r6&Z-jE19{!$(R_-Z4eFh0=?UaHUkM0(`P{L4vF68!E5sktlx-+XgV-H|9LA zQ9HZCWfD2ik?jR4=y8Yyhb1Vl`Elc5Fm{+h3`8inU7@@m9`qJkM+IatDkHil5=%r| zrcq*4TLv#1h{%z#a}YZrrHe~YE6O5P;y+71Zam*G82sxH^+JS#5-fQa!&XwN^F`W7 zn{!d3wyIA;`2Hv2*f18;IK+U3fjQb>+5=l|?VrK9NZ4vu@4opJ31ZsZ~sa+XrQW;r-X`D!}eie}nR3P%cCrEwdi zd9zF&gY`Bn(a*!4(9MOz!OFhh;Q#UP|6_eGl>kPem#_<==P_#E{~2NwelxVGh!r^z zavnT^wivtON+Bk@YuN05;G@;-y&$J%jLDaCVib!XnVMFWjBL2yOrYBb3W|hn`E2dYdE5eUwTBbGO4*WiZQN6iHXw; zu-K8YMY(;ZCWC%ajk|g!cHTvN^I#F3|4KwnaoC?j?QA!exHy#S9GC$g9@Ea;7W6TeT-QC$9O)QnB{PHCF39*nNW=>dmhSXZS?8|~jKV-}-LKJl_$qJXsC*x5H zCG=S+6|)JY#urT}jD}nt${7y}4J%okW_hjM+=E5jv&L?gLu`Vh+RVAeI%5j8*v%P> zQp3Ru!-(nadz|`iZG*vi@axgNwc2+C5`z@|UKF_uJ0-%9Ys(tS$*c_d2z^?5M_`#UXBMWmMOgeBuR)xw!tX6HsbcZ%#}a>&-J zene^IFk7ijgYL{8cparq@Z zyB{+^&;Kl$n8t`)@9rmPJUj<)sgWhUyB^&g(371@^*Q?7wxt^p_e3L;HQ3Ez#(t7e zaBnD0tZ1h3>F^~VqFU*2@K=a{p*AsIe!ZWh^|kYIM&De-b@TF{`&u2XI_#T~U+&|u zLin>Z_c_RWz3q(g8$T}(z>}cD-Us+N<&la#Rnomu>C`2@7k~Ykob&1gDkB2oeVnulilyiS*JB?EOiMa2R zip|vCV0L}1*k*?J{5RhP&F}Sqz+fdh@^OC`c2xY;FjpdJ3GIJdc ze|E)7cpIb7&&U5_iIs_+bu4xzi?oivB`?G``u5|OSbeoC%F`x&0y`!xFXS6s^CSjd z9L6!vm4qniP_P=%q@LkW@TnFB-b@|kD?ia+V)mk^?G3HyH|;OUnhq`EX0IkNS~Wq| zsrK2eZC2EGOQ;cW$vsBvUJ+6Qmg~#-17@jSdfT3H{A;Ne5nWo(*Vq$okrt-t(qc|o z@|PyL=FM?UaNQV*Ra^{uKQ&Y;2~!={p+W0ThhIl&o&XKqDrVu?89EGjf96jWKZjL^ zICj{nPVW{+itKS3%OF4ZVSq$ucSdv3os6c}r@x7Bqy8hRF+bnJ0wWZevG9Q`G69hcGjoZY ztEwE zFTbUz#Djg&)*5>hY!U)uxjFf_;GF&KjOG4nrsJH1U3spG<|BwbWY01)j=*Y7YZ0st z_y`;>PF7lA5p`*$geC#;Ulj#T$>8euBU(({7mhU zwf24=;&4bT z*($N7QGoT%EEPA6xN`(-S9#Ej@-}|}KO|$Zxb_qb2G0-Ojs3ZfSv|*{v#uV)Za!vv z0o~Jj*e8gtU>>{ z^xwnROZ`Heq;>3U;atQR`SRc2Z2FHk6bcj?pSKI;}-5PL{g-YS`nIMhEsX=|qzGv-bVrveV%|J3gFMHt2b;?4Vr( z{u2s*J^W5tdfD=1N`bn{Ua?Q~-JFsmnnVd+?*i<37;*ZyiXGM^mU{-*Q1HHCURrLa zUw%A%-p}XXoUHSZDc_vQESs3Wcj-a<3Gd#r#i?&EO;ni74BGTMw+S2fqph4a1zoXUblM#z8rO=VhfH55@{_SdV+H#woNUh>kE;vDv_6rviE7 z=S8r=Azdmag50?Az+W_+m)5}h`DC`n0!*J>z&8$w9 zII1OaT%w3blybAkb@4HpjD)tYg_W{K19{hen~lq` zVl~3_zg!{@obz)TU$n`7Cgan#fZslr?m~wr;|`KZqr*5c8N0*cDoJ|3PoG>-z~(cz za-zPm)~@lQWC?odB-r~?YbQQ^GkxmA*kd6*S{sjFB6orI`95A+11-(HfbxJOM>R0j z+WZzGX+B(P5(9oFN5~zQaS>0Dh38%J>mg^gmH|r}!SUb1UOdDvzth5Qr*Zy>R~n#_ z2#p$8g#7+?v?>1`QSTK6;7Qo~@#6kkkxbYE%Q3a1AfmnIP+)>i@H$fwjU>&(m&~+N zh+oS3YKy|`+LTJ)vb6y(nG|jQnfSi=e&7zBv+YnIQ+K;J5m%#bhWAjwqWhWm?SPeQ zZ-~k)X)|Ch<6GDP%`JtpTVMy&H)}c$1t#m#QPQgG^cBjCz}6)w%wD#Z_Tq%^?Z7P9 zEGpZg1IFmW9#7NJEw=}_Q)U0;Ju-sons3wzv!pEeNlrjA0H6$)iuIPEW# z1>qm~*9#~7@As0mhmIc#Jf)kAwpQyh@c)o*yf3ZCEseuYo5CKW^r8PyAOo?RwEiWa zL#)@*dG1$#L`?XbEwsO5#e40A@&@5ul$)Y!3zxIW_t&89%6jBItkWWotTnWtr_#S_ zCY9UpRtmp$ue|Y=R7To$OV8kKg^q1WLmwh}q;j4A?ZEvQ`G!-60;QOZ_o}1iFS!M< zXPv4(CGK)AQuw_TLPE<OJYQCMvB(XKK34YKeTyz^h6s>_$;6lVSH7WL9VQ8N%m1fg8Z9V2neA3;=PW^}9nebmNy zjkkW@;+8x@2F1sD6vj)#fuhl03kjBx`!QHzmO9GTUos zrJp{G8Yl$VcBZaw*7(!f0Ha~*zD&{eZ|EkOTxo>)&N=?W= zU_aP$EtyO-$Wgi!U#^(g!R#m;`(XpK<*zt}=DxB2^pizElhSt0WppfOhbAv7rDVtfrE-w_=MO}B<2XOUJS*DwsxrnnEhD*2g}YQ(()7ThY>Qb+3a|0a1vevJ$_T6N zp+M*km2Yn<{hkL6*p3>S{Mi=jr`5ks1OX`qiL{17s8eqgv(v}6(D}AtkI#$rif_nG z7U}l0>@@h0qG?Ur3w|*RC(oS5O1Y2DX=2MP)BT8fqVvSJ=g>Y`+tpJ}{w+YXlTDFX zx0((IqI46_M4l{?HehAYr)k>@Ixa9fDP@{JYhz2y+g8*3{OAA^y`d#uNl2x%Vtc_~ z!gIvuc7A<0@m-(8i(<_ zukLo|U)8e%4O;Ad*TWZ;DaS?AH_4cHBERa6dE1^JuOwwzGi>rp?u?$+;tW}Ty@UXJ z`m63_nSmecfZcudkM3Y{cSAvbeV0mmANuut$0<-BvX;WlfSvE>9^FZ);>zE7k-IXc z#n7;@;lt6J{dBy^|1sV)S*<-3Sg50;y9FF#C3m_ZDr0(ERAwH0Hk>?j;w|`%zU7uQ zTE8~HN43Lp%Xo?DD?e?a|9T7VhSr82f0{f>8x7lj1X7%LG=&s9&P(A`?so#BDHtu| z1x%fK%jF10;Hec>B!1O-Kb^4&CW;vmy0yKa=^Me)=6_#Y;CG0pMG5~8_>+6wCBUOx z(!u7oH>~u}ysBoCJ6YWwJrC{crgZ>I7}TNdQi0zcu=lp#pdl32<3ST6>z&pU4}3f% zdS_*}zu6^__h)^AeTS-iO6}5pOkHOF7;)7%1!6JUB=@NTtjZ!Kwk$MNhVk%g+A^06 z+IQBQ42rD^FyZ%^CZh>;-)Cr#A9|mc`{)`gDMLww!H^AnS+G0WEC01}$s(5yE9!^6riWnpK?<_T>MWJQ)tjovfZdK(sO&Ag5y(K5$|3%If$T!p{rT=bo zu||v)6RF16;`z>|WyUw+Kk%Kte-utP;2Fo5e3|8=lC8$S8eX>J))~e$cJp&>EHid>(jsgHoyGfcjD8uKK5+B%=7>B|Nh&VPapj5 zFX1+RdD31aSacH@Ym>I&Y2S5kA zd^w&8o!_ita+C9427MhR<~X^oKXbL;lBccTX|%upHuL-X`SynI_@|j^ZscnMD&b4z zb$EYX-#_oNUdv-|Hb%%jFOtpeDX^v0IdHPWZ_dsamxyG05u~%PLnL60StQOC$<`fW zv>7{573Oc8{6%P;xJkU=A9=m+=y$pCJ2gi7mvTB%zCF@!QG0*>jb2~&)q_^k9RZ>6 z@5)+2rcUfFk8u!p$1P7Dnd#^N{Wc(NM7%pyeN>-VqPZ57AyJmw|-WH4X$ycJwarAhGcznY9;K1S=W3D z^cWsm%qVd>cDNZ8T2~bGDypwEgtZzqA}v?R5>lqw|a)ajS6YdFv1dd&aDB~?yc^1QK!?P)DD#8C1@!ybFA zFCqW2vYqw=OI2;eM8BMiez&)ioBY14lvDnZfHk8q|9iM)=YYW4M}6^III=n(nbae&%5&eBE?7Vj-Cm-UCA9PY=@=VBH*E%&3f)hjdZCkDzf9 zxd?mRx%Ox$Yfi4Bsvfyua2C%dPo4|KrfX*Wk=eeWNhj<-$H5 z_&ZLb_k6nw{w5|j0oIOENIn5yzu%2=l_)RZ6bgT6SHQ?Ev@e_~z#0+-OF*n>u4FT4 ze{UMFpiPyJo|;3ge6WHZrv4q;mcB85wgvT$_0n@2RWo+llTPWJI8P@)O0nxKcc(q> z7%#&T39A&{Gs(-T3MY5K2FD{_JF+90$8M5vj1OT29%&=?zr++13yrJBt|h(6L;smu z?essF$L>$;Hmb~Rs+TNHtdW=hqU2cWF)liu)#wxJY;&E)Zo5Hfcr=gdPG2heniuzz z35V{~;G|WdegjUS;eUG&dUr#Ja^(G@&huA=R2-mQg`5AX2y=IcV=5Tb{*p4uL;b)c z6EnAt80mf()n*lBz^kzR^73j$RcD%^&%D^i>_$Ij(sFho5xS9(4JP zCPL8?BGe*XLAw1OBIuB4xi@h=fO0I_$;G^3*Ty=&jz%5xwfA+D^gIQBa9l_=w)JYT znwBwz%LJ;Wv3wf2`fc@>7x9HQHZGCyrfT@6lS1@smiyAro1QMbhW}p|cH@6dq3lnc zkOujT!wo$l#b$DBEA+dwJ~)ml_^c|9q&x3OsJT7 zHJ)QlnmuIFyO1YC!6U;W{IU#sG(UvXa8UEI5M#B;KdiM~(p#g_0WmR65%vWNXeV{zu_1jLC> z$bZK^(nnz4t6Ifjh4yz!VlR7(laN=hxba1 z3_<(M+-$eWYV@ux46!BQwlb^9gqy@E9c1#YzDHAU@oHWkUp!CxuxXp9#m+NMW@48J zAAsX(yfyM8j+&l4uBL}`)W}AT$-^l<*|Q<1c`z2g50EkGVXt7apSJPo?G1DXUrp!e zNj*M(8iny*`)6&KxjmZS;Y^!hIu>kky+CK=&%>I{NBndh(e;q_2O_+L`>jZnHmpfN zLOxx;bbV;{LXXsr5tkd;TG*&yd1$@*i5ohYmaRJs9gT8v*{7*v!|yNRJ$xo^w$6Z- z>V_AIn$6Tgb$&?a|IeD5!!_0a*P4!mYpO*}J2f`>H*O)5Tz$b!Yd!jXtxZF8TsCg) z1&WhzDD<82eWEy_;JZe!mm%AM$G6N(7wm9#)CGNxfMOyncBoU8MNEm6nEY zec^@WsO=GyUeNw=!`R7l+i2}iG4)@@`T|U>|Hp9JLB*~XWgtjd=s79?YH^9 zRJQG>C1!PMwTsYG?Qo?0h~b5^?&$f=d%)=giqjE{UOaU6o5-4-GbDRfrH=4XoY(AO zO_BG-L$n`n4n-t7y3$g(l*}klT12W~A07Sg7<$%z&U*~3>0FUCnchOi$~JI7%%NEt3xehnN|)t=@uVn<(| zZ8*ps!kIeoM{WZ?WC33`<5~%P0e78kgtuo>2y`*m0xFCBbjVL=)Pe4kOx@{NC#{pk z*%g$NQVvMH3sFv^%ODL0|Ix>oA)hctlz6R=N&61EqXv<)v<}(^ZI5+Ee2T36aTCoS z>#S&ujAFxnPQIfrrAUqO6ysdFyaR?J3X}Xk>j9iSgfjsg;oXnZ`?#?Q?eTNU#7vC1 zypOGeO;xn?yuXU>GGc3ou|6K6-Urb8rLgA;kf19t>J3dLJzq^#&1nY(_zJ5SIx}EE z$G-;sn1bt#@xM9zukq7&QBF-Gyr5~?_Q?2HIR~TT!=sCwcR?Z1=ci?Wx@uw>Q?ytuz_ zqIH8fxdHnuwKySo+FpBG3z<;=LJA_yqrIgsOm5iy5aR0_5JB#~e|m6Lt@o6{+Z0uZ zeVD8JRb$7slL^q2v`pQrcK+D8%689)CVZo+FK3{S)s<~8`1(c5OYsdU4LfnROnrIE zLn$6&Ho`Nqy@1y7SwE<3SPQef^!1BL4e<>d?PI;m%9-5x_oOr&?90*bpfIsmo~@S> zdOFssywMpNUxI9d9cqM`nMO6RR{w~MJ|DH58(N|f<47jA_$XB;nNBax@S8hQwyBEf z*-snYH0A~{qaITVv)$!%rw4_^*0Rk7U%hC4DGEK==%cw*t~^&s5F`22K6+i-vt}P4s(LC6!EgPv%)emrAadub9;Pr>m4#2M zs*&z3QwK(==nmvhaJr(Uy&JXM?8~i$q$!6qQ@zYhlj#_K;%uedC#glGyy-x>6&^6y zt;T9j)3cOlU8((4GL)<7ySnhZ1LfT5C6X2JzwECoW1tmmFQBDwsklJ<%AdpkXuiWA zY>Y(&fK5ZsANd9L|Gwa{Jh|*ihiu?-u}iA-QKyG_s{=}ChP2WEIMIYT^ zPES0jkz(QdrTCI6*wB}I`ZQzjY$uaT;1jg5#DcT!mypR*efnHJV2sLFEH1vQhAR|v zm*QNCzS0*C_DuxtGmv~K2JC(z9ym0?S13~a!}(c#6n~cWQT!_WzYF^sxTdbV|8wt6 zLVzd{v?3rT1T+Xp6s-7GLx3xa9aimX)viIrUaFP&Qj6Q38ilUZ+RhfYTJ2Vp*4dYB z1a*V97T0cFU)C+r_IXrvrFCtpv@VExF9gW*{oO?Cv$_92pU=&?FXxeb|aGwT$JAlb#o|&rtfseMI{Q9uN11dPo=cVU|$G0&L`gQ)%9Z@d<8P&S#w~ zcQ)gzY3%rYr@$lTVla}UdsQln54_cP8uVjl@?l_S(EO6T2C1w8??-6CjoObe2fu_h zfyz%~m-eFfRS(FofU4`ReGT*2R!v41AGi-YuxmaoBn}Hvx*fpmOzP~W@8F(1Om~9H z1wv|qzS_UpZguPQZ6&&h-B>cjw_k^Z@jhwHjIdi@k^FRF#F`WMb#JG0GQ^K7%*d48TaOZ20y3}}I!|1k(RPW($ z-Va_4$)8bIg->qRim_V^VwNYinsy7Z6vc9L7e11W-g$54bqOB56D3azNh?3=LUWr>aUOs z-1qjZ@OmlIes|_jbL~v%AkvJ}?4tF94@|}`k9O-CJkJ`cj66?dm67LWJcliNWc*>) z#XM~683X?2Z{5GU`qb9gGrzmKW9y{OgI9NK)=ruT9;7a?ZdYdJb1(kxs$*-G*M2!V z@0X4ugi#2SXx@Q*|6kib?dkJRg?ner+4Gn?lQDPhOZrg7y@TEwtku$i_lde$uTj)zpg@r(r@BC9(Td*!Act)THW4)fun05Ckr@6 zYozneZpl)y(LdGG3?!xVKvJ?)cRSZ$w2op@ghz7gl5RskeAo3vdm`F9Z?Lk~;-MA2 z3Mkmwq2}7G8@+=Iq=&*7x2`Z&nK{wM1?~?~Url0MU>Q(ks26jAyVbL7oGpaDM+&%J z)U&w2ybkI&T6NaG4SE|ucNij`g$}5q78hOjs%V_dOf!zzMJs%kv>cd$ptvfC>+`@M zzTp;jjugKWE0ASNa=S_m;XJGvOQ{;5V-u2J4t)&18OzX!x#G&5ziGWb`x$-TwY3IR z1S8r4qxTYY%EG#swQsBW;~ukq?QKTu?SC-2^}w1BRxASj)2yx~gfF(&(Ol2Y{;Hmv z`d7a~*vN%{dWubGK@Zh{4!Z+;<_HZ&rNy@`J$yv5xgkAM4wWQRl|W%gdz^J)_R;*3;h6D0@gY z4Bg?G3|*O#Xo*&}4lF=zrWM;#-5KTmRIqBjN!Gvk*d%>N)cC{xnDHLWRbgyDvW^0O zi0lhs2U`#N7-J4PEV0|{HS?r*8$NPFhq*d&E@2ro!Wx}r89FfQGm3?H_^11KGr_uB z#Q`L}4M8IO3zbq3ot{6rt>U13AP5Zy;L+wm(<>XgUet;vXmYrvUC1@TL$g%V+sM)E zmO}Erw~0!>bsuz}JY}f#Fl^~<`_uRd+{}8M6-u#ElFO9#;`(JSmbTY*#+9^w z+4?imBbCVu4REV1kJ@J)N>;)(6sR^q1+Vgcd8DX=4tWtY|UT7Ca5 z)@Ro9W4&Dr%2oZlmHSX1RX|)=4jiTmpQdMb^e4)0C!4guyFp^+yeEuOtD>o)K}FDs#E zU7W%DWV^5p-^~rm^I)SKl2_h9tsoo>_Sppy1l5P#;k7Jy8R7?(saZW&W?O zy;pJVB2JC)^L9mNb7mEEI4xY`C(2GY$)qM?*PwCQ2%Bh!un#F83-1^n!;XQOk0@!! zf7*IrXI%fzf%w+lCCR=DtXbiGg9iHsnwx0P6&~N&1K0OipS3d}hwE*$_t2_49>}L1 zU8jGw^T301T2I{PSaN`7DHkCcKIb@4&z*fqNpImMzvtKV>;#I&bFc}#+ca4$6}cH0 zGvK+)?)Ma%F?P+my6*|Wf|j9nu0j5Kw`Y!UBJBH%kdvMHgcgJ?Y%7RN92Y z|Bjrp638ihrXhly5=Kg)zI!u%N~7}=AXE&>ZG*puF2H&9&A)IbV9j2PHTxvi>_!P` zOs(A~w;uGeti(L$R3Sd$f-JWIgD#_sC>Cf!{wubj85CDE_5;s5^q{!Rn0KOEmnZ$9 zm2gGZ_{~p$iKm!BY&os-k}v;lW{>z+NPr zj;O5)q@#CpGduhhJ72z$dS3DsqGz<*)qY5bj(2(Vuv!_EA5qBqg=hfkV_20yq0suF z5gUO5<5rd!L-KzGyVeQPa#~+%!l4r(Q6|f!#JNP%8pOwFT;8H*gC~V~YDJL#PlTA| zTI-tn*XpTn!{-Kc$1yL1#tFZf==^$Vn8>o{d1-cgBQymzGEDh=T_$KXt7@aeEazA6 zuQ7WhtZC09zvc(VgR-|pVcyXHxAHY)DNf+W>}*xFiT~9b3Lv9cB~n{=r4puvENHbHTQ(n5v#73e^+J8N;!Rl zTQpadNp|xKPAX~YK+^-vy2X13WLWRQYAFjnx5bz$36?@25w*Y^*?ax&{o??FZ1qN_064xQ}mv1Q*cIqAHF_4SWBaJgcfX8M+o5VPR0uHv@&8=-lQ zdU`U_#vhz6q-?@9=eIu7@N@l>bFP}ah|MdSCM3wWc0m-;eI$=d=lEZ z46>Xs1qLg~AvWn6u;2?}|D)-7LFigH0=h-eMuprcuk(v_yqVhvnjww8=>y!$>`bef zEYiIxBGpI-?v3lLtO@s=&y-^K3r^}ge^3l=>e)oeXhl~$t+$5TxxBX>wB~;6Bi

zf-oYk^!*-lA1ncf4y)s;#ZTWx+?6=-S$K}QYs=5KEV#&J`?h#tTlcIY>=G|><9F4L zsI^~=o^(p7T{t4()Wr2aJ+#%g7&>@QU(wCfLOSrV`!ndB2p<*As9Mn8!^e&Hw)Y5e z+4$$MOMKg%Ex~729|N9_1sdR*lDh@k)0~hg9_p}(S?j-c*FvwF zzJ*z%&oIlHZ8~TYy}((K#}V+d{bVmymx-(CeX^frlIG3*{ZIGU@m+ST|8CrQ%$+6e zQI?pJASwKtpUeEQ4_7uWs}`KRCff@oJT%o_C|U1g*)`zQz*2M&&y938F2qc~^KZ+n zR8z35`~OdhyOClsdP|B6aCAq#L=#{^({J^xg;gGV@v~Np;vdVT!kw8zj zdzop^+}|NZN+j1r-*9-yo$A5?UCsp(_IxJ%dg@df=GA76;sM>d4O zgTyG0jnaAYodsf?N^KNfUSNgQLWTq5>a)IKJ;vea(XgDq$MyEtaqN#5EO-C$LdM;% zjIE5G6>N}azGXJn7Bht+_y$!M;}7nfhTZDT!La%sBtF_k~TY=EnKTO_fv z(s8Y;@_fDK>hg`vD~)BqEzFNA17B$utYd65MOAtkXbh5EVMs0xMFEM`gnh+;yn67U zf1$VpXS0J*ViWjRE5HY-$FUZN?dW4Ik4drfjpm;z_h8kKK>xae5_BWJ1ocX3w6I_3 zmme9h;>tcqUKS|5f;B^9B_0P0K$?}3%0}KvyPAW2;3=kjo{}ooIYxHcnPA~DuxLzWu4s$i^`z^4mvT&h=a+u{o#%0Y7Xkd11Q(|O=PQp|Y!tiX{{!ArdXVYc z5FW$9N?mjRj*tURcf6Okj`K=yMp?A!v91NKMb)M2R|dB7J6uBAhc0_s%=(zL9qSEU z*PoxLZdkv=#Ydj8wE61^9qszlXRF_Lg;P^Xddj7J=4a6IqBV`sjY+2XxEnkE@#z!L zOZ0k-k)Dv-!kPKB@<8=Wvh30?I{x~pEX zI@cl9MB8wr%0gMP5hK%CujC;1BpkUoY{%=B$?OvH%R`JQc$$xtQ}N_>W4$sBPYc+g zYA1HL^~!V{Gfef$OvIRl7_*IckoD0v+VL7;*PuT--xIrKJ~+i%A)%;jeS+tG%uSDj zuL0|cZy-L(XJe=gCB}GtSzP!PcS=VMSoRvqU3qG)@>S)H3OoO!CdX!?B?2E zIJ9SM=(}G8fiucty1tJm19E2&I`>nLIs7MAJAdV3CX#tE^MEX?`|QVRx!F-CoYH{& zXJAhdFQiXickH*!HNH|hb+4`1cn1rZ`fuwoLgSY149JIrU!t!l<>UvdK^!fPh2;_p zd^NBQ|8Ih0dPgVe=iVhgukg0Eju=QBKY(7qlhAmB-Z}cxbMUevoGC@@OM}*~Gtl9T z0XF7otVIHBua|bLaI8QpmS21lm^+&IM#;pTY=@<>Thsj!#WF2kDiq)E#F=e5&VG60 ztW-Sb#O?uO8DH_!lh7LC@{LpU+2^21$;D7<;BBtt^4ZuPo}+RsMmgT>qT)a zo}tc_YQ<l^FFR$+(*d;^XBjWjHK<*y+&hrgnnMW<4 z-a_d<8zdfOB`Eu5H&N8^OV_ps@&U4UX8|vEFGu=#SZsm62482eRB?y>2YFyka{x9( zRqkf@RrG606II&Mr@`t>wp(@XuP2;5^0*#R29yW}`$AI_g;y+gWl7?9QI* zPi!|H%bnQkgN02`qQK3+)M@9`rMQ;wYizl;Kj_NfWM{{lWKD( z>b(=@kF-sg#|1cL4Wvzl-YjQEnN>=WgxcJ!**-RYnz*1nO{@W(HL(I{`e|6Gy{J@(&@Q_D#?ECj?l~2w5XxaWo$Oykfi3@SOMV zF43O+;xtRAJk!LJ{z_?rm?C}z9mbDwFw2Mdg4i(jweW$T>fDAkjp`k~?NIJVJ^7%y zS4jp>(_9VsfGlt?{8ON8c_>>RaRl?>wJ!kv!(%OEtvSd0xWsaSdyzTA+u*p}OKmY0 zp5_ue^CZ@|Q9`>-gRNQP%<7Lk{ZA08gfMZKud}+*Q;pn-n$(DfGn6U=?L?#FvLrm4 zEG2j(JpCs$oHVZk(<8J7^u2zn@ZkYR39M>MFlrLSt3H-a@_4=GbL>QaB59yAG)la~ zb9?n99I2ibgTn`dQc=zg@S7 zy-B+zwa-6Jsi>bM_%bi*g+-$Fd3861TE7S?zCe5pbDY$NG}bS@1M+jq7pU>3xtII{ z@-s>&>PD+kabeNtNx$&*+G+PfPSZD4{Ox!J9mMPhp)l` zO)7teKSWl2Vc&*THKP^^k6KI#;hd%S4hRtucC&jr?{6NtcLT4A_YCulxhfGKynfz*}rQ&?WON!n7 z#|v7E0URuc?L`mEm<3LPx+i}814UE#nZGn$6V&a_&i?|vK(D2pX{@%zKU?fpS3ugW zjD@C1NcN$n<2!{{x#tN}$OP&lx)U}(>1UoGD{Du9@RE~=J11Rtu#M@H3}))LGKqF(_-c!+yKxgNE?H+A-NRq+u!k^ z)QFgki20J53&~C2M=o_}uOMW22Kf3r|3s%Q7FMQ>h!I{Ji`%%F$&l|fZws8GJMgje z7L9cc#;Ml>&np&g7k5avJG}nz#Tbu-|25d;i0VWycT{4`FLn}YJIkZBCMe73>w3%= zn!SzivrV(csgUFWF1K-hgnt=Y zlT@Oh{8xAnWPzuk>kOaiszx$oFsjU-2b$;}iFr&m*W6lQ%g4bdYBJ`)!+ohHHZkU0 z(xbwo(8oCuGp|pc4L&_Hk&g}?e0256Q-^qq$s?p4*qkOcZtnFrZ|?C^9^~T`IE=_; zdAlG9Xj zYZ&EyRjPv>xBw2dfkky$#iBJ9XWg1gAV#A;oCTolIC!q5{0ffV20xMDQ{6(YdCCKS zgy<cnDNr3D3Msqd35p(A}JMmidH$0fAyDAuj!Pi;(G zo{vJp*nV$#P5BdM3>|dRK=DBuK{{|~ZFp@OfwmkYo%#>xyy3D$pA#CW{|avGX(HZ~ zj6|t2MSUyT6VrE5FZ`d#%oHT=vNzT))enw!66u-ULi0mtn0qa>fuvsAi1tTKUxD+vG$$#!lPtw}$WR;(mC?>7ymvbT>1(}KTjYcu>+dFnf%0k~v9vp-6<2RKh zjNhTi@dGH$EjYhf4N02lPqN?@a`>lVo~k0p0QJ{(jAi$p<7UxZuqTw>deNWlzT^i_ zFcm9R=eW$#rxf|?Al4?x?5be{!WKTIaO5M##5MNG=P>gq@>keZg)yq{(ezPyHJ}H- zi8?BI^z)gyC3v})4<<0y2qRRiFg!L9#huske-xF~QGQeLA$kg9fz|^?xn;bb+VtQ2_{TTw@vFm{eSE(alnw5UFjwy8dmiX_o_9MXqw^hwGCdXM0< z(s$4FI;3$xVxM;13--VGko-(pg|@%e3R#>x2rLcqRY85BKr!~j#%V;Ik0cR9d#T!v zb&o@Pj!J$))#2jyOc$HG_53c!j*jC=%wz{|;rD%%LLDQ}$zvU%5BAO^eXi|~>L$iO z$0U^21ljARG&eYrERA2?C%Yko4}CL({P`A<6#p4fowIJCF&G8@cb~kXzrYU7L7Cgt z;^7V{vUP}Ly^4HdFvr8gUPLnovoN$&bXyeo10y_o;CRq(sV+}yUPT&Od0yIYGjnRG zrYBvGZ|^#u_?c3BJb8X$H@HleZ-T=)76=SjVAnKyK5=VL5yfZjlYcp2M+x>r4!^?4 zEaNhpSH%ObCJ1zf2FS-&I9bkn-X+Qkl%NtNIPTY>xAtMpruv9cwNnM%H|pFu2V;!J z*{`1f-|fS_GYKhWo{ITRdJ_8iaVCA4eqIu|^vj%zyr&Pi*=cd6Bqw=`k>&9np+!+_ zWEboDcn8xP5nk7!W&ys9aG8i#LKrr2qPd;WYLxPzJmszU_Mm(+nApYlj23`b5WB2| zT|VaC-=o(AA;r2!(XTyRVmPHau?(0Lq;c@=z-%@ADdbbc-+)GCil$o!1vplC26b4G z#Ii~y!p~ySOBCHMaMHVV!s{J+c=;8ImzVtb+~Av!O%g=k z1^%6P;6I6WgS_`c=`V`Gw8uf?WbWx0=|oaqag5UfR1;v~I959GKFl)Ao>=rD&Zu-^ z5GXCNz|`TVi%UgW{^!tm3HX=ZEb)FZraM}D26X!~&JnQQF_=DdrWQY6GTxbW`ju`$ z*gvNEE?}HM_Cz@fMRswGGbpD9ANNxk>me=O?tQ-G?1{tO(Sp%?r}&EBB3(|b1kM>N zN^@3HS!@E%6R0e6lO9=liondu@R7tFb`;I}N}5%3Ry)6s92^v4LdMQATo5nBXEf<}n(QoM@)!uhb` zy+sHyiB1|fn87#fip=M+PQI+Dh?&apg!-22Xfbr@c<6=b%f1|~FFhaROQPnR646>h zNjHZ+J}GEZHQs zuM;(gglEBrQqo{or#Tdq>xXneE;E8E56W*~MQ~D%dtnz77^4V#$)+PHZy3sV8sGtf zXpMei4Y zq)v@X$R()#GKy1&?`Iu%MV`aX(~SPoHVr##ZWnuY*J1j)^9nSH_l~X2$>F?NIj|Yd z;~LE(w+H)wB_}7&*V4nvqfZCrEx|MkSxqwgkK0Fev5as0{h+5n(^$1-y6o6qf|uRrJ4 zfH%W3gFb`!PqgkZ(ID+=x76BT&|g`*Iz<3V#?q{@K3+7MoA90#rOm_V8%^23Rx(&}SQ9PK({5O>FV7N}@rY4*#bkoSr3wD0+zy^& z9VoQP-nh&uYE9{5$4<_lGvc}u_xX^()p=pVRezZ^Fi`X-oz^}<<&WL zU|Gp{;=Jie`2HeCWp|O_ni~#Tp#0xreD|#Irf1jFEpDHW@`X!<5={=5>gn%F6&Qb0 zD%ac>zl~i}_|0hG2*fU*0IUADF1@#=?UGpb(xs9g4P5&3k8ZnUOd5S@R}8HgWY5pk zUto45EF_Mp{udf?Cs4xdfjajN$7pfx>DK=3mmQvWpx>O{&jQcbpR?O9a)#aZ@{6NO zErz#(%C&L*j}PgCxBa2$B3Gmf4)1y1f%PO&*Aue217o&5@33FKZ5w~b%;M!qFSQ;H zDgk}am{)vJC}zRoLkCNuPtO{|u*Y`?wA&7r*e~#IlJ{U8mvnNp_dd_z?&16c z-CR^uzhU6AeF4y9WIO6<0vjK$(@C(|hy`BVg|8CD!LN!+zC6*^%@41}u9PeI%&!+u z_?_ZY{%vBrf3^6q-v(@x*L!%w^x_0R^_g_cbJVw^uyPoWMw|2t6zmW%wkwlO7VJ{| z1IZ>u{s+7u2=gsKYzF72!wnx|r*J^`+XTrp_A#JXU7ga>A_lu97B~v;?|Q5e`+y%y z_R2&9AIANCeGKv4_zyp?fehO~epawfke`)B@QD%)Ud9NV>*u8f4lPQ%Vgz2IZa&?? zJc@HFE4*bdHR50_#AU1HKzW_CKQ9k(>TZe!Qs1Dr?g4E>Pc%0@8EPkVSn%c4y!79) zt~h9&!^548r1C|n!?TD77Ig}K(fdu%)eCcBr@VnL??GNy5iij2W?hul_uchz_^?LF za9rp2F(X@HDNCOpP`RC>8{X}WW#5;Q4WoY8=E(9s_e2}`Rh-F?p$)RM?@Ob8Sa=yG ztqzmdahXGyijb~TyoKtuzGGR&%G6kKQ)*0cqhs_5$SP}=raao7oxa_X;^I#4Nx9Eu zPYQr5_o@Hhj0^sy8GirwGrk1H!4z)omq5PwC)%0H^a9$0D?7eat;er>#=%FuRRAIf zSE|V8fL)Ost6|}LoO`NY*7fQejB|(l z`9Sz)HsvV%sC=*Jic3gvPAQu*KUos4ZcPxbKDu}x--zDT7Mz6iM}s-|Zw+#$YjQcX z7xJZRa@kO3*$WP6yQS0J;-Lwkf%2W#rNC;$-phhe6HJ2z_i$eZaGTBGiA4Lp zh2^*I6!k|op`_?_M?pun7>yoW1v&WA_Itqn-ZexyCJ&7$%f`sbL!P66q#j=yl#_;d z^9Y2ufiu9;u_qe>g+HXgO1mMSoA8dm0exkmoh@~v-l$*GH%GNZwKRe3c?rjT|E2wt zYO@WxVLlE4Jhr7pLYXPB)lR+b2-490o=?@5X-f?qdNXI{0~&seQ)9j+v$yFv?fXei z&Xr!1WsEhthEI930Bq-L@})r@IE6`0_!`6gM9M1?_wyTZp$Ukzrz6)IYj{OZI7MH* za!|;L24(_dhK_XD)iLv8Xu^ys(w2=uJuDt1%@JBLgZ@n@KYc4~nMM6k9<(St=YZcY z%LNMdONwTmwk$^CuXL?UfPcG=-twOg`3n=7LBXvo!^hbxCAVe1Ee)gDnu9mZfG6}zK zyF>9Mih6Lwhk?Vd9F4Ogr_~x@dWqr>RNUkTQ4ZMlK}IxBEo(z43VesTD)#97kj64Y zqMWL~(^RY`UfZuL3wd{?0K(B6bh&h{qsAy9jCeOJhW{Q{+< ztNCi2yVUP29x%K~lD~e4WCX^=8q7&zJDy)2I$Z1!TSVSsz#b$dUmQABGQ~@M zp`*9vr}rwRS@6VUyH`m;s6m*BP>*mVLIXl0LL)*0!jTB|2on)%5T@9!=R-MvJfwfK zQi_IkF!iwGHKV=E!lMr9*>8t1X7X6tNcHLvW)Hf@atg7J9+I0Pd^SBScYjN@p}j;q z^c(Sa4NZ~mK}q*I@_ZX`mV&cMzE+$i;w;m58D}GLmhPKUeGf{!*D=nw0cQrBCHY!$ zrpMVR-({R>aAt&dm1$bIlrcWj?LR2_v+w5jEmie*HCob+*JyvOw$g9>*0hkhUrvNv zQM-W3FT)nk%0hB4G+!#Q>c37pFTn9(K)W`8xNm-n(6s&_3(UQDxqhO~C30Cckl;dy91E!26VfbBr<{8`B4`L;;4 zk4XDU)p4%HP-PvI_r=jHy<)>do2TVGl#B3wsba&b)fEq~-889^&0Dp$%Cw|n;~EBC zy~vN`BOK027NWf=T#9RoLh>bgzB_VV9|^11Rc~U2E|+6N^|}Y{cCqRW>vM73|M0r1 zT31!&rm9ud8#mycv)0wDt9YpD4!rUm%IDKDz!i#wLL}7TK|Ah8;hI7{u78g(Ch|NQ z*OV?cat%q3dOa)>Zmil+S+j1&3{%OXMdgcTmMPzze3nE1$q)tKoz=;cL|KstX z1rWM37^C*57r+a>nqeG;pX0qogy?NNh4lgxC`3jYgr@ybOiR}v>Y0v0<8T&*ka*R4 zy52p4MI&^chaM?HcYP|0Mc9-vo*57xe=mm(L)i7eR5lzTi=NHm5LT~S&DK?{W7c(O z@JzIgikmmND2$^w;9zW5>L1^F525|b7i`ZWWCwO9-ACcqn@cB9Xu7}Yav?%i_3<$^ ztpA_K)zBNiSPfAYHDnKdZKrsw;cz&#Zw-fRUL+i8-}^lM+UNXennX^ z`?6Ub8@HY-AN4S`bvC=vdu`NLwpO73N$5XLraxC1?qi=;G5z9|;q&XUF-S93A;Ho0 zOyA>Tde26te+A*(2Vtjzueb%~g`sZNJN#)Lpgrg$&`w-9Z@Y*%( zp+z-p;nIAQV}S{$rlOj4n>N&}T~@WBrgWOAyyQ+6k50s+$zC6y4#$%mFK}v^sEtt` z%A0-w64H0$2wySw&&YYk7N)0z?&n0pjE%V&8(Hp6HIY4e^3*9d+l@Q;S{qxu + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ +#pragma once + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/* Clocking *************************************************************************/ +/* The board provides the following clock sources: + * + * X1: 24 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed internal oscillator + * HSE: 24 MHz crystal for HSE + */ +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 0 + +#define STM32_HSEBYP_ENABLE 1 + +/* Main PLL Configuration. + * + * PLL source is HSE = 24,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (24,000,000 / 2) * 80 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE|RCC_PLLCFGR_PLL1RGE_4_8_MHZ|RCC_PLLCFGR_DIVP1EN|RCC_PLLCFGR_DIVQ1EN|RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(2) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(80) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 80) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE|RCC_PLLCFGR_PLL2RGE_4_8_MHZ|RCC_PLLCFGR_DIVP2EN|RCC_PLLCFGR_DIVQ2EN|RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(4) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(32) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE|RCC_PLLCFGR_PLL3RGE_4_8_MHZ|RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(4) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(32) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * Note: look at Table 54 in ST Manual + */ +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI /* I2C123 clock source */ +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI /* I2C4 clock source */ +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */ +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */ +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */ +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */ +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + +/* FLASH wait states */ +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + + +/* UART/USART */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ + +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_CTS GPIO_USART3_CTS_NSS_2 /* PD11 */ +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 /* PD12 */ + +#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ +#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ + +#define GPIO_USART6_TX 0 /* USART6 is RX-only */ +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ + +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ + +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ + + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ + +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 /* PB12 */ +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* PB13 */ + + +/* SPI */ +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ + +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_3) /* PB10 */ +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ + +#define GPIO_SPI5_SCK ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7 */ +#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ +#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */ + + +/* I2C */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */ +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ + +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4 /* PB6 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4 /* PB7 */ diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h new file mode 100755 index 000000000000..99a2fe3ba9b9 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// DMAMUX1 +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_1 /* DMA1:71 */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_1 /* DMA1:72 */ + +#define DMAMAP_SPI5_RX DMAMAP_DMA12_SPI5RX_0 /* DMA1:83 */ +#define DMAMAP_SPI5_TX DMAMAP_DMA12_SPI5TX_0 /* DMA1:84 */ diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..c9ec46ed878e --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig @@ -0,0 +1,260 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1124 +CONFIG_CDCACM_PRODUCTSTR="3DRControlZeroH7OEM_revG" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3DR" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_STM32H7_PWR_EXTERNAL_SOURCE_SUPPLY=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDMMC1_SDIO_PULLUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C3=y +CONFIG_STM32H7_I2C4=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI5=y +CONFIG_STM32H7_SPI5_DMA=y +CONFIG_STM32H7_SPI5_DMA_BUFFER=1024 +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM2=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM8=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGTSTP=y +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_SERIAL_CONSOLE=y +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=3000 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld new file mode 100755 index 000000000000..3fb4cc1f33ce --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,221 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The board uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 128K + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > AXI_SRAM AT > FLASH + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } + + .ramfunc : { + _sramfuncs = .; + *(.ramfunc .ramfunc.*) + . = ALIGN(4); + _eramfuncs = .; + } > ITCM_RAM AT > FLASH + + _framfuncs = LOADADDR(.ramfunc); +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld new file mode 100755 index 000000000000..02e763a790fb --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2021 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The board uses an STM32H743XIH6 and has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt b/boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt new file mode 100755 index 000000000000..6a7d2ce306f7 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt @@ -0,0 +1,65 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + spi.cpp + timer_config.cpp + usb.c + ) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h b/boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h new file mode 100755 index 000000000000..bcc8bba1a619 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h @@ -0,0 +1,187 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * Board internal definitions + */ + +#pragma once + +#include +#include +#include +#include + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ +#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) +#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* ADC channels */ +#define PX4_ADC_GPIO \ + /* PA2 */ GPIO_ADC12_INP14, \ + /* PA3 */ GPIO_ADC12_INP15, \ + /* PA4 */ GPIO_ADC12_INP18, \ + /* PC1 */ GPIO_ADC123_INP11 + +/* Define Channel numbers must match above GPIO pins */ +#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */ +#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */ +#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */ +#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */ + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_RC_RSSI_CHANNEL)) + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* CAN Silence: Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) + +/* PWM */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +/* Power supply control and monitoring GPIOs */ +#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) + +#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define BOARD_NUMBER_BRICKS 1 + +#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_VDD_1V2_CORE_POWER_EN /* PH5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN5) + +/* Define True logic Power Control in arch agnostic form */ +#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true)) +#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */ + +#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 + +/* USB OTG FS */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer3 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1 + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS3" + +#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) + +/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ +#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ + +/* Power switch controls ******************************************************/ +#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true) + +/* + * Board has a separate RC_IN + * + * GPIO PPM_IN on PB0 T3CH3 + * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 + * Inversion is possible in the UART and can drive GPIO_PPM_IN as an output + */ +#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) +#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) +#define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED +#define BOARD_ADC_SERVO_VALID (1) /* never powers off the Servo rail */ +#define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID)) + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ +#define BOARD_HAS_ON_RESET 1 + +#define BOARD_HAS_STATIC_MANIFEST 1 + + +#define BOARD_NUM_IO_TIMERS 3 + + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN2_TX, \ + GPIO_CAN2_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_nPOWER_IN_A, \ + GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_VDD_1V2_CORE_POWER_EN, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_SAFETY_SWITCH_IN, \ + GPIO_OTGFS_VBUS, \ + } + +__BEGIN_DECLS +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); +extern void board_peripheral_reset(int ms); + +#include +#endif /* __ASSEMBLY__ */ +__END_DECLS diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c new file mode 100755 index 000000000000..bb6b4dc23aad --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_configgpio(GPIO_OTGFS_VBUS); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h b/boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h new file mode 100755 index 000000000000..b212ccd60273 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1124 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define OSC_FREQ 24 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_RED +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp b/boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp new file mode 100755 index 000000000000..1b8927c69939 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(3), + initI2CBusExternal(4), +}; diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c new file mode 100755 index 000000000000..daaace48fb2f --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file init.c + * + * board-specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +#include "board_config.h" + +#include + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN(); + /* Keep Spektum on to discharge rail*/ + VDD_3V3_SPEKTRUM_POWER_EN(false); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + VDD_3V3_SPEKTRUM_POWER_EN(last); +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ +__EXPORT void stm32_boardinitialize(void) +{ + /* Reset PWM first thing */ + board_on_reset(-1); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + board_control_spi_sensors_power_configgpio(); + + /* configure LEDs */ + board_autoled_initialize(); +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ +__EXPORT int board_app_initialize(uintptr_t arg) +{ + /* Power on Interfaces */ + board_control_spi_sensors_power(true, 0xffff); + VDD_3V3_SPEKTRUM_POWER_EN(true); + + px4_platform_init(); + + stm32_spiinitialize(); + + /* configure the DMA allocator */ + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_on(LED_GREEN); // Indicate Power. + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + +#ifdef CONFIG_MMCSD + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + struct sdio_dev_s *sdio_dev = sdio_initialize(0); // SDIO_SLOTNO 0 Only one slot + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); + } + + if (mmcsd_slotinitialize(0, sdio_dev) != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); + } + + /* Assume that the SD card is inserted. What choice do we have? */ + sdio_mediachange(sdio_dev, true); +#endif /* CONFIG_MMCSD */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/led.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/led.c new file mode 100755 index 000000000000..0e7beb3a9e3b --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/led.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED + GPIO_nLED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(led, !phy_get_led(led)); +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp b/boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp new file mode 100755 index 000000000000..4a4c3502bbd9 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + initSPIDevice(DRV_BARO_DEVTYPE_DPS310, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp b/boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp new file mode 100755 index 000000000000..e23880491444 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer8, DMA{DMA::Index1}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel1}, {GPIO::PortI, GPIO::Pin5}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel2}, {GPIO::PortI, GPIO::Pin6}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c new file mode 100755 index 000000000000..fe32ce71ac63 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file usb.c + * + * Board-specific USB functions. + */ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + + + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} From af0129dab7521bff7807feb0bfe5a7f338f7975c Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Wed, 14 Aug 2024 07:42:20 -0700 Subject: [PATCH 18/19] github: update bug report template Removes unnecessary required fields --- .github/ISSUE_TEMPLATE/bug_report.yml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml index 60d93ea9b811..ffbb271a900f 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.yml +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -20,14 +20,14 @@ body: 3. Took off '....' 4. See error validations: - required: true + required: false - type: textarea attributes: label: Expected behavior description: A clear and concise description of what you expected to happen. validations: - required: true + required: false - type: textarea attributes: @@ -45,7 +45,7 @@ body: placeholder: | # PASTE HERE THE LINK TO THE LOG validations: - required: true + required: false - type: markdown attributes: @@ -60,14 +60,14 @@ body: placeholder: | # If you don't know the version, paste the output of `ver all` in the MAVLink Shell of QGC validations: - required: true + required: false - type: input attributes: label: Flight controller description: Specify your flight controller model (what type is it, where was it bought from, ...). validations: - required: true + required: false - type: dropdown attributes: From 2a124fd99898166637e6bea3e3123aa6bfe176ef Mon Sep 17 00:00:00 2001 From: Vilius Date: Thu, 15 Aug 2024 12:29:02 +0300 Subject: [PATCH 19/19] Add Bosch BMM350 magnetometer (#23362) * Add Bosch BMM350 magnetometer * BMM350 replace info messages with debug messages * BMM350 update measurement interval * BMM350 fix offsets, update based on review * BMM350 Update default parameters to 50Hz * Update OTP Word LSB check * BMM350 fix styles and formatting * BMM350 update error checks --- src/drivers/drv_sensor.h | 2 + src/drivers/magnetometer/bosch/CMakeLists.txt | 1 + .../magnetometer/bosch/bmm350/BMM350.cpp | 773 ++++++++++++++++++ .../magnetometer/bosch/bmm350/BMM350.hpp | 210 +++++ .../bosch/bmm350/Bosch_BMM350_registers.hpp | 159 ++++ .../magnetometer/bosch/bmm350/CMakeLists.txt | 48 ++ src/drivers/magnetometer/bosch/bmm350/Kconfig | 5 + .../magnetometer/bosch/bmm350/bmm350_main.cpp | 89 ++ .../magnetometer/bosch/bmm350/module.yaml | 44 + 9 files changed, 1331 insertions(+) create mode 100755 src/drivers/magnetometer/bosch/bmm350/BMM350.cpp create mode 100644 src/drivers/magnetometer/bosch/bmm350/BMM350.hpp create mode 100644 src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp create mode 100644 src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt create mode 100644 src/drivers/magnetometer/bosch/bmm350/Kconfig create mode 100644 src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp create mode 100644 src/drivers/magnetometer/bosch/bmm350/module.yaml diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 378c92b6736c..53eecce4be71 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -242,6 +242,8 @@ #define DRV_INS_DEVTYPE_VN300 0xE3 #define DRV_DIFF_PRESS_DEVTYPE_ASP5033 0xE4 +#define DRV_MAG_DEVTYPE_BMM350 0xE5 + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/magnetometer/bosch/CMakeLists.txt b/src/drivers/magnetometer/bosch/CMakeLists.txt index b814e7224fb1..d7e848ec86a4 100644 --- a/src/drivers/magnetometer/bosch/CMakeLists.txt +++ b/src/drivers/magnetometer/bosch/CMakeLists.txt @@ -32,3 +32,4 @@ ############################################################################ add_subdirectory(bmm150) +add_subdirectory(bmm350) diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp new file mode 100755 index 000000000000..e4e8e19f9c5e --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -0,0 +1,773 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BMM350.hpp" +using namespace time_literals; + +BMM350::BMM350(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + ModuleParams(nullptr), + _px4_mag(get_device_id(), config.rotation) + +{ +} + +BMM350::~BMM350() +{ + perf_free(_reset_perf); + perf_free(_bad_read_perf); + perf_free(_self_test_failed_perf); +} + +int BMM350::init() +{ + ModuleParams::updateParams(); + ParametersUpdate(true); + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool BMM350::Reset() +{ + RegisterWrite(Register::CMD, SOFT_RESET); + _state = STATE::RESET; + ScheduleClear(); + ScheduleDelayed(1_ms); + return true; +} + +void BMM350::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_bad_read_perf); + perf_print_counter(_self_test_failed_perf); +} + +void BMM350::ParametersUpdate(bool force) +{ + if (_parameter_update_sub.updated() || force) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + updateParams(); + UpdateMagParams(); + } +} + +void BMM350::UpdateMagParams() +{ + uint8_t odr = GetODR(_param_bmm350_odr.get()); + uint8_t avg = GetAVG(_param_bmm350_avg.get()); + + _mag_odr_mode = odr; + _mag_avg_mode = avg; + _mag_pad_drive = static_cast(_param_bmm350_drive.get()); + PX4_DEBUG("Set params odr = %d, avg = %d, drive = %d", _mag_odr_mode, _mag_avg_mode, _mag_pad_drive); +} + +uint8_t BMM350::GetODR(int value) +{ + switch (value) { + case 0: return ODR_400HZ; + + case 1: return ODR_200HZ; + + case 2: return ODR_100HZ; + + case 3: return ODR_50HZ; + + case 4: return ODR_25HZ; + + case 5: return ODR_12_5HZ; + + case 6: return ODR_6_25HZ; + + case 7: return ODR_3_125HZ; + + case 8: return ODR_1_5625HZ; + + default: return ODR_200HZ; + } +} + +hrt_abstime BMM350::OdrToUs(uint8_t odr) +{ + switch (odr) { + case ODR_400HZ: + return 2500_us; + + case ODR_200HZ: + return 5000_us; + + case ODR_100HZ: + return 10000_us; + + case ODR_50HZ: + return 20000_us; + + case ODR_25HZ: + return 40000_us; + + case ODR_12_5HZ: + return 80000_us; + + case ODR_6_25HZ: + return 160000_us; + + case ODR_3_125HZ: + return 320000_us; + + case ODR_1_5625HZ: + return 640000_us; + + default: + return 5000_us; + } +} + +uint8_t BMM350::GetAVG(int value) +{ + switch (value) { + case 0: return AVG_NO_AVG; + + case 1: return AVG_2; + + case 2: return AVG_4; + + case 3: return AVG_8; + + default: return AVG_2; + } +} + + +int BMM350::probe() +{ + for (int i = 0; i < 3; i++) { + uint8_t chip_id; + + if (PX4_OK == RegisterRead(Register::CHIP_ID, &chip_id)) { + PX4_DEBUG("CHIP_ID: 0x%02hhX", chip_id); + + if (chip_id == chip_identification_number) { + return PX4_OK; + } + } + } + + return PX4_ERROR; +} + +void BMM350::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + int ret = PX4_OK; + ParametersUpdate(); + + switch (_state) { + case STATE::RESET: { + if (RegisterWrite(Register::CMD, SOFT_RESET) == PX4_OK) { + _reset_timestamp = now; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + } + + ScheduleDelayed(10_ms); + } + break; + + case STATE::WAIT_FOR_RESET: { + ret = probe(); + + if (ret == PX4_OK) { + _state = STATE::RESET; + uint8_t status_0; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + + if (ret == PX4_OK && (status_0 & PWR_NORMAL) != 0) { + ret = PX4_ERROR; + } + + if (ret == PX4_OK) { + ret = UpdateMagOffsets(); + } + + if (ret == PX4_OK) { + PX4_DEBUG("Going to FGR"); + _state = STATE::FGR; + + } + + ScheduleDelayed(10_ms); + + } else { + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + } + break; + + case STATE::FGR: { + _state = STATE::RESET; + uint8_t odr_reg_data = (ODR_100HZ & 0xf); + odr_reg_data = ((odr_reg_data & ~(0x30)) | ((AVG_2 << 0x4) & 0x30)); + ret = RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); + + if (ret == PX4_OK) { + ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, EN_XYZ); + } + + if (ret == PX4_OK) { + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FGR); + } + + if (ret == PX4_OK) { + PX4_DEBUG("Going to BR"); + _state = STATE::BR; + } + + ScheduleDelayed(30_ms); + } + break; + + case STATE::BR: { + uint8_t status_0; + _state = STATE::RESET; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + + if (ret == PX4_OK && PMU_CMD_STATUS_0_RES(status_0) == PMU_STATUS_FGR) { + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_BR_FAST); + + if (ret == PX4_OK) { + PX4_DEBUG("Going to after reset"); + _state = STATE::AFTER_RESET; + } + } + + ScheduleDelayed(4_ms); + } + break; + + case STATE::AFTER_RESET: { + uint8_t status_0; + _state = STATE::RESET; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + + if (ret == PX4_OK && PMU_CMD_STATUS_0_RES(status_0) == PMU_STATUS_BR_FAST) { + _state = STATE::MEASURE_FORCED; + _self_test_state = SELF_TEST_STATE::INIT; + PX4_DEBUG("Going to fast FM"); + } + + ScheduleNow(); + } + break; + + case STATE::MEASURE_FORCED: { + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); + + if (ret == PX4_OK) { + _state = STATE::SELF_TEST_CHECK; + + } else { + _state = STATE::RESET; + } + + ScheduleDelayed(OdrToUs(_mag_odr_mode)); + break; + } + + case STATE::SELF_TEST_CHECK: { + + float xyzt[4]; + _state = STATE::RESET; + + if (ReadOutRawData(xyzt) != PX4_OK) { + perf_count(_self_test_failed_perf); + ScheduleNow(); + break; + } + + switch (_self_test_state) { + case SELF_TEST_STATE::INIT: + memcpy(_initial_self_test_values, xyzt, sizeof(_initial_self_test_values)); + + if (RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_X) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::POS_X; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::POS_X: + if (xyzt[0] - _initial_self_test_values[0] >= 130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_X) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::NEG_X; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::NEG_X: + if (xyzt[0] - _initial_self_test_values[0] <= -130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_Y) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::POS_Y; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::POS_Y: + if (xyzt[1] - _initial_self_test_values[1] >= 130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_Y) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::NEG_Y; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::NEG_Y: + if (xyzt[1] - _initial_self_test_values[1] <= -130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_DISABLE) == PX4_OK) { + PX4_DEBUG("Self test good, going to configure"); + _state = STATE::CONFIGURE; + } + + break; + + } + + if (_state == STATE::RESET) { + PX4_DEBUG("Self test failed"); + perf_count(_self_test_failed_perf); + } + + ScheduleDelayed(1_ms); + } + break; + + + case STATE::CONFIGURE: + if (Configure() == PX4_OK) { + _state = STATE::READ; + PX4_DEBUG("Configure went fine"); + ScheduleOnInterval(OdrToUs(_mag_odr_mode), 50_ms); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + // -- start get_compensated_mag_xyz_temp_data + float out_data[4] = {0.0f}; + float dut_offset_coeff[3], dut_sensit_coeff[3], dut_tcos[3], dut_tcss[3]; + float cr_ax_comp_x, cr_ax_comp_y, cr_ax_comp_z; + + ret = ReadOutRawData(out_data); + + if (ret == PX4_OK) { + // Apply compensation to temperature reading + out_data[3] = (1 + _mag_comp_vals.dut_sensit_coef.t_sens) * out_data[3] + + _mag_comp_vals.dut_offset_coef.t_offs; + + // Store magnetic compensation structure to an array + dut_offset_coeff[0] = _mag_comp_vals.dut_offset_coef.offset_x; + dut_offset_coeff[1] = _mag_comp_vals.dut_offset_coef.offset_y; + dut_offset_coeff[2] = _mag_comp_vals.dut_offset_coef.offset_z; + + dut_sensit_coeff[0] = _mag_comp_vals.dut_sensit_coef.sens_x; + dut_sensit_coeff[1] = _mag_comp_vals.dut_sensit_coef.sens_y; + dut_sensit_coeff[2] = _mag_comp_vals.dut_sensit_coef.sens_z; + + dut_tcos[0] = _mag_comp_vals.dut_tco.tco_x; + dut_tcos[1] = _mag_comp_vals.dut_tco.tco_y; + dut_tcos[2] = _mag_comp_vals.dut_tco.tco_z; + + dut_tcss[0] = _mag_comp_vals.dut_tcs.tcs_x; + dut_tcss[1] = _mag_comp_vals.dut_tcs.tcs_y; + dut_tcss[2] = _mag_comp_vals.dut_tcs.tcs_z; + + for (int idx = 0; idx < 3; idx++) { + out_data[idx] *= 1 + dut_sensit_coeff[idx]; + out_data[idx] += dut_offset_coeff[idx]; + out_data[idx] += dut_tcos[idx] * (out_data[3] - _mag_comp_vals.dut_t0); + out_data[idx] /= 1 + dut_tcss[idx] * (out_data[3] - _mag_comp_vals.dut_t0); + } + + cr_ax_comp_x = (out_data[0] - _mag_comp_vals.cross_axis.cross_x_y * out_data[1]) / + (1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y); + cr_ax_comp_y = (out_data[1] - _mag_comp_vals.cross_axis.cross_y_x * out_data[0]) / + (1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y); + cr_ax_comp_z = + (out_data[2] + + (out_data[0] * + (_mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_z_y - + _mag_comp_vals.cross_axis.cross_z_x) - + out_data[1] * + (_mag_comp_vals.cross_axis.cross_z_y - _mag_comp_vals.cross_axis.cross_x_y * + _mag_comp_vals.cross_axis.cross_z_x)) / + (1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y)); + + out_data[0] = cr_ax_comp_x; + out_data[1] = cr_ax_comp_y; + out_data[2] = cr_ax_comp_z; + _px4_mag.set_error_count(perf_event_count(_bad_read_perf) + perf_event_count(_self_test_failed_perf)); + _px4_mag.set_temperature(out_data[3]); + _px4_mag.update(now, out_data[0], out_data[1], out_data[2]); + + } else { + perf_count(_bad_read_perf); + } + } + + break; + } +} + +int BMM350::Configure() +{ + PX4_DEBUG("Configuring"); + uint8_t readData = 0; + int ret; + + // Set pad drive + ret = RegisterWrite(Register::PAD_CTRL, (_mag_pad_drive & 0x7)); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't set pad drive, defaults to 7"); + return ret; + } + + // Set PMU data aggregation + uint8_t odr = _mag_odr_mode; + uint8_t avg = _mag_avg_mode; + + if (odr == ODR_400HZ && avg >= AVG_2) { + avg = AVG_NO_AVG; + + } else if (odr == ODR_200HZ && avg >= AVG_4) { + avg = AVG_2; + + } else if (odr == ODR_100HZ && avg >= AVG_8) { + avg = AVG_4; + } + + uint8_t odr_reg_data = (odr & 0xf); + odr_reg_data = ((odr_reg_data & ~(0x30)) | ((avg << 0x4) & 0x30)); + + ret = RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Cannot set PMU AGG REG"); + return ret; + } + + ret = RegisterRead(Register::PMU_CMD_AGGR_SET, &readData); + + if (ret != PX4_OK || readData != odr_reg_data) { + PX4_DEBUG("Couldn't check PMU AGGR REG"); + return ret; + } + + odr_reg_data = PMU_CMD_UPDATE_OAE; + ret = RegisterWrite(Register::PMU_CMD, odr_reg_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't write PMU CMD REG"); + return ret; + } + + ret = RegisterRead(Register::PMU_CMD, &readData); + + if (ret != PX4_OK || readData != odr_reg_data) { + PX4_DEBUG("Couldn't check PMU CMD REG"); + return ret; + } + + // Enable AXIS + uint8_t axis_data = EN_X | EN_Y | EN_Z; + // PMU_CMD_AXIS_EN + ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, axis_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't write AXIS data"); + return ret; + } + + ret = RegisterRead(Register::PMU_CMD_AXIS_EN, &readData); + + if (ret != PX4_OK || readData != axis_data) { + PX4_DEBUG("Couldn't cross check the set AXIS"); + return ret; + } + + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_NM); + + if (ret != PX4_OK) { + PX4_DEBUG("Failed to start mag in normal mode"); + return ret; + } + + // microTesla -> Gauss + _px4_mag.set_scale(0.01f); + + return ret; +} + +int32_t BMM350::FixSign(uint32_t inval, int8_t num_bits) +{ + int32_t power = 1 << (num_bits - 1); // Calculate 2^(num_bits - 1) + int32_t retval = static_cast(inval); + + if (retval >= power) { + retval -= (power << 1); // Equivalent to retval = retval - (power * 2) + } + + return retval; +} + +int BMM350::ReadOutRawData(float *out_data) +{ + if (out_data == NULL) { + return -1; + } + + float temp = 0.0; + struct BMM350::raw_mag_data raw_data = {0}; + + // --- Start read_uncomp_mag_temp_data + uint8_t mag_data[14] = {0}; + + uint32_t raw_mag_x, raw_mag_y, raw_mag_z, raw_temp; + uint8_t cmd = static_cast(Register::DATAX_XLSB); + + uint8_t res = transfer(&cmd, 1, (uint8_t *)&mag_data, sizeof(mag_data)); + + if (res != PX4_OK) { + return -1; + } + + raw_mag_x = mag_data[2] + ((uint32_t)mag_data[3] << 8) + ((uint32_t)mag_data[4] << 16); + raw_mag_y = mag_data[5] + ((uint32_t)mag_data[6] << 8) + ((uint32_t)mag_data[7] << 16); + raw_mag_z = mag_data[8] + ((uint32_t)mag_data[9] << 8) + ((uint32_t)mag_data[10] << 16); + raw_temp = mag_data[11] + ((uint32_t)mag_data[12] << 8) + ((uint32_t)mag_data[13] << 16); + + raw_data.raw_x = FixSign(raw_mag_x, 24); + raw_data.raw_y = FixSign(raw_mag_y, 24); + raw_data.raw_z = FixSign(raw_mag_z, 24); + raw_data.raw_t = FixSign(raw_temp, 24); + // --- End read_uncomp_mag_temp_data + + // --- Start read_out_raw_data + out_data[0] = (float)raw_data.raw_x * lsb_to_utc_degc[0]; + out_data[1] = (float)raw_data.raw_y * lsb_to_utc_degc[1]; + out_data[2] = (float)raw_data.raw_z * lsb_to_utc_degc[2]; + out_data[3] = (float)raw_data.raw_t * lsb_to_utc_degc[3]; + + // Adjust temperature + if (out_data[3] > 0.0f) { + temp = (float)(out_data[3] - (1 * 25.49f)); + + } else if (out_data[3] < 0.0f) { + temp = (float)(out_data[3] - (-1 * 25.49f)); + + } else { + temp = (float)(out_data[3]); + } + + out_data[3] = temp; + + return res; +} + +int BMM350::ReadOTPWord(uint8_t addr, uint16_t *lsb_msb) +{ + uint8_t otp_cmd = OTP_DIR_READ | (addr & OTP_WORD_MSK); + int ret = RegisterWrite(Register::OTP_CMD, otp_cmd); + uint8_t otp_status = 0; + + if (ret == PX4_OK) { + do { + ret = RegisterRead(Register::OTP_STATUS, &otp_status); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + } while (!(otp_status & 0x01)); + + uint8_t msb = 0, lsb = 0; + ret = RegisterRead(Register::OTP_DATA_MSB, &msb); + + if (ret == PX4_OK) { + ret = RegisterRead(Register::OTP_DATA_LSB, &lsb); + + if (ret == PX4_OK) { + *lsb_msb = ((msb << 8) | lsb) & 0xffff; + } + } + } + + return ret; +} + +int BMM350::UpdateMagOffsets() +{ + PX4_DEBUG("DUMPING OTP"); + uint16_t otp_data[32] = {0}; + + for (int idx = 0; idx < 32; idx++) { + if (ReadOTPWord(idx, &otp_data[idx]) != PX4_OK) { + return PX4_ERROR; + } + + PX4_DEBUG("i: %i, val = %i", idx, otp_data[idx]); + } + + if (RegisterWrite(Register::OTP_CMD, PWR_OFF_OTP) != PX4_OK) { + return PX4_ERROR; + } + + PX4_DEBUG("var_id: %i", (otp_data[30] & 0x7f00) >> 9); + + PX4_DEBUG("UPDATING OFFSETS"); + uint16_t off_x_lsb_msb, off_y_lsb_msb, off_z_lsb_msb, t_off; + uint8_t sens_x, sens_y, sens_z, t_sens; + uint8_t tco_x, tco_y, tco_z; + uint8_t tcs_x, tcs_y, tcs_z; + uint8_t cross_x_y, cross_y_x, cross_z_x, cross_z_y; + + off_x_lsb_msb = otp_data[0x0E] & 0x0FFF; + off_y_lsb_msb = ((otp_data[0x0E] & 0xF000) >> 4) + + (otp_data[0x0F] & 0x00FF); + off_z_lsb_msb = (otp_data[0x0F] & 0x0F00) + + (otp_data[0x10] & 0x00FF); + t_off = otp_data[0x0D] & 0x00FF; + + _mag_comp_vals.dut_offset_coef.offset_x = FixSign(off_x_lsb_msb, 12); + _mag_comp_vals.dut_offset_coef.offset_y = FixSign(off_y_lsb_msb, 12); + _mag_comp_vals.dut_offset_coef.offset_z = FixSign(off_z_lsb_msb, 12); + _mag_comp_vals.dut_offset_coef.t_offs = FixSign(t_off, 8) / 5.0f; + + sens_x = (otp_data[0x10] & 0xFF00) >> 8; + sens_y = (otp_data[0x11] & 0x00FF); + sens_z = (otp_data[0x11] & 0xFF00) >> 8; + t_sens = (otp_data[0x0D] & 0xFF00) >> 8; + + _mag_comp_vals.dut_sensit_coef.sens_x = FixSign(sens_x, 8) / 256.0f; + _mag_comp_vals.dut_sensit_coef.sens_y = (FixSign(sens_y, 8) / 256.0f) + 0.01f; + _mag_comp_vals.dut_sensit_coef.sens_z = FixSign(sens_z, 8) / 256.0f; + _mag_comp_vals.dut_sensit_coef.t_sens = FixSign(t_sens, 8) / 512.0f; + + tco_x = (otp_data[0x12] & 0x00FF); + tco_y = (otp_data[0x13] & 0x00FF); + tco_z = (otp_data[0x14] & 0x00FF); + + _mag_comp_vals.dut_tco.tco_x = FixSign(tco_x, 8) / 32.0f; + _mag_comp_vals.dut_tco.tco_y = FixSign(tco_y, 8) / 32.0f; + _mag_comp_vals.dut_tco.tco_z = FixSign(tco_z, 8) / 32.0f; + + tcs_x = (otp_data[0x12] & 0xFF00) >> 8; + tcs_y = (otp_data[0x13] & 0xFF00) >> 8; + tcs_z = (otp_data[0x14] & 0xFF00) >> 8; + + _mag_comp_vals.dut_tcs.tcs_x = FixSign(tcs_x, 8) / 16384.0f; + _mag_comp_vals.dut_tcs.tcs_y = FixSign(tcs_y, 8) / 16384.0f; + _mag_comp_vals.dut_tcs.tcs_z = (FixSign(tcs_z, 8) / 16384.0f) - 0.0001f; + + _mag_comp_vals.dut_t0 = (FixSign(otp_data[0x18], 16) / 512.0f) + 23.0f; + + cross_x_y = (otp_data[0x15] & 0x00FF); + cross_y_x = (otp_data[0x15] & 0xFF00) >> 8; + cross_z_x = (otp_data[0x16] & 0x00FF); + cross_z_y = (otp_data[0x16] & 0xFF00) >> 8; + + _mag_comp_vals.cross_axis.cross_x_y = FixSign(cross_x_y, 8) / 800.0f; + _mag_comp_vals.cross_axis.cross_y_x = FixSign(cross_y_x, 8) / 800.0f; + _mag_comp_vals.cross_axis.cross_z_x = FixSign(cross_z_x, 8) / 800.0f; + _mag_comp_vals.cross_axis.cross_z_y = FixSign(cross_z_y, 8) / 800.0f; + return PX4_OK; +} + +int BMM350::RegisterRead(Register reg, uint8_t *value) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer[3] = {0}; + int ret = transfer(&cmd, 1, buffer, 3); + + if (ret != PX4_OK) { + PX4_DEBUG("register read 0x%02hhX failed, ret = %d", cmd, ret); + + } else { + *value = buffer[2]; + } + + return ret; +} + +int BMM350::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] {(uint8_t)reg, value}; + int ret = transfer(buffer, sizeof(buffer), nullptr, 0); + + if (ret != PX4_OK) { + PX4_DEBUG("register write 0x%02hhX failed, ret = %d", (uint8_t)reg, ret); + } + + return ret; +} diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp new file mode 100644 index 000000000000..4ba7f535dbd9 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp @@ -0,0 +1,210 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file BMM350.hpp + * + * Driver for the Bosch BMM350 connected via I2C. + * + */ + +#pragma once + +#include "Bosch_BMM350_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Bosch_BMM350; +using namespace time_literals; + +class BMM350 : public device::I2C, public I2CSPIDriver, public ModuleParams +{ +public: + BMM350(const I2CSPIDriverConfig &config); + ~BMM350() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + struct mag_temp_data { + float x; + float y; + float z; + float temp; + }; + + struct raw_mag_data { + int32_t raw_x; + int32_t raw_y; + int32_t raw_z; + int32_t raw_t; + }; + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + struct dut_offset_coef { + float t_offs; + float offset_x; + float offset_y; + float offset_z; + }; + struct dut_sensit_coef { + float t_sens; + float sens_x; + float sens_y; + float sens_z; + }; + + struct dut_tco { + float tco_x; + float tco_y; + float tco_z; + }; + + struct dut_tcs { + float tcs_x; + float tcs_y; + float tcs_z; + }; + + struct cross_axis { + float cross_x_y; + float cross_y_x; + float cross_z_x; + float cross_z_y; + }; + + struct mag_compensate_vals { + struct dut_offset_coef dut_offset_coef; + struct dut_sensit_coef dut_sensit_coef; + struct dut_tco dut_tco; + struct dut_tcs dut_tcs; + float dut_t0; + struct cross_axis cross_axis; + }; + + int probe() override; + bool Reset(); + int Configure(); + + int RegisterRead(Register reg, uint8_t *value); + int RegisterWrite(Register reg, uint8_t value); + + int8_t CompensateAxisAndTemp(); + int ReadOutRawData(float *out_data); + int ReadOTPWord(uint8_t addr, uint16_t *lsb_msb); + int32_t FixSign(uint32_t inval, int8_t num_bits); + + int UpdateMagOffsets(); + void ParametersUpdate(bool force = false); + void UpdateMagParams(); + uint8_t GetODR(int value); + hrt_abstime OdrToUs(uint8_t value); + uint8_t GetAVG(int value); + + PX4Magnetometer _px4_mag; + + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME ": reset")}; + perf_counter_t _bad_read_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad read")}; + perf_counter_t _self_test_failed_perf{perf_alloc(PC_COUNT, MODULE_NAME ": self test failed")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + + mag_compensate_vals _mag_comp_vals{0}; + + float _initial_self_test_values[4]; + + uint8_t _mag_odr_mode = ODR_200HZ; + uint8_t _mag_avg_mode = AVG_2; + uint8_t _mag_pad_drive = 7; + + + static constexpr float BXY_SENS = 14.55f; + static constexpr float BZ_SENS = 9.0f; + static constexpr float TEMP_SENS = 0.00204f; + static constexpr float INA_XY_GAIN_TRT = 19.46f; + static constexpr float INA_Z_GAIN_TRT = 31.0f; + static constexpr float ADC_GAIN = 1 / 1.5f; + static constexpr float LUT_GAIN = 0.714607238769531f; + static constexpr float POWER = 1000000.0f / 1048576.0f; + float lsb_to_utc_degc[4] = { + (POWER / (BXY_SENS *INA_XY_GAIN_TRT *ADC_GAIN * LUT_GAIN)), + (POWER / (BXY_SENS *INA_XY_GAIN_TRT *ADC_GAIN * LUT_GAIN)), + (POWER / (BZ_SENS *INA_Z_GAIN_TRT *ADC_GAIN * LUT_GAIN)), + 1 / (TEMP_SENS *ADC_GAIN *LUT_GAIN * 1048576) + }; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + FGR, + BR, + AFTER_RESET, + MEASURE_FORCED, + SELF_TEST_CHECK, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + enum class SELF_TEST_STATE : uint8_t { + INIT, + POS_X, + NEG_X, + POS_Y, + NEG_Y + } _self_test_state{SELF_TEST_STATE::INIT}; + + DEFINE_PARAMETERS( + (ParamInt) _param_bmm350_odr, + (ParamInt) _param_bmm350_avg, + (ParamInt) _param_bmm350_drive + ) + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; +}; diff --git a/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp new file mode 100644 index 000000000000..dc391f8824aa --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Bosch_BMM350_registers.hpp + * + * Bosch BMM350 registers. + * + */ + +#pragma once + +#include + +namespace Bosch_BMM350 +{ +#define ENABLE_X_AXIS(axis_data) (axis_data = (axis_data & ~(0x01)) | (1 & 0x01)) +#define ENABLE_Y_AXIS(axis_data) (axis_data = ((axis_data & ~(0x02)) | ((1 << 0x1) & 0x02))) +#define ENABLE_Z_AXIS(axis_data) (axis_data = ((axis_data & ~(0x04)) | ((1 << 0x2) & 0x04))) + +static constexpr uint32_t I2C_SPEED = 400 * 1000; +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x14; +static constexpr uint8_t chip_identification_number = 0x33; + +enum class Register : uint8_t { + CHIP_ID = 0x00, + + PAD_CTRL = 0x03, + PMU_CMD_AGGR_SET = 0x04, + PMU_CMD_AXIS_EN = 0x05, + PMU_CMD = 0x06, + PMU_STATUS_0 = 0x07, + PMU_STATUS_1 = 0x08, + + I2C_WDT_SET = 0x0a, + + DATAX_XLSB = 0x31, + + OTP_CMD = 0x50, + OTP_DATA_MSB = 0x52, + OTP_DATA_LSB = 0x53, + OTP_STATUS = 0x55, + + TMR_SELF_TEST_USER = 0x60, + CMD = 0x7e +}; + +enum CONTROL_CMD : uint8_t { + SOFT_RESET = 0xb6, + EN_XYZ = 0x7 +}; + +enum PMU_CONTROL_CMD : uint8_t { + PMU_CMD_SUSPEND = 0x00, + PMU_CMD_NM = 0x01, + PMU_CMD_UPDATE_OAE = 0x02, + PMU_CMD_FM = 0x03, + PMU_CMD_FAST_FM = 0x04, + PMU_CMD_FGR = 0x05, + PMU_CMD_FAST_FGR = 0x06, + PMU_CMD_BR = 0x07, + PMU_CMD_BR_FAST = 0x08, + PMU_CMD_NM_TC = 0x09 +}; + +static inline uint8_t PMU_CMD_STATUS_0_RES(uint8_t val) +{ + return (val >> 5) & 0x7; +}; + +enum PMU_STATUS_0_BIT : uint8_t { + PMU_BUSY = (1 << 0), + ODR_OVWR = (1 << 1), + AVG_OVWR = (1 << 2), + PWR_NORMAL = (1 << 3), + ILLEGAL_CMD = (1 << 4) +}; + +enum PMU_STATUS_VALUE { + PMU_STATUS_SUS = 0x0, + PMU_STATUS_NM = 0x1, + PMU_STATUS_UPDATE_OAE = 0x2, + PMU_STATUS_FM = 0x3, + PMU_STATUS_FM_FAST = 0x4, + PMU_STATUS_FGR = 0x5, + PMU_STATUS_FGR_FAST = 0x6, + PMU_STATUS_BR = 0x7, + PMU_STATUS_BR_FAST = 0x7, +}; + +enum ODR_CONTROL_CMD : uint8_t { + ODR_400HZ = 0x2, + ODR_200HZ = 0x3, + ODR_100HZ = 0x4, + ODR_50HZ = 0x5, + ODR_25HZ = 0x6, + ODR_12_5HZ = 0x7, + ODR_6_25HZ = 0x8, + ODR_3_125HZ = 0x9, + ODR_1_5625HZ = 0xa +}; + +enum AVG_CONTROL_CMD : uint8_t { + AVG_NO_AVG = 0x0, + AVG_2 = 0x1, + AVG_4 = 0x2, + AVG_8 = 0x3 +}; + +enum ENABLE_AXIS_BIT : uint8_t { + EN_X = (1 << 0), + EN_Y = (1 << 1), + EN_Z = (1 << 2) +}; + +enum OTP_CONTROL_CMD : uint8_t { + PWR_OFF_OTP = 0x80, + OTP_DIR_READ = 0x20, + OTP_WORD_MSK = 0x1F, +}; + +enum SELF_TEST_CMD : uint8_t { + SELF_TEST_DISABLE = 0x00, + SELF_TEST_POS_X = 0x0d, + SELF_TEST_NEG_X = 0x0b, + SELF_TEST_POS_Y = 0x15, + SELF_TEST_NEG_Y = 0x13, +}; +} // namespace Bosch_BMM350 diff --git a/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt b/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt new file mode 100644 index 000000000000..ca253077287e --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE drivers__magnetometer__bosch__bmm350 + MAIN bmm350 + COMPILE_FLAGS + SRCS + BMM350.cpp + BMM350.hpp + bmm350_main.cpp + Bosch_BMM350_registers.hpp + DEPENDS + drivers_magnetometer + px4_work_queue + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/magnetometer/bosch/bmm350/Kconfig b/src/drivers/magnetometer/bosch/bmm350/Kconfig new file mode 100644 index 000000000000..834a2c694780 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_BOSCH_BMM350 + bool "bmm350" + default n + ---help--- + Enable support for bosch bmm350 diff --git a/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp b/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp new file mode 100644 index 000000000000..c707d7957b6b --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BMM350.hpp" + +#include +#include + +void BMM350::print_usage() +{ + PRINT_MODULE_USAGE_NAME("bmm350", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x14); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int bmm350_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = BMM350; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_BMM350); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/magnetometer/bosch/bmm350/module.yaml b/src/drivers/magnetometer/bosch/bmm350/module.yaml new file mode 100644 index 000000000000..140c5c321a8a --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/module.yaml @@ -0,0 +1,44 @@ +module_name: BMM350 +parameters: + - group: Magnetometer + definitions: + BMM350_ODR: + description: + short: BMM350 ODR rate + long: | + Defines which ODR rate to use during data polling. + type: enum + values: + 0: 400 Hz + 1: 200 Hz + 2: 100 Hz + 3: 50 Hz + 4: 25 Hz + 5: 12.5 Hz + 6: 6.25 Hz + 7: 3.125 Hz + 8: 1.5625 Hz + reboot_required: true + default: [3] + BMM350_AVG: + description: + short: BMM350 data averaging + long: | + Defines which averaging mode to use during data polling. + type: enum + values: + 0: No averaging + 1: 2 sample averaging + 2: 4 sample averaging + 3: 8 sample averaging + reboot_required: true + default: [1] + BMM350_DRIVE: + description: + short: BMM350 pad drive strength setting + long: | + This setting helps avoid signal problems like overshoot or undershoot. + type: int32 + min: 0 + max: 7 + default: [7]