From 6e49333a40d8200dcf1442a68a5d8018d2d6b4c9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 17 Dec 2024 12:05:16 +1100 Subject: [PATCH 01/23] AP_HAL_ChibiOS: do not build QuickTune on peripherals kills heavy peripherals ../../libraries/AP_Quicktune/AP_Quicktune.cpp: In member function 'void AP_Quicktune::update(bool)': ../../libraries/AP_Quicktune/AP_Quicktune.cpp:177:32: error: 'vehicle' is not a member of 'AP' 177 | const auto &vehicle = *AP::vehicle(); | ^~~~~~~ compilation terminated due to -Wfatal-errors. [ 688/1225] Compiling libraries/AP_TemperatureSensor/AP_TemperatureSensor_TSYS03.cpp Waf: Leaving directory `/home/pbarker/rc/ardupilot/build/CubeOrange-periph-heavy' --- libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h index 4954c9be65..a262f9fa13 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h @@ -468,3 +468,7 @@ #ifndef AP_CUSTOMROTATIONS_ENABLED #define AP_CUSTOMROTATIONS_ENABLED 0 #endif + +#ifndef AP_QUICKTUNE_ENABLED +#define AP_QUICKTUNE_ENABLED 0 +#endif From c5284c6fdf0e154c7d264f073438c1440ad50789 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 16 Dec 2024 10:36:27 +1100 Subject: [PATCH 02/23] Rover: allow WATER_DEPTH mavlink message rate to be specified ... and reduce the default rate this is currently unconditionally streamed at 50Hz, chewing up all available bandwidth on some telemetry radios. --- Rover/GCS_Mavlink.cpp | 1 + Rover/Log.cpp | 4 ---- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index a903e591c6..eaa26eee7d 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -589,6 +589,7 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_WIND, #if AP_RANGEFINDER_ENABLED MSG_RANGEFINDER, + MSG_WATER_DEPTH, #endif MSG_DISTANCE_SENSOR, MSG_SYSTEM_TIME, diff --git a/Rover/Log.cpp b/Rover/Log.cpp index f13ad700ad..e474cda125 100644 --- a/Rover/Log.cpp +++ b/Rover/Log.cpp @@ -79,10 +79,6 @@ void Rover::Log_Write_Depth() (double)(s->distance()), temp_C); } -#if AP_RANGEFINDER_ENABLED - // send water depth and temp to ground station - gcs().send_message(MSG_WATER_DEPTH); -#endif } #endif From 33a788ebb560bf6131f19fe0598519082463a05a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 16 Dec 2024 10:47:14 +1100 Subject: [PATCH 03/23] GCS_MAVLink: move sending of WATER_DEPTH into Rover code only compiled in on Rover at the moment. need to add an additional Rover-specific check for frame type, so move this code into Rover for now. --- libraries/GCS_MAVLink/GCS.h | 1 - libraries/GCS_MAVLink/GCS_Common.cpp | 58 ---------------------------- 2 files changed, 59 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 649a12a6ad..aa67f61e8c 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -403,7 +403,6 @@ public: #if AP_WINCH_ENABLED virtual void send_winch_status() const {}; #endif - void send_water_depth() const; int8_t battery_remaining_pct(const uint8_t instance) const; #if HAL_HIGH_LATENCY2_ENABLED diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index e670339e7b..91dfd299c3 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -5966,57 +5966,6 @@ void GCS_MAVLINK::send_generator_status() const } #endif -#if AP_RANGEFINDER_ENABLED && APM_BUILD_TYPE(APM_BUILD_Rover) -void GCS_MAVLINK::send_water_depth() const -{ - if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) { - return; - } - - RangeFinder *rangefinder = RangeFinder::get_singleton(); - - if (rangefinder == nullptr || !rangefinder->has_orientation(ROTATION_PITCH_270)){ - return; - } - - // get position - const AP_AHRS &ahrs = AP::ahrs(); - Location loc; - IGNORE_RETURN(ahrs.get_location(loc)); - - for (uint8_t i=0; inum_sensors(); i++) { - const AP_RangeFinder_Backend *s = rangefinder->get_backend(i); - - if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) { - continue; - } - - // get temperature - float temp_C; - if (!s->get_temp(temp_C)) { - temp_C = 0.0f; - } - - const bool sensor_healthy = (s->status() == RangeFinder::Status::Good); - - mavlink_msg_water_depth_send( - chan, - AP_HAL::millis(), // time since system boot TODO: take time of measurement - i, // rangefinder instance - sensor_healthy, // sensor healthy - loc.lat, // latitude of vehicle - loc.lng, // longitude of vehicle - loc.alt * 0.01f, // altitude of vehicle (MSL) - ahrs.get_roll(), // roll in radians - ahrs.get_pitch(), // pitch in radians - ahrs.get_yaw(), // yaw in radians - s->distance(), // distance in meters - temp_C); // temperature in degC - } - -} -#endif // AP_RANGEFINDER_ENABLED && APM_BUILD_TYPE(APM_BUILD_Rover) - #if HAL_ADSB_ENABLED void GCS_MAVLINK::send_uavionix_adsb_out_status() const { @@ -6547,13 +6496,6 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) break; #endif -#if AP_RANGEFINDER_ENABLED && APM_BUILD_TYPE(APM_BUILD_Rover) - case MSG_WATER_DEPTH: - CHECK_PAYLOAD_SIZE(WATER_DEPTH); - send_water_depth(); - break; -#endif - #if HAL_HIGH_LATENCY2_ENABLED case MSG_HIGH_LATENCY2: CHECK_PAYLOAD_SIZE(HIGH_LATENCY2); From 9f2253a109b59eadcd792363c650b11aa2811f03 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 16 Dec 2024 10:47:14 +1100 Subject: [PATCH 04/23] Rover: move sending of WATER_DEPTH into Rover code only compiled in on Rover at the moment. need to add an additional Rover-specific check for frame type, so move this code into Rover for now. --- Rover/GCS_Mavlink.cpp | 56 +++++++++++++++++++++++++++++++++++++++++++ Rover/GCS_Mavlink.h | 3 +++ 2 files changed, 59 insertions(+) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index eaa26eee7d..55cfc4ec91 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -185,6 +185,55 @@ void GCS_MAVLINK_Rover::send_rangefinder() const distance, voltage); } + +void GCS_MAVLINK_Rover::send_water_depth() const +{ + if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) { + return; + } + + RangeFinder *rangefinder = RangeFinder::get_singleton(); + + if (rangefinder == nullptr || !rangefinder->has_orientation(ROTATION_PITCH_270)){ + return; + } + + // get position + const AP_AHRS &ahrs = AP::ahrs(); + Location loc; + IGNORE_RETURN(ahrs.get_location(loc)); + + for (uint8_t i=0; inum_sensors(); i++) { + const AP_RangeFinder_Backend *s = rangefinder->get_backend(i); + + if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) { + continue; + } + + // get temperature + float temp_C; + if (!s->get_temp(temp_C)) { + temp_C = 0.0f; + } + + const bool sensor_healthy = (s->status() == RangeFinder::Status::Good); + + mavlink_msg_water_depth_send( + chan, + AP_HAL::millis(), // time since system boot TODO: take time of measurement + i, // rangefinder instance + sensor_healthy, // sensor healthy + loc.lat, // latitude of vehicle + loc.lng, // longitude of vehicle + loc.alt * 0.01f, // altitude of vehicle (MSL) + ahrs.get_roll(), // roll in radians + ahrs.get_pitch(), // pitch in radians + ahrs.get_yaw(), // yaw in radians + s->distance(), // distance in meters + temp_C); // temperature in degC + } + +} #endif // AP_RANGEFINDER_ENABLED /* @@ -400,6 +449,13 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) } #endif +#if AP_RANGEFINDER_ENABLED + case MSG_WATER_DEPTH: + CHECK_PAYLOAD_SIZE(WATER_DEPTH); + send_water_depth(); + break; +#endif // AP_RANGEFINDER_ENABLED + default: return GCS_MAVLINK::try_send_message(id); } diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index fd92cac3d5..75d4534b1b 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -44,6 +44,9 @@ protected: // Index starts at 1 uint8_t send_available_mode(uint8_t index) const override; + // send WATER_DEPTH - metres and temperature + void send_water_depth() const; + private: void handle_message(const mavlink_message_t &msg) override; From 07edfdd32304ca49f7255b0e084b8a057c2f5279 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 16 Dec 2024 10:51:41 +1100 Subject: [PATCH 05/23] Rover: only send WATER_DEPTH for boat frames --- Rover/GCS_Mavlink.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 55cfc4ec91..b4a53e3c9f 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -192,6 +192,11 @@ void GCS_MAVLINK_Rover::send_water_depth() const return; } + // only send for boats: + if (!rover.is_boat()) { + return; + } + RangeFinder *rangefinder = RangeFinder::get_singleton(); if (rangefinder == nullptr || !rangefinder->has_orientation(ROTATION_PITCH_270)){ From 73710d888de1c1f8a1153327624ac893473a11e4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 16 Dec 2024 10:53:09 +1100 Subject: [PATCH 06/23] Rover: tidy WATER_DEPTH send checks --- Rover/GCS_Mavlink.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index b4a53e3c9f..841f6e97cb 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -199,9 +199,14 @@ void GCS_MAVLINK_Rover::send_water_depth() const RangeFinder *rangefinder = RangeFinder::get_singleton(); - if (rangefinder == nullptr || !rangefinder->has_orientation(ROTATION_PITCH_270)){ + if (rangefinder == nullptr) { return; - } + } + + // depth can only be measured by a downward-facing rangefinder: + if (!rangefinder->has_orientation(ROTATION_PITCH_270)) { + return; + } // get position const AP_AHRS &ahrs = AP::ahrs(); From 84440108e3ad4333b6f581d87ab8f5e4bf0fcc12 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 25 Nov 2024 10:50:17 +1100 Subject: [PATCH 07/23] AP_AHRS: rename DCM members to clarify EAS vs TAS --- libraries/AP_AHRS/AP_AHRS.cpp | 28 ++++++++++++------------- libraries/AP_AHRS/AP_AHRS.h | 10 ++++----- libraries/AP_AHRS/AP_AHRS_Backend.h | 9 ++++---- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 32 ++++++++++++++--------------- libraries/AP_AHRS/AP_AHRS_DCM.h | 16 +++++++-------- libraries/AP_AHRS/AP_AHRS_SIM.cpp | 6 +++--- libraries/AP_AHRS/AP_AHRS_SIM.h | 4 ++-- 7 files changed, 53 insertions(+), 52 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 945ad67d41..e8fa5cd623 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -346,9 +346,9 @@ void AP_AHRS::update_state(void) state.primary_core = _get_primary_core_index(); state.wind_estimate_ok = _wind_estimate(state.wind_estimate); state.EAS2TAS = AP_AHRS_Backend::get_EAS2TAS(); - state.airspeed_ok = _airspeed_estimate(state.airspeed, state.airspeed_estimate_type); - state.airspeed_true_ok = _airspeed_estimate_true(state.airspeed_true); - state.airspeed_vec_ok = _airspeed_vector_true(state.airspeed_vec); + state.airspeed_ok = _airspeed_EAS(state.airspeed, state.airspeed_estimate_type); + state.airspeed_true_ok = _airspeed_TAS(state.airspeed_true); + state.airspeed_vec_ok = _airspeed_TAS(state.airspeed_vec); state.quat_ok = _get_quaternion(state.quat); state.secondary_attitude_ok = _get_secondary_attitude(state.secondary_attitude); state.secondary_quat_ok = _get_secondary_quaternion(state.secondary_quat); @@ -931,7 +931,7 @@ bool AP_AHRS::_should_use_airspeed_sensor(uint8_t airspeed_index) const // return an airspeed estimate if available. return true // if we have an estimate -bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const +bool AP_AHRS::_airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const { #if AP_AHRS_DCM_ENABLED || (AP_AIRSPEED_ENABLED && AP_GPS_ENABLED) const uint8_t idx = get_active_airspeed_index(); @@ -970,20 +970,20 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs #if AP_AHRS_DCM_ENABLED case EKFType::DCM: airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(idx, airspeed_ret); + return dcm.airspeed_EAS(idx, airspeed_ret); #endif #if AP_AHRS_SIM_ENABLED case EKFType::SIM: airspeed_estimate_type = AirspeedEstimateType::SIM; - return sim.airspeed_estimate(airspeed_ret); + return sim.airspeed_EAS(airspeed_ret); #endif #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: #if AP_AHRS_DCM_ENABLED airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(idx, airspeed_ret); + return dcm.airspeed_EAS(idx, airspeed_ret); #else return false; #endif @@ -999,7 +999,7 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs case EKFType::EXTERNAL: #if AP_AHRS_DCM_ENABLED airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(idx, airspeed_ret); + return dcm.airspeed_EAS(idx, airspeed_ret); #else return false; #endif @@ -1027,18 +1027,18 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs #if AP_AHRS_DCM_ENABLED // fallback to DCM airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(idx, airspeed_ret); + return dcm.airspeed_EAS(idx, airspeed_ret); #endif return false; } -bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const +bool AP_AHRS::_airspeed_TAS(float &airspeed_ret) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED case EKFType::DCM: - return dcm.airspeed_estimate_true(airspeed_ret); + return dcm.airspeed_TAS(airspeed_ret); #endif #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: @@ -1064,7 +1064,7 @@ bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const // return estimate of true airspeed vector in body frame in m/s // returns false if estimate is unavailable -bool AP_AHRS::_airspeed_vector_true(Vector3f &vec) const +bool AP_AHRS::_airspeed_TAS(Vector3f &vec) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED @@ -1126,14 +1126,14 @@ bool AP_AHRS::airspeed_health_data(float &innovation, float &innovationVariance, return false; } -// return a synthetic airspeed estimate (one derived from sensors +// return a synthetic EAS estimate (one derived from sensors // other than an actual airspeed sensor), if available. return // true if we have a synthetic airspeed. ret will not be modified // on failure. bool AP_AHRS::synthetic_airspeed(float &ret) const { #if AP_AHRS_DCM_ENABLED - return dcm.synthetic_airspeed(ret); + return dcm.synthetic_airspeed_EAS(ret); #endif return false; } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 10f9deed01..04ce489dad 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -152,7 +152,7 @@ public: // get air density / sea level density - decreases as altitude climbs float get_air_density_ratio(void) const; - // return an airspeed estimate if available. return true + // return an (equivalent) airspeed estimate if available. return true // if we have an estimate bool airspeed_estimate(float &airspeed_ret) const; @@ -195,7 +195,7 @@ public: return AP_AHRS_Backend::airspeed_sensor_enabled(airspeed_index); } - // return a synthetic airspeed estimate (one derived from sensors + // return a synthetic (equivalent) airspeed estimate (one derived from sensors // other than an actual airspeed sensor), if available. return // true if we have a synthetic airspeed. ret will not be modified // on failure. @@ -873,7 +873,7 @@ private: // return an airspeed estimate if available. return true // if we have an estimate - bool _airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &status) const; + bool _airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &status) const; // return secondary attitude solution if available, as eulers in radians bool _get_secondary_attitude(Vector3f &eulers) const; @@ -892,11 +892,11 @@ private: // return a true airspeed estimate (navigation airspeed) if // available. return true if we have an estimate - bool _airspeed_estimate_true(float &airspeed_ret) const; + bool _airspeed_TAS(float &airspeed_ret) const; // return estimate of true airspeed vector in body frame in m/s // returns false if estimate is unavailable - bool _airspeed_vector_true(Vector3f &vec) const; + bool _airspeed_TAS(Vector3f &vec) const; // return the quaternion defining the rotation from NED to XYZ (body) axes bool _get_quaternion(Quaternion &quat) const WARN_IF_UNUSED; diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 0e70ce9662..99ad8f2dc8 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -135,13 +135,13 @@ public: // return an airspeed estimate if available. return true // if we have an estimate - virtual bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED { return false; } - virtual bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const { return false; } + virtual bool airspeed_EAS(float &airspeed_ret) const WARN_IF_UNUSED { return false; } + virtual bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const { return false; } // return a true airspeed estimate (navigation airspeed) if // available. return true if we have an estimate - bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED { - if (!airspeed_estimate(airspeed_ret)) { + bool airspeed_TAS(float &airspeed_ret) const WARN_IF_UNUSED { + if (!airspeed_EAS(airspeed_ret)) { return false; } airspeed_ret *= get_EAS2TAS(); @@ -156,6 +156,7 @@ public: // get apparent to true airspeed ratio static float get_EAS2TAS(void); + static float get_TAS2EAS(void) { return 1.0/get_EAS2TAS(); } // return true if airspeed comes from an airspeed sensor, as // opposed to an IMU estimate diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index d7aa5c7cd3..a64821c145 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -724,16 +724,16 @@ AP_AHRS_DCM::drift_correction(float deltat) return; } - float airspeed = _last_airspeed; + float airspeed_TAS = _last_airspeed_TAS; #if AP_AIRSPEED_ENABLED if (airspeed_sensor_enabled()) { - airspeed = AP::airspeed()->get_airspeed(); + airspeed_TAS = AP::airspeed()->get_airspeed(); } #endif // use airspeed to estimate our ground velocity in // earth frame by subtracting the wind - velocity = _dcm_matrix.colx() * airspeed; + velocity = _dcm_matrix.colx() * airspeed_TAS; // add in wind estimate velocity += _wind; @@ -762,7 +762,7 @@ AP_AHRS_DCM::drift_correction(float deltat) // take positive component in X direction. This mimics a pitot // tube - _last_airspeed = MAX(airspeed.x, 0); + _last_airspeed_TAS = MAX(airspeed.x, 0); } if (have_gps()) { @@ -1084,31 +1084,31 @@ bool AP_AHRS_DCM::get_location(Location &loc) const return _have_position; } -bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const +bool AP_AHRS_DCM::airspeed_EAS(float &airspeed_ret) const { #if AP_AIRSPEED_ENABLED const auto *airspeed = AP::airspeed(); if (airspeed != nullptr) { - return airspeed_estimate(airspeed->get_primary(), airspeed_ret); + return airspeed_EAS(airspeed->get_primary(), airspeed_ret); } #endif // airspeed_estimate will also make this nullptr check and act // appropriately when we call it with a dummy sensor ID. - return airspeed_estimate(0, airspeed_ret); + return airspeed_EAS(0, airspeed_ret); } -// return an airspeed estimate: +// return an (equivalent) airspeed estimate: // - from a real sensor if available // - otherwise from a GPS-derived wind-triangle estimate (if GPS available) // - otherwise from a cached wind-triangle estimate value (but returning false) -bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const +bool AP_AHRS_DCM::airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const { - // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order: + // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order: // airspeed as filled-in by an enabled airspeed sensor // if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation // Or if none of the above, fills-in using the previous airspeed estimate // Return false: if we are using the previous airspeed estimate - if (!get_unconstrained_airspeed_estimate(airspeed_index, airspeed_ret)) { + if (!get_unconstrained_airspeed_EAS(airspeed_index, airspeed_ret)) { return false; } @@ -1127,12 +1127,12 @@ bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) return true; } -// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order: +// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order: // airspeed as filled-in by an enabled airspeed sensor // if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation // Or if none of the above, fills-in using the previous airspeed estimate // Return false: if we are using the previous airspeed estimate -bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const +bool AP_AHRS_DCM::get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const { #if AP_AIRSPEED_ENABLED if (airspeed_sensor_enabled(airspeed_index)) { @@ -1143,13 +1143,13 @@ bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, fl if (AP::ahrs().get_wind_estimation_enabled() && have_gps()) { // estimated via GPS speed and wind - airspeed_ret = _last_airspeed; + airspeed_ret = _last_airspeed_TAS; return true; } // Else give the last estimate, but return false. // This is used by the dead-reckoning code - airspeed_ret = _last_airspeed; + airspeed_ret = _last_airspeed_TAS; return false; } @@ -1182,7 +1182,7 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void) Vector2f gndVelADS; Vector2f gndVelGPS; float airspeed = 0; - const bool gotAirspeed = airspeed_estimate_true(airspeed); + const bool gotAirspeed = airspeed_TAS(airspeed); const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D); if (gotAirspeed) { const Vector2f airspeed_vector{_cos_yaw * airspeed, _sin_yaw * airspeed}; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index e4e13ded40..8f9288d014 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -82,18 +82,18 @@ public: // return an airspeed estimate if available. return true // if we have an estimate - bool airspeed_estimate(float &airspeed_ret) const override; + bool airspeed_EAS(float &airspeed_ret) const override; // return an airspeed estimate if available. return true // if we have an estimate from a specific sensor index - bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const override; + bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const override; - // return a synthetic airspeed estimate (one derived from sensors + // return a synthetic EAS estimate (one derived from sensors // other than an actual airspeed sensor), if available. return // true if we have a synthetic airspeed. ret will not be modified // on failure. - bool synthetic_airspeed(float &ret) const WARN_IF_UNUSED { - ret = _last_airspeed; + bool synthetic_airspeed_EAS(float &ret) const WARN_IF_UNUSED { + ret = _last_airspeed_TAS; return true; } @@ -173,12 +173,12 @@ private: // DCM matrix from the eulers. Called internally we may. void reset(bool recover_eulers); - // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order: + // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order: // airspeed as filled-in by an enabled airspeed sensor // if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation // Or if none of the above, fills-in using the previous airspeed estimate // Return false: if we are using the previous airspeed estimate - bool get_unconstrained_airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const; + bool get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const; // primary representation of attitude of board used for all inertial calculations Matrix3f _dcm_matrix; @@ -262,7 +262,7 @@ private: Vector3f _last_fuse; Vector3f _last_vel; uint32_t _last_wind_time; - float _last_airspeed; + float _last_airspeed_TAS; uint32_t _last_consistent_heading; // estimated wind in m/s diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.cpp b/libraries/AP_AHRS/AP_AHRS_SIM.cpp index c1e26f1890..4f9b223cdb 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_SIM.cpp @@ -41,7 +41,7 @@ bool AP_AHRS_SIM::wind_estimate(Vector3f &wind) const return true; } -bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const +bool AP_AHRS_SIM::airspeed_EAS(float &airspeed_ret) const { if (_sitl == nullptr) { return false; @@ -52,9 +52,9 @@ bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const return true; } -bool AP_AHRS_SIM::airspeed_estimate(uint8_t index, float &airspeed_ret) const +bool AP_AHRS_SIM::airspeed_EAS(uint8_t index, float &airspeed_ret) const { - return airspeed_estimate(airspeed_ret); + return airspeed_EAS(airspeed_ret); } bool AP_AHRS_SIM::get_quaternion(Quaternion &quat) const diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.h b/libraries/AP_AHRS/AP_AHRS_SIM.h index 037b64c388..924a0d6bea 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.h +++ b/libraries/AP_AHRS/AP_AHRS_SIM.h @@ -68,11 +68,11 @@ public: // return an airspeed estimate if available. return true // if we have an estimate - bool airspeed_estimate(float &airspeed_ret) const override; + bool airspeed_EAS(float &airspeed_ret) const override; // return an airspeed estimate if available. return true // if we have an estimate from a specific sensor index - bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const override; + bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const override; // return a ground vector estimate in meters/second, in North/East order Vector2f groundspeed_vector() override; From 31ec6f66d9f1cd3027a302976eadc8fe69e2018d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 16 Dec 2024 21:21:17 +1100 Subject: [PATCH 08/23] autotest: fix FRSkyPassThroughSensorIDs test need to clear the read buffers before re-polling sensors - just in case we don't need to! --- Tools/autotest/vehicle_test_suite.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index a5535d73da..e847a3e5ce 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -1715,8 +1715,8 @@ class FRSkySPort(FRSky): if not self.connect(): self.progress("Failed to connect") return - self.check_poll() self.do_sport_read() + self.check_poll() def do_sport_read(self): self.buffer += self.do_read() From 7c084e99551d7e6331c67b3033e5c4d91a704c17 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 17 Dec 2024 17:00:31 +1100 Subject: [PATCH 09/23] autotest: use perfect sim gps to avoid races in frsky test can't get mavlink and frsky to agree on a value! --- Tools/autotest/vehicle_test_suite.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index e847a3e5ce..6ff5fc7c34 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -13738,8 +13738,11 @@ switch value''' def FRSkySPort(self): '''Test FrSky SPort mode''' - self.set_parameter("SERIAL5_PROTOCOL", 4) # serial5 is FRSky sport - self.set_parameter("RPM1_TYPE", 10) # enable SITL RPM sensor + self.set_parameters({ + "SERIAL5_PROTOCOL": 4, # serial5 is FRSky sport + "RPM1_TYPE": 10, # enable SITL RPM sensor + "GPS1_TYPE": 100, # use simulated backend for consistent position + }) port = self.spare_network_port() self.customise_SITL_commandline([ "--serial5=tcp:%u" % port # serial5 spews to localhost port From 58c385432563823433e2a2d793395c1f577a11b0 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 17 Dec 2024 12:44:34 +1100 Subject: [PATCH 10/23] SITL: make SIM_ENGINE_FAIL a mask The param docs already claimed it was one. --- libraries/AP_HAL_SITL/SITL_State.cpp | 17 +++++++++-------- libraries/SITL/SITL.cpp | 2 +- libraries/SITL/SITL.h | 2 +- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 4850e40b02..06fe08506f 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -400,17 +400,18 @@ void SITL_State::_simulator_servos(struct sitl_input &input) } float engine_mul = _sitl?_sitl->engine_mul.get():1; - uint8_t engine_fail = _sitl?_sitl->engine_fail.get():0; + uint32_t engine_fail = _sitl?_sitl->engine_fail.get():0; float throttle = 0.0f; - if (engine_fail >= ARRAY_SIZE(input.servos)) { - engine_fail = 0; - } // apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter - if (_vehicle != Rover) { - input.servos[engine_fail] = ((input.servos[engine_fail]-1000) * engine_mul) + 1000; - } else { - input.servos[engine_fail] = static_cast(((input.servos[engine_fail] - 1500) * engine_mul) + 1500); + for (uint8_t i=0; i(((input.servos[i] - 1500) * engine_mul) + 1500); + } + } } if (_vehicle == ArduPlane) { diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 03ba5f1771..b4541b822d 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -246,7 +246,7 @@ const AP_Param::GroupInfo SIM::var_info[] = { // @Param: ENGINE_FAIL // @DisplayName: Engine Fail Mask // @Description: mask of motors which SIM_ENGINE_MUL will be applied to - // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8 + // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15, 15: Servo 16, 16: Servo 17, 17: Servo 18, 18: Servo 19, 19: Servo 20, 20: Servo 21, 21: Servo 22, 22: Servo 23, 23: Servo 24, 24: Servo 25, 25: Servo 26, 26: Servo 27, 27: Servo 28, 28: Servo 29, 29: Servo 30, 30: Servo 31, 31: Servo 32 AP_GROUPINFO("ENGINE_FAIL", 58, SIM, engine_fail, 0), AP_SUBGROUPINFO(models, "", 59, SIM, SIM::ModelParm), AP_SUBGROUPEXTENSION("", 60, SIM, var_mag), diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 54d9350437..d80f641c5f 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -197,7 +197,7 @@ public: AP_Float drift_speed; // degrees/second/minute AP_Float drift_time; // period in minutes AP_Float engine_mul; // engine multiplier - AP_Int8 engine_fail; // engine servo to fail (0-7) + AP_Int32 engine_fail; // mask of engine/motor servo outputs to fail // initial offset on GPS lat/lon, used to shift origin AP_Float gps_init_lat_ofs; From dee88b4ecb51770d62079a71573abd5f567d5faf Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 17 Dec 2024 12:46:00 +1100 Subject: [PATCH 11/23] SITL: default SIM_ENGINE_MUL to 0 This makes SIM_ENGINE_FAIL work a little more intuitively, since it is usually used to simulate a complete failure. Also, drive-by fix of the SIM_ENGINE_MUL documentation. --- Tools/autotest/default_params/vee-gull_005.param | 2 +- libraries/SITL/SITL.cpp | 4 ++-- .../examples/SilentWings/Params/Rolladen-Schneider-LS8b.param | 2 +- .../SITL/examples/SilentWings/Params/Schleicher-ASW27B.param | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Tools/autotest/default_params/vee-gull_005.param b/Tools/autotest/default_params/vee-gull_005.param index ab340c7eaf..9415f055bd 100644 --- a/Tools/autotest/default_params/vee-gull_005.param +++ b/Tools/autotest/default_params/vee-gull_005.param @@ -620,7 +620,7 @@ SIM_BATT_VOLTAGE,12.6 SIM_DRIFT_SPEED,0.05 SIM_DRIFT_TIME,5 SIM_ENGINE_FAIL,0 -SIM_ENGINE_MUL,1 +SIM_ENGINE_MUL,0 SIM_FLOAT_EXCEPT,1 SIM_FLOW_DELAY,0 SIM_FLOW_ENABLE,0 diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index b4541b822d..3adb9cc673 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -78,8 +78,8 @@ const AP_Param::GroupInfo SIM::var_info[] = { // @Param: ENGINE_MUL // @DisplayName: Engine failure thrust scaler // @Description: Thrust from Motors in SIM_ENGINE_FAIL will be multiplied by this factor - // @Units: ms - AP_GROUPINFO("ENGINE_MUL", 8, SIM, engine_mul, 1), + // @Range: 0 1 + AP_GROUPINFO("ENGINE_MUL", 8, SIM, engine_mul, 0), // @Param: WIND_SPD // @DisplayName: Simulated Wind speed // @Description: Allows you to emulate wind in sim diff --git a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param index 98d5e0b7a0..ccba94be53 100644 --- a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param +++ b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param @@ -612,7 +612,7 @@ SIM_BATT_VOLTAGE,12.6 SIM_DRIFT_SPEED,0.05 SIM_DRIFT_TIME,5 SIM_ENGINE_FAIL,0 -SIM_ENGINE_MUL,1 +SIM_ENGINE_MUL,0 SIM_FLOAT_EXCEPT,1 SIM_FLOW_DELAY,0 SIM_FLOW_ENABLE,0 diff --git a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param index 960c2c713c..353379fa5a 100644 --- a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param +++ b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param @@ -632,7 +632,7 @@ SIM_BATT_VOLTAGE,12.6 SIM_DRIFT_SPEED,0.05 SIM_DRIFT_TIME,5 SIM_ENGINE_FAIL,0 -SIM_ENGINE_MUL,1 +SIM_ENGINE_MUL,0 SIM_FLOAT_EXCEPT,1 SIM_FLOW_DELAY,0 SIM_FLOW_ENABLE,0 From 726e05afb2952f058d1e454b37e54ecbed9c7fd8 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 17 Dec 2024 13:12:23 +1100 Subject: [PATCH 12/23] autotest: update SIM_ENGINE_FAIL to mask --- Tools/autotest/arducopter.py | 20 +++++++++----------- Tools/autotest/quadplane.py | 10 ++++++---- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 1de6f855c4..fe808916d4 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1374,7 +1374,10 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): # cut motor 1's to efficiency self.progress("Cutting motor 1 to 65% efficiency") - self.set_parameter("SIM_ENGINE_MUL", 0.65) + self.set_parameters({ + "SIM_ENGINE_MUL": 0.65, + "SIM_ENGINE_FAIL": 1 << 0, # motor 1 + }) while self.get_sim_time_cached() < tstart + holdtime: m = self.mav.recv_match(type='VFR_HUD', blocking=True) @@ -3285,8 +3288,8 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.progress("Killing motor %u (%u%%)" % (fail_servo+1, fail_mul)) self.set_parameters({ - "SIM_ENGINE_FAIL": fail_servo, "SIM_ENGINE_MUL": fail_mul, + "SIM_ENGINE_FAIL": 1 << fail_servo, }) failed = True @@ -3345,10 +3348,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): raise NotAchievedException("Vehicle is descending") self.progress("Fixing motors") - self.set_parameters({ - "SIM_ENGINE_FAIL": 0, - "SIM_ENGINE_MUL": 1.0, - }) + self.set_parameter("SIM_ENGINE_FAIL", 0) self.do_RTL() @@ -4170,8 +4170,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.takeoff(40) self.set_rc(9, 1500) self.set_parameters({ - "SIM_ENGINE_MUL": 0, - "SIM_ENGINE_FAIL": 1, + "SIM_ENGINE_FAIL": 1 << 1, # motor 2 }) self.wait_statustext('BANG! Parachute deployed', timeout=60) self.set_rc(9, 1000) @@ -4184,8 +4183,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.takeoff(loiter_alt, mode='LOITER') self.set_rc(9, 1100) self.set_parameters({ - "SIM_ENGINE_MUL": 0, - "SIM_ENGINE_FAIL": 1, + "SIM_ENGINE_FAIL": 1 << 1, # motor 2 }) tstart = self.get_sim_time() while self.get_sim_time_cached() < tstart + 5: @@ -12024,8 +12022,8 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.context_push() self.context_collect('STATUSTEXT') self.set_parameters({ - "SIM_ENGINE_FAIL": 1, "SIM_ENGINE_MUL": 0.5, + "SIM_ENGINE_FAIL": 1 << 1, # motor 2 "FLIGHT_OPTIONS": 4, }) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 721752a5c3..95e72e276f 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -767,8 +767,7 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite): "Q_WVANE_ENABLE": 1, "Q_WVANE_GAIN": 1, "STICK_MIXING": 0, - "Q_FWD_THR_USE": 2, - "SIM_ENGINE_FAIL": 2}) # we want to fail the forward thrust motor only + "Q_FWD_THR_USE": 2}) self.takeoff(10, mode="QLOITER") self.set_rc(2, 1000) @@ -785,7 +784,7 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite): self.set_rc(2, 1500) self.delay_sim_time(5) loc1 = self.mav.location() - self.set_parameter("SIM_ENGINE_MUL", 0) # simulate a complete loss of forward motor thrust + self.set_parameter("SIM_ENGINE_FAIL", 1 << 2) # simulate a complete loss of forward motor thrust self.delay_sim_time(20) self.change_mode('QLAND') self.wait_disarmed(timeout=60) @@ -1475,7 +1474,10 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite): (p, new_values[p], threshold)) self.progress("ensure we are not overtuned") - self.set_parameter('SIM_ENGINE_MUL', 0.9) + self.set_parameters({ + 'SIM_ENGINE_MUL': 0.9, + 'SIM_ENGINE_FAIL': 1 << 0, + }) self.delay_sim_time(5) From f02b1ddea22b1fdbcb9c0afb9e7686616a392e82 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 17 Dec 2024 10:49:46 +0900 Subject: [PATCH 13/23] Rover: mavlink stream rate requests not saved to params --- Rover/GCS_Mavlink.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index 75d4534b1b..d26a00f894 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -29,8 +29,6 @@ protected: void send_position_target_global_int() override; - bool persist_streamrates() const override { return true; } - uint64_t capabilities() const override; void send_nav_controller_output() const override; From 74c702674b2f5a5762524f42fa450f037a88f78c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 16 Dec 2024 20:45:50 +0900 Subject: [PATCH 14/23] AC_PosControl: add get_vel_target and get_accel_target --- .../AC_AttitudeControl/AC_PosControl.cpp | 22 +++++++++++++++++++ libraries/AC_AttitudeControl/AC_PosControl.h | 6 +++++ 2 files changed, 28 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index c50c7581e8..5b768c99f9 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -1208,6 +1208,28 @@ bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &v accel_offset_NED.z = -_accel_offset_target.z * 0.01; return true; } + +// get target velocity in m/s in NED frame +bool AC_PosControl::get_vel_target(Vector3f &vel_target_NED) +{ + if (!is_active_xy() || !is_active_z()) { + return false; + } + vel_target_NED.xy() = _vel_target.xy() * 0.01; + vel_target_NED.z = -_vel_target.z * 0.01; + return true; +} + +// get target acceleration in m/s/s in NED frame +bool AC_PosControl::get_accel_target(Vector3f &accel_target_NED) +{ + if (!is_active_xy() || !is_active_z()) { + return false; + } + accel_target_NED.xy() = _accel_target.xy() * 0.01; + accel_target_NED.z = -_accel_target.z * 0.01; + return true; +} #endif /// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index f52c7a9c5c..ff5de98498 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -339,6 +339,12 @@ public: // units are m, m/s and m/s/s in NED frame bool set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED); bool get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED); + + // get target velocity in m/s in NED frame + bool get_vel_target(Vector3f &vel_target_NED); + + // get target acceleration in m/s/s in NED frame + bool get_accel_target(Vector3f &accel_target_NED); #endif /// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame From 56cb20ac507e27d27eadecf776fff968ed17f4d9 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 16 Dec 2024 20:46:33 +0900 Subject: [PATCH 15/23] AP_Scripting: add get_vel_target poscontrol binding --- libraries/AP_Scripting/docs/docs.lua | 8 ++++++++ .../AP_Scripting/generator/description/bindings.desc | 2 ++ 2 files changed, 10 insertions(+) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 660c1650fa..da3eda133e 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -3785,6 +3785,14 @@ function poscontrol:set_posvelaccel_offset(pos_offset_NED, vel_offset_NED, accel ---@return Vector3f_ud|nil function poscontrol:get_posvelaccel_offset() end +-- get position controller's target velocity in m/s in NED frame +---@return Vector3f_ud|nil +function poscontrol:get_vel_target() end + +-- get position controller's target acceleration in m/s/s in NED frame +---@return Vector3f_ud|nil +function poscontrol:get_accel_target() end + -- desc AR_PosControl = {} diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index d0ff5b1e49..08a48ecb58 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -774,6 +774,8 @@ singleton AC_PosControl depends APM_BUILD_COPTER_OR_HELI singleton AC_PosControl rename poscontrol singleton AC_PosControl method set_posvelaccel_offset boolean Vector3f Vector3f Vector3f singleton AC_PosControl method get_posvelaccel_offset boolean Vector3f'Null Vector3f'Null Vector3f'Null +singleton AC_PosControl method get_vel_target boolean Vector3f'Null +singleton AC_PosControl method get_accel_target boolean Vector3f'Null include APM_Control/AR_AttitudeControl.h depends APM_BUILD_TYPE(APM_BUILD_Rover) singleton AR_AttitudeControl depends APM_BUILD_TYPE(APM_BUILD_Rover) From 5c7cfa13cfd3fe240d01f9e49bcc82fa20013020 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 16 Dec 2024 19:34:42 +0900 Subject: [PATCH 16/23] AP_Scripting: add FOLLOW_TARGET msg definition --- .../MAVLink/mavlink_msg_FOLLOW_TARGET.lua | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 libraries/AP_Scripting/modules/MAVLink/mavlink_msg_FOLLOW_TARGET.lua diff --git a/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_FOLLOW_TARGET.lua b/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_FOLLOW_TARGET.lua new file mode 100644 index 0000000000..e5b853917c --- /dev/null +++ b/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_FOLLOW_TARGET.lua @@ -0,0 +1,16 @@ +local FOLLOW_TARGET = {} +FOLLOW_TARGET.id = 144 +FOLLOW_TARGET.fields = { + { "timestamp", " Date: Mon, 16 Dec 2024 19:35:00 +0900 Subject: [PATCH 17/23] AP_Scripting: add follow-target-send applet --- .../applets/follow-target-send.lua | 121 ++++++++++++++++++ .../applets/follow-target-send.md | 15 +++ 2 files changed, 136 insertions(+) create mode 100644 libraries/AP_Scripting/applets/follow-target-send.lua create mode 100644 libraries/AP_Scripting/applets/follow-target-send.md diff --git a/libraries/AP_Scripting/applets/follow-target-send.lua b/libraries/AP_Scripting/applets/follow-target-send.lua new file mode 100644 index 0000000000..461979aa1a --- /dev/null +++ b/libraries/AP_Scripting/applets/follow-target-send.lua @@ -0,0 +1,121 @@ +-- Send the FOLLOW_TARGET mavlink message to allow other vehicles to follow this one +-- +-- How To Use +-- 1. copy this script to the autopilot's "scripts" directory +-- 2. within the "scripts" directory create a "modules" directory +-- 3. copy the MAVLink/mavlink_msgs_xxx files to the "scripts" directory +-- 4. the FOLLOW_TARGET message will be published at 10hz + +-- load mavlink message definitions from modules/MAVLink directory +local mavlink_msgs = require("MAVLink/mavlink_msgs") + +-- global definitions +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +local UPDATE_INTERVAL_MS = 100 -- update at about 10hz +local FOLLOW_TARGET_CAPABILITIES = {POS=2^0, VEL=2^1, ACCEL=2^2, ATT_RATES=2^3} + + -- setup script specific parameters +local PARAM_TABLE_KEY = 88 +local PARAM_TABLE_PREFIX = "FOLT_" +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 2), 'could not add param table') + +-- add a parameter and bind it to a variable +local function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', PARAM_TABLE_PREFIX .. name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end + +--[[ + // @Param: FOLT_ENABLE + // @DisplayName: Follow Target Send Enable + // @Description: Follow Target Send Enable + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local FOLT_ENABLE = bind_add_param("ENABLE", 1, 1) + +--[[ + // @Param: FOLT_MAV_CHAN + // @DisplayName: Follow Target Send MAVLink Channel + // @Description: MAVLink channel to which FOLLOW_TARGET should be sent + // @Range: 0 10 + // @User: Standard +--]] +local FOLT_MAV_CHAN = bind_add_param("MAV_CHAN", 2, 0) + +-- initialize MAVLink rx with number of messages, and buffer depth +mavlink:init(2, 5) + +-- send FOLLOW_TARGET message +local function send_follow_target_msg() + + -- get vehicle location + local curr_loc = ahrs:get_location() + if curr_loc == nil then + do return end + end + local capabilities = FOLLOW_TARGET_CAPABILITIES.POS + + -- get vehicle target velocity in m/s in NED frame + local vel_target_NED = poscontrol:get_vel_target() + if vel_target_NED ~= nil then + capabilities = capabilities + FOLLOW_TARGET_CAPABILITIES.VEL + else + vel_target_NED = Vector3f() + end + + -- get vehicle target acceleration in m/s/s in NED frame + local accel_target_NED = poscontrol:get_accel_target() + if accel_target_NED ~= nil then + capabilities = capabilities + FOLLOW_TARGET_CAPABILITIES.ACCEL + else + accel_target_NED = Vector3f() + end + + -- get vehicle current attitude as quaternion and rates + local attitude_quat = ahrs:get_quaternion() + local curr_rot_rate = ahrs:get_gyro() + if attitude_quat ~= nil and curr_rot_rate ~= nil then + capabilities = capabilities + FOLLOW_TARGET_CAPABILITIES.ATT_RATES + else + attitude_quat = Quaternion() + curr_rot_rate = Vector3f() + end + local curr_rot_rate_NED = ahrs:body_to_earth(curr_rot_rate) + + -- prepare FOLLOW_TARGET message + local follow_target_msg = {} + follow_target_msg.timestamp = millis():toint() + follow_target_msg.est_capabilities = capabilities + follow_target_msg.lat = curr_loc:lat() + follow_target_msg.lon = curr_loc:lng() + follow_target_msg.alt = curr_loc:alt() * 0.01 + follow_target_msg.vel = {vel_target_NED:x(), vel_target_NED:y(), vel_target_NED:z()} + follow_target_msg.acc = {accel_target_NED:x(), accel_target_NED:y(), accel_target_NED:z()} + follow_target_msg.attitude_q = {attitude_quat:q1(), attitude_quat:q2(), attitude_quat:q3(), attitude_quat:q4()} + follow_target_msg.rates = {curr_rot_rate_NED:x(), curr_rot_rate_NED:y(), curr_rot_rate_NED:z()} + follow_target_msg.position_cov = {0, 0, 0} + follow_target_msg.custom_state = 0 + + -- send FOLLOW_TARGET message + mavlink:send_chan(FOLT_MAV_CHAN:get(), mavlink_msgs.encode("FOLLOW_TARGET", follow_target_msg)) +end + +-- display welcome message +gcs:send_text(MAV_SEVERITY.INFO, "follow-target-send script loaded") + +-- update function to receive location from payload and move vehicle to reduce payload's oscillation +local function update() + + -- exit immediately if not enabled + if (FOLT_ENABLE:get() <= 0) then + return update, 1000 + end + + -- send FOLLOW_TARGET message + send_follow_target_msg() + + return update, UPDATE_INTERVAL_MS +end + +return update() diff --git a/libraries/AP_Scripting/applets/follow-target-send.md b/libraries/AP_Scripting/applets/follow-target-send.md new file mode 100644 index 0000000000..a0b203d989 --- /dev/null +++ b/libraries/AP_Scripting/applets/follow-target-send.md @@ -0,0 +1,15 @@ +# Follow Target Send + +Sends the FOLLOW_TARGET mavlink message to allow other vehicles to follow this one + +# Parameters + +- FOLT_ENABLE : Set to 1 to enable this script +- FOLT_MAV_CHAN : MAVLink channel to which FOLLOW_TARGET should be sent + +# How To Use + +1. copy this script to the autopilot's "scripts" directory +2. within the "scripts" directory create a "modules" directory +3. copy the MAVLink/mavlink_msgs_xxx files to the "scripts" directory +4. the FOLLOW_TARGET message will be published at 10hz From 92241daef16a1bd2e1cd2b3a98663611d501fd2c Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Mon, 9 Dec 2024 21:10:01 +1100 Subject: [PATCH 18/23] AP_InertialSensor: do not read FIFO faster than requested rate for ICM45686 --- .../AP_InertialSensor_Invensensev3.cpp | 74 +++++++++---------- .../AP_InertialSensor_Invensensev3.h | 3 +- 2 files changed, 36 insertions(+), 41 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index d22a7a224f..36d69db3fb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -343,15 +343,11 @@ void AP_InertialSensor_Invensensev3::start() // pre-calculate backend period backend_period_us = 1000000UL / backend_rate_hz; - if (!_imu.register_gyro(gyro_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype)) || - !_imu.register_accel(accel_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype))) { + if (!_imu.register_gyro(gyro_instance, sampling_rate_hz, dev->get_bus_id_devtype(devtype)) || + !_imu.register_accel(accel_instance, sampling_rate_hz, dev->get_bus_id_devtype(devtype))) { return; } - // update backend sample rate - _set_accel_raw_sample_rate(accel_instance, backend_rate_hz); - _set_gyro_raw_sample_rate(gyro_instance, backend_rate_hz); - // indicate what multiplier is appropriate for the sensors' // readings to fit them into an int16_t: _set_raw_sample_accel_multiplier(accel_instance, multiplier_accel); @@ -381,16 +377,15 @@ void AP_InertialSensor_Invensensev3::start() // get a startup banner to output to the GCS bool AP_InertialSensor_Invensensev3::get_output_banner(char* banner, uint8_t banner_len) { - if (fast_sampling) { - snprintf(banner, banner_len, "IMU%u: fast%s sampling enabled %.1fkHz", - gyro_instance, + snprintf(banner, banner_len, "IMU%u:%s%s sampling %.1fkHz/%.1fkHz", + gyro_instance, + fast_sampling ? " fast" : " normal", #if HAL_INS_HIGHRES_SAMPLE - highres_sampling ? ", high-resolution" : + highres_sampling ? " hi-res" : #endif - "" , backend_rate_hz * 0.001); - return true; - } - return false; + "", backend_rate_hz * 0.001, + sampling_rate_hz * 0.001); + return true; } /* @@ -669,21 +664,21 @@ void AP_InertialSensor_Invensensev3::register_write_bank(uint8_t bank, uint8_t r } // calculate the fast sampling backend rate -uint16_t AP_InertialSensor_Invensensev3::calculate_fast_sampling_backend_rate(uint16_t base_odr, uint16_t max_odr) const +uint16_t AP_InertialSensor_Invensensev3::calculate_fast_sampling_backend_rate(uint16_t base_backend_rate, uint16_t max_backend_rate) const { // constrain the gyro rate to be at least the loop rate - uint8_t loop_limit = 1; - if (get_loop_rate_hz() > base_odr) { - loop_limit = 2; + uint8_t min_base_rate_multiplier = 1; + if (get_loop_rate_hz() > base_backend_rate) { + min_base_rate_multiplier = 2; } - if (get_loop_rate_hz() > base_odr * 2) { - loop_limit = 4; + if (get_loop_rate_hz() > base_backend_rate * 2) { + min_base_rate_multiplier = 4; } // constrain the gyro rate to be a 2^N multiple - uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8); + uint8_t fast_sampling_rate_multiplier = constrain_int16(get_fast_sampling_rate(), min_base_rate_multiplier, 8); // calculate rate we will be giving samples to the backend - return constrain_int16(base_odr * fast_sampling_rate, base_odr, max_odr); + return constrain_int16(base_backend_rate * fast_sampling_rate_multiplier, base_backend_rate, max_backend_rate); } /* @@ -806,6 +801,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void) } } } + sampling_rate_hz = backend_rate_hz; // sampling rate and backend rate are the same // disable gyro and accel as per 12.9 in the ICM-42688 docs register_write(INV3REG_PWR_MGMT0, 0x00); @@ -862,6 +858,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void) void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void) { backend_rate_hz = 1600; + sampling_rate_hz = 1600; // use low-noise mode register_write(INV3REG_70_PWR_MGMT0, 0x0f); hal.scheduler->delay_microseconds(300); @@ -886,27 +883,24 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void) */ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void) { - uint8_t odr_config = 4; - backend_rate_hz = 1600; - // always fast sampling - fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; + uint8_t odr_config; + backend_rate_hz = 800; - if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() > 1) { - backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, backend_rate_hz * 4); + fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; + if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() && fast_sampling) { + // For ICM45686 we only set 3200 or 6400Hz as sampling rates + // we don't need to read FIFO faster than requested rate + backend_rate_hz = calculate_fast_sampling_backend_rate(800, 6400); + } else { + fast_sampling = false; } - // this sensor actually only supports 2 speeds - backend_rate_hz = constrain_int16(backend_rate_hz, 3200, 6400); - - switch (backend_rate_hz) { - case 6400: // 6.4Khz - odr_config = 3; - break; - case 3200: // 3.2Khz - odr_config = 4; - break; - default: - break; + if (backend_rate_hz <= 3200) { + odr_config = 0x04; // 3200Hz + sampling_rate_hz = 3200; + } else { + odr_config = 0x03; // 6400Hz + sampling_rate_hz = 6400; } // Disable FIFO first diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index 6b4fc49a8c..889fda4f76 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -58,7 +58,7 @@ private: void set_filter_and_scaling_icm42670(void); void set_filter_and_scaling_icm456xy(void); void fifo_reset(); - uint16_t calculate_fast_sampling_backend_rate(uint16_t base_odr, uint16_t max_odr) const; + uint16_t calculate_fast_sampling_backend_rate(uint16_t base_backend_rate, uint16_t max_backend_rate) const; /* Read samples from FIFO */ void read_fifo(); @@ -141,4 +141,5 @@ private: float temp_filtered; LowPassFilter2pFloat temp_filter; + uint32_t sampling_rate_hz; }; From 46b9f0601166952fa702ae2bda27196a42b7b89d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 25 Nov 2024 11:01:25 +1100 Subject: [PATCH 19/23] AP_AHRS: use TAS in DCM drift correction code Co-authored-by: luweiagi --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index a64821c145..23baa244ca 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -727,7 +727,7 @@ AP_AHRS_DCM::drift_correction(float deltat) float airspeed_TAS = _last_airspeed_TAS; #if AP_AIRSPEED_ENABLED if (airspeed_sensor_enabled()) { - airspeed_TAS = AP::airspeed()->get_airspeed(); + airspeed_TAS = AP::airspeed()->get_airspeed() * get_EAS2TAS(); } #endif From f5d81abb424c3a59aa59ba1fd7595f969468ac56 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 25 Nov 2024 11:05:09 +1100 Subject: [PATCH 20/23] AP_AHRS: return EAS from get_unconstrained_airspeed_EAS Co-authored-by: luweiagi --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 23baa244ca..a9179502d3 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1143,13 +1143,13 @@ bool AP_AHRS_DCM::get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float & if (AP::ahrs().get_wind_estimation_enabled() && have_gps()) { // estimated via GPS speed and wind - airspeed_ret = _last_airspeed_TAS; + airspeed_ret = _last_airspeed_TAS * get_TAS2EAS(); return true; } // Else give the last estimate, but return false. // This is used by the dead-reckoning code - airspeed_ret = _last_airspeed_TAS; + airspeed_ret = _last_airspeed_TAS * get_TAS2EAS(); return false; } From 300e5ccedd5c78fa1729b3294c238cc741af959a Mon Sep 17 00:00:00 2001 From: Andreas Gising Date: Fri, 28 Jun 2024 09:19:26 +0200 Subject: [PATCH 21/23] AP_Scripting: add glide-into-wind example without RC override --- .../AP_Scripting/examples/glide_into_wind.lua | 399 ++++++++++++++++++ 1 file changed, 399 insertions(+) create mode 100644 libraries/AP_Scripting/examples/glide_into_wind.lua diff --git a/libraries/AP_Scripting/examples/glide_into_wind.lua b/libraries/AP_Scripting/examples/glide_into_wind.lua new file mode 100644 index 0000000000..389ac155eb --- /dev/null +++ b/libraries/AP_Scripting/examples/glide_into_wind.lua @@ -0,0 +1,399 @@ +-- Glide into wind, LUA script for glide into wind functionality + +-- Background +-- When flying a fixed-wing drone on ad-hoc BVLOS missions, it might not be +-- suitable for the drone to return to home if the C2 link is lost, since that +-- might mean flying without control for an extended time and distance. One +-- option in ArduPlane is to set FS_Long to Glide, which makes the drone glide +-- and land in the direction it happened to have when the command was invoked, +-- without regard to the wind. This script offers a way to decrease the kinetic +-- energy in this blind landing by means of steering the drone towards the wind +-- as GLIDE is initiated, hence lowering the ground speed. The intention is to +-- minimize impact energy at landing - foremost for any third party, but also to +-- minimize damage to the drone. + +-- Functionality and setup +-- 1. Set SCR_ENABLE = 1 +-- 2. Put script in scripts folder, boot twice +-- 3. A new parameter has appeared: +-- - GLIDE_WIND_ENABL (0=disable, 1=enable) +-- 4. Set GLIDE_WIND_ENABL = 1 +-- 5. Read the docs on FS: +-- https://ardupilot.org/plane/docs/apms-failsafe-function.html#failsafe-parameters-and-their-meanings +-- 6. Set FS_LONG_ACTN = 2 +-- 7. Set FS_LONG_TIMEOUT as appropriate +-- 8. Set FS_GCS_ENABL = 1 +-- 9. If in simulation, set SIM_WIND_SPD = 4 to get a reliable wind direction. +-- 10. Test in simulation: Fly a mission, disable heartbeats by typing 'set +-- heartbeat 0' into mavproxy/SITL, monitor what happens in the console. If +-- QGC or similar GCS is used, make sure it does not send heartbeats. +-- 11. Test in flight: Fly a mission, monitor estimated wind direction from GCS, +-- then fail GCS link and see what happens. +-- 12. Once heading is into wind script will stop steering and not steer again +-- until state machine is reset and failsafe is triggered again. Steering in low +-- airspeeds (thr=0) increases risks of stall and it is preferable touch +-- ground in level roll attitude. If the script parameter hdg_ok_lim is set +-- to tight or the wind estimate is not stable, the script will anyhow stop +-- steering after override_time_lim and enter FBWA - otherwise the script +-- would hinder the GLIDE fail safe. +-- 13. Script will stop interfering as soon as a new goto-point is received or +-- the flight mode is changed by the operator or the remote pilot. + +-- During the fail safe maneuver a warning tune is played. + +-- State machine +-- CAN_TRIGGER +-- - Do: Nothing +-- - Change state: If the failsafe GLIDE is triggered: if FS_GCS_ENABL is set +-- and FS_LONG_ACTN is 2, change to TRIGGERED else change to CANCELED +-- +-- TRIGGERED +-- - Do: First use GUIDED mode to steer into wind, then switch to FBWA to +-- Glide into wind. Play warning tune. +-- - Change state: If flight mode is changed by operator/remote pilot or +-- operator/remote pilot sends a new goto point, change state to CANCELED +-- +-- CANCELED +-- - Do: Nothing +-- - Change state: When new heart beat arrive, change state to CAN_TRIGGER + +-- Credits +-- This script is developed by agising at UASolutions, commissioned by, and in +-- cooperation with Remote.aero, with funding from Swedish AeroEDIH, in response +-- to a need from the Swedish Sea Rescue Society (Sjöräddningssällskapet, SSRS). + +-- Disable diagnostics related to reading parameters to pass linter +---@diagnostic disable: need-check-nil +---@diagnostic disable: param-type-mismatch + +-- Tuning parameters +local looptime = 250 -- Short looptime +local long_looptime = 2000 -- Long looptime, GLIDE_WIND is not enabled +local tune_repeat_t = 1000 -- How often to play tune in glide into wind, [ms] +local hdg_ok_lim = 15 -- Acceptable heading error in deg (when to stop steering) +local hdg_ok_t_lim = 5000 -- Stop steering towards wind after hdg_ok_t_lim ms with error less than hdg_ok_lim +local override_time_lim = 15000 -- Max time in GUIDED during GLIDE, after limit set FBWA independent of hdg + +-- GCS text levels +local _INFO = 6 +local _WARNING = 4 + +-- Plane flight modes mapping +local mode_FBWA = 5 +local mode_GUIDED = 15 + +-- Tunes +local _tune_glide_warn = "MFT240 L16 cdefgfgfgfg" -- The warning tone played during GLIDE_WIND + +--State variable +local fs_state = nil + +-- Flags +local override_enable = false -- Flag to allow RC channel loverride + +-- Variables +local wind_dir_rad = nil -- param for wind dir in rad +local wind_dir_180 = nil -- param for wind dir in deg +local hdg_error = nil -- Heading error, hdg vs wind_dir_180 +local gw_enable = nil -- glide into wind enable flag +local hdg = nil -- vehicle heading +local wind = Vector3f() -- wind 3Dvector +local link_lost_for = nil -- link loss time counter +local last_seen = nil -- timestamp last received heartbeat +local tune_time_since = 0 -- Timer for last played tune +local hdg_ok_t = 0 -- Timer +local expected_flight_mode = nil -- Flight mode set by this script +local location_here = nil -- Current location +local location_upwind = nil -- Location to hold the target location +local user_notified = false -- Flag to keep track user being notified or not +local failed_location_counter = 0 -- Counter for failed location requests, possible GPS denied +local upwind_distance = 500 -- Distance to the upwind location, minimum 4x turn radius +local override_time = 0 -- Time since override started in ms + +-- Add param table +local PARAM_TABLE_KEY = 74 +local PARAM_TABLE_PREFIX = "GLIDE_WIND_" +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 30), 'could not add param table') + +------- +-- Init +------- + +function _init() + -- Add and init paramters + GLIDE_WIND_ENABL = bind_add_param('ENABL', 1, 0) + + -- Init parameters + FS_GCS_ENABL = bind_param('FS_GCS_ENABL') -- Is set to 1 if GCS lol should trigger FS after FS_LONG_TIMEOUT + FS_LONG_TIMEOUT = bind_param('FS_LONG_TIMEOUT') -- FS long timeout in seconds + FS_LONG_ACTN = bind_param('FS_LONG_ACTN') -- Is set to 2 for Glide + + send_to_gcs(_INFO, 'LUA: FS_LONG_TIMEOUT timeout: ' .. FS_LONG_TIMEOUT:get() .. 's') + + -- Test paramter + if GLIDE_WIND_ENABL:get() == nil then + send_to_gcs(_INFO, 'LUA: Something went wrong, GLIDE_WIND_ENABL not created') + return _init(), looptime + else + gw_enable = GLIDE_WIND_ENABL:get() + send_to_gcs(_INFO, 'LUA: GLIDE_WIND_ENABL: ' .. gw_enable) + end + + -- Init last_seen. + last_seen = gcs:last_seen() + + -- Init link_lost_for [ms] to FS_LONG_TIMEOUT [s] to prevent link to recover without + -- new heartbeat. This is to properly init the state machine. + link_lost_for = FS_LONG_TIMEOUT:get() * 1000 + + -- Warn if GLIDE_WIND_ENABL is set and FS_LONG_ACTN is not GLIDE + if gw_enable == 1 and FS_LONG_ACTN:get() ~= 2 then + send_to_gcs(_WARNING, 'GLIDE_WIND_ENABL is set, but FS_LONG_ACTN is not GLIDE.') + end + + -- Init fs_state machine to CANCELED. A heartbeat is required to set the state + -- to CAN_TRIGGER from where Glide into wind can be triggered. + fs_state = 'CANCELED' + + -- All set, go to update + return update(), long_looptime +end + + +------------ +-- Main loop +------------ + +function update() + -- Check if state of GLIDE_WIND_ENABL parameter changed, print every change + if gw_enable ~= GLIDE_WIND_ENABL:get() then + gw_enable = GLIDE_WIND_ENABL:get() + send_to_gcs(_INFO, 'LUA: GLIDE_WIND_ENABL: ' .. gw_enable) + -- If GLIDE_WIND_ENABL was enabled, warn if not FS_LONG_ACTN is set accordingly + if gw_enable == 1 then + if FS_LONG_ACTN:get() ~=2 then + send_to_gcs(_WARNING, 'GLIDE_WIND_ENABL is set, but FS_LONG_ACTN is not GLIDE.') + end + end + end + + -- -- If feature is not enabled, loop slowly + if gw_enable == 0 then + return update, long_looptime + end + + -- GLIDE_WIND_ENABL is enabled, look for triggers + -- Monitor time since last gcs heartbeat + if last_seen == gcs:last_seen() then + link_lost_for = link_lost_for + looptime + else + -- There has been a new heartbeat, update last_seen and reset link_lost_for + last_seen = gcs:last_seen() + link_lost_for = 0 + end + + -- Run the state machine + -- State CAN_TRIGGER + if fs_state == 'CAN_TRIGGER' then + if link_lost_for > FS_LONG_TIMEOUT:get() * 1000 then + -- Double check that FS_GCS_ENABL is set + if FS_GCS_ENABL:get() == 1 and FS_LONG_ACTN:get() == 2 then + fs_state = "TRIGGERED" + -- Reset some variables + hdg_ok_t = 0 + user_notified = false + override_enable = true + override_time = 0 + failed_location_counter = 0 + -- Set mode to GUIDED before entering TRIGGERED state + set_flight_mode(mode_GUIDED, 'LUA: Glide into wind state TRIGGERED') + else + -- Do not trigger glide into wind, require new heart beats to get here again + fs_state = "CANCELED" + end + end + -- State TRIGGERED + elseif fs_state == "TRIGGERED" then + -- Check for flight mode changes from outside script + if vehicle:get_mode() ~= expected_flight_mode then + fs_state = "CANCELED" + send_to_gcs(_INFO, 'LUA: Glide into wind state CANCELED: flight mode change') + end + + -- In GUIDED, check for target location changes from outside script (operator) + if vehicle:get_mode() == mode_GUIDED then + if not locations_are_equal(vehicle:get_target_location(), location_upwind) then + fs_state = "CANCELED" + send_to_gcs(_INFO, 'LUA: Glide into wind state CANCELED: new goto-point') + end + end + + -- State CANCELED + elseif fs_state == "CANCELED" then + -- Await link is not lost + if link_lost_for < FS_LONG_TIMEOUT:get() * 1000 then + fs_state = "CAN_TRIGGER" + send_to_gcs(_INFO, 'LUA: Glide into wind state CAN_TRIGGER') + end + end + + -- State TRIGGERED actions + if fs_state == "TRIGGERED" then + -- Get the heading angle + hdg = math.floor(math.deg(ahrs:get_yaw())) + + -- Get wind direction. Function wind_estimate returns x and y for direction wind blows in, add pi to get true wind dir + wind = ahrs:wind_estimate() + wind_dir_rad = math.atan(wind:y(), wind:x())+math.pi + wind_dir_180 = math.floor(wrap_180(math.deg(wind_dir_rad))) + hdg_error = wrap_180(wind_dir_180 - hdg) + + -- Check if we are close to target heading + if math.abs(hdg_error) < hdg_ok_lim then + -- If we have been close to target heading for hdg_ok_t_lim, switch back to FBWA + if hdg_ok_t > hdg_ok_t_lim then + if override_enable then + set_flight_mode(mode_FBWA,'LUA: Glide into wind steering complete, GLIDE in FBWA') + end + -- Do not override again until state machine has triggered again + override_enable = false + else + hdg_ok_t = hdg_ok_t + looptime + end + -- Heading error is big, reset timer hdg_ok_t + else + hdg_ok_t = 0 + end + + -- Play tune every tune_repeat_t [ms] + if tune_time_since > tune_repeat_t then + -- Play tune and reset timer + send_to_gcs(_INFO, 'LUA: Play warning tune') + play_tune(_tune_glide_warn) + tune_time_since = 0 + else + tune_time_since = tune_time_since + looptime + end + + -- If not steered into wind yet, update goto point into wind + if override_enable then + -- Check override time, if above limit, switch back to FBWA + override_time = override_time + looptime + if override_time > override_time_lim then + set_flight_mode(mode_FBWA, "LUA: Glide into wind override time out, GLIDE in current heading") + override_enable = false + end + -- Get current position and handle if not valid + location_here = ahrs:get_location() + if location_here == nil then + -- In case we cannot get location for some time we must give up and continue with GLIDE + failed_location_counter = failed_location_counter + 1 + if failed_location_counter > 5 then + set_flight_mode(mode_FBWA, "LUA: Glide failed to get location, GLIDE in current heading") + override_enable = false + return update, looptime + end + gcs:send_text(_WARNING, "LUA: Glide failed to get location") + return update, looptime + end + -- Calc upwind position, copy and modify location_here + location_upwind = location_here:copy() + location_upwind:offset_bearing(wind_dir_180, upwind_distance) + + -- Set location_upwind as GUIDED target + if vehicle:set_target_location(location_upwind) then + if not user_notified then + send_to_gcs(_INFO, "LUA: Guided target set " .. upwind_distance .. "m away at bearing " .. wind_dir_180) + -- Just notify once + user_notified = true + end + else + -- Most likely we are not in GUIDED anymore (operator changed mode), state machine will handle this in next loop. + gcs:send_text(_WARNING, "LUA: Glide failed to set upwind target") + end + end + end + return update, looptime +end + + +------------------- +-- Helper functions +------------------- + +-- Set mode and wait for mode change +function set_flight_mode(mode, message) + expected_flight_mode = mode + vehicle:set_mode(expected_flight_mode) + return wait_for_mode_change(mode, message, 0) +end + +-- Wait for mode change +function wait_for_mode_change(mode, message, attempt) + -- If mode change does not go through after 10 attempts, give up + if attempt > 10 then + send_to_gcs(_WARNING, 'LUA: Glide into wind mode change failed.') + return update, looptime + -- If mode change does not go through, wait and try again + elseif vehicle:get_mode() ~= mode then + return wait_for_mode_change(mode, message, attempt + 1), 5 + -- Mode change has gone through + else + send_to_gcs(_INFO, message) + return update, looptime + end +end + +-- Function to compare two Location objects +function locations_are_equal(loc1, loc2) + -- If either location is nil, they are not equal + if not loc1 or not loc2 then + return false + end + -- Compare latitude and longitude, return bool + return loc1:lat() == loc2:lat() and loc1:lng() == loc2:lng() +end + +-- bind a parameter to a variable +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p +end + +-- Add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end + +-- Print to GCS +function send_to_gcs(level, mess) + gcs:send_text(level, mess) +end + +-- Play tune +function play_tune(tune) + notify:play_tune(tune) +end + +-- Returns the angle in range 0-360 +function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +-- Returns the angle in range -180-180 +function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + +-- Start up the script +return _init, 2000 From 42e2e23464d489bcd3c982555e3dc737772dc42f Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 18 Dec 2024 17:42:30 +1100 Subject: [PATCH 22/23] AP_Networking: fix memory error generated by CAN mcast driver --- libraries/AP_Networking/AP_Networking_CAN.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Networking/AP_Networking_CAN.cpp b/libraries/AP_Networking/AP_Networking_CAN.cpp index 875fece574..250e78b022 100644 --- a/libraries/AP_Networking/AP_Networking_CAN.cpp +++ b/libraries/AP_Networking/AP_Networking_CAN.cpp @@ -182,7 +182,11 @@ void AP_Networking_CAN::mcast_server(void) const uint16_t timeout_us = 2000; while (frame_buffers[bus]->peek(frame)) { - auto retcode = get_caniface(bus)->send(frame, + auto *cbus = get_caniface(bus); + if (cbus == nullptr) { + break; + } + auto retcode = cbus->send(frame, AP_HAL::micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN); if (retcode == 0) { From 5c9003dd027ded0c1ea7d79b83b0dc430777807a Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Tue, 17 Dec 2024 09:19:14 -0800 Subject: [PATCH 23/23] AP_Volz_Protocol: Moved member variable definition within proper #define block to prevent unused variable warning --- libraries/AP_Volz_Protocol/AP_Volz_Protocol.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h index d17fa76570..c6c9bc338f 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -88,7 +88,6 @@ private: // Loop in thread to output to uart void loop(); - uint8_t sent_count; void init(void); @@ -120,6 +119,8 @@ private: void read_telem(); void process_response(const CMD &cmd); + uint8_t sent_count; + struct { CMD_ID types[3] { CMD_ID::READ_CURRENT,