From 81a30e8d107bad2f9b8683e102b2ab6e605bcf6a Mon Sep 17 00:00:00 2001 From: Hubert <1701213518@sz.pku.edu.cn> Date: Sat, 9 Nov 2024 16:43:11 +0800 Subject: [PATCH 001/117] AP_Bootloader: ID reserved for MicoAir743AIOv1 --- Tools/AP_Bootloader/board_types.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index a0ab11ed0a3f2..16eda3f59e8dc 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -291,6 +291,7 @@ AP_HW_PhenixH7_Pro 1172 AP_HW_2RAWH743 1173 AP_HW_X-MAV-AP-H743V2 1174 AP_HW_BETAFPV_F4_2-3S_20A 1175 +AP_HW_MicoAir743AIOv1 1176 AP_HW_FlywooF405HD_AIOv2 1180 AP_HW_FlywooH743Pro 1181 From ce0babc8f15f100a47523ed422532e9dbbf830b4 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 9 Nov 2024 15:29:57 +0000 Subject: [PATCH 002/117] Plane: QuadPlane: Clear pilot corrections on mode change to avoid getting stuck in QLand --- ArduPlane/mode_qland.cpp | 1 - ArduPlane/quadplane.cpp | 5 +++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index f3a08aabde762..e59e3c9a50e95 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -9,7 +9,6 @@ bool ModeQLand::_enter() quadplane.throttle_wait = false; quadplane.setup_target_position(); poscontrol.set_state(QuadPlane::QPOS_LAND_DESCEND); - poscontrol.pilot_correction_done = false; quadplane.last_land_final_agl = plane.relative_ground_altitude(plane.g.rangefinder_landing); quadplane.landing_detect.lower_limit_start_ms = 0; quadplane.landing_detect.land_start_ms = 0; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index e7fc346758491..87be3e1160436 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -4596,6 +4596,11 @@ void QuadPlane::mode_enter(void) poscontrol.last_velocity_match_ms = 0; poscontrol.set_state(QuadPlane::QPOS_NONE); + // Clear any pilot corrections + poscontrol.pilot_correction_done = false; + poscontrol.pilot_correction_active = false; + poscontrol.target_vel_cms.zero(); + // clear guided takeoff wait on any mode change, but remember the // state for special behaviour guided_wait_takeoff_on_mode_enter = guided_wait_takeoff; From 47342db4164603dc980b1d36808e3276ddfe8b2d Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 10 Nov 2024 12:12:24 +0000 Subject: [PATCH 003/117] Plane: remove unused `ChannelMixing` enum --- ArduPlane/defines.h | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b2939557adfdf..a3da572611c77 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -64,18 +64,6 @@ enum class RtlAutoland { }; -enum ChannelMixing { - MIXING_DISABLED = 0, - MIXING_UPUP = 1, - MIXING_UPDN = 2, - MIXING_DNUP = 3, - MIXING_DNDN = 4, - MIXING_UPUP_SWP = 5, - MIXING_UPDN_SWP = 6, - MIXING_DNUP_SWP = 7, - MIXING_DNDN_SWP = 8, -}; - // PID broadcast bitmask enum tuning_pid_bits { TUNING_BITS_ROLL = (1 << 0), From 8a5556cb4efc395262c8bbe0b51b4c7ef30b92be Mon Sep 17 00:00:00 2001 From: muramura Date: Sat, 5 Oct 2024 11:06:54 +0900 Subject: [PATCH 004/117] Copter: Consolidate processing --- ArduCopter/mode_poshold.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index c06072c597f90..40a86a8cff42e 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -535,11 +535,7 @@ void ModePosHold::update_brake_angle_from_velocity(float &brake_angle, float vel } // calculate velocity-only based lean angle - if (velocity >= 0) { - lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (velocity + 60.0f)); - } else { - lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (-velocity + 60.0f)); - } + lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (fabsf(velocity) + 60.0f)); // do not let lean_angle be too far from brake_angle brake_angle = constrain_float(lean_angle, brake_angle - brake_rate, brake_angle + brake_rate); From da69e2267392788b709eefa24395595a98301921 Mon Sep 17 00:00:00 2001 From: muramura Date: Sat, 5 Oct 2024 14:43:57 +0900 Subject: [PATCH 005/117] Copter: Use GRAVITY_MSS --- ArduCopter/mode_poshold.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index 40a86a8cff42e..5474aaa84151f 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -588,7 +588,7 @@ void ModePosHold::update_wind_comp_estimate() } // limit acceleration - const float accel_lim_cmss = tanf(radians(POSHOLD_WIND_COMP_LEAN_PCT_MAX * copter.aparm.angle_max * 0.01f)) * 981.0f; + const float accel_lim_cmss = tanf(radians(POSHOLD_WIND_COMP_LEAN_PCT_MAX * copter.aparm.angle_max * 0.01f)) * (GRAVITY_MSS*100); const float wind_comp_ef_len = wind_comp_ef.length(); if (!is_zero(accel_lim_cmss) && (wind_comp_ef_len > accel_lim_cmss)) { wind_comp_ef *= accel_lim_cmss / wind_comp_ef_len; From cc1ebe65299ace499f7a6fda9438948a788aa5c3 Mon Sep 17 00:00:00 2001 From: Pradeep CK Date: Mon, 11 Nov 2024 13:37:02 +1100 Subject: [PATCH 006/117] AP_BattMonitor : update metadata for fuellevel param ranges --- .../AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp index ecd2c79b89b25..d5d6c324bf7e8 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp @@ -63,28 +63,28 @@ const AP_Param::GroupInfo AP_BattMonitor_FuelLevel_Analog::var_info[] = { // @Param: FL_FF // @DisplayName: First order term // @Description: First order polynomial fit term - // @Range: 0 10 + // @Range: -10 10 // @User: Advanced AP_GROUPINFO("FL_FF", 45, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_first_order_coeff, 1), // @Param: FL_FS // @DisplayName: Second order term // @Description: Second order polynomial fit term - // @Range: 0 10 + // @Range: -10 10 // @User: Advanced AP_GROUPINFO("FL_FS", 46, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_second_order_coeff, 0), // @Param: FL_FT // @DisplayName: Third order term // @Description: Third order polynomial fit term - // @Range: 0 10 + // @Range: -10 10 // @User: Advanced AP_GROUPINFO("FL_FT", 47, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_third_order_coeff, 0), // @Param: FL_OFF // @DisplayName: Offset term // @Description: Offset polynomial fit term - // @Range: 0 10 + // @Range: -10 10 // @User: Advanced AP_GROUPINFO("FL_OFF", 48, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_offset, 0), From 34b306e10731fd84fd01a218913c9bb38e8611f7 Mon Sep 17 00:00:00 2001 From: kfruson Date: Thu, 7 Nov 2024 14:49:02 -0700 Subject: [PATCH 007/117] AP_Volz_Protocol: bugfix with scaling integer --- libraries/AP_Volz_Protocol/AP_Volz_Protocol.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h index 02b18fc4ea421..f14df3023090d 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -140,8 +140,8 @@ class AP_Volz_Protocol { float secondary_current; float primary_voltage; float secondary_voltage; - uint16_t motor_temp_deg; - uint16_t pcb_temp_deg; + int16_t motor_temp_deg; + int16_t pcb_temp_deg; } data[NUM_SERVO_CHANNELS]; uint32_t last_log_ms; } telem; From f7aabed164b7be868396c8aa145af88389c1ea28 Mon Sep 17 00:00:00 2001 From: kfruson <6444332+DjMixMasterDragon@users.noreply.github.com> Date: Mon, 11 Nov 2024 13:19:11 -0700 Subject: [PATCH 008/117] AP_Volz_Protocol: update logging format with integer change --- libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index 43f1c17feb705..4dac4792b3bc3 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -405,7 +405,7 @@ void AP_Volz_Protocol::update() "TimeUS,I,Dang,ang,pc,sc,pv,sv,mt,pt", "s#ddAAvvOO", "F000000000", - "QBffffffHH", + "QBffffffhh", AP_HAL::micros64(), i + 1, // convert to 1 indexed to match actuator IDs and SERVOx numbering telem.data[i].desired_angle, From 9dfcb487cfb9c7a466a4035f432b5e0139c83b74 Mon Sep 17 00:00:00 2001 From: Hwurzburg Date: Mon, 28 Dec 2020 16:07:46 -0600 Subject: [PATCH 009/117] AP_Mount: add RC failsafe action --- libraries/AP_Mount/AP_Mount_Backend.h | 1 + libraries/AP_Mount/AP_Mount_Params.cpp | 2 +- libraries/AP_Mount/AP_Mount_Servo.cpp | 8 ++++++++ 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 16082d58f450b..a25bd4870a4e2 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -249,6 +249,7 @@ class AP_Mount_Backend // options parameter bitmask handling enum class Options : uint8_t { RCTARGETING_LOCK_FROM_PREVMODE = (1U << 0), // RC_TARGETING mode's lock/follow state maintained from previous mode + NEUTRAL_ON_RC_FS = (1U << 1), // move mount to netral position on RC failsafe }; bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; } diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index e2c727f4b41c4..d077c603ec976 100644 --- a/libraries/AP_Mount/AP_Mount_Params.cpp +++ b/libraries/AP_Mount/AP_Mount_Params.cpp @@ -168,7 +168,7 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = { // @Param: _OPTIONS // @DisplayName: Mount options // @Description: Mount options bitmask - // @Bitmask: 0:RC lock state from previous mode + // @Bitmask: 0:RC lock state from previous mode, 1:Return to neutral angles on RC failsafe // @User: Standard AP_GROUPINFO("_OPTIONS", 16, AP_Mount_Params, options, 0), diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 1fba00e92a1be..7c97656c1232f 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -56,6 +56,13 @@ void AP_Mount_Servo::update() // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { + // update targets using pilot's rc inputs or go to neutral or retracted targets if no rc + if (rc().in_rc_failsafe()) { + if (option_set(Options::NEUTRAL_ON_RC_FS)) { + mnt_target.angle_rad.set(_angle_bf_output_rad, false); + mnt_target.target_type = MountTargetType::ANGLE; + } + } else { // update targets using pilot's RC inputs MountTarget rc_target; get_rc_target(mnt_target.target_type, rc_target); @@ -67,6 +74,7 @@ void AP_Mount_Servo::update() mnt_target.rate_rads = rc_target; break; } + } break; } From 500ec85e52002f36472dea2bc1c2cf77d2e59dda Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 Apr 2021 20:15:20 +1000 Subject: [PATCH 010/117] autotest: add test for mount retract on rc failsafe --- Tools/autotest/helicopter.py | 43 ++++++++++++++++++++++++++++ Tools/autotest/vehicle_test_suite.py | 7 +++-- 2 files changed, 48 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/helicopter.py b/Tools/autotest/helicopter.py index 9f3f57cc4ddaf..c02608dbb2e3e 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -708,6 +708,48 @@ def NastyMission(self): self.change_mode('LOITER') self.fly_mission_points(self.scurve_nasty_up_mission()) + def MountFailsafeAction(self): + """Fly Mount Failsafe action""" + self.context_push() + + self.progress("Setting up servo mount") + roll_servo = 12 + pitch_servo = 11 + yaw_servo = 10 + open_servo = 9 + roll_limit = 50 + self.set_parameters({ + "MNT1_TYPE": 1, + "SERVO%u_MIN" % roll_servo: 1000, + "SERVO%u_MAX" % roll_servo: 2000, + "SERVO%u_FUNCTION" % yaw_servo: 6, # yaw + "SERVO%u_FUNCTION" % pitch_servo: 7, # roll + "SERVO%u_FUNCTION" % roll_servo: 8, # pitch + "SERVO%u_FUNCTION" % open_servo: 9, # mount open + "MNT1_OPTIONS": 2, # retract + "MNT1_DEFLT_MODE": 3, # RC targettting + "MNT1_ROLL_MIN": -roll_limit, + "MNT1_ROLL_MAX": roll_limit, + }) + + self.reboot_sitl() + + retract_roll = 25.0 + self.set_parameter("MNT1_NEUTRAL_X", retract_roll) + self.progress("Killing RC") + self.set_parameter("SIM_RC_FAIL", 2) + self.delay_sim_time(10) + want_servo_channel_value = int(1500 + 500*retract_roll/roll_limit) + self.wait_servo_channel_value(roll_servo, want_servo_channel_value, epsilon=1) + + self.progress("Resurrecting RC") + self.set_parameter("SIM_RC_FAIL", 0) + self.wait_servo_channel_value(roll_servo, 1500) + + self.context_pop() + + self.reboot_sitl() + def set_rc_default(self): super(AutoTestHelicopter, self).set_rc_default() self.progress("Lowering rotor speed") @@ -1122,6 +1164,7 @@ def tests(self): self.NastyMission, self.PIDNotches, self.AutoTune, + self.MountFailsafeAction, ]) return ret diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 3cab908b5951c..5885c21267fc9 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -7939,7 +7939,7 @@ def get_servo_channel_value(self, channel, timeout=2): (str(m), channel_field)) return m_value - def wait_servo_channel_value(self, channel, value, timeout=2, comparator=operator.eq): + def wait_servo_channel_value(self, channel, value, epsilon=0, timeout=2, comparator=operator.eq): """wait for channel value comparison (default condition is equality)""" channel_field = "servo%u_raw" % channel opstring = ("%s" % comparator)[-3:-1] @@ -7957,8 +7957,11 @@ def wait_servo_channel_value(self, channel, value, timeout=2, comparator=operato if m_value is None: raise ValueError("message (%s) has no field %s" % (str(m), channel_field)) - self.progress("want SERVO_OUTPUT_RAW.%s=%u %s %u" % + self.progress("SERVO_OUTPUT_RAW.%s got=%u %s want=%u" % (channel_field, m_value, opstring, value)) + if comparator == operator.eq: + if abs(m_value - value) <= epsilon: + return m_value if comparator(m_value, value): return m_value From dcc04d685f2b8fa91fba329eb65306af316583c2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 14:47:30 +1100 Subject: [PATCH 011/117] AP_Mount: factor out update_mnt_target_from_rc_target from servo, use it elsewhere this gives all backends the neutral-on-RC-failsafe behaviour --- libraries/AP_Mount/AP_Mount_Alexmos.cpp | 15 ++---------- libraries/AP_Mount/AP_Mount_Backend.cpp | 23 +++++++++++++++++++ libraries/AP_Mount/AP_Mount_Backend.h | 3 +++ libraries/AP_Mount/AP_Mount_Gremsy.cpp | 15 ++---------- libraries/AP_Mount/AP_Mount_SToRM32.cpp | 15 ++---------- .../AP_Mount/AP_Mount_SToRM32_serial.cpp | 15 ++---------- libraries/AP_Mount/AP_Mount_Scripting.cpp | 15 ++---------- libraries/AP_Mount/AP_Mount_Servo.cpp | 23 ++----------------- libraries/AP_Mount/AP_Mount_Siyi.cpp | 15 ++---------- libraries/AP_Mount/AP_Mount_SoloGimbal.cpp | 14 ++--------- libraries/AP_Mount/AP_Mount_Topotek.cpp | 15 ++---------- libraries/AP_Mount/AP_Mount_Viewpro.cpp | 15 ++---------- libraries/AP_Mount/AP_Mount_Xacti.cpp | 15 ++---------- 13 files changed, 48 insertions(+), 150 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 693d197860897..7db333f6b7849 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -100,20 +100,9 @@ void AP_Mount_Alexmos::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 9d064633b1916..94f9e7e31b5c9 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -66,6 +66,29 @@ bool AP_Mount_Backend::set_mode(MAV_MOUNT_MODE mode) return true; } +// called when mount mode is RC-targetting, updates the mnt_target object from RC inputs: +void AP_Mount_Backend::update_mnt_target_from_rc_target() +{ + if (rc().in_rc_failsafe()) { + if (option_set(Options::NEUTRAL_ON_RC_FS)) { + mnt_target.angle_rad.set(_params.neutral_angles.get() * DEG_TO_RAD, false); + mnt_target.target_type = MountTargetType::ANGLE; + return; + } + } + + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + mnt_target.angle_rad = rc_target; + break; + case MountTargetType::RATE: + mnt_target.rate_rads = rc_target; + break; + } +} + // set angle target in degrees // roll and pitch are in earth-frame // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index a25bd4870a4e2..67b673d656ebe 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -253,6 +253,9 @@ class AP_Mount_Backend }; bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; } + // called when mount mode is RC-targetting, updates the mnt_target object from RC inputs: + void update_mnt_target_from_rc_target(); + // returns true if user has configured a valid roll angle range // allows user to disable roll even on 3-axis gimbal bool roll_range_valid() const { return (_params.roll_angle_min < _params.roll_angle_max); } diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index 259de8e90cc4d..631acc87784e4 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -48,20 +48,9 @@ void AP_Mount_Gremsy::update() } // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index 59e4da66bcbeb..bd638fa42ab74 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -52,21 +52,10 @@ void AP_Mount_SToRM32::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); resend_now = true; break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 82387a0e93ec1..9b003de0fe4b0 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -49,21 +49,10 @@ void AP_Mount_SToRM32_serial::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); resend_now = true; break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index c2b93304e67ee..e70c03003616e 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -45,21 +45,10 @@ void AP_Mount_Scripting::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); target_loc_valid = false; break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 7c97656c1232f..743708fa9dd8b 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -55,28 +55,9 @@ void AP_Mount_Servo::update() } // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's rc inputs or go to neutral or retracted targets if no rc - if (rc().in_rc_failsafe()) { - if (option_set(Options::NEUTRAL_ON_RC_FS)) { - mnt_target.angle_rad.set(_angle_bf_output_rad, false); - mnt_target.target_type = MountTargetType::ANGLE; - } - } else { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index d0e82cd503ee1..1c91af6a43fc9 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -121,20 +121,9 @@ void AP_Mount_Siyi::update() break; } - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 02c8c3f2475c1..251c859693b2d 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -65,20 +65,10 @@ void AP_Mount_SoloGimbal::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { + case MAV_MOUNT_MODE_RC_TARGETING: _gimbal.set_lockedToBody(false); - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 09682a8429b64..bfe93ec83a8ed 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -156,20 +156,9 @@ void AP_Mount_Topotek::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index f9b06f3b80ad3..1de89fbc6722b 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -94,20 +94,9 @@ void AP_Mount_Viewpro::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index 6353ce41d00d3..c5727f6c023d8 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -135,20 +135,9 @@ void AP_Mount_Xacti::update() break; } - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: From b7e3c8c71d50fe975a3f144e8c5053c1cc05ee61 Mon Sep 17 00:00:00 2001 From: Simon Hancock Date: Sun, 18 Feb 2024 18:14:17 +0000 Subject: [PATCH 012/117] AP_HAL: Add @LoggerEnum tags around BOARD/SUBTYPE #defines --- libraries/AP_HAL/AP_HAL_Boards.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 1c3aa1b1648e7..26a47e2c8cd99 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -6,6 +6,7 @@ */ #pragma once +// @LoggerEnum: HAL_BOARD #define HAL_BOARD_SITL 3 // #define HAL_BOARD_SMACCM 4 // unused // #define HAL_BOARD_PX4 5 // unused @@ -16,7 +17,9 @@ #define HAL_BOARD_ESP32 12 #define HAL_BOARD_QURT 13 #define HAL_BOARD_EMPTY 99 +// @LoggerEnumEnd +// @LoggerEnum: HAL_BOARD_SUBTYPE /* Default board subtype is -1 */ #define HAL_BOARD_SUBTYPE_NONE -1 @@ -70,6 +73,7 @@ #define HAL_BOARD_SUBTYPE_ESP32_NICK 6006 #define HAL_BOARD_SUBTYPE_ESP32_S3DEVKIT 6007 #define HAL_BOARD_SUBTYPE_ESP32_S3EMPTY 6008 +// @LoggerEnumEnd /* InertialSensor driver types */ #define HAL_INS_NONE 0 From df9c36fee303074a0a9688834c18a90eb4725044 Mon Sep 17 00:00:00 2001 From: Simon Hancock Date: Sun, 18 Feb 2024 18:15:26 +0000 Subject: [PATCH 013/117] AP_Vehicle: Add @LoggerEnum tags around APM_BUILD #defines --- libraries/AP_Vehicle/AP_Vehicle_Type.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle_Type.h b/libraries/AP_Vehicle/AP_Vehicle_Type.h index 0bc298658f8c8..3636f2364983c 100644 --- a/libraries/AP_Vehicle/AP_Vehicle_Type.h +++ b/libraries/AP_Vehicle/AP_Vehicle_Type.h @@ -19,6 +19,7 @@ Also note that code needs to support other APM_BUILD_DIRECTORY values for example sketches */ +// @LoggerEnum: APM_BUILD #define APM_BUILD_Rover 1 #define APM_BUILD_ArduCopter 2 #define APM_BUILD_ArduPlane 3 @@ -32,6 +33,7 @@ #define APM_BUILD_AP_Bootloader 11 #define APM_BUILD_Blimp 12 #define APM_BUILD_Heli 13 +// @LoggerEnumEnd #ifdef APM_BUILD_DIRECTORY /* From 43272dd9ee449440ac0303d89dde571d6bad6734 Mon Sep 17 00:00:00 2001 From: Simon Hancock Date: Sun, 18 Feb 2024 18:18:20 +0000 Subject: [PATCH 014/117] autotest: Handle @LoggerEnum tags for #define sets --- Tools/autotest/logger_metadata/enum_parse.py | 28 ++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/logger_metadata/enum_parse.py b/Tools/autotest/logger_metadata/enum_parse.py index d9ab17f4b41af..3f37a9837f443 100755 --- a/Tools/autotest/logger_metadata/enum_parse.py +++ b/Tools/autotest/logger_metadata/enum_parse.py @@ -92,6 +92,11 @@ def match_enum_line(self, line): if m is not None: return (m.group(1), 1 << int(m.group(2)), m.group(3)) + # Match: "#define FRED 1 // optional comment" + m = re.match(r"#define\s*([A-Z0-9_a-z]+)\s+(-?\d+) *(// *(.*) *)?$", line) + if m is not None: + return (m.group(1), m.group(2), m.group(4)) + if m is None: raise ValueError("Failed to match (%s)" % line) @@ -116,7 +121,13 @@ def debug(x): break line = line.rstrip() # print("state=%s line: %s" % (state, line)) - if re.match(r"\s*//.*", line): + # Skip single-line comments - unless they contain LoggerEnum tags + if re.match(r"\s*//.*", line) and "LoggerEnum" not in line: + continue + # Skip multi-line comments + if re.match(r"\s*/\*.*", line): + while "*/" not in line: + line = f.readline() continue if state == "outside": if re.match("class .*;", line) is not None: @@ -154,6 +165,19 @@ def debug(x): last_value = None state = state_inside skip_enumeration = False + continue + + # // @LoggerEnum: NAME - can be used around for #define sets + m = re.match(r".*@LoggerEnum: *([\w]+)", line) + if m is not None: + enum_name = m.group(1) + debug("%s: %s" % (source_file, enum_name)) + entries = [] + last_value = None + state = state_inside + skip_enumeration = False + continue + continue if state == "inside": if re.match(r"\s*$", line): @@ -164,7 +188,7 @@ def debug(x): continue if re.match(r"#else", line): continue - if re.match(r".*}\s*\w*(\s*=\s*[\w:]+)?;", line): + if re.match(r".*}\s*\w*(\s*=\s*[\w:]+)?;", line) or "@LoggerEnumEnd" in line: # potential end of enumeration if not skip_enumeration: if enum_name is None: From 67412c99978336a205cabbc201e1f9fc0caaa0ba Mon Sep 17 00:00:00 2001 From: Simon Hancock Date: Sun, 18 Feb 2024 18:16:27 +0000 Subject: [PATCH 015/117] AP_Logger: Add enums to VER message --- libraries/AP_Logger/LogStructure.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index ae5197297c373..432ce26376bb5 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1134,7 +1134,9 @@ struct PACKED log_VER { // @Description: Ardupilot version // @Field: TimeUS: Time since system startup // @Field: BT: Board type +// @FieldValueEnum: BT: HAL_BOARD // @Field: BST: Board subtype +// @FieldValueEnum: BST: HAL_BOARD_SUBTYPE // @Field: Maj: Major version number // @Field: Min: Minor version number // @Field: Pat: Patch number @@ -1143,6 +1145,7 @@ struct PACKED log_VER { // @Field: FWS: Firmware version string // @Field: APJ: Board ID // @Field: BU: Build vehicle type +// @FieldValueEnum: BU: APM_BUILD // @Field: FV: Filter version // @LoggerMessage: MOTB From 1f54cf39d57d6ad48eb8fc9cb93dbcf122fbe21d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 13 Mar 2024 08:32:42 +0000 Subject: [PATCH 016/117] AP_HAL_ChibiOS: FoxeerH743v2 --- libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat index 97e2b86c68c14..8f0f642cdebb7 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FoxeerH743v1/hwdef.dat @@ -159,7 +159,9 @@ BARO DPS310 I2C:0:0x76 # IMU setup SPIDEV imu1 SPI2 DEVID1 GYRO1_CS MODE3 1*MHZ 8*MHZ +SPIDEV imu2 SPI2 DEVID1 GYRO1_CS MODE3 1*MHZ 16*MHZ IMU Invensense SPI:imu1 ROTATION_YAW_270 +IMU Invensensev3 SPI:imu2 ROTATION_YAW_180 DMA_NOSHARE TIM3_UP TIM4_UP TIM8_UP SPI2* DMA_PRIORITY TIM3_UP TIM4_UP TIM8_UP SPI2* From 7d426f4741c7f77b03c26b98899e5d3d89503e8f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 8 Aug 2024 12:13:25 +1000 Subject: [PATCH 017/117] AP_Mission: do not use float functions on integers pitch is int8_t, yaw is int16_t --- libraries/AP_Mission/AP_Mission_Commands.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index bc0374232050d..ab69c468245a6 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -330,8 +330,8 @@ bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Miss } // handle angle target - const bool pitch_angle_valid = !isnan(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) && (fabsF(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) <= 90); - const bool yaw_angle_valid = !isnan(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg) && (fabsF(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg) <= 360); + const bool pitch_angle_valid = abs(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) <= 90; + const bool yaw_angle_valid = abs(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg) <= 360; if (pitch_angle_valid && yaw_angle_valid) { mount->set_angle_target(gimbal_instance, 0, cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg, cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg, cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); return true; From d76132ec632e6691d4118fb76a0be68eab75f71d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 19 Sep 2024 15:26:53 +0100 Subject: [PATCH 018/117] AP_InertialSensor: ensure fifo reads use transfer() to optimize buffer allocation and copying --- .../AP_InertialSensor_Invensensev3.cpp | 25 +++++++++++++++++-- 1 file changed, 23 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index 024d1f56a23b7..fc0bc9ecad79b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -555,9 +555,30 @@ void AP_InertialSensor_Invensensev3::read_fifo() while (n_samples > 0) { uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN); - if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * fifo_sample_size)) { - goto check_registers; + + if (!dev->set_chip_select(true)) { + if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * fifo_sample_size)) { + goto check_registers; + } + } else { + // we don't use read_registers() here to ensure that the fifo buffer that we have allocated + // gets passed all the way down to the SPI DMA handling. This involves one transfer to send + // the register read and then another using the same buffer and length which is handled specially + // for the read + uint8_t reg = reg_data | BIT_READ_FLAG; + if (!dev->transfer(®, 1, nullptr, 0)) { + dev->set_chip_select(false); + goto check_registers; + } + // transfer will also be sending data, make sure that data is zero + memset((uint8_t*)fifo_buffer, 0, n * fifo_sample_size); + if (!dev->transfer((uint8_t*)fifo_buffer, n * fifo_sample_size, (uint8_t*)fifo_buffer, n * fifo_sample_size)) { + dev->set_chip_select(false); + goto check_registers; + } + dev->set_chip_select(false); } + #if HAL_INS_HIGHRES_SAMPLE if (highres_sampling) { if (!accumulate_highres_samples((FIFODataHighRes*)fifo_buffer, n)) { From c0ce5e5ed0edb46c962e5393196d79346e9225b3 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 23 Sep 2024 11:37:11 +0100 Subject: [PATCH 019/117] AP_InertialSensor: optimize Invensense v3 FIF read --- .../AP_InertialSensor_Invensensev3.cpp | 45 ++++++++----------- 1 file changed, 18 insertions(+), 27 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index fc0bc9ecad79b..daf54ed64e7ec 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -185,12 +185,12 @@ AP_InertialSensor_Invensensev3::~AP_InertialSensor_Invensensev3() #if HAL_INS_HIGHRES_SAMPLE if (highres_sampling) { if (fifo_buffer != nullptr) { - hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE + 1, AP_HAL::Util::MEM_DMA_SAFE); } } else #endif if (fifo_buffer != nullptr) { - hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE + 1, AP_HAL::Util::MEM_DMA_SAFE); } } @@ -358,10 +358,10 @@ void AP_InertialSensor_Invensensev3::start() // allocate fifo buffer #if HAL_INS_HIGHRES_SAMPLE if (highres_sampling) { - fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE + 1, AP_HAL::Util::MEM_DMA_SAFE); } else #endif - fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE + 1, AP_HAL::Util::MEM_DMA_SAFE); if (fifo_buffer == nullptr) { AP_HAL::panic("Invensensev3: Unable to allocate FIFO buffer"); @@ -519,6 +519,8 @@ void AP_InertialSensor_Invensensev3::read_fifo() uint8_t reg_counth; uint8_t reg_data; + uint8_t* samples = nullptr; + uint8_t* tfr_buffer = (uint8_t*)fifo_buffer; switch (inv3_type) { case Invensensev3_Type::ICM45686: @@ -556,38 +558,27 @@ void AP_InertialSensor_Invensensev3::read_fifo() while (n_samples > 0) { uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN); - if (!dev->set_chip_select(true)) { - if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * fifo_sample_size)) { - goto check_registers; - } - } else { - // we don't use read_registers() here to ensure that the fifo buffer that we have allocated - // gets passed all the way down to the SPI DMA handling. This involves one transfer to send - // the register read and then another using the same buffer and length which is handled specially - // for the read - uint8_t reg = reg_data | BIT_READ_FLAG; - if (!dev->transfer(®, 1, nullptr, 0)) { - dev->set_chip_select(false); - goto check_registers; - } - // transfer will also be sending data, make sure that data is zero - memset((uint8_t*)fifo_buffer, 0, n * fifo_sample_size); - if (!dev->transfer((uint8_t*)fifo_buffer, n * fifo_sample_size, (uint8_t*)fifo_buffer, n * fifo_sample_size)) { - dev->set_chip_select(false); - goto check_registers; - } - dev->set_chip_select(false); + // we don't use read_registers() here to ensure that the fifo buffer that we have allocated + // gets passed all the way down to the SPI DMA handling. This involves one transfer to send + // the register read and then another using the same buffer and length which is handled specially + // for the read + tfr_buffer[0] = reg_data | BIT_READ_FLAG; + // transfer will also be sending data, make sure that data is zero + memset(tfr_buffer + 1, 0, n * fifo_sample_size); + if (!dev->transfer(tfr_buffer, n * fifo_sample_size + 1, tfr_buffer, n * fifo_sample_size + 1)) { + goto check_registers; } + samples = tfr_buffer + 1; #if HAL_INS_HIGHRES_SAMPLE if (highres_sampling) { - if (!accumulate_highres_samples((FIFODataHighRes*)fifo_buffer, n)) { + if (!accumulate_highres_samples((FIFODataHighRes*)samples, n)) { need_reset = true; break; } } else #endif - if (!accumulate_samples((FIFOData*)fifo_buffer, n)) { + if (!accumulate_samples((FIFOData*)samples, n)) { need_reset = true; break; } From 405401218d9ce423501ca8d4e1a869cd6775635b Mon Sep 17 00:00:00 2001 From: "paul.quillen" Date: Thu, 3 Oct 2024 14:57:57 -0500 Subject: [PATCH 020/117] AP_DDS: Add set/get parameters service. --- .../test_parameter_service.py | 197 +++++++++++++++++ libraries/AP_DDS/AP_DDS_Client.cpp | 203 +++++++++++++++++- libraries/AP_DDS/AP_DDS_Client.h | 16 ++ libraries/AP_DDS/AP_DDS_Service_Table.h | 28 ++- libraries/AP_DDS/AP_DDS_config.h | 8 + .../Idl/rcl_interfaces/msg/Parameter.idl | 23 ++ .../Idl/rcl_interfaces/msg/ParameterType.idl | 28 +++ .../Idl/rcl_interfaces/msg/ParameterValue.idl | 58 +++++ .../msg/SetParametersResult.idl | 20 ++ .../Idl/rcl_interfaces/srv/GetParameters.idl | 29 +++ .../Idl/rcl_interfaces/srv/SetParameters.idl | 21 ++ 11 files changed, 629 insertions(+), 2 deletions(-) create mode 100644 Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_parameter_service.py create mode 100644 libraries/AP_DDS/Idl/rcl_interfaces/msg/Parameter.idl create mode 100644 libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterType.idl create mode 100644 libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterValue.idl create mode 100644 libraries/AP_DDS/Idl/rcl_interfaces/msg/SetParametersResult.idl create mode 100644 libraries/AP_DDS/Idl/rcl_interfaces/srv/GetParameters.idl create mode 100644 libraries/AP_DDS/Idl/rcl_interfaces/srv/SetParameters.idl diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_parameter_service.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_parameter_service.py new file mode 100644 index 0000000000000..8e6e95601c971 --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_parameter_service.py @@ -0,0 +1,197 @@ +# Copyright 2023 ArduPilot.org. +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +""" +Bring up ArduPilot SITL and check that the get/set_parameters services are up and running. + +Checks whether a parameter is changed using service call. + +colcon test --packages-select ardupilot_dds_tests \ +--event-handlers=console_cohesion+ --pytest-args -k test_parameter_service + +""" + +import launch_pytest +import pytest +import rclpy +import rclpy.node +import threading +import time + +from launch import LaunchDescription + +from launch_pytest.tools import process as process_tools + +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from rclpy.qos import QoSHistoryPolicy + +from rcl_interfaces.srv import GetParameters +from rcl_interfaces.srv import SetParameters +from rcl_interfaces.msg import Parameter + +# Enums for parameter type +PARAMETER_NOT_SET = 0 +PARAMETER_INTEGER = 2 +PARAMETER_DOUBLE = 3 + + +class ParameterClient(rclpy.node.Node): + """Send GetParameters and SetParameters Requests.""" + + def __init__(self): + """Initialize the node.""" + super().__init__('parameter_client') + self.get_param_event_object = threading.Event() + self.set_param_event_object = threading.Event() + self.set_correct_object = threading.Event() + + def start_client(self): + """Start the parameter client.""" + qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + ) + + # Define clients for getting and setting parameter + self.get_cli = self.create_client(GetParameters, 'ap/get_parameters') + while not self.get_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('GetParameters service not available, waiting again...') + + self.set_cli = self.create_client(SetParameters, 'ap/set_parameters') + while not self.set_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('SetParameters service not availabel, waiting again...') + + # Add a spin thread + self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def send_get_param_req(self, param_name): + self.get_req = GetParameters.Request() + self.get_req.names.append(param_name) + + self.get_future = self.get_cli.call_async(self.get_req) + while not self.get_future.done(): + self.get_logger().info("Waiting for GetParameters service response...") + time.sleep(0.1) + + resp = self.get_future.result() + value = resp.values[0] + + if value.type != PARAMETER_NOT_SET: + self.get_param_event_object.set() + + def send_set_param_req(self, param_name, param_value, param_type): + self.set_req = SetParameters.Request() + param_update = Parameter() + param_update.name = param_name + param_update.value.type = param_type + if param_type == PARAMETER_INTEGER: + param_update.value.integer_value = int(param_value) + else: + param_update.value.double_value = float(param_value) + + self.set_req.parameters.append(param_update) + + self.desired_value = param_value + get_response = self.get_future.result() + initial_value = get_response.values[0] + + if initial_value.type == PARAMETER_INTEGER: + self.param_value = initial_value.integer_value + elif initial_value.type == PARAMETER_DOUBLE: + self.param_value = initial_value.double_value + else: + self.param_value = 'nan' + + self.set_future = self.set_cli.call_async(self.set_req) + + while not self.set_future.done(): + self.get_logger().info("Waiting for SetParameters service response...") + time.sleep(0.1) + + if self.set_future.done(): + response = self.set_future.result() + if response.results[0].successful: + self.set_param_event_object.set() + + def check_param_change(self): + param_name = self.set_req.parameters[0].name + + self.send_get_param_req(param_name) + + get_response = self.get_future.result() + new_value = get_response.values[0] + + if new_value.type == PARAMETER_INTEGER: + updated_value = new_value.integer_value + elif new_value.type == PARAMETER_DOUBLE: + updated_value = new_value.double_value + else: + updated_value = 'nan' + + if updated_value == self.desired_value: + self.set_correct_object.set() + + +@launch_pytest.fixture +def launch_sitl_copter_dds_udp(sitl_copter_dds_udp): + """Fixture to create the launch description.""" + sitl_ld, sitl_actions = sitl_copter_dds_udp + + ld = LaunchDescription( + [ + sitl_ld, + launch_pytest.actions.ReadyToTest(), + ] + ) + actions = sitl_actions + yield ld, actions + + +@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp) +def test_dds_udp_parameter_services(launch_context, launch_sitl_copter_dds_udp): + """Test Get/Set parameter services broadcast by AP_DDS.""" + _, actions = launch_sitl_copter_dds_udp + micro_ros_agent = actions["micro_ros_agent"].action + mavproxy = actions["mavproxy"].action + sitl = actions["sitl"].action + + # Wait for process to start. + process_tools.wait_for_start_sync(launch_context, micro_ros_agent, timeout=2) + process_tools.wait_for_start_sync(launch_context, mavproxy, timeout=2) + process_tools.wait_for_start_sync(launch_context, sitl, timeout=2) + + rclpy.init() + try: + node = ParameterClient() + node.start_client() + parameter_name = "LOIT_SPEED" + param_change_value = 1250 + param_type = PARAMETER_DOUBLE + node.send_get_param_req(parameter_name) + get_param_received_flag = node.get_param_event_object.wait(timeout=10.0) + assert get_param_received_flag, f"Did not get '{parameter_name}' param." + node.send_set_param_req(parameter_name, param_change_value, param_type) + set_param_received_flag = node.set_param_event_object.wait(timeout=10.0) + assert set_param_received_flag, f"Could not set '{parameter_name}' to '{param_change_value}'" + node.check_param_change() + set_param_changed_flag = node.set_correct_object.wait(timeout=10.0) + assert set_param_changed_flag, f"Did not confirm '{parameter_name}' value change" + + finally: + rclpy.shutdown() + yield diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index b7fcf460c37b2..f5aaa5883929d 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -81,6 +81,17 @@ geometry_msgs_msg_TwistStamped AP_DDS_Client::rx_velocity_control_topic {}; ardupilot_msgs_msg_GlobalPosition AP_DDS_Client::rx_global_position_control_topic {}; #endif // AP_DDS_GLOBAL_POS_CTRL_ENABLED +// Define the parameter server data members, which are static class scope. +// If these are created on the stack, then the AP_DDS_Client::on_request +// frame size is exceeded. +#if AP_DDS_PARAMETER_SERVER_ENABLED +rcl_interfaces_srv_SetParameters_Request AP_DDS_Client::set_parameter_request {}; +rcl_interfaces_srv_SetParameters_Response AP_DDS_Client::set_parameter_response {}; +rcl_interfaces_srv_GetParameters_Request AP_DDS_Client::get_parameters_request {}; +rcl_interfaces_srv_GetParameters_Response AP_DDS_Client::get_parameters_response {}; +rcl_interfaces_msg_Parameter AP_DDS_Client::param {}; +#endif + const AP_Param::GroupInfo AP_DDS_Client::var_info[] { // @Param: _ENABLE @@ -801,6 +812,196 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u break; } #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_PARAMETER_SERVER_ENABLED + case services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id: { + const bool deserialize_success = rcl_interfaces_srv_SetParameters_Request_deserialize_topic(ub, &set_parameter_request); + if (deserialize_success == false) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Set Parameters Request : Failed to deserialize request.", msg_prefix); + break; + } + + if (set_parameter_request.parameters_size > 8U) { + break; + } + + // Set parameters and responses for each one requested + set_parameter_response.results_size = set_parameter_request.parameters_size; + for (size_t i = 0; i < set_parameter_request.parameters_size; i++) { + param = set_parameter_request.parameters[i]; + + enum ap_var_type var_type; + + // set parameter + AP_Param *vp; + char param_key[AP_MAX_NAME_SIZE + 1]; + strncpy(param_key, (char *)param.name, AP_MAX_NAME_SIZE); + param_key[AP_MAX_NAME_SIZE] = 0; + + // Currently only integer and double value types can be set. + // The following parameter value types are not handled: + // bool, string, byte_array, bool_array, integer_array, double_array and string_array + bool param_isnan = true; + bool param_isinf = true; + float param_value; + switch (param.value.type) { + case PARAMETER_INTEGER: { + param_isnan = isnan(param.value.integer_value); + param_isinf = isinf(param.value.integer_value); + param_value = float(param.value.integer_value); + break; + } + case PARAMETER_DOUBLE: { + param_isnan = isnan(param.value.double_value); + param_isinf = isinf(param.value.double_value); + param_value = float(param.value.double_value); + break; + } + default: { + break; + } + } + + // find existing param to get the old value + uint16_t parameter_flags = 0; + vp = AP_Param::find(param_key, &var_type, ¶meter_flags); + if (vp == nullptr || param_isnan || param_isinf) { + set_parameter_response.results[i].successful = false; + strncpy(set_parameter_response.results[i].reason, "Parameter not found", sizeof(set_parameter_response.results[i].reason)); + continue; + } + + // Add existing parameter checks used in GCS_Param.cpp + if (parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) { + // The user can set BRD_OPTIONS to enable set of internal + // parameters, for developer testing or unusual use cases + if (AP_BoardConfig::allow_set_internal_parameters()) { + parameter_flags &= ~AP_PARAM_FLAG_INTERNAL_USE_ONLY; + } + } + + if ((parameter_flags & AP_PARAM_FLAG_INTERNAL_USE_ONLY) || vp->is_read_only()) { + set_parameter_response.results[i].successful = false; + strncpy(set_parameter_response.results[i].reason, "Parameter is read only",sizeof(set_parameter_response.results[i].reason)); + continue; + } + + // Set and save the value if it changed + bool force_save = vp->set_and_save_by_name_ifchanged(param_key, param_value); + + if (force_save && (parameter_flags & AP_PARAM_FLAG_ENABLE)) { + AP_Param::invalidate_count(); + } + + set_parameter_response.results[i].successful = true; + strncpy(set_parameter_response.results[i].reason, "Parameter accepted", sizeof(set_parameter_response.results[i].reason)); + } + + const uxrObjectId replier_id = { + .id = services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id, + .type = UXR_REPLIER_ID + }; + + uint32_t reply_size = rcl_interfaces_srv_SetParameters_Response_size_of_topic(&set_parameter_response, 0U); + uint8_t reply_buffer[reply_size] {}; + ucdrBuffer reply_ub; + + ucdr_init_buffer(&reply_ub, reply_buffer, reply_size); + const bool serialize_success = rcl_interfaces_srv_SetParameters_Response_serialize_topic(&reply_ub, &set_parameter_response); + if (serialize_success == false) { + break; + } + + uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub)); + bool successful_params = true; + for (size_t i = 0; i < set_parameter_response.results_size; i++) { + // Check that all the parameters were set successfully + successful_params &= set_parameter_response.results[i].successful; + } + GCS_SEND_TEXT(successful_params ? MAV_SEVERITY_INFO : MAV_SEVERITY_WARNING, "%s Set Parameters Request : %s", msg_prefix, successful_params ? "SUCCESSFUL" : "ONE OR MORE PARAMS FAILED" ); + break; + } + case services[to_underlying(ServiceIndex::GET_PARAMETERS)].rep_id: { + const bool deserialize_success = rcl_interfaces_srv_GetParameters_Request_deserialize_topic(ub, &get_parameters_request); + if (deserialize_success == false) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s Get Parameters Request : Failed to deserialize request.", msg_prefix); + break; + } + + if (get_parameters_request.names_size > 8U) { + break; + } + + bool successful_read = true; + get_parameters_response.values_size = get_parameters_request.names_size; + for (size_t i = 0; i < get_parameters_request.names_size; i++) { + enum ap_var_type var_type; + + AP_Param *vp; + char param_key[AP_MAX_NAME_SIZE + 1]; + strncpy(param_key, (char *)get_parameters_request.names[i], AP_MAX_NAME_SIZE); + param_key[AP_MAX_NAME_SIZE] = 0; + + vp = AP_Param::find(param_key, &var_type); + if (vp == nullptr) { + get_parameters_response.values[i].type = PARAMETER_NOT_SET; + successful_read &= false; + continue; + } + + switch (var_type) { + case AP_PARAM_INT8: { + get_parameters_response.values[i].type = PARAMETER_INTEGER; + get_parameters_response.values[i].integer_value = ((AP_Int8 *)vp)->get(); + successful_read &= true; + break; + } + case AP_PARAM_INT16: { + get_parameters_response.values[i].type = PARAMETER_INTEGER; + get_parameters_response.values[i].integer_value = ((AP_Int16 *)vp)->get(); + successful_read &= true; + break; + } + case AP_PARAM_INT32: { + get_parameters_response.values[i].type = PARAMETER_INTEGER; + get_parameters_response.values[i].integer_value = ((AP_Int32 *)vp)->get(); + successful_read &= true; + break; + } + case AP_PARAM_FLOAT: { + get_parameters_response.values[i].type = PARAMETER_DOUBLE; + get_parameters_response.values[i].double_value = vp->cast_to_float(var_type); + successful_read &= true; + break; + } + default: { + get_parameters_response.values[i].type = PARAMETER_NOT_SET; + successful_read &= false; + break; + } + } + } + + const uxrObjectId replier_id = { + .id = services[to_underlying(ServiceIndex::GET_PARAMETERS)].rep_id, + .type = UXR_REPLIER_ID + }; + + uint32_t reply_size = rcl_interfaces_srv_GetParameters_Response_size_of_topic(&get_parameters_response, 0U); + uint8_t reply_buffer[reply_size] {}; + ucdrBuffer reply_ub; + + ucdr_init_buffer(&reply_ub, reply_buffer, reply_size); + const bool serialize_success = rcl_interfaces_srv_GetParameters_Response_serialize_topic(&reply_ub, &get_parameters_response); + if (serialize_success == false) { + break; + } + + uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub)); + + GCS_SEND_TEXT(successful_read ? MAV_SEVERITY_INFO : MAV_SEVERITY_WARNING, "%s Get Parameters Request : %s", msg_prefix, successful_read ? "SUCCESSFUL" : "ONE OR MORE PARAM NOT FOUND"); + break; + } +#endif // AP_DDS_PARAMETER_SERVER_ENABLED } } @@ -947,7 +1148,7 @@ bool AP_DDS_Client::create() .id = 0x01, .type = UXR_PARTICIPANT_ID }; - const char* participant_name = "ardupilot_dds"; + const char* participant_name = AP_DDS_PARTICIPANT_NAME; const auto participant_req_id = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id, static_cast(domain_id), participant_name, UXR_REPLACE); diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 92bdbf2a4bd30..ded3451f99228 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -46,6 +46,14 @@ #if AP_DDS_CLOCK_PUB_ENABLED #include "rosgraph_msgs/msg/Clock.h" #endif // AP_DDS_CLOCK_PUB_ENABLED +#if AP_DDS_PARAMETER_SERVER_ENABLED +#include "rcl_interfaces/srv/SetParameters.h" +#include "rcl_interfaces/msg/Parameter.h" +#include "rcl_interfaces/msg/SetParametersResult.h" +#include "rcl_interfaces/msg/ParameterValue.h" +#include "rcl_interfaces/msg/ParameterType.h" +#include "rcl_interfaces/srv/GetParameters.h" +#endif //AP_DDS_PARAMETER_SERVER_ENABLED #include #include @@ -201,6 +209,14 @@ class AP_DDS_Client #endif // AP_DDS_DYNAMIC_TF_SUB_ENABLED HAL_Semaphore csem; +#if AP_DDS_PARAMETER_SERVER_ENABLED + static rcl_interfaces_srv_SetParameters_Request set_parameter_request; + static rcl_interfaces_srv_SetParameters_Response set_parameter_response; + static rcl_interfaces_srv_GetParameters_Request get_parameters_request; + static rcl_interfaces_srv_GetParameters_Response get_parameters_response; + static rcl_interfaces_msg_Parameter param; +#endif + // connection parametrics bool status_ok{false}; bool connected{false}; diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h index 0e86496dd5777..667149d6aa756 100644 --- a/libraries/AP_DDS/AP_DDS_Service_Table.h +++ b/libraries/AP_DDS/AP_DDS_Service_Table.h @@ -6,8 +6,12 @@ enum class ServiceIndex: uint8_t { ARMING_MOTORS, #endif // #if AP_DDS_ARM_SERVER_ENABLED #if AP_DDS_MODE_SWITCH_SERVER_ENABLED - MODE_SWITCH + MODE_SWITCH, #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_PARAMETER_SERVER_ENABLED + SET_PARAMETERS, + GET_PARAMETERS +#endif // AP_DDS_PARAMETER_SERVER_ENABLED }; static inline constexpr uint8_t to_underlying(const ServiceIndex index) @@ -47,4 +51,26 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = { .reply_topic_name = "rr/ap/mode_switchReply", }, #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED +#if AP_DDS_PARAMETER_SERVER_ENABLED + { + .req_id = to_underlying(ServiceIndex::SET_PARAMETERS), + .rep_id = to_underlying(ServiceIndex::SET_PARAMETERS), + .service_rr = Service_rr::Replier, + .service_name = "rs/ap/set_parametersService", + .request_type = "rcl_interfaces::srv::dds_::SetParameters_Request_", + .reply_type = "rcl_interfaces::srv::dds_::SetParameters_Response_", + .request_topic_name = "rq/ap/set_parametersRequest", + .reply_topic_name = "rr/ap/set_parametersReply", + }, + { + .req_id = to_underlying(ServiceIndex::GET_PARAMETERS), + .rep_id = to_underlying(ServiceIndex::GET_PARAMETERS), + .service_rr = Service_rr::Replier, + .service_name = "rs/ap/get_parameterService", + .request_type = "rcl_interfaces::srv::dds_::GetParameters_Request_", + .reply_type = "rcl_interfaces::srv::dds_::GetParameters_Response_", + .request_topic_name = "rq/ap/get_parametersRequest", + .reply_topic_name = "rr/ap/get_parametersReply", + }, +#endif // AP_DDS_PARAMETER_SERVER_ENABLED }; diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index b8f31e368c8a4..9bc3a5512656d 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -126,6 +126,10 @@ #define AP_DDS_MODE_SWITCH_SERVER_ENABLED 1 #endif +#ifndef AP_DDS_PARAMETER_SERVER_ENABLED +#define AP_DDS_PARAMETER_SERVER_ENABLED 1 +#endif + // Whether to include Twist support #define AP_DDS_NEEDS_TWIST AP_DDS_VEL_CTRL_ENABLED || AP_DDS_LOCAL_VEL_PUB_ENABLED @@ -139,3 +143,7 @@ #define AP_DDS_DEFAULT_UDP_IP_ADDR "127.0.0.1" #endif #endif + +#ifndef AP_DDS_PARTICIPANT_NAME +#define AP_DDS_PARTICIPANT_NAME "ap" +#endif diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/msg/Parameter.idl b/libraries/AP_DDS/Idl/rcl_interfaces/msg/Parameter.idl new file mode 100644 index 0000000000000..992213a287458 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/msg/Parameter.idl @@ -0,0 +1,23 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from rcl_interfaces/msg/Parameter.msg +// generated code does not contain a copyright notice + +#include "rcl_interfaces/msg/ParameterValue.idl" + +module rcl_interfaces { + module msg { + @verbatim (language="comment", text= + "This is the message to communicate a parameter. It is an open struct with an enum in" "\n" + "the descriptor to select which value is active.") + struct Parameter { + @verbatim (language="comment", text= + "The full name of the parameter.") + string name; + + @verbatim (language="comment", text= + "The parameter's value which can be one of several types, see" "\n" + "`ParameterValue.msg` and `ParameterType.msg`.") + rcl_interfaces::msg::ParameterValue value; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterType.idl b/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterType.idl new file mode 100644 index 0000000000000..e6a6bfcca7da7 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterType.idl @@ -0,0 +1,28 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from rcl_interfaces/msg/ParameterType.msg +// generated code does not contain a copyright notice + + +module rcl_interfaces { + module msg { + module ParameterType_Constants { + @verbatim (language="comment", text= + "Default value, which implies this is not a valid parameter.") + const uint8 PARAMETER_NOT_SET = 0; + const uint8 PARAMETER_BOOL = 1; + const uint8 PARAMETER_INTEGER = 2; + const uint8 PARAMETER_DOUBLE = 3; + const uint8 PARAMETER_STRING = 4; + const uint8 PARAMETER_BYTE_ARRAY = 5; + const uint8 PARAMETER_BOOL_ARRAY = 6; + const uint8 PARAMETER_INTEGER_ARRAY = 7; + const uint8 PARAMETER_DOUBLE_ARRAY = 8; + const uint8 PARAMETER_STRING_ARRAY = 9; + }; + @verbatim (language="comment", text= + "These types correspond to the value that is set in the ParameterValue message.") + struct ParameterType { + uint8 structure_needs_at_least_one_member; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterValue.idl b/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterValue.idl new file mode 100644 index 0000000000000..3adbc5233cac2 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/msg/ParameterValue.idl @@ -0,0 +1,58 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from rcl_interfaces/msg/ParameterValue.msg +// generated code does not contain a copyright notice + + +module rcl_interfaces { + module msg { + @verbatim (language="comment", text= + "Used to determine which of the next *_value fields are set." "\n" + "ParameterType.PARAMETER_NOT_SET indicates that the parameter was not set" "\n" + "(if gotten) or is uninitialized." "\n" + "Values are enumerated in `ParameterType.msg`.") + struct ParameterValue { + @verbatim (language="comment", text= + "The type of this parameter, which corresponds to the appropriate field below.") + uint8 type; + + @verbatim (language="comment", text= + "\"Variant\" style storage of the parameter value. Only the value corresponding" "\n" + "the type field will have valid information." "\n" + "Boolean value, can be either true or false.") + boolean bool_value; + + @verbatim (language="comment", text= + "Integer value ranging from -9,223,372,036,854,775,808 to" "\n" + "9,223,372,036,854,775,807.") + int64 integer_value; + + @verbatim (language="comment", text= + "A double precision floating point value following IEEE 754.") + double double_value; + + @verbatim (language="comment", text= + "A textual value with no practical length limit.") + string string_value; + + @verbatim (language="comment", text= + "An array of bytes, used for non-textual information.") + sequence byte_array_value; + + @verbatim (language="comment", text= + "An array of boolean values.") + sequence bool_array_value; + + @verbatim (language="comment", text= + "An array of 64-bit integer values.") + sequence integer_array_value; + + @verbatim (language="comment", text= + "An array of 64-bit floating point values.") + sequence double_array_value; + + @verbatim (language="comment", text= + "An array of string values.") + sequence string_array_value; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/msg/SetParametersResult.idl b/libraries/AP_DDS/Idl/rcl_interfaces/msg/SetParametersResult.idl new file mode 100644 index 0000000000000..d1c68fe1200f1 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/msg/SetParametersResult.idl @@ -0,0 +1,20 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from rcl_interfaces/msg/SetParametersResult.msg +// generated code does not contain a copyright notice + + +module rcl_interfaces { + module msg { + @verbatim (language="comment", text= + "A true value of the same index indicates that the parameter was set" "\n" + "successfully. A false value indicates the change was rejected.") + struct SetParametersResult { + boolean successful; + + @verbatim (language="comment", text= + "Reason why the setting was either successful or a failure. This should only be" "\n" + "used for logging and user interfaces.") + string reason; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/srv/GetParameters.idl b/libraries/AP_DDS/Idl/rcl_interfaces/srv/GetParameters.idl new file mode 100644 index 0000000000000..d6579f51b2c12 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/srv/GetParameters.idl @@ -0,0 +1,29 @@ +// generated from rosidl_adapter/resource/srv.idl.em +// with input from rcl_interfaces/srv/GetParameters.srv +// generated code does not contain a copyright notice + +#include "rcl_interfaces/msg/ParameterValue.idl" + +module rcl_interfaces { + module srv { + @verbatim (language="comment", text= + "TODO(wjwwood): Decide on the rules for grouping, nodes, and parameter \"names\"" "\n" + "in general, then link to that." "\n" + "" "\n" + "For more information about parameters and naming rules, see:" "\n" + "https://design.ros2.org/articles/ros_parameters.html" "\n" + "https://github.com/ros2/design/pull/241") + struct GetParameters_Request { + @verbatim (language="comment", text= + "A list of parameter names to get.") + sequence names; + }; + @verbatim (language="comment", text= + "List of values which is the same length and order as the provided names. If a" "\n" + "parameter was not yet set, the value will have PARAMETER_NOT_SET as the" "\n" + "type.") + struct GetParameters_Response { + sequence values; + }; + }; +}; diff --git a/libraries/AP_DDS/Idl/rcl_interfaces/srv/SetParameters.idl b/libraries/AP_DDS/Idl/rcl_interfaces/srv/SetParameters.idl new file mode 100644 index 0000000000000..ade6df7994030 --- /dev/null +++ b/libraries/AP_DDS/Idl/rcl_interfaces/srv/SetParameters.idl @@ -0,0 +1,21 @@ +// generated from rosidl_adapter/resource/srv.idl.em +// with input from rcl_interfaces/srv/SetParameters.srv +// generated code does not contain a copyright notice + +#include "rcl_interfaces/msg/Parameter.idl" +#include "rcl_interfaces/msg/SetParametersResult.idl" + +module rcl_interfaces { + module srv { + @verbatim (language="comment", text= + "A list of parameters to set.") + struct SetParameters_Request { + sequence parameters; + }; + @verbatim (language="comment", text= + "Indicates whether setting each parameter succeeded or not and why.") + struct SetParameters_Response { + sequence results; + }; + }; +}; From 601d9ef430e1d904e3d7e4405d426ad107d77931 Mon Sep 17 00:00:00 2001 From: Tiziano Fiorenzani Date: Tue, 29 Oct 2024 15:06:50 +0000 Subject: [PATCH 021/117] AP_DDS: Vehicle status interface --- Tools/ros2/ardupilot_msgs/CMakeLists.txt | 1 + Tools/ros2/ardupilot_msgs/msg/Status.msg | 27 +++++++ libraries/AP_DDS/AP_DDS_Client.cpp | 80 ++++++++++++++++++- libraries/AP_DDS/AP_DDS_Client.h | 15 ++++ libraries/AP_DDS/AP_DDS_Topic_Table.h | 21 +++++ libraries/AP_DDS/AP_DDS_config.h | 8 ++ .../AP_DDS/Idl/ardupilot_msgs/msg/Status.idl | 56 +++++++++++++ 7 files changed, 207 insertions(+), 1 deletion(-) create mode 100644 Tools/ros2/ardupilot_msgs/msg/Status.msg create mode 100644 libraries/AP_DDS/Idl/ardupilot_msgs/msg/Status.idl diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt index d066a2d32b2e4..f8af788e8b81e 100644 --- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt +++ b/Tools/ros2/ardupilot_msgs/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/GlobalPosition.msg" + "msg/Status.msg" "srv/ArmMotors.srv" "srv/ModeSwitch.srv" DEPENDENCIES geometry_msgs std_msgs diff --git a/Tools/ros2/ardupilot_msgs/msg/Status.msg b/Tools/ros2/ardupilot_msgs/msg/Status.msg new file mode 100644 index 0000000000000..38ff02804d12c --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/msg/Status.msg @@ -0,0 +1,27 @@ +std_msgs/Header header + +uint8 APM_ROVER = 1 +uint8 APM_ARDUCOPTER = 2 +uint8 APM_ARDUPLANE = 3 +uint8 APM_ANTENNATRACKER = 4 +uint8 APM_UNKNOWN = 5 +uint8 APM_REPLAY = 6 +uint8 APM_ARDUSUB = 7 +uint8 APM_IOFIRMWARE = 8 +uint8 APM_AP_PERIPH = 9 +uint8 APM_AP_DAL_STANDALONE = 10 +uint8 APM_AP_BOOTLOADER = 11 +uint8 APM_BLIMP = 12 +uint8 APM_HELI = 13 +uint8 vehicle_type # From AP_Vehicle_Type.h + +bool armed # true if vehicle is armed. +uint8 mode # Vehicle mode, enum depending on vehicle type. +bool flying # True if flying/driving/diving/tracking. +bool external_control # True is external control is enabled. + +uint8 FS_RADIO = 21 +uint8 FS_BATTERY = 22 +uint8 FS_GCS = 23 +uint8 FS_EKF = 24 +uint8[] failsafe # Array containing all active failsafe. diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index f5aaa5883929d..ef4e14a35132f 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -17,6 +17,7 @@ #include # endif // AP_DDS_ARM_SERVER_ENABLED #include +#include #include #if AP_DDS_ARM_SERVER_ENABLED @@ -66,6 +67,9 @@ static constexpr uint16_t DELAY_CLOCK_TOPIC_MS =AP_DDS_DELAY_CLOCK_TOPIC_MS; static constexpr uint16_t DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS = AP_DDS_DELAY_GPS_GLOBAL_ORIGIN_TOPIC_MS; #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED static constexpr uint16_t DELAY_PING_MS = 500; +#ifdef AP_DDS_STATUS_PUB_ENABLED +static constexpr uint16_t DELAY_STATUS_TOPIC_MS = AP_DDS_DELAY_STATUS_TOPIC_MS; +#endif // AP_DDS_STATUS_PUB_ENABLED // Define the subscriber data members, which are static class scope. // If these are created on the stack in the subscriber, @@ -615,6 +619,56 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg) } #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED +bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Status& msg) +{ + // Fill the new message. + const auto &vehicle = AP::vehicle(); + const auto &battery = AP::battery(); + msg.vehicle_type = static_cast(AP::fwversion().vehicle_type); + msg.armed = hal.util->get_soft_armed(); + msg.mode = vehicle->get_mode(); + msg.flying = vehicle->get_likely_flying(); + msg.external_control = true; // Always true for now. To be filled after PR#28429. + uint8_t fs_iter = 0; + msg.failsafe_size = 0; + if (AP_Notify::flags.failsafe_radio) { + msg.failsafe[fs_iter++] = FS_RADIO; + } + if (battery.has_failsafed()) { + msg.failsafe[fs_iter++] = FS_BATTERY; + } + if (AP_Notify::flags.failsafe_gcs) { + msg.failsafe[fs_iter++] = FS_GCS; + } + if (AP_Notify::flags.failsafe_ekf) { + msg.failsafe[fs_iter++] = FS_EKF; + } + msg.failsafe_size = fs_iter; + + // Compare with the previous one. + bool is_message_changed {false}; + is_message_changed |= (last_status_msg_.flying != msg.flying); + is_message_changed |= (last_status_msg_.armed != msg.armed); + is_message_changed |= (last_status_msg_.mode != msg.mode); + is_message_changed |= (last_status_msg_.vehicle_type != msg.vehicle_type); + is_message_changed |= (last_status_msg_.failsafe_size != msg.failsafe_size); + is_message_changed |= (last_status_msg_.external_control != msg.external_control); + + if ( is_message_changed ) { + last_status_msg_.flying = msg.flying; + last_status_msg_.armed = msg.armed; + last_status_msg_.mode = msg.mode; + last_status_msg_.vehicle_type = msg.vehicle_type; + last_status_msg_.failsafe_size = msg.failsafe_size; + last_status_msg_.external_control = msg.external_control; + update_topic(msg.header.stamp); + return true; + } else { + return false; + } +} +#endif // AP_DDS_STATUS_PUB_ENABLED /* start the DDS thread */ @@ -1458,6 +1512,23 @@ void AP_DDS_Client::write_gps_global_origin_topic() } #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED +void AP_DDS_Client::write_status_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub {}; + const uint32_t topic_size = ardupilot_msgs_msg_Status_size_of_topic(&status_topic, 0); + uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::STATUS_PUB)].dw_id, &ub, topic_size); + const bool success = ardupilot_msgs_msg_Status_serialize_topic(&ub, &status_topic); + if (!success) { + // TODO sometimes serialization fails on bootup. Determine why. + // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + } + } +} +#endif // AP_DDS_STATUS_PUB_ENABLED + void AP_DDS_Client::update() { WITH_SEMAPHORE(csem); @@ -1537,10 +1608,17 @@ void AP_DDS_Client::update() write_gps_global_origin_topic(); } #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED + if (cur_time_ms - last_status_check_time_ms > DELAY_STATUS_TOPIC_MS) { + if (update_topic(status_topic)) { + write_status_topic(); + } + last_status_check_time_ms = cur_time_ms; + } status_ok = uxr_run_session_time(&session, 1); } - +#endif // AP_DDS_STATUS_PUB_ENABLED #if CONFIG_HAL_BOARD != HAL_BOARD_SITL extern "C" { int clock_gettime(clockid_t clockid, struct timespec *ts); diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index ded3451f99228..8453577ec1edd 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -25,6 +25,9 @@ #if AP_DDS_IMU_PUB_ENABLED #include "sensor_msgs/msg/Imu.h" #endif // AP_DDS_IMU_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED +#include "ardupilot_msgs/msg/Status.h" +#endif // AP_DDS_STATUS_PUB_ENABLED #if AP_DDS_JOY_SUB_ENABLED #include "sensor_msgs/msg/Joy.h" #endif // AP_DDS_JOY_SUB_ENABLED @@ -183,6 +186,17 @@ class AP_DDS_Client static void update_topic(rosgraph_msgs_msg_Clock& msg); #endif // AP_DDS_CLOCK_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED + ardupilot_msgs_msg_Status status_topic; + bool update_topic(ardupilot_msgs_msg_Status& msg); + // The last ms timestamp AP_DDS wrote/checked a status message + uint64_t last_status_check_time_ms; + // last status values; + ardupilot_msgs_msg_Status last_status_msg_; + //! @brief Serialize the current status and publish to the IO stream(s) + void write_status_topic(); +#endif // AP_DDS_STATUS_PUB_ENABLED + #if AP_DDS_STATIC_TF_PUB_ENABLED // outgoing transforms tf2_msgs_msg_TFMessage tx_static_transforms_topic; @@ -271,6 +285,7 @@ class AP_DDS_Client // client key we present static constexpr uint32_t key = 0xAAAABBBB; + public: ~AP_DDS_Client(); diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index f578d2e8a486b..b9bde199dc050 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -48,6 +48,9 @@ enum class TopicIndex: uint8_t { #if AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED GPS_GLOBAL_ORIGIN_PUB, #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED + STATUS_PUB, +#endif // AP_DDS_STATUS_PUB_ENABLED #if AP_DDS_JOY_SUB_ENABLED JOY_SUB, #endif // AP_DDS_JOY_SUB_ENABLED @@ -268,6 +271,24 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { }, }, #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED +#if AP_DDS_STATUS_PUB_ENABLED + { + .topic_id = to_underlying(TopicIndex::STATUS_PUB), + .pub_id = to_underlying(TopicIndex::STATUS_PUB), + .sub_id = to_underlying(TopicIndex::STATUS_PUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::STATUS_PUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::STATUS_PUB), .type=UXR_DATAREADER_ID}, + .topic_rw = Topic_rw::DataWriter, + .topic_name = "rt/ap/status", + .type_name = "ardupilot_msgs::msg::dds_::Status_", + .qos = { + .durability = UXR_DURABILITY_TRANSIENT_LOCAL, + .reliability = UXR_RELIABILITY_RELIABLE, + .history = UXR_HISTORY_KEEP_LAST, + .depth = 1, + }, + }, +#endif // AP_DDS_STATUS_PUB_ENABLED #if AP_DDS_JOY_SUB_ENABLED { .topic_id = to_underlying(TopicIndex::JOY_SUB), diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index 9bc3a5512656d..5785ca23b1212 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -94,6 +94,10 @@ #define AP_DDS_DELAY_BATTERY_STATE_TOPIC_MS 1000 #endif +#ifndef AP_DDS_DELAY_STATUS_TOPIC_MS +#define AP_DDS_DELAY_STATUS_TOPIC_MS 100 +#endif + #ifndef AP_DDS_CLOCK_PUB_ENABLED #define AP_DDS_CLOCK_PUB_ENABLED 1 #endif @@ -102,6 +106,10 @@ #define AP_DDS_DELAY_CLOCK_TOPIC_MS 10 #endif +#ifndef AP_DDS_STATUS_PUB_ENABLED +#define AP_DDS_STATUS_PUB_ENABLED 1 +#endif + #ifndef AP_DDS_JOY_SUB_ENABLED #define AP_DDS_JOY_SUB_ENABLED 1 #endif diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Status.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Status.idl new file mode 100644 index 0000000000000..a06a0da8cb2e6 --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/Status.idl @@ -0,0 +1,56 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from ardupilot_msgs/msg/Status.msg +// generated code does not contain a copyright notice + +#include "std_msgs/msg/Header.idl" + +module ardupilot_msgs { + module msg { + module Status_Constants { + const uint8 APM_ROVER = 1; + const uint8 APM_ARDUCOPTER = 2; + const uint8 APM_ARDUPLANE = 3; + const uint8 APM_ANTENNATRACKER = 4; + const uint8 APM_UNKNOWN = 5; + const uint8 APM_REPLAY = 6; + const uint8 APM_ARDUSUB = 7; + const uint8 APM_IOFIRMWARE = 8; + const uint8 APM_AP_PERIPH = 9; + const uint8 APM_AP_DAL_STANDALONE = 10; + const uint8 APM_AP_BOOTLOADER = 11; + const uint8 APM_BLIMP = 12; + const uint8 APM_HELI = 13; + const uint8 FS_RADIO = 21; + const uint8 FS_BATTERY = 22; + const uint8 FS_GCS = 23; + const uint8 FS_EKF = 24; + }; + struct Status { + std_msgs::msg::Header header; + + @verbatim (language="comment", text= + "From AP_Vehicle_Type.h") + uint8 vehicle_type; + + @verbatim (language="comment", text= + "true if vehicle is armed.") + boolean armed; + + @verbatim (language="comment", text= + "Vehicle mode, enum depending on vehicle type.") + uint8 mode; + + @verbatim (language="comment", text= + "True if flying/driving/diving/tracking.") + boolean flying; + + @verbatim (language="comment", text= + "True is external control is enabled.") + boolean external_control; + + @verbatim (language="comment", text= + "Array containing all active failsafe.") + sequence failsafe; + }; + }; +}; From 1e17278bda4b1026b4614d14f12f334700c8ea3b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 17 Oct 2024 22:51:58 +1100 Subject: [PATCH 022/117] AP_NavEKF3: add an option_is_enabled method --- libraries/AP_NavEKF3/AP_NavEKF3.h | 5 ++++- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 415d7424a4777..6bfba2ee17eb8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -453,9 +453,12 @@ class NavEKF3 { AP_Int32 _options; // bit mask of processing options // enum for processing options - enum class Options { + enum class Option { JammingExpected = (1<<0), }; + bool option_is_enabled(Option option) const { + return (_options & (uint32_t)option) != 0; + } // Possible values for _flowUse #define FLOW_USE_NONE 0 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 9e1eab32eb29d..a7146dd06e71b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -638,7 +638,7 @@ void NavEKF3_core::readGpsData() useGpsVertVel = false; } - if ((frontend->_options & (int32_t)NavEKF3::Options::JammingExpected) && + if (frontend->option_is_enabled(NavEKF3::Option::JammingExpected) && (lastTimeGpsReceived_ms - secondLastGpsTime_ms) > frontend->gpsNoFixTimeout_ms) { const bool doingBodyVelNav = (imuSampleTime_ms - prevBodyVelFuseTime_ms < 1000); const bool doingFlowNav = (imuSampleTime_ms - prevFlowFuseTime_ms < 1000);; From 281ea91ee556402cbba5d890daa4a44c4c069da2 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Mon, 28 Oct 2024 19:56:24 -0700 Subject: [PATCH 023/117] ArduCopter: Update clang pragma to check for the version of clang that introduces the warning AP_Arming: Update clang pragma to check for the version of clang that introduces the warning --- ArduCopter/AP_Arming.cpp | 2 +- libraries/AP_Arming/AP_Arming.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index e33abf569df02..6ef06d421641a 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -1,7 +1,7 @@ #include "Copter.h" #pragma GCC diagnostic push -#if defined(__clang__) +#if defined(__clang_major__) && __clang_major__ >= 14 #pragma GCC diagnostic ignored "-Wbitwise-instead-of-logical" #endif diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 8ff983a692ded..ffa148d6b5af1 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -175,7 +175,7 @@ extern AP_IOMCU iomcu; #endif #pragma GCC diagnostic push -#if defined (__clang__) +#if defined(__clang_major__) && __clang_major__ >= 14 #pragma GCC diagnostic ignored "-Wbitwise-instead-of-logical" #endif From 7dbb22d3b726c8ce44b647518e852d42b4c6c0f8 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 5 Nov 2024 13:51:29 +0800 Subject: [PATCH 024/117] MAVLink: use the new MAVLink GUIDED HEADING_TYPE_DEFAULT --- modules/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/mavlink b/modules/mavlink index 83e75ffdb2709..08da786b2d94c 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 83e75ffdb2709f4821b9746477639e2ae6df103f +Subproject commit 08da786b2d94ce0955a82f92ab2166c347259623 From 292f7bd78519cc89cc0f36fb1f1417443938d708 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 5 Nov 2024 13:58:36 +0800 Subject: [PATCH 025/117] ArduPlane: use the new MAVLink GUIDED HEADING_TYPE_DEFAULT --- ArduPlane/GCS_Mavlink.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index d2800c367631f..9256fdfe01cb3 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -863,6 +863,9 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) || (plane.control_mode == &plane.mode_guided)) { plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND); +#if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; +#endif // add home alt if needed if (requested_position.relative_alt) { @@ -973,14 +976,20 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavl float new_target_heading = radians(wrap_180(packet.param2)); - // course over ground - if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int + switch(HEADING_TYPE(packet.param1)) { + case HEADING_TYPE_COURSE_OVER_GROUND: + // course over ground plane.guided_state.target_heading_type = GUIDED_HEADING_COG; plane.prev_WP_loc = plane.current_loc; - // normal vehicle heading - } else if (int(packet.param1) == HEADING_TYPE_HEADING) { // compare as nearest int + break; + case HEADING_TYPE_HEADING: + // normal vehicle heading plane.guided_state.target_heading_type = GUIDED_HEADING_HEADING; - } else { + break; + case HEADING_TYPE_DEFAULT: + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; + return MAV_RESULT_ACCEPTED; + default: // MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters). return MAV_RESULT_DENIED; } From 2c401ccec5aa24b0ca79933b834072764215302e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 6 Nov 2024 19:09:47 +1100 Subject: [PATCH 026/117] Copter: rename ADVANCED_FAILSAFE to AP_COPTER_ADVANCED_FAILSAFE_ENABLED to make integration with custom build server work --- ArduCopter/APM_Config.h | 2 +- ArduCopter/Copter.cpp | 2 +- ArduCopter/Copter.h | 6 +++--- ArduCopter/GCS_Mavlink.cpp | 2 +- ArduCopter/Parameters.cpp | 4 ++-- ArduCopter/Parameters.h | 4 ++-- ArduCopter/afs_copter.cpp | 4 ++-- ArduCopter/afs_copter.h | 6 ++++-- ArduCopter/config.h | 4 ++-- ArduCopter/events.cpp | 2 +- ArduCopter/failsafe.cpp | 2 +- ArduCopter/mode.h | 12 ++++++------ ArduCopter/motors.cpp | 2 +- 13 files changed, 27 insertions(+), 25 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 5b86ade1f5069..7f1ecabb7f14c 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -27,7 +27,7 @@ // features below are disabled by default on all boards //#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes //#define DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE // disable mode changes from GCS during Radio failsafes. Avoids a race condition for vehicle like Solo in which the RC and telemetry travel along the same link -//#define ADVANCED_FAILSAFE 1 // enabled advanced failsafe which allows running a portion of the mission in failsafe events +//#define AP_COPTER_ADVANCED_FAILSAFE_ENABLED 1 // enabled advanced failsafe which allows running a portion of the mission in failsafe events // other settings //#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 675b3b07eab03..417468160cdfc 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -232,7 +232,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if HAL_ADSB_ENABLED SCHED_TASK(avoidance_adsb_update, 10, 100, 138), #endif -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED SCHED_TASK(afs_fs_check, 10, 100, 141), #endif #if AP_TERRAIN_AVAILABLE diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 60c0818e6339d..48b6141c9a84d 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -137,7 +137,7 @@ #include #endif -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED # include "afs_copter.h" #endif #if TOY_MODE_ENABLED @@ -183,7 +183,7 @@ class Copter : public AP_Vehicle { friend class ParametersG2; friend class AP_Avoidance_Copter; -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED friend class AP_AdvancedFailsafe_Copter; #endif friend class AP_Arming_Copter; @@ -804,7 +804,7 @@ class Copter : public AP_Vehicle { // failsafe.cpp void failsafe_enable(); void failsafe_disable(); -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED void afs_fs_check(void); #endif diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 1e4e6da5984a1..6c2d169db8af6 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1519,7 +1519,7 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg) } MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_int_t &packet) { -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED if (GCS_MAVLINK::handle_flight_termination(packet) == MAV_RESULT_ACCEPTED) { return MAV_RESULT_ACCEPTED; } diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 8f5425482143a..ef7f785bea07a 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -771,7 +771,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("GND_EFFECT_COMP", 5, ParametersG2, gndeffect_comp_enabled, 1), -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // @Group: AFS_ // @Path: ../libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp AP_SUBGROUPINFO(afs, "AFS_", 6, ParametersG2, AP_AdvancedFailsafe), @@ -1251,7 +1251,7 @@ ParametersG2::ParametersG2(void) #if HAL_PROXIMITY_ENABLED , proximity() #endif -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED ,afs() #endif #if MODE_SMARTRTL_ENABLED diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 0eee4058331d7..7ba292bc5fed5 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -530,8 +530,8 @@ class ParametersG2 { // whether to enforce acceptance of packets only from sysid_my_gcs AP_Int8 sysid_enforce; - -#if ADVANCED_FAILSAFE + +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // advanced failsafe library AP_AdvancedFailsafe_Copter afs; #endif diff --git a/ArduCopter/afs_copter.cpp b/ArduCopter/afs_copter.cpp index a0b83b36cad3a..0d639e1276b12 100644 --- a/ArduCopter/afs_copter.cpp +++ b/ArduCopter/afs_copter.cpp @@ -4,7 +4,7 @@ #include "Copter.h" -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED /* setup radio_out values for all channels to termination values @@ -63,4 +63,4 @@ AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void) { copter.set_mode(Mode::Number::AUTO,ModeReason::GCS_FAILSAFE); } -#endif // ADVANCED_FAILSAFE +#endif // AP_COPTER_ADVANCED_FAILSAFE_ENABLED diff --git a/ArduCopter/afs_copter.h b/ArduCopter/afs_copter.h index 29e2dfa7a347c..4d133f6276a3d 100644 --- a/ArduCopter/afs_copter.h +++ b/ArduCopter/afs_copter.h @@ -18,7 +18,9 @@ advanced failsafe support for copter */ -#if ADVANCED_FAILSAFE +#include "config.h" + +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED #include /* @@ -44,5 +46,5 @@ class AP_AdvancedFailsafe_Copter : public AP_AdvancedFailsafe void set_mode_auto(void) override; }; -#endif // ADVANCED_FAILSAFE +#endif // AP_COPTER_ADVANCED_FAILSAFE_ENABLED diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 3faf4609c6672..0da560ea09099 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -574,8 +574,8 @@ // Developer Items // -#ifndef ADVANCED_FAILSAFE -# define ADVANCED_FAILSAFE 0 +#ifndef AP_COPTER_ADVANCED_FAILSAFE_ENABLED +# define AP_COPTER_ADVANCED_FAILSAFE_ENABLED 0 #endif #ifndef CH_MODE_DEFAULT diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index d49ce3d01aaf0..3002e773c02e5 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -492,7 +492,7 @@ void Copter::do_failsafe_action(FailsafeAction action, ModeReason reason){ set_mode_SmartRTL_or_land_with_pause(reason); break; case FailsafeAction::TERMINATE: { -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED g2.afs.gcs_terminate(true, "Failsafe"); #else arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE); diff --git a/ArduCopter/failsafe.cpp b/ArduCopter/failsafe.cpp index 57864b01be009..f143505977923 100644 --- a/ArduCopter/failsafe.cpp +++ b/ArduCopter/failsafe.cpp @@ -72,7 +72,7 @@ void Copter::failsafe_check() } -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED /* check for AFS failsafe check */ diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index faf22b0180f45..5579b51fb786e 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -4,7 +4,7 @@ #include #include // TODO why is this needed if Copter.h includes this -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED #include "afs_copter.h" #endif @@ -134,7 +134,7 @@ class Mode { virtual bool allows_flip() const { return false; } virtual bool crash_check_enabled() const { return true; } -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // Return the type of this mode for use by advanced failsafe virtual AP_AdvancedFailsafe_Copter::control_mode afs_mode() const { return AP_AdvancedFailsafe_Copter::control_mode::AFS_STABILIZED; } #endif @@ -517,7 +517,7 @@ class ModeAuto : public Mode { bool allows_inverted() const override { return true; }; #endif -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // Return the type of this mode for use by advanced failsafe AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; } #endif @@ -1068,7 +1068,7 @@ class ModeGuided : public Mode { bool requires_terrain_failsafe() const override { return true; } -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // Return the type of this mode for use by advanced failsafe AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; } #endif @@ -1243,7 +1243,7 @@ class ModeLand : public Mode { bool is_landing() const override { return true; }; -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // Return the type of this mode for use by advanced failsafe AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; } #endif @@ -1425,7 +1425,7 @@ class ModeRTL : public Mode { bool requires_terrain_failsafe() const override { return true; } -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // Return the type of this mode for use by advanced failsafe AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; } #endif diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 45eccc3e84f99..e382cb5941f65 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -135,7 +135,7 @@ void Copter::auto_disarm_check() // motors_output - send output to motors library which will adjust and send to ESCs and servos void Copter::motors_output() { -#if ADVANCED_FAILSAFE +#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED // this is to allow the failsafe module to deliberately crash // the vehicle. Only used in extreme circumstances to meet the // OBC rules From ca424a165dcb0d14be01ba918e0682619de96fad Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 6 Nov 2024 19:10:19 +1100 Subject: [PATCH 027/117] Tools: add entry for Copter advanced failsafe to custom build server --- Tools/autotest/test_build_options.py | 1 + Tools/scripts/build_options.py | 1 + Tools/scripts/extract_features.py | 1 + 3 files changed, 3 insertions(+) diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index 12f5b8c3946c9..a11b8c63b169a 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -287,6 +287,7 @@ def define_is_whitelisted_for_feature_in_code(self, target, define): feature_define_whitelist.add('AP_WINCH_DAIWA_ENABLED') feature_define_whitelist.add('AP_WINCH_PWM_ENABLED') feature_define_whitelist.add(r'AP_MOTORS_FRAME_.*_ENABLED') + feature_define_whitelist.add('AP_COPTER_ADVANCED_FAILSAFE_ENABLED') if target.lower() != "plane": # only on Plane: diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 3f5dd534ff8f1..54be58a3bd074 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -156,6 +156,7 @@ def config_option(self): Feature('Copter', 'MODE_FLOWHOLD', 'MODE_FLOWHOLD_ENABLED', 'Enable Mode Flowhold', 0, "OPTICALFLOW"), Feature('Copter', 'MODE_FLIP', 'MODE_FLIP_ENABLED', 'Enable Mode Flip', 0, None), Feature('Copter', 'MODE_BRAKE', 'MODE_BRAKE_ENABLED', 'Enable Mode Brake', 0, None), + Feature('Copter', 'COPTER_ADVANCED_FAILSAFE', 'AP_COPTER_ADVANCED_FAILSAFE_ENABLED', 'Enable Advanced Failsafe', 0, "ADVANCED_FAILSAFE"), # NOQA: 501 Feature('Rover', 'ROVER_ADVANCED_FAILSAFE', 'AP_ROVER_ADVANCED_FAILSAFE_ENABLED', 'Enable Advanced Failsafe', 0, "ADVANCED_FAILSAFE"), # NOQA: 501 diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index eec1399c02a3b..36a375522aa05 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -278,6 +278,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_RSSI_ENABLED', r'AP_RSSI::init'), ('AP_ROVER_ADVANCED_FAILSAFE_ENABLED', r'Rover::afs_fs_check'), + ('AP_COPTER_ADVANCED_FAILSAFE_ENABLED', r'Copter::afs_fs_check'), ('AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED', r'GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands'), ('AP_SERIALMANAGER_REGISTER_ENABLED', r'AP_SerialManager::register_port'), From d7b207fd7794adcaec5180dfa27baa6fff60899a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Nov 2024 13:23:18 +1100 Subject: [PATCH 028/117] AP_HAL_ChibiOS: tidy defaulting of OpticalFlow sensor type --- libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat index 5140f1130b1a9..64d400165f889 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat @@ -130,6 +130,7 @@ define HAL_BARO_20789_I2C_ADDR_ICM 0x68 define HAL_I2C_CLEAR_BUS define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 +define OPTICAL_FLOW_TYPE_DEFAULT Type::PIXART # the web UI uses an abin file for firmware uploads env BUILD_ABIN True From 8b008a2a19e8e6d683a5c55fce9c2d3725212c80 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Nov 2024 13:23:18 +1100 Subject: [PATCH 029/117] AP_OpticalFlow: tidy defaulting of OpticalFlow sensor type --- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 3492718ea4c40..1f0188917da43 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -19,9 +19,7 @@ extern const AP_HAL::HAL& hal; #ifndef OPTICAL_FLOW_TYPE_DEFAULT - #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 || defined(HAL_HAVE_PIXARTFLOW_SPI) - #define OPTICAL_FLOW_TYPE_DEFAULT Type::PIXART - #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP #define OPTICAL_FLOW_TYPE_DEFAULT Type::BEBOP #else #define OPTICAL_FLOW_TYPE_DEFAULT Type::NONE From 0c5741364e7ff3da40c87024675002f961668976 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Nov 2024 13:25:50 +1100 Subject: [PATCH 030/117] AP_HAL: tidy defaulting of Bebop OpticalFlow sensor type --- libraries/AP_HAL/board/linux.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL/board/linux.h b/libraries/AP_HAL/board/linux.h index b92ea9c40663b..a7e6c3fc1740f 100644 --- a/libraries/AP_HAL/board/linux.h +++ b/libraries/AP_HAL/board/linux.h @@ -57,6 +57,7 @@ #define HAL_LINUX_HEAT_TARGET_TEMP 50 #define BEBOP_CAMV_PWM 9 #define BEBOP_CAMV_PWM_FREQ 43333333 + #define OPTICAL_FLOW_TYPE_DEFAULT Type::BEBOP #define HAL_OPTFLOW_ONBOARD_VDEV_PATH "/dev/video0" #define HAL_OPTFLOW_ONBOARD_SUBDEV_PATH "/dev/v4l-subdev0" #define HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH 320 From 25a6d579ebba573d62fb53022025b168b7ddb701 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Nov 2024 13:25:50 +1100 Subject: [PATCH 031/117] AP_OpticalFlow: tidy defaulting of Bebop OpticalFlow sensor type --- libraries/AP_OpticalFlow/AP_OpticalFlow.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 1f0188917da43..2b86e63f73d4f 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -19,11 +19,7 @@ extern const AP_HAL::HAL& hal; #ifndef OPTICAL_FLOW_TYPE_DEFAULT - #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP - #define OPTICAL_FLOW_TYPE_DEFAULT Type::BEBOP - #else #define OPTICAL_FLOW_TYPE_DEFAULT Type::NONE - #endif #endif const AP_Param::GroupInfo AP_OpticalFlow::var_info[] = { From 1e7cd71ad604c8e124ee65d8d532b234745e74a3 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Fri, 8 Nov 2024 16:01:30 -0700 Subject: [PATCH 032/117] Tools: Add astyle dependency Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- Tools/environment_install/install-prereqs-ubuntu.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index 7f5c075a824f9..d7b7d8c31440d 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -162,7 +162,7 @@ else fi # Lists of packages to install -BASE_PKGS="build-essential ccache g++ gawk git make wget valgrind screen python3-pexpect" +BASE_PKGS="build-essential ccache g++ gawk git make wget valgrind screen python3-pexpect astyle" PYTHON_PKGS="future lxml pymavlink pyserial MAVProxy geocoder empy==3.3.4 ptyprocess dronecan" PYTHON_PKGS="$PYTHON_PKGS flake8 junitparser wsproto" From 99e314f49affdfcb96c87b1b2bf19dcc275e16c9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 10:29:06 +1100 Subject: [PATCH 033/117] AP_Mount: tidy header includes our pattern is to include the config file and then use the relevant define, with nothing in between --- libraries/AP_Mount/AP_Mount.cpp | 6 ++++-- libraries/AP_Mount/AP_Mount_Alexmos.cpp | 5 ++++- libraries/AP_Mount/AP_Mount_Alexmos.h | 5 ++++- libraries/AP_Mount/AP_Mount_Backend.cpp | 6 +++++- libraries/AP_Mount/AP_Mount_Backend_Serial.cpp | 5 ++++- libraries/AP_Mount/AP_Mount_Gremsy.cpp | 4 +++- libraries/AP_Mount/AP_Mount_Gremsy.h | 4 +++- libraries/AP_Mount/AP_Mount_SToRM32.cpp | 5 ++++- libraries/AP_Mount/AP_Mount_SToRM32.h | 4 +++- libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp | 5 ++++- libraries/AP_Mount/AP_Mount_SToRM32_serial.h | 4 +++- libraries/AP_Mount/AP_Mount_Scripting.cpp | 5 ++++- libraries/AP_Mount/AP_Mount_Scripting.h | 6 ++++-- libraries/AP_Mount/AP_Mount_Servo.cpp | 5 ++++- libraries/AP_Mount/AP_Mount_Servo.h | 4 +++- libraries/AP_Mount/AP_Mount_Siyi.cpp | 5 ++++- libraries/AP_Mount/AP_Mount_Siyi.h | 6 ++++-- libraries/AP_Mount/AP_Mount_Topotek.cpp | 4 +++- libraries/AP_Mount/AP_Mount_Viewpro.cpp | 5 ++++- libraries/AP_Mount/AP_Mount_Viewpro.h | 4 +++- libraries/AP_Mount/AP_Mount_Xacti.cpp | 5 ++++- libraries/AP_Mount/AP_Mount_Xacti.h | 4 +++- libraries/AP_Mount/AP_Mount_config.h | 2 ++ libraries/AP_Mount/SoloGimbal.cpp | 6 ++++-- libraries/AP_Mount/SoloGimbalEKF.cpp | 5 ++++- libraries/AP_Mount/SoloGimbal_Parameters.cpp | 5 ++++- 26 files changed, 95 insertions(+), 29 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index fe7f29f7eac3c..8dc31bbfa33de 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -1,9 +1,11 @@ +#include "AP_Mount_config.h" + +#if HAL_MOUNT_ENABLED + #include #include #include "AP_Mount.h" -#if HAL_MOUNT_ENABLED - #include "AP_Mount_Backend.h" #include "AP_Mount_Servo.h" #include "AP_Mount_SoloGimbal.h" diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 7db333f6b7849..79a8a5eaf0586 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Alexmos.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_ALEXMOS_ENABLED + +#include "AP_Mount_Alexmos.h" + #include //definition of the commands id for the Alexmos Serial Protocol diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.h b/libraries/AP_Mount/AP_Mount_Alexmos.h index a8765e17436f2..10225afa7255d 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.h +++ b/libraries/AP_Mount/AP_Mount_Alexmos.h @@ -3,9 +3,12 @@ */ #pragma once -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_ALEXMOS_ENABLED + +#include "AP_Mount_Backend.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 94f9e7e31b5c9..531d01c30a6e3 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -1,5 +1,9 @@ -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" + #if HAL_MOUNT_ENABLED + +#include "AP_Mount_Backend.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Backend_Serial.cpp b/libraries/AP_Mount/AP_Mount_Backend_Serial.cpp index ec27051057565..35fe60265b6a7 100644 --- a/libraries/AP_Mount/AP_Mount_Backend_Serial.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend_Serial.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Backend_Serial.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_ENABLED + +#include "AP_Mount_Backend_Serial.h" + #include // Default init function for every mount diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index 631acc87784e4..9e88a32417a7e 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -1,7 +1,9 @@ -#include "AP_Mount_Gremsy.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_GREMSY_ENABLED +#include "AP_Mount_Gremsy.h" + #include #include diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.h b/libraries/AP_Mount/AP_Mount_Gremsy.h index 79bd42af77670..581eda36908ab 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.h +++ b/libraries/AP_Mount/AP_Mount_Gremsy.h @@ -3,10 +3,12 @@ */ #pragma once -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_GREMSY_ENABLED +#include "AP_Mount_Backend.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index bd638fa42ab74..71875ad1592dc 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_SToRM32.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_STORM32MAVLINK_ENABLED + +#include "AP_Mount_SToRM32.h" + #include #include diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.h b/libraries/AP_Mount/AP_Mount_SToRM32.h index 33aef375c410f..6ff1a1ad1abaf 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32.h @@ -3,10 +3,12 @@ */ #pragma once -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_STORM32MAVLINK_ENABLED +#include "AP_Mount_Backend.h" + #include #include diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 9b003de0fe4b0..ace890366352d 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_SToRM32_serial.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_STORM32SERIAL_ENABLED + +#include "AP_Mount_SToRM32_serial.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h index 895143e80bb72..07b3d3d165de8 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h @@ -3,10 +3,12 @@ */ #pragma once -#include "AP_Mount_Backend_Serial.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_STORM32SERIAL_ENABLED +#include "AP_Mount_Backend_Serial.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index e70c03003616e..165d0d1a58a07 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Scripting.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_SCRIPTING_ENABLED + +#include "AP_Mount_Scripting.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Scripting.h b/libraries/AP_Mount/AP_Mount_Scripting.h index ecf1f7cbd89c7..52537f171dd1f 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.h +++ b/libraries/AP_Mount/AP_Mount_Scripting.h @@ -4,10 +4,12 @@ #pragma once -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_SCRIPTING_ENABLED +#include "AP_Mount_Backend.h" + #include #include #include @@ -50,4 +52,4 @@ class AP_Mount_Scripting : public AP_Mount_Backend bool target_loc_valid; // true if target_loc holds a valid target location }; -#endif // HAL_MOUNT_SIYISERIAL_ENABLED +#endif // HAL_MOUNT_SCRIPTING_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 743708fa9dd8b..f2446c2af7a5f 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Servo.h" +#include "AP_Mount_config.h" + #if HAL_MOUNT_SERVO_ENABLED +#include "AP_Mount_Servo.h" + #include #include diff --git a/libraries/AP_Mount/AP_Mount_Servo.h b/libraries/AP_Mount/AP_Mount_Servo.h index 8ead11561fd08..a1da63210bc5c 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.h +++ b/libraries/AP_Mount/AP_Mount_Servo.h @@ -3,10 +3,12 @@ */ #pragma once -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_SERVO_ENABLED +#include "AP_Mount_Backend.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 1c91af6a43fc9..e16a29586de5f 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Siyi.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_SIYI_ENABLED + +#include "AP_Mount_Siyi.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 20285b0b5bd6c..7f7f280b224aa 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/libraries/AP_Mount/AP_Mount_Siyi.h @@ -19,10 +19,12 @@ #pragma once -#include "AP_Mount_Backend_Serial.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_SIYI_ENABLED +#include "AP_Mount_Backend_Serial.h" + #include #include #include @@ -381,4 +383,4 @@ class AP_Mount_Siyi : public AP_Mount_Backend_Serial uint8_t sent_time_count; }; -#endif // HAL_MOUNT_SIYISERIAL_ENABLED +#endif // HAL_MOUNT_SIYI_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index bfe93ec83a8ed..dc1fae7007b9f 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -1,7 +1,9 @@ -#include "AP_Mount_Topotek.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_TOPOTEK_ENABLED +#include "AP_Mount_Topotek.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index 1de89fbc6722b..aa443483e82b2 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Viewpro.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_VIEWPRO_ENABLED + +#include "AP_Mount_Viewpro.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.h b/libraries/AP_Mount/AP_Mount_Viewpro.h index 1e045c8fd1ae2..1100586cd1ce1 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.h +++ b/libraries/AP_Mount/AP_Mount_Viewpro.h @@ -16,10 +16,12 @@ #pragma once -#include "AP_Mount_Backend_Serial.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_VIEWPRO_ENABLED +#include "AP_Mount_Backend_Serial.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index c5727f6c023d8..86296d814be01 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -1,6 +1,9 @@ -#include "AP_Mount_Xacti.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_XACTI_ENABLED + +#include "AP_Mount_Xacti.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_Xacti.h b/libraries/AP_Mount/AP_Mount_Xacti.h index 31a6684023561..0057f4ae371ad 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.h +++ b/libraries/AP_Mount/AP_Mount_Xacti.h @@ -8,10 +8,12 @@ #pragma once -#include "AP_Mount_Backend.h" +#include "AP_Mount_config.h" #if HAL_MOUNT_XACTI_ENABLED +#include "AP_Mount_Backend.h" + #include #include #include diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index c7b063b20e2ec..c6e0791c2a6b0 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -2,6 +2,8 @@ #include #include +#include +#include #ifndef HAL_MOUNT_ENABLED #define HAL_MOUNT_ENABLED 1 diff --git a/libraries/AP_Mount/SoloGimbal.cpp b/libraries/AP_Mount/SoloGimbal.cpp index eba243eabac4a..6e270de10251a 100644 --- a/libraries/AP_Mount/SoloGimbal.cpp +++ b/libraries/AP_Mount/SoloGimbal.cpp @@ -1,9 +1,11 @@ +#include "AP_Mount_config.h" + +#if HAL_SOLO_GIMBAL_ENABLED + #include #include #include "SoloGimbal.h" -#if HAL_SOLO_GIMBAL_ENABLED - #include #include #include diff --git a/libraries/AP_Mount/SoloGimbalEKF.cpp b/libraries/AP_Mount/SoloGimbalEKF.cpp index b599b4c2516b8..b514706f159a9 100644 --- a/libraries/AP_Mount/SoloGimbalEKF.cpp +++ b/libraries/AP_Mount/SoloGimbalEKF.cpp @@ -1,3 +1,7 @@ +#include "AP_Mount_config.h" + +#if HAL_SOLO_GIMBAL_ENABLED + #include // uncomment this to force the optimisation of this code, note that @@ -9,7 +13,6 @@ #endif #include "SoloGimbalEKF.h" -#if HAL_SOLO_GIMBAL_ENABLED #include #include #include diff --git a/libraries/AP_Mount/SoloGimbal_Parameters.cpp b/libraries/AP_Mount/SoloGimbal_Parameters.cpp index 60bf156acb3ba..ff76f0583dfed 100644 --- a/libraries/AP_Mount/SoloGimbal_Parameters.cpp +++ b/libraries/AP_Mount/SoloGimbal_Parameters.cpp @@ -1,5 +1,8 @@ -#include "SoloGimbal_Parameters.h" +#include "AP_Mount_config.h" + #if HAL_SOLO_GIMBAL_ENABLED + +#include "SoloGimbal_Parameters.h" #include #include #include From cf27ba09d006752b79862a5de3e53a59d9b4ff60 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 19 Sep 2024 12:25:23 +1000 Subject: [PATCH 034/117] AP_IOMCU: use RC_Channel to populate IOMCU mappings --- libraries/AP_IOMCU/AP_IOMCU.cpp | 23 +++++++++++++---------- libraries/AP_IOMCU/AP_IOMCU.h | 3 +-- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index 77617cdb02ea4..e1c6f5d863444 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -1208,7 +1208,7 @@ void AP_IOMCU::bind_dsm(uint8_t mode) setup for mixing. This allows fixed wing aircraft to fly in manual mode if the FMU dies */ -bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan, +bool AP_IOMCU::setup_mixing(int8_t override_chan, float mixing_gain, uint16_t manual_rc_mask) { if (!is_chibios_backend) { @@ -1229,16 +1229,19 @@ bool AP_IOMCU::setup_mixing(RCMapper *rcmap, int8_t override_chan, MIX_UPDATE(mixing.servo_function[i], c->get_function()); MIX_UPDATE(mixing.servo_reversed[i], c->get_reversed()); } - // update RCMap - MIX_UPDATE(mixing.rc_channel[0], rcmap->roll()); - MIX_UPDATE(mixing.rc_channel[1], rcmap->pitch()); - MIX_UPDATE(mixing.rc_channel[2], rcmap->throttle()); - MIX_UPDATE(mixing.rc_channel[3], rcmap->yaw()); + auto &xrc = rc(); + // note that if not all of these channels are specified correctly + // in parameters then these may be a "dummy" RC channel pointer. + // In that case c->ch() will be zero. + const RC_Channel *channels[] { + &xrc.get_roll_channel(), + &xrc.get_pitch_channel(), + &xrc.get_throttle_channel(), + &xrc.get_yaw_channel() + }; for (uint8_t i=0; i<4; i++) { - const RC_Channel *c = RC_Channels::rc_channel(mixing.rc_channel[i]-1); - if (!c) { - continue; - } + const auto *c = channels[i]; + MIX_UPDATE(mixing.rc_channel[i], c->ch()); MIX_UPDATE(mixing.rc_min[i], c->get_radio_min()); MIX_UPDATE(mixing.rc_max[i], c->get_radio_max()); MIX_UPDATE(mixing.rc_trim[i], c->get_radio_trim()); diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index 1d03debb9a86b..da6d55a03dce7 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -11,7 +11,6 @@ #if HAL_WITH_IO_MCU #include "iofirmware/ioprotocol.h" -#include #include #include @@ -151,7 +150,7 @@ class AP_IOMCU void soft_reboot(); // setup for FMU failsafe mixing - bool setup_mixing(RCMapper *rcmap, int8_t override_chan, + bool setup_mixing(int8_t override_chan, float mixing_gain, uint16_t manual_rc_mask); // Check if pin number is valid and configured for GPIO From 2ad8477f98f4bc0e73ddfae8ded2548aedaf2a5e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 19 Sep 2024 12:25:23 +1000 Subject: [PATCH 035/117] ArduPlane: use RC_Channel to populate IOMCU mappings --- ArduPlane/ArduPlane.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 0181f392d804b..d69e3a3842e4b 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -329,7 +329,7 @@ void Plane::one_second_loop() set_control_channels(); #if HAL_WITH_IO_MCU - iomcu.setup_mixing(&rcmap, g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask); + iomcu.setup_mixing(g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask); #endif #if HAL_ADSB_ENABLED From 3716f5513d307e4731972819d095e8315dd8bf2d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 13:39:20 +1100 Subject: [PATCH 036/117] AP_HAL_SITL: process inbound data in outqueue-length delay loop this is the loop which ensures the amount of data sent to the mavlink client (usually Python) is limited - if we don't do this then we lose vast amounts of data when running at large speedups. By attempting to process inbound data we may realise that the TCP connection has been dropped, and in that case we will start to listen for another connection. This allows you to terminate the sim_vehicle.py MAVProxy and have it automagically restart (when running under GDB). This is very useful for testing MAVProxy patches with SITL; it's a different workflow to opening an output and connecting a new version of MAVProxy to that outout. --- libraries/AP_HAL_SITL/SITL_State.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index af55acaa1f844..4850e40b027a4 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -185,12 +185,14 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) // conditions. if (speedup > 1 && hal.scheduler->in_main_thread()) { while (true) { - const int queue_length = ((HALSITL::UARTDriver*)hal.serial(0))->get_system_outqueue_length(); + HALSITL::UARTDriver *uart = (HALSITL::UARTDriver*)hal.serial(0); + const int queue_length = uart->get_system_outqueue_length(); // ::fprintf(stderr, "queue_length=%d\n", (signed)queue_length); if (queue_length < 1024) { break; } _serial_0_outqueue_full_count++; + uart->handle_reading_from_device_to_readbuffer(); usleep(1000); } } From 8085c44840aaa31ad454ab7852a978cdb9f52915 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 11 Nov 2024 21:27:06 +1100 Subject: [PATCH 037/117] AP_HAL_SITL: remove redundant gps state shadows stuff in base class --- libraries/AP_HAL_SITL/SITL_State.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 4ff1c0c046b22..b5452aceda2df 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -110,9 +110,6 @@ class HALSITL::SITL_State : public SITL_State_Common { uint32_t wind_start_delay_micros; uint32_t last_wind_update_us; - // simulated GPS devices - SITL::GPS *gps[2]; // constrained by # of parameter sets - // returns a voltage between 0V to 5V which should appear as the // voltage from the sensor float _sonar_pin_voltage() const; From 3bc0207f22d48648d0c46d301904fa782861733d Mon Sep 17 00:00:00 2001 From: zhou <3177821352@qq.com> Date: Wed, 23 Oct 2024 18:03:36 +0800 Subject: [PATCH 038/117] AP_Mount: topotek: Change the type of gimbal angle acquisition ... also convert the lowercase characters in the command to uppercase --- libraries/AP_Mount/AP_Mount_Topotek.cpp | 10 +++++----- libraries/AP_Mount/AP_Mount_Topotek.h | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index dc1fae7007b9f..4e468a7ae5173 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -33,7 +33,7 @@ extern const AP_HAL::HAL& hal; # define AP_MOUNT_TOPOTEK_ID3CHAR_START_TRACKING "LOC" // start image tracking # define AP_MOUNT_TOPOTEK_ID3CHAR_LRF "LRF" // laser rangefinder control, data bytes: 00:ranging stop, 01:ranging start, 02:single measurement, 03:continuous measurement # define AP_MOUNT_TOPOTEK_ID3CHAR_PIP "PIP" // set picture-in-picture setting, data bytes: // 00:main only, 01:main+sub, 02:sub+main, 03:sub only, 0A:next -# define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT "GAA" // get gimbal attitude, data bytes: 00:stop, 01:start +# define AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT "GIA" // get gimbal attitude, data bytes: 00:stop, 01:start # define AP_MOUNT_TOPOTEK_ID3CHAR_SD_CARD "SDC" // get SD card state, data bytes: 00:get remaining capacity, 01:get total capacity # define AP_MOUNT_TOPOTEK_ID3CHAR_TIME "UTC" // set time and date, data bytes: HHMMSSDDMMYY # define AP_MOUNT_TOPOTEK_ID3CHAR_GET_VERSION "VSN" // get firmware version, data bytes always 00 @@ -756,7 +756,7 @@ void AP_Mount_Topotek::read_incoming_packets() // request gimbal attitude void AP_Mount_Topotek::request_gimbal_attitude() { - // sample command: #TPUG2wGAA01 + // sample command: #TPUG2wGIA01 send_fixedlen_packet(AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_GIMBAL_ATT, true, 1); } @@ -802,7 +802,7 @@ void AP_Mount_Topotek::send_angle_target(const MountTarget& angle_rad) // calculate and send yaw target // sample command #tpUG6wGIY - const char* format_str = "%04x%02x"; + const char* format_str = "%04X%02X"; const uint8_t speed = 99; const uint16_t yaw_angle_cd = (uint16_t)constrain_int16(degrees(angle_rad.get_bf_yaw()) * 100, MAX(-18000, _params.yaw_angle_min * 100), MIN(18000, _params.yaw_angle_max * 100)); @@ -863,7 +863,7 @@ void AP_Mount_Topotek::send_rate_target(const MountTarget& rate_rads) // prepare and send command // sample command: #tpUG6wYPR uint8_t databuff[7]; - hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02x%02x%02x", yaw_angle_speed, pitch_angle_speed, roll_angle_speed); + hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02X%02X%02X", yaw_angle_speed, pitch_angle_speed, roll_angle_speed); send_variablelen_packet(HeaderType::VARIABLE_LEN, AddressByte::GIMBAL, AP_MOUNT_TOPOTEK_ID3CHAR_YPR_RATE, true, databuff, ARRAY_SIZE(databuff)-1); } @@ -1141,7 +1141,7 @@ int16_t AP_Mount_Topotek::hexchar4_to_int16(char high, char mid_high, char mid_l bool AP_Mount_Topotek::send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value) { uint8_t databuff[3]; - hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02x", value); + hal.util->snprintf((char *)databuff, ARRAY_SIZE(databuff), "%02X", value); return send_variablelen_packet(HeaderType::FIXED_LEN, address, id, write, databuff, ARRAY_SIZE(databuff)-1); } diff --git a/libraries/AP_Mount/AP_Mount_Topotek.h b/libraries/AP_Mount/AP_Mount_Topotek.h index 3d52f0ff62827..9c8882936c758 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.h +++ b/libraries/AP_Mount/AP_Mount_Topotek.h @@ -272,7 +272,7 @@ class AP_Mount_Topotek : public AP_Mount_Backend_Serial // stores command ID and corresponding member functions that are compared with the command received by the gimbal UartCmdFunctionHandler uart_recv_cmd_compare_list[AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM] = { - {{"GAC"}, &AP_Mount_Topotek::gimbal_angle_analyse}, + {{"GIA"}, &AP_Mount_Topotek::gimbal_angle_analyse}, {{"REC"}, &AP_Mount_Topotek::gimbal_record_analyse}, {{"SDC"}, &AP_Mount_Topotek::gimbal_sdcard_analyse}, {{"LRF"}, &AP_Mount_Topotek::gimbal_dist_info_analyse}, From be769a6a7fa1cfc44c1a2fa6b31dc897c043d4ce Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Nov 2024 22:00:05 +1100 Subject: [PATCH 039/117] Tools: correct powr_change.py output for accumulated flags --- Tools/scripts/powr_change.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/Tools/scripts/powr_change.py b/Tools/scripts/powr_change.py index 332f9814dcd9e..d7c295a08c757 100755 --- a/Tools/scripts/powr_change.py +++ b/Tools/scripts/powr_change.py @@ -70,8 +70,6 @@ def run(self): if new_acc_bit_set and not old_acc_bit_set: line += " ACCFLAGS+%s" % self.bit_description(bit) - elif not new_bit_set and old_bit_set: - line += " ACCFLAGS-%s" % self.bit_description(bit) if len(line) == 0: continue From 7e7f56df79e7a8dd1f71db0bc394a9793be2f8b7 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Fri, 8 Nov 2024 02:35:08 -0700 Subject: [PATCH 040/117] Tools: Add mavcesium option to sim_vehicle.py Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- Tools/autotest/sim_vehicle.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index 19dbbb5ee7cd3..6899e55e9912c 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -964,6 +964,8 @@ def start_mavproxy(opts, stuff): cmd.extend(['--aircraft', opts.aircraft]) if opts.moddebug: cmd.append('--moddebug=%u' % opts.moddebug) + if opts.mavcesium: + cmd.extend(["--load-module", "cesium"]) if opts.fresh_params: # these were built earlier: @@ -1373,6 +1375,11 @@ def generate_frame_help(): default=False, action='store_true', help="load map module on startup") +group.add_option("", "--mavcesium", + default=False, + action='store_true', + help="load MAVCesium module on startup") + group.add_option("", "--console", default=False, action='store_true', From 8413ab2bf2ca9d8b60d62af73727baaaeed2be4e Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Sun, 10 Nov 2024 14:03:50 -0900 Subject: [PATCH 041/117] AP_InertialSensor: added IIM42653 support --- .../AP_InertialSensor/AP_InertialSensor_Backend.h | 1 + .../AP_InertialSensor_Invensensev3.cpp | 13 +++++++++++++ .../AP_InertialSensor_Invensensev3.h | 1 + 3 files changed, 15 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 00d149e149223..817cb2e947c85 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -136,6 +136,7 @@ class AP_InertialSensor_Backend DEVTYPE_INS_ICM42670 = 0x3A, DEVTYPE_INS_ICM45686 = 0x3B, DEVTYPE_INS_SCHA63T = 0x3C, + DEVTYPE_INS_IIM42653 = 0x3D, }; protected: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index daf54ed64e7ec..d22a7a224f30b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -136,6 +136,7 @@ extern const AP_HAL::HAL& hal; #define INV3_ID_ICM42605 0x42 #define INV3_ID_ICM42688 0x47 #define INV3_ID_IIM42652 0x6f +#define INV3_ID_IIM42653 0x56 #define INV3_ID_ICM42670 0x67 #define INV3_ID_ICM45686 0xE9 @@ -258,6 +259,12 @@ void AP_InertialSensor_Invensensev3::start() devtype = DEVTYPE_INS_IIM42652; temp_sensitivity = 1.0 / 2.07; break; + case Invensensev3_Type::IIM42653: + devtype = DEVTYPE_INS_IIM42653; + temp_sensitivity = 1.0 / 2.07; + gyro_scale = GYRO_SCALE_4000DPS; + accel_scale = ACCEL_SCALE_32G; + break; case Invensensev3_Type::ICM42688: devtype = DEVTYPE_INS_ICM42688; temp_sensitivity = 1.0 / 2.07; @@ -294,6 +301,7 @@ void AP_InertialSensor_Invensensev3::start() switch (inv3_type) { case Invensensev3_Type::ICM42688: // HiRes 19bit case Invensensev3_Type::IIM42652: // HiRes 19bit + case Invensensev3_Type::IIM42653: // HiRes 19bit case Invensensev3_Type::ICM45686: // HiRes 20bit highres_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; break; @@ -824,6 +832,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void) case Invensensev3_Type::ICM42688: case Invensensev3_Type::ICM42605: case Invensensev3_Type::IIM42652: + case Invensensev3_Type::IIM42653: case Invensensev3_Type::ICM42670: { /* fix for the "stuck gyro" issue, which affects all IxM42xxx @@ -967,6 +976,9 @@ bool AP_InertialSensor_Invensensev3::check_whoami(void) case INV3_ID_IIM42652: inv3_type = Invensensev3_Type::IIM42652; return true; + case INV3_ID_IIM42653: + inv3_type = Invensensev3_Type::IIM42653; + return true; case INV3_ID_ICM42670: inv3_type = Invensensev3_Type::ICM42670; return true; @@ -1042,6 +1054,7 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void) switch (inv3_type) { case Invensensev3_Type::ICM45686: case Invensensev3_Type::ICM40609: + case Invensensev3_Type::IIM42653: _clip_limit = 29.5f * GRAVITY_MSS; break; case Invensensev3_Type::ICM42688: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index b3946aa81dd47..6b4fc49a8c5c4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -37,6 +37,7 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend ICM42605, // No HiRes ICM40605, // No HiRes IIM42652, // HiRes 19bit + IIM42653, // HiRes 19bit ICM42670, // HiRes 19bit ICM45686 // HiRes 20bit }; From 3e0c0132c8625ba515ac8cbac9df191db0119271 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Sun, 10 Nov 2024 14:05:25 -0900 Subject: [PATCH 042/117] Tools: scripts: decode_devid.py: added IIM42653 --- Tools/scripts/decode_devid.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/scripts/decode_devid.py b/Tools/scripts/decode_devid.py index ee80f080ae0c8..8841d06d8769e 100755 --- a/Tools/scripts/decode_devid.py +++ b/Tools/scripts/decode_devid.py @@ -104,6 +104,7 @@ def num(s): 0x3A : "DEVTYPE_INS_ICM42670", 0x3B : "DEVTYPE_INS_ICM45686", 0x3C : "DEVTYPE_INS_SCHA63T", + 0x3D : "DEVTYPE_INS_IIM42653", } baro_types = { From 8a58642cd10fe5c8d1a636b795d0a0d5101f15d1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 20 Sep 2024 16:09:11 +1000 Subject: [PATCH 043/117] waf: make initialiser reordering fatal we were bitten by a nasty bug in CAN because of constructor reordering --- Tools/ardupilotwaf/boards.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 154ef72f8bcf4..7ccddaad109e0 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -361,10 +361,10 @@ def configure_env(self, cfg, env): '-Wpointer-arith', '-Wno-unused-parameter', '-Wno-missing-field-initializers', - '-Wno-reorder', '-Wno-redundant-decls', '-Wno-unknown-pragmas', '-Wno-expansion-to-defined', + '-Werror=reorder', '-Werror=cast-align', '-Werror=attributes', '-Werror=format-security', From 6ee1d94ec7b8f8f4a02f1597d24ba6739da046b8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 20 Sep 2024 22:19:18 +1000 Subject: [PATCH 044/117] Copter: reorder initialisation of member variables to make -Werror=reorder work --- ArduCopter/Parameters.cpp | 19 ++++++++++--------- ArduCopter/Parameters.h | 5 +++++ 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index ef7f785bea07a..fd5eea3e8626f 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1240,8 +1240,11 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { /* constructor for g2 object */ -ParametersG2::ParametersG2(void) - : command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f) +ParametersG2::ParametersG2(void) : + unused_integer{17} +#if HAL_BUTTON_ENABLED + ,button_ptr(&copter.button) +#endif #if AP_TEMPCALIBRATION_ENABLED , temp_calibration() #endif @@ -1257,15 +1260,15 @@ ParametersG2::ParametersG2(void) #if MODE_SMARTRTL_ENABLED ,smart_rtl() #endif +#if USER_PARAMS_ENABLED + ,user_parameters() +#endif #if MODE_FLOWHOLD_ENABLED ,mode_flowhold_ptr(&copter.mode_flowhold) #endif #if MODE_FOLLOW_ENABLED ,follow() #endif -#if USER_PARAMS_ENABLED - ,user_parameters() -#endif #if AUTOTUNE_ENABLED ,autotune_ptr(&copter.mode_autotune.autotune) #endif @@ -1275,13 +1278,9 @@ ParametersG2::ParametersG2(void) #if MODE_AUTOROTATE_ENABLED ,arot() #endif -#if HAL_BUTTON_ENABLED - ,button_ptr(&copter.button) -#endif #if MODE_ZIGZAG_ENABLED ,mode_zigzag_ptr(&copter.mode_zigzag) #endif - #if MODE_ACRO_ENABLED || MODE_SPORT_ENABLED ,command_model_acro_rp(ACRO_RP_RATE_DEFAULT, ACRO_RP_EXPO_DEFAULT, 0.0f) #endif @@ -1290,6 +1289,8 @@ ParametersG2::ParametersG2(void) ,command_model_acro_y(ACRO_Y_RATE_DEFAULT, ACRO_Y_EXPO_DEFAULT, 0.0f) #endif + ,command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f) + #if WEATHERVANE_ENABLED ,weathervane() #endif diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 7ba292bc5fed5..0cf0a8cb51959 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -499,6 +499,11 @@ class ParametersG2 { // altitude at which nav control can start in takeoff AP_Float wp_navalt_min; + // unused_integer simply exists so that the constructor for + // ParametersG2 can be created with a relatively easy syntax in + // the face of many #ifs: + uint8_t unused_integer; + // button checking #if HAL_BUTTON_ENABLED AP_Button *button_ptr; From b7ccee5ebe2b604a7bd39f69515651d3f4ffca20 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 22 Sep 2024 15:53:10 +1000 Subject: [PATCH 045/117] Plane: reorder initialisation of member variables to make -Werror=reorder work --- ArduPlane/Parameters.h | 6 +++--- ArduPlane/mode.cpp | 7 ++++--- ArduPlane/mode.h | 3 +++ 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 377eb9ded865f..d29243d1ef2bb 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -483,6 +483,9 @@ class ParametersG2 { // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; + // just to make compilation easier when all things are compiled out... + uint8_t unused_integer; + // button reporting library #if HAL_BUTTON_ENABLED AP_Button *button_ptr; @@ -579,9 +582,6 @@ class ParametersG2 { AP_Int8 axis_bitmask; // axes to be autotuned - // just to make compilation easier when all things are compiled out... - uint8_t unused_integer; - #if AP_RANGEFINDER_ENABLED // orientation of rangefinder to use for landing AP_Int8 rangefinder_land_orient; diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index e4f97acc2c089..8bece6d4187c3 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -1,14 +1,15 @@ #include "Plane.h" Mode::Mode() : - ahrs(plane.ahrs) + unused_integer{17}, #if HAL_QUADPLANE_ENABLED - , quadplane(plane.quadplane), pos_control(plane.quadplane.pos_control), attitude_control(plane.quadplane.attitude_control), loiter_nav(plane.quadplane.loiter_nav), - poscontrol(plane.quadplane.poscontrol) + quadplane(plane.quadplane), + poscontrol(plane.quadplane.poscontrol), #endif + ahrs(plane.ahrs) { } diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 230b372499ff4..8260924afca89 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -159,6 +159,9 @@ class Mode // Output pilot throttle, this is used in stabilized modes without auto throttle control void output_pilot_throttle(); + // makes the initialiser list in the constructor manageable + uint8_t unused_integer; + #if HAL_QUADPLANE_ENABLED // References for convenience, used by QModes AC_PosControl*& pos_control; From 35f2dce575d755fb533d85db1705db391879b811 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 14:31:39 +1100 Subject: [PATCH 046/117] AP_HAL_ESP32: re-order initialiser lines so -Werror=reorder will work --- libraries/AP_HAL_ESP32/AnalogIn.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_ESP32/AnalogIn.cpp b/libraries/AP_HAL_ESP32/AnalogIn.cpp index 05487e62a0052..7f027a31a31e9 100644 --- a/libraries/AP_HAL_ESP32/AnalogIn.cpp +++ b/libraries/AP_HAL_ESP32/AnalogIn.cpp @@ -141,8 +141,8 @@ void adc_calibration_deinit(adc_cali_handle_t handle) //ardupin is the ardupilot assigned number, starting from 1-8(max) AnalogSource::AnalogSource(int16_t ardupin, adc_channel_t adc_channel, float scaler, float initial_value) : - _ardupin(ardupin), _adc_channel(adc_channel), + _ardupin(ardupin), _scaler(scaler), _value(initial_value), _latest_value(initial_value), From cca1edb78d3f38ce03752bd3746fb78c23d453e3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 14:31:39 +1100 Subject: [PATCH 047/117] AP_HAL: re-order initialiser lines so -Werror=reorder will work AP_HAL: correct compilation for sim-on-hardware with -werror=reorder --- libraries/AP_HAL/HAL.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL/HAL.h b/libraries/AP_HAL/HAL.h index 723e68f64ffc2..a2bef78e470de 100644 --- a/libraries/AP_HAL/HAL.h +++ b/libraries/AP_HAL/HAL.h @@ -67,9 +67,6 @@ class AP_HAL::HAL { scheduler(_scheduler), util(_util), opticalflow(_opticalflow), -#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL - simstate(_simstate), -#endif flash(_flash), #if HAL_WITH_DSP dsp(_dsp), @@ -85,6 +82,9 @@ class AP_HAL::HAL { _serial7, _serial8, _serial9} +#if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL + ,simstate(_simstate) +#endif { #if HAL_NUM_CAN_IFACES > 0 if (_can_ifaces == nullptr) { From 43970c0c7a6bc2d37212decf984fbd2a695fe83f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 14:31:39 +1100 Subject: [PATCH 048/117] AP_HAL_Linux: re-order initialiser lines so -Werror=reorder will work --- libraries/AP_HAL_Linux/Flow_PX4.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_HAL_Linux/Flow_PX4.cpp b/libraries/AP_HAL_Linux/Flow_PX4.cpp index e285763c4fa57..08939bccf9193 100644 --- a/libraries/AP_HAL_Linux/Flow_PX4.cpp +++ b/libraries/AP_HAL_Linux/Flow_PX4.cpp @@ -53,8 +53,8 @@ Flow_PX4::Flow_PX4(uint32_t width, uint32_t bytesperline, float bottom_flow_feature_threshold, float bottom_flow_value_threshold) : _width(width), - _bytesperline(bytesperline), _search_size(max_flow_pixel), + _bytesperline(bytesperline), _bottom_flow_feature_threshold(bottom_flow_feature_threshold), _bottom_flow_value_threshold(bottom_flow_value_threshold) { From d1674b089a6f4d6aace91a075dd5d7776f5f5be2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 25 Sep 2024 14:32:02 +1000 Subject: [PATCH 049/117] AP_Periph: rearrange apd periph initialiser for --error=reorder --- Tools/AP_Periph/esc_apd_telem.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Tools/AP_Periph/esc_apd_telem.cpp b/Tools/AP_Periph/esc_apd_telem.cpp index ed438d60f36c5..e7173bdca9e83 100644 --- a/Tools/AP_Periph/esc_apd_telem.cpp +++ b/Tools/AP_Periph/esc_apd_telem.cpp @@ -17,8 +17,9 @@ extern const AP_HAL::HAL& hal; #define TELEM_LEN 0x16 ESC_APD_Telem::ESC_APD_Telem (AP_HAL::UARTDriver *_uart, float num_poles) : - pole_count(num_poles), - uart(_uart) { + uart(_uart), + pole_count(num_poles) +{ uart->begin(115200); } From e14045898d9f7876cef8f6d7371a4e243fa8a546 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 12 Nov 2024 13:31:58 +0000 Subject: [PATCH 050/117] AP_DDS: move closing #endif for status publisher - Must be before the status_ok check. Signed-off-by: Rhys Mainwaring --- libraries/AP_DDS/AP_DDS_Client.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index ef4e14a35132f..b70c026b6a9d4 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -1615,10 +1615,11 @@ void AP_DDS_Client::update() } last_status_check_time_ms = cur_time_ms; } +#endif // AP_DDS_STATUS_PUB_ENABLED status_ok = uxr_run_session_time(&session, 1); } -#endif // AP_DDS_STATUS_PUB_ENABLED + #if CONFIG_HAL_BOARD != HAL_BOARD_SITL extern "C" { int clock_gettime(clockid_t clockid, struct timespec *ts); From 2bd4e15f761d7263e3eb41d0557eb9ea6b6c1d27 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Tue, 12 Nov 2024 12:18:17 +0000 Subject: [PATCH 051/117] AP_DDS: use memset to initialise variable size array Signed-off-by: Rhys Mainwaring --- libraries/AP_DDS/AP_DDS_Client.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index b70c026b6a9d4..05ba28f06dc7a 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -955,8 +955,9 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u .type = UXR_REPLIER_ID }; - uint32_t reply_size = rcl_interfaces_srv_SetParameters_Response_size_of_topic(&set_parameter_response, 0U); - uint8_t reply_buffer[reply_size] {}; + const uint32_t reply_size = rcl_interfaces_srv_SetParameters_Response_size_of_topic(&set_parameter_response, 0U); + uint8_t reply_buffer[reply_size]; + memset(reply_buffer, 0, reply_size * sizeof(uint8_t)); ucdrBuffer reply_ub; ucdr_init_buffer(&reply_ub, reply_buffer, reply_size); @@ -1040,8 +1041,9 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u .type = UXR_REPLIER_ID }; - uint32_t reply_size = rcl_interfaces_srv_GetParameters_Response_size_of_topic(&get_parameters_response, 0U); - uint8_t reply_buffer[reply_size] {}; + const uint32_t reply_size = rcl_interfaces_srv_GetParameters_Response_size_of_topic(&get_parameters_response, 0U); + uint8_t reply_buffer[reply_size]; + memset(reply_buffer, 0, reply_size * sizeof(uint8_t)); ucdrBuffer reply_ub; ucdr_init_buffer(&reply_ub, reply_buffer, reply_size); From 3bbcd8b0a3566a90483544ef6c8f7cc7916965e8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 12 Nov 2024 20:43:06 +0900 Subject: [PATCH 052/117] AP_Scripting: copter-slung-payload minor format fix --- .../AP_Scripting/applets/copter-slung-payload.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Scripting/applets/copter-slung-payload.md b/libraries/AP_Scripting/applets/copter-slung-payload.md index 32d76256c43a7..b4953565ccf22 100644 --- a/libraries/AP_Scripting/applets/copter-slung-payload.md +++ b/libraries/AP_Scripting/applets/copter-slung-payload.md @@ -4,13 +4,13 @@ This script moves a Copter so as to reduce a slung payload's oscillation. Requi # Parameters -SLUP_ENABLE : Set to 1 to enable this script -SLUP_VEL_P : Oscillation controller velocity P gain. Higher values will result in the vehicle moving more quickly in sync with the payload -SLUP_DIST_MAX : maximum acceptable distance between vehicle and payload. Within this distance oscillation suppression will operate -SLUP_SYSID : System id of payload's autopilot. If zero any system id is accepted -SLUP_WP_POS_P : Return to waypoint position P gain. Higher values result in the vehicle returning more quickly to the latest waypoint -SLUP_RESTOFS_TC : Slung Payload resting offset estimate filter time constant. Higher values result in smoother estimate but slower response -SLUP_DEBUG : Slung payload debug output, set to 1 to enable debug +- SLUP_ENABLE : Set to 1 to enable this script +- SLUP_VEL_P : Oscillation controller velocity P gain. Higher values will result in the vehicle moving more quickly in sync with the payload +- SLUP_DIST_MAX : maximum acceptable distance between vehicle and payload. Within this distance oscillation suppression will operate +- SLUP_SYSID : System id of payload's autopilot. If zero any system id is accepted +- SLUP_WP_POS_P : Return to waypoint position P gain. Higher values result in the vehicle returning more quickly to the latest waypoint +- SLUP_RESTOFS_TC : Slung Payload resting offset estimate filter time constant. Higher values result in smoother estimate but slower response +- SLUP_DEBUG : Slung payload debug output, set to 1 to enable debug # How To Use From 9f29606d1c915f0dd64a06babf628c623311783a Mon Sep 17 00:00:00 2001 From: Patrick Menschel Date: Mon, 11 Nov 2024 17:33:10 +0100 Subject: [PATCH 053/117] AP_Tramp: Fix _configuration_finished indication The flag _configuration_finished in AP_VideoTX is not set by AP_Tramp. Therefore OSD item VTX_PWR blinks forever. --- libraries/AP_VideoTX/AP_Tramp.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_VideoTX/AP_Tramp.cpp b/libraries/AP_VideoTX/AP_Tramp.cpp index 54f62a382f051..afe6f07ac7bc2 100644 --- a/libraries/AP_VideoTX/AP_Tramp.cpp +++ b/libraries/AP_VideoTX/AP_Tramp.cpp @@ -143,6 +143,8 @@ char AP_Tramp::handle_response(void) debug("device config: freq: %u, cfg pwr: %umw, act pwr: %umw, pitmode: %u", unsigned(freq), unsigned(power), unsigned(cur_act_power), unsigned(pit_mode)); + // update the "_configuration_finished" flag, otherwise OSD item VTX_POWER blinks forever + vtx.set_configuration_finished(!update_pending); return 'v'; } From d149150a452bc395ed3faa6b0cbdbcf58e97c08b Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Tue, 8 Oct 2024 20:38:08 +0200 Subject: [PATCH 054/117] Plane: Added parameter TKOFF_THR_IDLE --- ArduPlane/Parameters.cpp | 9 +++++++++ ArduPlane/Parameters.h | 1 + ArduPlane/servos.cpp | 5 +++++ 3 files changed, 15 insertions(+) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index d643d8b013754..b2db9099f8f51 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -158,6 +158,15 @@ const AP_Param::Info Plane::var_info[] = { // @User: Standard ASCALAR(takeoff_throttle_min, "TKOFF_THR_MIN", 0), + // @Param: TKOFF_THR_IDLE + // @DisplayName: Takeoff idle throttle + // @Description: The idle throttle to hold after arming and before a takeoff. Applicable in TAKEOFF and AUTO modes. + // @Units: % + // @Range: 0 100 + // @Increment: 1 + // @User: Standard + ASCALAR(takeoff_throttle_idle, "TKOFF_THR_IDLE", 0), + // @Param: TKOFF_OPTIONS // @DisplayName: Takeoff options // @Description: This selects the mode of the takeoff in AUTO and TAKEOFF flight modes. diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index d29243d1ef2bb..24645b3b88471 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -359,6 +359,7 @@ class Parameters { k_param_autotune_options, k_param_takeoff_throttle_min, k_param_takeoff_options, + k_param_takeoff_throttle_idle, k_param_pullup = 270, }; diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 49e3e07b460fc..672631e373e74 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -619,6 +619,11 @@ void Plane::set_throttle(void) // throttle is suppressed (above) to zero in final flare in auto mode, but we allow instead thr_min if user prefers, eg turbines: SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.throttle_min.get()); + } else if ((flight_stage == AP_FixedWing::FlightStage::TAKEOFF) + && (aparm.takeoff_throttle_idle.get() > 0) + ) { + // we want to spin at idle throttle before the takeoff conditions are met + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.takeoff_throttle_idle.get()); } else { // default SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); From db6a52581e39642f13433364263cf9f53e5e9b59 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Tue, 8 Oct 2024 20:38:28 +0200 Subject: [PATCH 055/117] AP_Vehicle: Added parameter TKOFF_THR_IDLE --- libraries/AP_Vehicle/AP_FixedWing.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index 722c53dcaf9af..8135a7d160352 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/libraries/AP_Vehicle/AP_FixedWing.h @@ -12,6 +12,7 @@ struct AP_FixedWing { AP_Int8 throttle_cruise; AP_Int8 takeoff_throttle_max; AP_Int8 takeoff_throttle_min; + AP_Int8 takeoff_throttle_idle; AP_Int32 takeoff_options; AP_Int16 airspeed_min; AP_Int16 airspeed_max; From 19bce3b1714d84388c3a3f13e79edb02a106a1e7 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Tue, 8 Oct 2024 21:10:02 +0200 Subject: [PATCH 056/117] autotest: added test for TKOFF_THR_IDLE --- Tools/autotest/arduplane.py | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index bc0d157258f51..48334ab8d7c2e 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4831,6 +4831,34 @@ def TakeoffGround(self): self.fly_home_land_and_disarm() + def TakeoffIdleThrottle(self): + '''Apply idle throttle before takeoff.''' + self.customise_SITL_commandline( + [], + model='plane-catapult', + defaults_filepath=self.model_defaults_filepath("plane") + ) + self.set_parameters({ + "TKOFF_THR_IDLE": 20.0, + "TKOFF_THR_MINSPD": 3.0, + }) + self.change_mode("TAKEOFF") + + self.wait_ready_to_arm() + self.arm_vehicle() + + # Ensure that the throttle rises to idle throttle. + expected_idle_throttle = 1000+10*self.get_parameter("TKOFF_THR_IDLE") + self.assert_servo_channel_range(3, expected_idle_throttle-10, expected_idle_throttle+10) + + # Launch the catapult + self.set_servo(6, 1000) + + self.delay_sim_time(5) + self.change_mode('RTL') + + self.fly_home_land_and_disarm() + def DCMFallback(self): '''Really annoy the EKF and force fallback''' self.reboot_sitl() @@ -6389,6 +6417,7 @@ def tests1b(self): self.TakeoffTakeoff3, self.TakeoffTakeoff4, self.TakeoffGround, + self.TakeoffIdleThrottle, self.ForcedDCM, self.DCMFallback, self.MAVFTP, From 199074ce34b383d9d244de17e61fff718045455e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 18:15:08 +1100 Subject: [PATCH 057/117] AP_GPS: add specific defines for sending of GPS mavlink messages --- libraries/AP_GPS/AP_GPS.cpp | 8 +++++--- libraries/AP_GPS/AP_GPS_config.h | 17 +++++++++++++++++ libraries/AP_GPS/GPS_Backend.cpp | 5 +++-- libraries/AP_GPS/GPS_Backend.h | 2 ++ 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 070276dd45c9f..79a69b88b2962 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1367,6 +1367,7 @@ uint16_t AP_GPS::gps_yaw_cdeg(uint8_t instance) const return yaw_cd; } +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) { const Location &loc = location(0); @@ -1400,8 +1401,9 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) 0, // TODO one-sigma heading accuracy standard deviation gps_yaw_cdeg(0)); } +#endif // AP_GPS_GPS_RAW_INT_SENDING_ENABLED -#if GPS_MAX_RECEIVERS > 1 +#if AP_GPS_GPS2_RAW_SENDING_ENABLED void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) { // always send the message if 2nd GPS is configured @@ -1442,9 +1444,9 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) sacc * 1000, // one-sigma standard deviation in mm/s 0); // TODO one-sigma heading accuracy standard deviation } -#endif // GPS_MAX_RECEIVERS +#endif // AP_GPS_GPS2_RAW_SENDING_ENABLED -#if HAL_GCS_ENABLED +#if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst) { if (inst >= GPS_MAX_RECEIVERS) { diff --git a/libraries/AP_GPS/AP_GPS_config.h b/libraries/AP_GPS/AP_GPS_config.h index 08ebdac605938..467c056ab4fcc 100644 --- a/libraries/AP_GPS/AP_GPS_config.h +++ b/libraries/AP_GPS/AP_GPS_config.h @@ -104,3 +104,20 @@ #ifndef HAL_GPS_COM_PORT_DEFAULT #define HAL_GPS_COM_PORT_DEFAULT 1 #endif + + +#ifndef AP_GPS_GPS_RAW_INT_SENDING_ENABLED +#define AP_GPS_GPS_RAW_INT_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED +#endif + +#ifndef AP_GPS_GPS2_RAW_SENDING_ENABLED +#define AP_GPS_GPS2_RAW_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && GPS_MAX_RECEIVERS > 1 +#endif + +#ifndef AP_GPS_GPS_RTK_SENDING_ENABLED +#define AP_GPS_GPS_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED +#endif + +#ifndef AP_GPS_GPS2_RTK_SENDING_ENABLED +#define AP_GPS_GPS2_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && GPS_MAX_RECEIVERS > 1 +#endif diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index eb55bd76e433d..380730a9af679 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -172,7 +172,7 @@ bool AP_GPS_Backend::should_log() const #endif -#if HAL_GCS_ENABLED +#if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan) { const uint8_t instance = state.instance; @@ -212,7 +212,8 @@ void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan) break; } } -#endif +#endif // AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED + /* diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 70f4cb36fc215..2a70a1c57153e 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -71,7 +71,9 @@ class AP_GPS_Backend #if HAL_GCS_ENABLED //MAVLink methods virtual bool supports_mavlink_gps_rtk_message() const { return false; } +#if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED virtual void send_mavlink_gps_rtk(mavlink_channel_t chan); +#endif virtual void handle_msg(const mavlink_message_t &msg) { return ; } #endif From d4e15b1ae7cad783ad63973f8cd31d550fed776b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 18:24:49 +1100 Subject: [PATCH 058/117] GCS_MAVLink: add specific defines for sending of GPS mavlink messages --- libraries/GCS_MAVLink/GCS_Common.cpp | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 9a1528b9dca4e..69598bf6959cc 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1030,13 +1030,17 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_SCALED_PRESSURE, MSG_SCALED_PRESSURE}, { MAVLINK_MSG_ID_SCALED_PRESSURE2, MSG_SCALED_PRESSURE2}, { MAVLINK_MSG_ID_SCALED_PRESSURE3, MSG_SCALED_PRESSURE3}, -#if AP_GPS_ENABLED +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED { MAVLINK_MSG_ID_GPS_RAW_INT, MSG_GPS_RAW}, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED { MAVLINK_MSG_ID_GPS_RTK, MSG_GPS_RTK}, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED { MAVLINK_MSG_ID_GPS2_RAW, MSG_GPS2_RAW}, - { MAVLINK_MSG_ID_GPS2_RTK, MSG_GPS2_RTK}, #endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED + { MAVLINK_MSG_ID_GPS2_RTK, MSG_GPS2_RTK}, #endif { MAVLINK_MSG_ID_SYSTEM_TIME, MSG_SYSTEM_TIME}, { MAVLINK_MSG_ID_RC_CHANNELS_SCALED, MSG_SERVO_OUT}, @@ -6237,28 +6241,32 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) CHECK_PAYLOAD_SIZE(SYSTEM_TIME); send_system_time(); break; -#if AP_GPS_ENABLED + +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED case MSG_GPS_RAW: CHECK_PAYLOAD_SIZE(GPS_RAW_INT); AP::gps().send_mavlink_gps_raw(chan); break; +#endif // AP_GPS_GPS_RAW_INT_SENDING_ENABLED +#if AP_GPS_GPS_RTK_SENDING_ENABLED case MSG_GPS_RTK: CHECK_PAYLOAD_SIZE(GPS_RTK); AP::gps().send_mavlink_gps_rtk(chan, 0); break; -#if GPS_MAX_RECEIVERS > 1 +#endif // AP_GPS_GPS_RTK_SENDING_ENABLED +#if AP_GPS_GPS2_RAW_SENDING_ENABLED case MSG_GPS2_RAW: CHECK_PAYLOAD_SIZE(GPS2_RAW); AP::gps().send_mavlink_gps2_raw(chan); break; -#endif -#if GPS_MAX_RECEIVERS > 1 +#endif // AP_GPS_GPS2_RAW_SENDING_ENABLED +#if AP_GPS_GPS2_RTK_SENDING_ENABLED case MSG_GPS2_RTK: CHECK_PAYLOAD_SIZE(GPS2_RTK); AP::gps().send_mavlink_gps_rtk(chan, 1); break; -#endif -#endif // AP_GPS_ENABLED +#endif // AP_GPS_GPS2_RTK_SENDING_ENABLED + #if AP_AHRS_ENABLED case MSG_LOCAL_POSITION: CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED); From 36722f6d3a300c5b399f7009fcebdc9140b9a82c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 18:24:49 +1100 Subject: [PATCH 059/117] AntennaTracker: add specific defines for sending of GPS mavlink messages --- AntennaTracker/GCS_Mavlink.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 31c1148db9ea8..93e6638304440 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -276,10 +276,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_NAV_CONTROLLER_OUTPUT, +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif }; From 8df33ce22985e6b12a06790910a026338724c933 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 18:24:50 +1100 Subject: [PATCH 060/117] ArduCopter: add specific defines for sending of GPS mavlink messages --- ArduCopter/GCS_Mavlink.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 6c2d169db8af6..c07b4807a2e09 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -505,10 +505,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, // MISSION_CURRENT +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, From 4d8a0fac33cc30f01840266d5bbdd1214c455572 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 18:24:50 +1100 Subject: [PATCH 061/117] ArduPlane: add specific defines for sending of GPS mavlink messages --- ArduPlane/GCS_Mavlink.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 9256fdfe01cb3..b733f7b7bff13 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -630,10 +630,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, From 187953b297b17bd4fc3e286d68957d74aaba1504 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 18:24:50 +1100 Subject: [PATCH 062/117] ArduSub: add specific defines for sending of GPS mavlink messages --- ArduSub/GCS_Mavlink.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 0d0bad47ebd89..c2bbf8b5c42bb 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -369,10 +369,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, From b26e37e54c1ff3b17fd585902c6d3148c6addb97 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 18:24:50 +1100 Subject: [PATCH 063/117] Blimp: add specific defines for sending of GPS mavlink messages --- Blimp/GCS_Mavlink.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index b132d80160a5c..4d91191a02863 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -327,10 +327,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, // MISSION_CURRENT +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, From 0ce765aac10096dfca1fd27257d2d70768868fbe Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 18:24:50 +1100 Subject: [PATCH 064/117] Rover: add specific defines for sending of GPS mavlink messages --- Rover/GCS_Mavlink.cpp | 8 +++++++- Tools/AP_Periph/GCS_MAVLink.cpp | 4 +++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 6a39311eade23..3704110ec9e2f 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -541,10 +541,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, diff --git a/Tools/AP_Periph/GCS_MAVLink.cpp b/Tools/AP_Periph/GCS_MAVLink.cpp index a3df4631fef87..f35ee3a580c8c 100644 --- a/Tools/AP_Periph/GCS_MAVLink.cpp +++ b/Tools/AP_Periph/GCS_MAVLink.cpp @@ -32,8 +32,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_MCU_STATUS, #endif MSG_MEMINFO, -#if AP_GPS_ENABLED +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, #endif }; From 20fa2b0741982c77750e817114ffa01a32d9a986 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Nov 2024 15:14:16 +1100 Subject: [PATCH 065/117] AP_GPS: don't compile support for sending rtk messages if no backend supports it --- libraries/AP_GPS/AP_GPS_config.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_config.h b/libraries/AP_GPS/AP_GPS_config.h index 467c056ab4fcc..550add0e1fa62 100644 --- a/libraries/AP_GPS/AP_GPS_config.h +++ b/libraries/AP_GPS/AP_GPS_config.h @@ -115,9 +115,9 @@ #endif #ifndef AP_GPS_GPS_RTK_SENDING_ENABLED -#define AP_GPS_GPS_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED +#define AP_GPS_GPS_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && (AP_GPS_SBF_ENABLED || AP_GPS_ERB_ENABLED) #endif #ifndef AP_GPS_GPS2_RTK_SENDING_ENABLED -#define AP_GPS_GPS2_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && GPS_MAX_RECEIVERS > 1 +#define AP_GPS_GPS2_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && GPS_MAX_RECEIVERS > 1 && (AP_GPS_SBF_ENABLED || AP_GPS_ERB_ENABLED) #endif From f38668bd6a2b457babb3bf9fe79fb4bfda307cf4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Nov 2024 14:56:10 +1100 Subject: [PATCH 066/117] AP_GPS: remove handling of HIL_GPS ... per deprecation/removal schedule --- libraries/AP_GPS/AP_GPS_MAV.cpp | 45 +-------------------------------- 1 file changed, 1 insertion(+), 44 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp index b9100f5f17c46..2a059af35b018 100644 --- a/libraries/AP_GPS/AP_GPS_MAV.cpp +++ b/libraries/AP_GPS/AP_GPS_MAV.cpp @@ -143,51 +143,8 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg) state.last_gps_time_ms = now_ms; _new_data = true; break; - } - -#if AP_MAVLINK_MSG_HIL_GPS_ENABLED - case MAVLINK_MSG_ID_HIL_GPS: { - mavlink_hil_gps_t packet; - mavlink_msg_hil_gps_decode(&msg, &packet); - - // check if target instance belongs to incoming gps data. - if (state.instance != packet.id) { - return; - } - - state.time_week = 0; - state.time_week_ms = packet.time_usec/1000; - state.status = (AP_GPS::GPS_Status)packet.fix_type; + } - Location loc = {}; - loc.lat = packet.lat; - loc.lng = packet.lon; - loc.alt = packet.alt * 0.1f; - state.location = loc; - state.hdop = MIN(packet.eph, GPS_UNKNOWN_DOP); - state.vdop = MIN(packet.epv, GPS_UNKNOWN_DOP); - if (packet.vel < 65535) { - state.ground_speed = packet.vel * 0.01f; - } - Vector3f vel(packet.vn*0.01f, packet.ve*0.01f, packet.vd*0.01f); - state.velocity = vel; - if (packet.vd != 0) { - state.have_vertical_velocity = true; - } - if (packet.cog < 36000) { - state.ground_course = packet.cog * 0.01f; - } - state.have_speed_accuracy = false; - state.have_horizontal_accuracy = false; - state.have_vertical_accuracy = false; - if (packet.satellites_visible < 255) { - state.num_sats = packet.satellites_visible; - } - state.last_gps_time_ms = AP_HAL::millis(); - _new_data = true; - break; - } -#endif // AP_MAVLINK_MSG_HIL_GPS_ENABLED default: // ignore all other messages break; From 866e00f143721fd98b58e62739bce993edcb16b0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Nov 2024 14:56:10 +1100 Subject: [PATCH 067/117] GCS_MAVLink: remove handling of HIL_GPS ... per deprecation/removal schedule --- libraries/GCS_MAVLink/GCS_Common.cpp | 5 ----- libraries/GCS_MAVLink/GCS_config.h | 9 --------- 2 files changed, 14 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 69598bf6959cc..806831a345f4f 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4303,11 +4303,6 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) #endif #if AP_GPS_ENABLED -#if AP_MAVLINK_MSG_HIL_GPS_ENABLED - case MAVLINK_MSG_ID_HIL_GPS: - send_received_message_deprecation_warning("HIL_GPS"); - FALLTHROUGH; -#endif case MAVLINK_MSG_ID_GPS_RTCM_DATA: case MAVLINK_MSG_ID_GPS_INPUT: case MAVLINK_MSG_ID_GPS_INJECT_DATA: diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index 5f059cb9a1042..2c8800a959c25 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -71,15 +71,6 @@ #define AP_MAVLINK_RALLY_POINT_PROTOCOL_ENABLED HAL_GCS_ENABLED && HAL_RALLY_ENABLED #endif -// CODE_REMOVAL -// handling of HIL_GPS is slated to be removed in 4.7; GPS_INPUT can be used -// in its place -// ArduPilot 4.6 stops compiling support in -// ArduPilot 4.7 removes the code entirely -#ifndef AP_MAVLINK_MSG_HIL_GPS_ENABLED -#define AP_MAVLINK_MSG_HIL_GPS_ENABLED 0 -#endif - // CODE_REMOVAL // ArduPilot 4.5 sends deprecation warnings for MOUNT_CONTROL/MOUNT_CONFIGURE // ArduPilot 4.6 stops compiling them in From fb4b52fae3e8f43d8913b3d8f6a7004bb044a06c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Nov 2024 14:56:10 +1100 Subject: [PATCH 068/117] Tools: remove handling of HIL_GPS ... per deprecation/removal schedule --- Tools/autotest/test_build_options.py | 1 - Tools/scripts/build_options.py | 1 - Tools/scripts/extract_features.py | 1 - 3 files changed, 3 deletions(-) diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index a11b8c63b169a..7cbc005c310aa 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -258,7 +258,6 @@ def define_is_whitelisted_for_feature_in_code(self, target, define): 'AP_COMPASS_MAG3110_ENABLED', # must be in hwdef, not probed 'AP_COMPASS_MMC5XX3_ENABLED', # must be in hwdef, not probed 'AP_MAVLINK_AUTOPILOT_VERSION_REQUEST_ENABLED', # completely elided - 'AP_MAVLINK_MSG_HIL_GPS_ENABLED', # no symbol available 'AP_MAVLINK_MSG_RELAY_STATUS_ENABLED', # no symbol available 'AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED', # no symbol available 'HAL_MSP_SENSORS_ENABLED', # no symbol available diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 54be58a3bd074..f31edfaf87ff3 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -364,7 +364,6 @@ def config_option(self): Feature('MAVLink', 'MAVLINK_VERSION_REQUEST', 'AP_MAVLINK_AUTOPILOT_VERSION_REQUEST_ENABLED', 'Enable Old AUTOPILOT_VERSION_REQUEST mesage', 0, None), # noqa Feature('MAVLink', 'REQUEST_AUTOPILOT_CAPA', 'AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED', 'Enable Old REQUEST_AUTOPILOT_CAPABILITIES command', 0, None), # noqa Feature('MAVLink', 'MAV_MSG_RELAY_STATUS', 'AP_MAVLINK_MSG_RELAY_STATUS_ENABLED', 'Enable Send RELAY_STATUS message', 0, 'RELAY'), # noqa - Feature('MAVLink', 'MAV_MSG_HIL_GPS', 'AP_MAVLINK_MSG_HIL_GPS_ENABLED', 'Enable Receive HIL_GPS messages', 0, 'MAV'), # noqa Feature('MAVLink', 'MAV_MSG_MOUNT_CONTROL', 'AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED', 'Enable Deprecated MOUNT_CONTROL message', 0, "MOUNT"), # noqa Feature('MAVLink', 'MAV_MSG_MOUNT_CONFIGURE', 'AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED', 'Enable Deprecated MOUNT_CONFIGURE message', 0, "MOUNT"), # noqa Feature('MAVLink', 'AP_MAVLINK_BATTERY2_ENABLED', 'AP_MAVLINK_BATTERY2_ENABLED', 'Enable Send old BATTERY2 message', 0, None), # noqa diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 36a375522aa05..de6eec6589523 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -273,7 +273,6 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_CUSTOMROTATIONS_ENABLED', 'AP_CustomRotations::init'), ('AP_OSD_LINK_STATS_EXTENSIONS_ENABLED', r'AP_OSD_Screen::draw_rc_tx_power'), ('HAL_ENABLE_DRONECAN_DRIVERS', r'AP_DroneCAN::init'), - ('AP_MAVLINK_MSG_HIL_GPS_ENABLED', r'mavlink_msg_hil_gps_decode'), ('AP_BARO_PROBE_EXTERNAL_I2C_BUSES', r'AP_Baro::_probe_i2c_barometers'), ('AP_RSSI_ENABLED', r'AP_RSSI::init'), From 02f25e81f8c391f0d74dda4ee2078fbd77185ba9 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 25 May 2024 09:31:12 +0100 Subject: [PATCH 069/117] AP_AHRS: correct get_accel() to use primary accel rather than first usable for scripting --- libraries/AP_AHRS/AP_AHRS.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 00988f1a0a6b9..10f9deed01df9 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -618,9 +618,9 @@ class AP_AHRS { // return current vibration vector for primary IMU Vector3f get_vibration(void) const; - // return primary accels, for lua + // return primary accels const Vector3f &get_accel(void) const { - return AP::ins().get_accel(); + return AP::ins().get_accel(_get_primary_accel_index()); } // return primary accel bias. This should be subtracted from From f60d0596188a8a89eb93a1e6fdb2aed3775811a2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 18:17:27 +1100 Subject: [PATCH 070/117] AP_Vehicle: create and use a singleton for SRV_Channels avoid creation of static pointers to objects held within SRV_Channels --- libraries/AP_Vehicle/AP_Vehicle.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 562ac606075a4..3005e40d3adcb 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -438,7 +438,7 @@ void AP_Vehicle::setup() #if AP_SRV_CHANNELS_ENABLED - SRV_Channels::init(); + AP::srv().init(); #endif // gyro FFT needs to be initialized really late From c1f04b507eb7b5b0676759122df107f174065985 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 18:17:28 +1100 Subject: [PATCH 071/117] AR_Motors: create and use a singleton for SRV_Channels avoid creation of static pointers to objects held within SRV_Channels --- libraries/AR_Motors/AP_MotorsUGV.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AR_Motors/AP_MotorsUGV.cpp b/libraries/AR_Motors/AP_MotorsUGV.cpp index e27ef29cdc11a..70f9754d625fa 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.cpp +++ b/libraries/AR_Motors/AP_MotorsUGV.cpp @@ -343,7 +343,7 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt) SRV_Channels::calc_pwm(); SRV_Channels::cork(); SRV_Channels::output_ch_all(); - SRV_Channels::push(); + AP::srv().push(); } // test steering or throttle output as a percentage of the total (range -100 to +100) @@ -411,7 +411,7 @@ bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct) SRV_Channels::calc_pwm(); SRV_Channels::cork(); SRV_Channels::output_ch_all(); - SRV_Channels::push(); + AP::srv().push(); return true; } @@ -477,7 +477,7 @@ bool AP_MotorsUGV::output_test_pwm(motor_test_order motor_seq, float pwm) SRV_Channels::calc_pwm(); SRV_Channels::cork(); SRV_Channels::output_ch_all(); - SRV_Channels::push(); + AP::srv().push(); return true; } From a0eef1039c49e59c73330bee14da512948e5805c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 18:17:28 +1100 Subject: [PATCH 072/117] SRV_Channel: create and use a singleton for SRV_Channels avoid creation of static pointers to objects held within SRV_Channels --- libraries/SRV_Channel/SRV_Channel.h | 15 +++--- libraries/SRV_Channel/SRV_Channel_aux.cpp | 2 +- libraries/SRV_Channel/SRV_Channels.cpp | 63 ++++++----------------- 3 files changed, 24 insertions(+), 56 deletions(-) diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 24631a448d2c4..e19c52a4bf574 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -473,7 +473,7 @@ class SRV_Channels { int16_t value, int16_t angle_min, int16_t angle_max); // assign and enable auxiliary channels - static void enable_aux_servos(void); + void enable_aux_servos(void); // enable channels by mask static void enable_by_mask(uint32_t mask); @@ -541,7 +541,7 @@ class SRV_Channels { static void cork(); - static void push(); + void push(); // disable PWM output to a set of channels given by a mask. This is used by the AP_BLHeli code static void set_disabled_channel_mask(uint32_t mask) { disabled_mask = mask; } @@ -570,7 +570,7 @@ class SRV_Channels { static void zero_rc_outputs(); // initialize before any call to push - static void init(uint32_t motor_mask = 0, AP_HAL::RCOutput::output_mode mode = AP_HAL::RCOutput::MODE_PWM_NONE); + void init(uint32_t motor_mask = 0, AP_HAL::RCOutput::output_mode mode = AP_HAL::RCOutput::MODE_PWM_NONE); // return true if a channel is set to type GPIO static bool is_GPIO(uint8_t channel); @@ -610,30 +610,25 @@ class SRV_Channels { #if AP_VOLZ_ENABLED // support for Volz protocol AP_Volz_Protocol volz; - static AP_Volz_Protocol *volz_ptr; #endif #if AP_SBUSOUTPUT_ENABLED // support for SBUS protocol AP_SBusOut sbus; - static AP_SBusOut *sbus_ptr; #endif #if AP_ROBOTISSERVO_ENABLED // support for Robotis servo protocol AP_RobotisServo robotis; - static AP_RobotisServo *robotis_ptr; #endif #if HAL_SUPPORT_RCOUT_SERIAL // support for BLHeli protocol AP_BLHeli blheli; - static AP_BLHeli *blheli_ptr; #endif #if AP_FETTEC_ONEWIRE_ENABLED AP_FETtecOneWire fetteconwire; - static AP_FETtecOneWire *fetteconwire_ptr; #endif // AP_FETTEC_ONEWIRE_ENABLED // mask of disabled channels @@ -692,3 +687,7 @@ class SRV_Channels { // semaphore for multi-thread use of override_counter array HAL_Semaphore override_counter_sem; }; + +namespace AP { + SRV_Channels &srv(); +}; diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index c117915e8a474..3fb4e922744d7 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -270,7 +270,7 @@ void SRV_Channels::enable_aux_servos() hal.rcout->update_channel_masks(); #if HAL_SUPPORT_RCOUT_SERIAL - blheli_ptr->update(); + blheli.update(); #endif } diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 67a9af5b00ad6..3ef0c7e34318f 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -41,28 +41,8 @@ extern const AP_HAL::HAL& hal; SRV_Channel *SRV_Channels::channels; SRV_Channels *SRV_Channels::_singleton; -#if AP_VOLZ_ENABLED -AP_Volz_Protocol *SRV_Channels::volz_ptr; -#endif - -#if AP_SBUSOUTPUT_ENABLED -AP_SBusOut *SRV_Channels::sbus_ptr; -#endif - -#if AP_ROBOTISSERVO_ENABLED -AP_RobotisServo *SRV_Channels::robotis_ptr; -#endif - -#if AP_FETTEC_ONEWIRE_ENABLED -AP_FETtecOneWire *SRV_Channels::fetteconwire_ptr; -#endif - uint16_t SRV_Channels::override_counter[NUM_SERVO_CHANNELS]; -#if HAL_SUPPORT_RCOUT_SERIAL -AP_BLHeli *SRV_Channels::blheli_ptr; -#endif - uint32_t SRV_Channels::disabled_mask; uint32_t SRV_Channels::digital_mask; uint32_t SRV_Channels::reversible_mask; @@ -379,26 +359,6 @@ SRV_Channels::SRV_Channels(void) } #endif } - -#if AP_FETTEC_ONEWIRE_ENABLED - fetteconwire_ptr = &fetteconwire; -#endif - -#if AP_VOLZ_ENABLED - volz_ptr = &volz; -#endif - -#if AP_SBUSOUTPUT_ENABLED - sbus_ptr = &sbus; -#endif - -#if AP_ROBOTISSERVO_ENABLED - robotis_ptr = &robotis; -#endif // AP_ROBOTISSERVO_ENABLED - -#if HAL_SUPPORT_RCOUT_SERIAL - blheli_ptr = &blheli; -#endif } // SRV_Channels initialization @@ -406,7 +366,7 @@ void SRV_Channels::init(uint32_t motor_mask, AP_HAL::RCOutput::output_mode mode) { // initialize BLHeli late so that all of the masks it might setup don't get trodden on by motor initialization #if HAL_SUPPORT_RCOUT_SERIAL - blheli_ptr->init(motor_mask, mode); + blheli.init(motor_mask, mode); #endif #ifndef HAL_BUILD_AP_PERIPH hal.rcout->set_dshot_rate(_singleton->dshot_rate, AP::scheduler().get_loop_rate_hz()); @@ -519,26 +479,26 @@ void SRV_Channels::push() #if AP_VOLZ_ENABLED // give volz library a chance to update - volz_ptr->update(); + volz.update(); #endif #if AP_SBUSOUTPUT_ENABLED // give sbus library a chance to update - sbus_ptr->update(); + sbus.update(); #endif #if AP_ROBOTISSERVO_ENABLED // give robotis library a chance to update - robotis_ptr->update(); + robotis.update(); #endif #if HAL_SUPPORT_RCOUT_SERIAL // give blheli telemetry a chance to update - blheli_ptr->update_telemetry(); + blheli.update_telemetry(); #endif #if AP_FETTEC_ONEWIRE_ENABLED - fetteconwire_ptr->update(); + fetteconwire.update(); #endif #if AP_KDECAN_ENABLED @@ -589,7 +549,7 @@ void SRV_Channels::zero_rc_outputs() for (uint8_t i=0; iwrite(i, 0); } - push(); + AP::srv().push(); } /* @@ -619,3 +579,12 @@ void SRV_Channels::set_emergency_stop(bool state) { #endif emergency_stop = state; } + +namespace AP { + +SRV_Channels &srv() +{ + return *SRV_Channels::get_singleton(); +} + +}; From fbb28a0c3ac36f269a25dbcd243427140627fdd0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 18:17:28 +1100 Subject: [PATCH 073/117] AntennaTracker: create and use a singleton for SRV_Channels avoid creation of static pointers to objects held within SRV_Channels --- AntennaTracker/Tracker.cpp | 2 +- AntennaTracker/servos.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/AntennaTracker/Tracker.cpp b/AntennaTracker/Tracker.cpp index 435134d6189e1..a35a6f3251ad5 100644 --- a/AntennaTracker/Tracker.cpp +++ b/AntennaTracker/Tracker.cpp @@ -82,7 +82,7 @@ void Tracker::one_second_loop() mavlink_system.sysid = g.sysid_this_mav; // update assigned functions and enable auxiliary servos - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); // updated armed/disarmed status LEDs update_armed_disarmed(); diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp index 23e1e0cab5bfd..5ca8bc0b54656 100644 --- a/AntennaTracker/servos.cpp +++ b/AntennaTracker/servos.cpp @@ -8,7 +8,7 @@ void Tracker::init_servos() { // update assigned functions and enable auxiliary servos - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); SRV_Channels::set_default_function(CH_YAW, SRV_Channel::k_tracker_yaw); SRV_Channels::set_default_function(CH_PITCH, SRV_Channel::k_tracker_pitch); From e29b6c30368bdd17b901347d4dc18a26bac27b25 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 18:17:28 +1100 Subject: [PATCH 074/117] ArduCopter: create and use a singleton for SRV_Channels avoid creation of static pointers to objects held within SRV_Channels --- ArduCopter/Copter.cpp | 2 +- ArduCopter/compassmot.cpp | 2 +- ArduCopter/esc_calibration.cpp | 8 ++++---- ArduCopter/motors.cpp | 2 +- ArduCopter/radio.cpp | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 417468160cdfc..84e31f3ebb326 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -727,7 +727,7 @@ void Copter::one_hz_loop() } // update assigned functions and enable auxiliary servos - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); #if HAL_LOGGING_ENABLED // log terrain data diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index 703f5cf722a2e..c0d26041c0901 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -148,7 +148,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) // pass through throttle to motors SRV_Channels::cork(); motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f); - SRV_Channels::push(); + AP::srv().push(); // read some compass values compass.read(); diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index b4c5c11906399..722127898e225 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -97,7 +97,7 @@ void Copter::esc_calibration_passthrough() // pass through to motors SRV_Channels::cork(); motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f); - SRV_Channels::push(); + AP::srv().push(); } #endif // FRAME_CONFIG != HELI_FRAME } @@ -114,14 +114,14 @@ void Copter::esc_calibration_auto() // raise throttle to maximum SRV_Channels::cork(); motors->set_throttle_passthrough_for_esc_calibration(1.0f); - SRV_Channels::push(); + AP::srv().push(); // delay for 5 seconds while outputting pulses uint32_t tstart = millis(); while (millis() - tstart < 5000) { SRV_Channels::cork(); motors->set_throttle_passthrough_for_esc_calibration(1.0f); - SRV_Channels::push(); + AP::srv().push(); esc_calibration_notify(); hal.scheduler->delay(3); } @@ -130,7 +130,7 @@ void Copter::esc_calibration_auto() while(1) { SRV_Channels::cork(); motors->set_throttle_passthrough_for_esc_calibration(0.0f); - SRV_Channels::push(); + AP::srv().push(); esc_calibration_notify(); hal.scheduler->delay(3); } diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index e382cb5941f65..328eb831500d5 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -181,7 +181,7 @@ void Copter::motors_output() } // push all channels - SRV_Channels::push(); + AP::srv().push(); } // check for pilot stick input to trigger lost vehicle alarm diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index b92178d0981ea..9873d1a2846f8 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -45,7 +45,7 @@ void Copter::init_rc_out() motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get()); // enable aux servos to cope with multiple output channels per motor - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); // update rate must be set after motors->init() to allow for motor mapping motors->set_update_rate(g.rc_speed); From c23f777ae654832f7714be3a951f4dafb9c6680e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 18:17:28 +1100 Subject: [PATCH 075/117] ArduPlane: create and use a singleton for SRV_Channels avoid creation of static pointers to objects held within SRV_Channels --- ArduPlane/ArduPlane.cpp | 2 +- ArduPlane/radio.cpp | 2 +- ArduPlane/servos.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index d69e3a3842e4b..1e10df8c42784 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -346,7 +346,7 @@ void Plane::one_second_loop() // sync MAVLink system ID mavlink_system.sysid = g.sysid_this_mav; - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); // update notify flags AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false); diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index cb2048be6d42f..49835adcfdf66 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -107,7 +107,7 @@ void Plane::init_rc_out_main() */ void Plane::init_rc_out_aux() { - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); servos_output(); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 672631e373e74..b8db6e14b8648 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -1055,7 +1055,7 @@ void Plane::servos_output(void) SRV_Channels::output_ch_all(); - SRV_Channels::push(); + AP::srv().push(); if (g2.servo_channels.auto_trim_enabled()) { servos_auto_trim(); From a8b07a854f87eee3c2c5fc2fae19090f0b4981dc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 18:17:28 +1100 Subject: [PATCH 076/117] ArduSub: create and use a singleton for SRV_Channels avoid creation of static pointers to objects held within SRV_Channels --- ArduSub/ArduSub.cpp | 2 +- ArduSub/motors.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index df4729ba01692..002fbfbaad591 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -290,7 +290,7 @@ void Sub::one_hz_loop() } // update assigned functions and enable auxiliary servos - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); #if HAL_LOGGING_ENABLED // log terrain data diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 5d45a2c00fbe2..7bfe51c24904c 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -22,7 +22,7 @@ void Sub::motors_output() SRV_Channels::calc_pwm(); SRV_Channels::output_ch_all(); motors.output(); - SRV_Channels::push(); + AP::srv().push(); } } From 52e2a162c969807d2cd48f612106146832a42a24 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 18:24:59 +1100 Subject: [PATCH 077/117] Blimp: create and use a singleton for SRV_Channels avoid creation of static pointers to objects held within SRV_Channels --- Blimp/Blimp.cpp | 2 +- Blimp/motors.cpp | 4 ++-- Blimp/radio.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 83f8e6207394b..8ae8c0b704aa3 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -210,7 +210,7 @@ void Blimp::one_hz_loop() #endif // update assigned functions and enable auxiliary servos - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); AP_Notify::flags.flying = !ap.land_complete; diff --git a/Blimp/motors.cpp b/Blimp/motors.cpp index 35ba08b90cce9..c2993c99bd48f 100644 --- a/Blimp/motors.cpp +++ b/Blimp/motors.cpp @@ -88,5 +88,5 @@ void Blimp::motors_output() motors->output(); // push all channels - SRV_Channels::push(); -} \ No newline at end of file + AP::srv().push(); +} diff --git a/Blimp/radio.cpp b/Blimp/radio.cpp index 4b0c5a9a709e2..47212c501272c 100644 --- a/Blimp/radio.cpp +++ b/Blimp/radio.cpp @@ -38,7 +38,7 @@ void Blimp::init_rc_in() void Blimp::init_rc_out() { // enable aux servos to cope with multiple output channels per motor - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); // refresh auxiliary channel to function map SRV_Channels::update_aux_servo_function(); From ccd12e3e1239e6dcd476ee8468cd44848714783f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 18:24:59 +1100 Subject: [PATCH 078/117] Rover: create and use a singleton for SRV_Channels avoid creation of static pointers to objects held within SRV_Channels --- Rover/Rover.cpp | 2 +- Rover/system.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index a8fc94074d2fa..7460a561e056b 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -435,7 +435,7 @@ void Rover::one_second_loop(void) set_control_channels(); // cope with changes to aux functions - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); // update notify flags AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false); diff --git a/Rover/system.cpp b/Rover/system.cpp index 6911b78b74701..06405d5e31d4f 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -70,7 +70,7 @@ void Rover::init_ardupilot() init_rc_in(); // sets up rc channels deadzone g2.motors.init(get_frame_type()); // init motors including setting servo out channels ranges - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); // init wheel encoders g2.wheel_encoder.init(); From 573b02fc2365ccaeba20d83fbcd9e741f0142ed4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 9 Nov 2024 18:40:03 +1100 Subject: [PATCH 079/117] AP_Periph: create and use a singleton for SRV_Channels --- Tools/AP_Periph/rc_out.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index 2dbfbe07f5149..b4d9d410a5307 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/Tools/AP_Periph/rc_out.cpp @@ -71,7 +71,7 @@ void AP_Periph_FW::rcout_init() hal.rcout->set_freq(esc_mask, g.esc_rate.get()); // setup ESCs with the desired PWM type, allowing for DShot - SRV_Channels::init(esc_mask, (AP_HAL::RCOutput::output_mode)g.esc_pwm_type.get()); + AP::srv().init(esc_mask, (AP_HAL::RCOutput::output_mode)g.esc_pwm_type.get()); // run DShot at 1kHz hal.rcout->set_dshot_rate(SRV_Channels::get_dshot_rate(), 400); @@ -83,7 +83,7 @@ void AP_Periph_FW::rcout_init() void AP_Periph_FW::rcout_init_1Hz() { // this runs at 1Hz to allow for run-time param changes - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); for (uint8_t i=0; i= esc_telem_update_period_ms) { last_esc_telem_update_ms = now_ms; From d204f22fe0b767a4bbe9ed3bddaf6eacb5d1dd54 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 10 Nov 2024 09:50:00 +1100 Subject: [PATCH 080/117] AP_Motors: create and use a singleton for SRV_Channels --- libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index f28db5c9243fb..336eea61e3d80 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -518,7 +518,7 @@ void stability_test() // arm motors motors->armed(true); motors->set_interlock(true); - SRV_Channels::enable_aux_servos(); + AP::srv().enable_aux_servos(); hal.console->printf("Roll,Pitch,Yaw,Thr,"); for (uint8_t i=0; i Date: Wed, 13 Nov 2024 08:09:59 +1100 Subject: [PATCH 081/117] AR_Motors: make SRV_Channels::cork non-static for symmetry with the push function --- libraries/AR_Motors/AP_MotorsUGV.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/libraries/AR_Motors/AP_MotorsUGV.cpp b/libraries/AR_Motors/AP_MotorsUGV.cpp index 70f9754d625fa..4b63101d5f5bb 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.cpp +++ b/libraries/AR_Motors/AP_MotorsUGV.cpp @@ -340,10 +340,11 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt) output_sail(); // send values to the PWM timers for output + auto &srv = AP::srv(); SRV_Channels::calc_pwm(); - SRV_Channels::cork(); + srv.cork(); SRV_Channels::output_ch_all(); - AP::srv().push(); + srv.push(); } // test steering or throttle output as a percentage of the total (range -100 to +100) @@ -408,10 +409,11 @@ bool AP_MotorsUGV::output_test_pct(motor_test_order motor_seq, float pct) case MOTOR_TEST_LAST: return false; } + auto &srv = AP::srv(); SRV_Channels::calc_pwm(); - SRV_Channels::cork(); + srv.cork(); SRV_Channels::output_ch_all(); - AP::srv().push(); + srv.push(); return true; } @@ -474,10 +476,11 @@ bool AP_MotorsUGV::output_test_pwm(motor_test_order motor_seq, float pwm) default: return false; } + auto &srv = AP::srv(); SRV_Channels::calc_pwm(); - SRV_Channels::cork(); + srv.cork(); SRV_Channels::output_ch_all(); - AP::srv().push(); + srv.push(); return true; } From 89936bab3d2bf3f9d1b7d4362180ad024aa846b4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Nov 2024 08:09:59 +1100 Subject: [PATCH 082/117] SRV_Channel: make SRV_Channels::cork non-static for symmetry with the push function --- libraries/SRV_Channel/SRV_Channel.h | 3 +-- libraries/SRV_Channel/SRV_Channels.cpp | 5 +++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index e19c52a4bf574..e4778b27738bf 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -538,9 +538,8 @@ class SRV_Channels { } return SRV_Channel::Aux_servo_function_t((SRV_Channel::k_motor9+(channel-8))); } - - static void cork(); + void cork(); void push(); // disable PWM output to a set of channels given by a mask. This is used by the AP_BLHeli code diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 3ef0c7e34318f..c1fda6226c37c 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -545,11 +545,12 @@ void SRV_Channels::zero_rc_outputs() * send an invalid signal to all channels to prevent * undesired/unexpected behavior */ - cork(); + auto &srv = AP::srv(); + srv.cork(); for (uint8_t i=0; iwrite(i, 0); } - AP::srv().push(); + srv.push(); } /* From a2f35b3150157db3522690fedea597602389a888 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Nov 2024 08:09:59 +1100 Subject: [PATCH 083/117] ArduCopter: make SRV_Channels::cork non-static for symmetry with the push function --- ArduCopter/compassmot.cpp | 5 +++-- ArduCopter/esc_calibration.cpp | 18 ++++++++++-------- ArduCopter/motors.cpp | 6 ++++-- 3 files changed, 17 insertions(+), 12 deletions(-) diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index c0d26041c0901..28282c35399f1 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -146,9 +146,10 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) read_radio(); // pass through throttle to motors - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f); - AP::srv().push(); + srv.push(); // read some compass values compass.read(); diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index 722127898e225..0c1d7ea70e636 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -95,9 +95,10 @@ void Copter::esc_calibration_passthrough() hal.scheduler->delay(3); // pass through to motors - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() * 0.001f); - AP::srv().push(); + srv.push(); } #endif // FRAME_CONFIG != HELI_FRAME } @@ -112,25 +113,26 @@ void Copter::esc_calibration_auto() esc_calibration_setup(); // raise throttle to maximum - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(1.0f); - AP::srv().push(); + srv.push(); // delay for 5 seconds while outputting pulses uint32_t tstart = millis(); while (millis() - tstart < 5000) { - SRV_Channels::cork(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(1.0f); - AP::srv().push(); + srv.push(); esc_calibration_notify(); hal.scheduler->delay(3); } // block until we restart while(1) { - SRV_Channels::cork(); + srv.cork(); motors->set_throttle_passthrough_for_esc_calibration(0.0f); - AP::srv().push(); + srv.push(); esc_calibration_notify(); hal.scheduler->delay(3); } diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 328eb831500d5..c37a4d55a2fb3 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -156,8 +156,10 @@ void Copter::motors_output() // output any servo channels SRV_Channels::calc_pwm(); + auto &srv = AP::srv(); + // cork now, so that all channel outputs happen at once - SRV_Channels::cork(); + srv.cork(); // update output on any aux channels, for manual passthru SRV_Channels::output_ch_all(); @@ -181,7 +183,7 @@ void Copter::motors_output() } // push all channels - AP::srv().push(); + srv.push(); } // check for pilot stick input to trigger lost vehicle alarm From aadc37ebeb58dd6162806ed7a8b049ba8ba9969a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Nov 2024 08:09:59 +1100 Subject: [PATCH 084/117] ArduPlane: make SRV_Channels::cork non-static for symmetry with the push function --- ArduPlane/servos.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index b8db6e14b8648..aa3de533d3cf6 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -858,8 +858,8 @@ void Plane::set_servos(void) // start with output corked. the cork is released when we run // servos_output(), which is run from all code paths in this // function - SRV_Channels::cork(); - + AP::srv().cork(); + // this is to allow the failsafe module to deliberately crash // the plane. Only used in extreme circumstances to meet the // OBC rules @@ -1017,7 +1017,8 @@ void Plane::indicate_waiting_for_rud_neutral_to_takeoff(void) */ void Plane::servos_output(void) { - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); // support twin-engine aircraft servos_twin_engine_mix(); @@ -1055,7 +1056,7 @@ void Plane::servos_output(void) SRV_Channels::output_ch_all(); - AP::srv().push(); + srv.push(); if (g2.servo_channels.auto_trim_enabled()) { servos_auto_trim(); From afadb7e6c06baf45f60590b31e830a11f698f14a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Nov 2024 08:09:59 +1100 Subject: [PATCH 085/117] ArduSub: make SRV_Channels::cork non-static for symmetry with the push function --- ArduSub/motors.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 7bfe51c24904c..504c6651d933b 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -18,11 +18,12 @@ void Sub::motors_output() verify_motor_test(); } else { motors.set_interlock(true); - SRV_Channels::cork(); + auto &srv = AP::srv(); + srv.cork(); SRV_Channels::calc_pwm(); SRV_Channels::output_ch_all(); motors.output(); - AP::srv().push(); + srv.push(); } } From cd2c5a1697f33cf5055354161daacac0a965feb5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Nov 2024 08:10:00 +1100 Subject: [PATCH 086/117] Blimp: make SRV_Channels::cork non-static for symmetry with the push function --- Blimp/motors.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Blimp/motors.cpp b/Blimp/motors.cpp index c2993c99bd48f..90fb3308a925b 100644 --- a/Blimp/motors.cpp +++ b/Blimp/motors.cpp @@ -78,8 +78,10 @@ void Blimp::motors_output() // output any servo channels SRV_Channels::calc_pwm(); + auto &srv = AP::srv(); + // cork now, so that all channel outputs happen at once - SRV_Channels::cork(); + srv.cork(); // update output on any aux channels, for manual passthru SRV_Channels::output_ch_all(); @@ -88,5 +90,5 @@ void Blimp::motors_output() motors->output(); // push all channels - AP::srv().push(); + srv.push(); } From dce439643072aea71efe905fb8327fba1f7a1763 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Nov 2024 08:10:00 +1100 Subject: [PATCH 087/117] Tools: make SRV_Channels::cork non-static for symmetry with the push function --- Tools/AP_Periph/rc_out.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index b4d9d410a5307..8feb3e0805ce8 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/Tools/AP_Periph/rc_out.cpp @@ -156,10 +156,11 @@ void AP_Periph_FW::rcout_update() } rcout_has_new_data_to_update = false; + auto &srv = AP::srv(); SRV_Channels::calc_pwm(); - SRV_Channels::cork(); + srv.cork(); SRV_Channels::output_ch_all(); - AP::srv().push(); + srv.push(); #if HAL_WITH_ESC_TELEM if (now_ms - last_esc_telem_update_ms >= esc_telem_update_period_ms) { last_esc_telem_update_ms = now_ms; From 88a80993dcb626f2344785d22e670bde5fe6ced4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 13 Nov 2024 12:20:08 +1100 Subject: [PATCH 088/117] RC_Channel: remove unused method get_channel_pos --- libraries/RC_Channel/RC_Channel.cpp | 8 -------- libraries/RC_Channel/RC_Channel.h | 1 - 2 files changed, 9 deletions(-) diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 4b4f30d79e288..1d4c7cc77fd0c 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -1921,14 +1921,6 @@ RC_Channel::AuxSwitchPos RC_Channel::get_aux_switch_pos() const return position; } -// return switch position value as LOW, MIDDLE, HIGH -// if reading the switch fails then it returns LOW -RC_Channel::AuxSwitchPos RC_Channels::get_channel_pos(const uint8_t rcmapchan) const -{ - const RC_Channel* chan = rc().channel(rcmapchan-1); - return chan != nullptr ? chan->get_aux_switch_pos() : RC_Channel::AuxSwitchPos::LOW; -} - RC_Channel *RC_Channels::find_channel_for_option(const RC_Channel::AUX_FUNC option) { for (uint8_t i=0; i Date: Sat, 9 Nov 2024 11:45:20 +0000 Subject: [PATCH 089/117] GCS_MAVLink: add support for `AVAILABLE_MODES` msg --- libraries/GCS_MAVLink/GCS.h | 14 ++++++++ libraries/GCS_MAVLink/GCS_Common.cpp | 50 ++++++++++++++++++++++++++++ libraries/GCS_MAVLink/GCS_Dummy.h | 1 + libraries/GCS_MAVLink/ap_message.h | 1 + 4 files changed, 66 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 8edbc60474a7c..8bb1108de3d18 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -412,6 +412,10 @@ class GCS_MAVLINK void send_uavionix_adsb_out_status() const; void send_autopilot_state_for_gimbal_device() const; + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + virtual uint8_t send_available_mode(uint8_t index) const = 0; + // lock a channel, preventing use by MAVLink void lock(bool _lock) { _locked = _lock; @@ -1126,6 +1130,16 @@ class GCS_MAVLINK // true if we should NOT do MAVLink on this port (usually because // someone's doing SERIAL_CONTROL over mavlink) bool _locked; + + // Handling of AVAILABLE_MODES + struct { + bool should_send; + // Note these start at 1 + uint8_t requested_index; + uint8_t next_index; + } available_modes; + bool send_available_modes(); + }; /// @class GCS diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 806831a345f4f..9922a4f8377b0 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1152,6 +1152,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c #if AP_AIRSPEED_ENABLED { MAVLINK_MSG_ID_AIRSPEED, MSG_AIRSPEED}, #endif + { MAVLINK_MSG_ID_AVAILABLE_MODES, MSG_AVAILABLE_MODES}, }; for (uint8_t i=0; i mode_count)) { + // Sending all and just sent the last + available_modes.should_send = false; + } + + return true; +} + bool GCS_MAVLINK::try_send_message(const enum ap_message id) { bool ret = true; @@ -6521,6 +6567,10 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) break; #endif + case MSG_AVAILABLE_MODES: + ret = send_available_modes(); + break; + default: // try_send_message must always at some stage return true for // a message, or we will attempt to infinitely retry the diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index f12155294df31..17e74ee0ba7ed 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -35,6 +35,7 @@ class GCS_MAVLINK_Dummy : public GCS_MAVLINK void send_nav_controller_output() const override {}; void send_pid_tuning() override {}; + uint8_t send_available_mode(uint8_t index) const override { return 0; } }; /* diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index a14be7cd53897..b82e64611f9dc 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -103,5 +103,6 @@ enum ap_message : uint8_t { MSG_HIGHRES_IMU, #endif MSG_AIRSPEED, + MSG_AVAILABLE_MODES, MSG_LAST // MSG_LAST must be the last entry in this enum }; From 67bca38151f3c14d2fd4cca797233f935a95d6af Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 9 Nov 2024 11:47:28 +0000 Subject: [PATCH 090/117] Copter: add support for `AVAILABLE_MODES` msg --- ArduCopter/GCS_Mavlink.cpp | 120 ++++++++++++++++++++++++++++++++++++- ArduCopter/GCS_Mavlink.h | 4 ++ 2 files changed, 123 insertions(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index c07b4807a2e09..b87375320753a 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -589,7 +589,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #endif }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, @@ -1663,3 +1664,120 @@ uint8_t GCS_MAVLINK_Copter::high_latency_wind_direction() const return 0; } #endif // HAL_HIGH_LATENCY2_ENABLED + +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Copter::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { +#if MODE_AUTO_ENABLED + &copter.mode_auto, // This auto is actually auto RTL! + &copter.mode_auto, // This one is really is auto! +#endif +#if MODE_ACRO_ENABLED + &copter.mode_acro, +#endif + &copter.mode_stabilize, + &copter.mode_althold, +#if MODE_CIRCLE_ENABLED + &copter.mode_circle, +#endif +#if MODE_LOITER_ENABLED + &copter.mode_loiter, +#endif +#if MODE_GUIDED_ENABLED + &copter.mode_guided, +#endif + &copter.mode_land, +#if MODE_RTL_ENABLED + &copter.mode_rtl, +#endif +#if MODE_DRIFT_ENABLED + &copter.mode_drift, +#endif +#if MODE_SPORT_ENABLED + &copter.mode_sport, +#endif +#if MODE_FLIP_ENABLED + &copter.mode_flip, +#endif +#if AUTOTUNE_ENABLED + &copter.mode_autotune, +#endif +#if MODE_POSHOLD_ENABLED + &copter.mode_poshold, +#endif +#if MODE_BRAKE_ENABLED + &copter.mode_brake, +#endif +#if MODE_THROW_ENABLED + &copter.mode_throw, +#endif +#if HAL_ADSB_ENABLED + &copter.mode_avoid_adsb, +#endif +#if MODE_GUIDED_NOGPS_ENABLED + &copter.mode_guided_nogps, +#endif +#if MODE_SMARTRTL_ENABLED + &copter.mode_smartrtl, +#endif +#if MODE_FLOWHOLD_ENABLED + (Mode*)copter.g2.mode_flowhold_ptr, +#endif +#if MODE_FOLLOW_ENABLED + &copter.mode_follow, +#endif +#if MODE_ZIGZAG_ENABLED + &copter.mode_zigzag, +#endif +#if MODE_SYSTEMID_ENABLED + (Mode *)copter.g2.mode_systemid_ptr, +#endif +#if MODE_AUTOROTATE_ENABLED + &copter.mode_autorotate, +#endif +#if MODE_TURTLE_ENABLED + &copter.mode_turtle, +#endif + }; + + const uint8_t mode_count = ARRAY_SIZE(modes); + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name = modes[index_zero]->name(); + uint8_t mode_number = (uint8_t)modes[index_zero]->mode_number(); + +#if MODE_AUTO_ENABLED + // Auto RTL is odd + // Have to deal with is separately becase its number and name can change depending on if were in it or not + if (index_zero == 0) { + mode_number = (uint8_t)Mode::Number::AUTO_RTL; + name = "AUTO RTL"; + + } else if (index_zero == 1) { + mode_number = (uint8_t)Mode::Number::AUTO; + name = "AUTO"; + + } +#endif + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index e38c9b6e7aa02..558804c05e8c3 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -64,6 +64,10 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK uint32_t log_radio_bit() const override { return MASK_LOG_PM; } #endif + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: // sanity check velocity or acceleration vector components are numbers From 684881d13a6faf7b806534112b7bcc288066fea0 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 9 Nov 2024 12:08:01 +0000 Subject: [PATCH 091/117] Plane: add support `AVAILABLE_MODES` msg --- ArduPlane/GCS_Mavlink.cpp | 99 ++++++++++++++++++++++++++++++++++++++- ArduPlane/GCS_Mavlink.h | 4 ++ 2 files changed, 102 insertions(+), 1 deletion(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index b733f7b7bff13..298b4e47a5195 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -716,7 +716,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_VIBRATION, }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, @@ -1545,3 +1546,99 @@ MAV_LANDED_STATE GCS_MAVLINK_Plane::landed_state() const return MAV_LANDED_STATE_ON_GROUND; } +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Plane::send_available_mode(uint8_t index) const +{ + // Fixed wing modes + const Mode* fw_modes[] { + &plane.mode_manual, + &plane.mode_circle, + &plane.mode_stabilize, + &plane.mode_training, + &plane.mode_acro, + &plane.mode_fbwa, + &plane.mode_fbwb, + &plane.mode_cruise, + &plane.mode_autotune, + &plane.mode_auto, + &plane.mode_rtl, + &plane.mode_loiter, +#if HAL_ADSB_ENABLED + &plane.mode_avoidADSB, +#endif + &plane.mode_guided, + &plane.mode_initializing, + &plane.mode_takeoff, +#if HAL_SOARING_ENABLED + &plane.mode_thermal, +#endif + }; + + const uint8_t fw_mode_count = ARRAY_SIZE(fw_modes); + + // Fixedwing modes are always present + uint8_t mode_count = fw_mode_count; + +#if HAL_QUADPLANE_ENABLED + // Quadplane modes + const Mode* q_modes[] { + &plane.mode_qstabilize, + &plane.mode_qhover, + &plane.mode_qloiter, + &plane.mode_qland, + &plane.mode_qrtl, + &plane.mode_qacro, + &plane.mode_loiter_qland, +#if QAUTOTUNE_ENABLED + &plane.mode_qautotune, +#endif + }; + + // Quadplane modes must be enabled + if (plane.quadplane.available()) { + mode_count += ARRAY_SIZE(q_modes); + } +#endif // HAL_QUADPLANE_ENABLED + + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name; + uint8_t mode_number; + + if (index_zero < fw_mode_count) { + // A fixedwing mode + name = fw_modes[index_zero]->name(); + mode_number = (uint8_t)fw_modes[index_zero]->mode_number(); + + } else { +#if HAL_QUADPLANE_ENABLED + // A Quadplane mode + const uint8_t q_index = index_zero - fw_mode_count; + name = q_modes[q_index]->name(); + mode_number = (uint8_t)q_modes[q_index]->mode_number(); +#else + // Should not endup here + return mode_count; +#endif + } + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 9e03940882713..2fa88274e1655 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -50,6 +50,10 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override; void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override; + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: void send_pid_info(const struct AP_PIDInfo *pid_info, const uint8_t axis, const float achieved); From 984daeabfd231445a45c2f70fa8f4d7c996ee389 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 9 Nov 2024 12:17:28 +0000 Subject: [PATCH 092/117] Rover: add support for `AVAILABLE_MODES` msg --- Rover/GCS_Mavlink.cpp | 54 ++++++++++++++++++++++++++++++++++++++++++- Rover/GCS_Mavlink.h | 4 ++++ 2 files changed, 57 insertions(+), 1 deletion(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 3704110ec9e2f..a903e591c6e62 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -619,7 +619,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #endif }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, @@ -1154,3 +1155,54 @@ uint8_t GCS_MAVLINK_Rover::high_latency_wind_direction() const return 0; } #endif // HAL_HIGH_LATENCY2_ENABLED + +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Rover::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { + &rover.mode_manual, + &rover.mode_acro, + &rover.mode_steering, + &rover.mode_hold, + &rover.mode_loiter, +#if MODE_FOLLOW_ENABLED + &rover.mode_follow, +#endif + &rover.mode_simple, + &rover.g2.mode_circle, + &rover.mode_auto, + &rover.mode_rtl, + &rover.mode_smartrtl, + &rover.mode_guided, + &rover.mode_initializing, +#if MODE_DOCK_ENABLED + (Mode *)rover.g2.mode_dock_ptr, +#endif + }; + + const uint8_t mode_count = ARRAY_SIZE(modes); + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name = modes[index_zero]->name4(); + const uint8_t mode_number = (uint8_t)modes[index_zero]->mode_number(); + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index b39a8cfb3630e..fd92cac3d5cfa 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -40,6 +40,10 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK uint32_t log_radio_bit() const override { return MASK_LOG_PM; } #endif + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: void handle_message(const mavlink_message_t &msg) override; From c0a31344390d7ddac426db6f6c790100702f300a Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 9 Nov 2024 12:27:36 +0000 Subject: [PATCH 093/117] Tracker: add support for `AVAILABLE_MODES` msg --- AntennaTracker/GCS_Mavlink.cpp | 43 +++++++++++++++++++++++++++++++++- AntennaTracker/GCS_Mavlink.h | 4 ++++ AntennaTracker/mode.h | 8 +++++++ 3 files changed, 54 insertions(+), 1 deletion(-) diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 93e6638304440..c9ec2050c746f 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -321,7 +321,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_BATTERY_STATUS, }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { @@ -649,3 +650,43 @@ void GCS_MAVLINK_Tracker::send_global_position_int() 0, // Z speed cm/s (+ve Down) tracker.ahrs.yaw_sensor); // compass heading in 1/100 degree } + +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Tracker::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { + &tracker.mode_manual, + &tracker.mode_stop, + &tracker.mode_scan, + &tracker.mode_guided, + &tracker.mode_servotest, + &tracker.mode_auto, + &tracker.mode_initialising, + }; + + const uint8_t mode_count = ARRAY_SIZE(modes); + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name = modes[index_zero]->name(); + const uint8_t mode_number = (uint8_t)modes[index_zero]->number(); + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index a431f5217e230..af98ee5a39fd7 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -30,6 +30,10 @@ class GCS_MAVLINK_Tracker : public GCS_MAVLINK void send_nav_controller_output() const override; void send_pid_tuning() override; + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override; diff --git a/AntennaTracker/mode.h b/AntennaTracker/mode.h index d6a10022dc8c5..bfdcad9ac7860 100644 --- a/AntennaTracker/mode.h +++ b/AntennaTracker/mode.h @@ -22,6 +22,7 @@ class Mode { // returns a unique number specific to this mode virtual Mode::Number number() const = 0; + virtual const char* name() const = 0; virtual bool requires_armed_servos() const = 0; @@ -41,6 +42,7 @@ class Mode { class ModeAuto : public Mode { public: Mode::Number number() const override { return Mode::Number::AUTO; } + const char* name() const override { return "Auto"; } bool requires_armed_servos() const override { return true; } void update() override; }; @@ -48,6 +50,7 @@ class ModeAuto : public Mode { class ModeGuided : public Mode { public: Mode::Number number() const override { return Mode::Number::GUIDED; } + const char* name() const override { return "Guided"; } bool requires_armed_servos() const override { return true; } void update() override; @@ -66,6 +69,7 @@ class ModeGuided : public Mode { class ModeInitialising : public Mode { public: Mode::Number number() const override { return Mode::Number::INITIALISING; } + const char* name() const override { return "Initialising"; } bool requires_armed_servos() const override { return false; } void update() override {}; }; @@ -73,6 +77,7 @@ class ModeInitialising : public Mode { class ModeManual : public Mode { public: Mode::Number number() const override { return Mode::Number::MANUAL; } + const char* name() const override { return "Manual"; } bool requires_armed_servos() const override { return true; } void update() override; }; @@ -80,6 +85,7 @@ class ModeManual : public Mode { class ModeScan : public Mode { public: Mode::Number number() const override { return Mode::Number::SCAN; } + const char* name() const override { return "Scan"; } bool requires_armed_servos() const override { return true; } void update() override; }; @@ -87,6 +93,7 @@ class ModeScan : public Mode { class ModeServoTest : public Mode { public: Mode::Number number() const override { return Mode::Number::SERVOTEST; } + const char* name() const override { return "ServoTest"; } bool requires_armed_servos() const override { return true; } void update() override {}; @@ -96,6 +103,7 @@ class ModeServoTest : public Mode { class ModeStop : public Mode { public: Mode::Number number() const override { return Mode::Number::STOP; } + const char* name() const override { return "Stop"; } bool requires_armed_servos() const override { return false; } void update() override {}; }; From deca687d303e17534a2182b3df1c7545d08b508d Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 9 Nov 2024 12:35:04 +0000 Subject: [PATCH 094/117] Blimp: add support for `AVAILABLE_MODES` msg --- Blimp/GCS_Mavlink.cpp | 41 ++++++++++++++++++++++++++++++++++++++++- Blimp/GCS_Mavlink.h | 4 ++++ Blimp/mode.h | 14 ++++++++++++++ 3 files changed, 58 insertions(+), 1 deletion(-) diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 4d91191a02863..1eb5985e81a1c 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -401,7 +401,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #endif }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; static const ap_message STREAM_ADSB_msgs[] = { MSG_ADSB_VEHICLE, @@ -614,3 +615,41 @@ uint8_t GCS_MAVLINK_Blimp::high_latency_wind_direction() const return wrap_360(degrees(atan2f(-wind.y, -wind.x))) / 2; } #endif // HAL_HIGH_LATENCY2_ENABLED + +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Blimp::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { + &blimp.mode_land, + &blimp.mode_manual, + &blimp.mode_velocity, + &blimp.mode_loiter, + &blimp.mode_rtl, + }; + + const uint8_t mode_count = ARRAY_SIZE(modes); + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name = modes[index_zero]->name(); + const uint8_t mode_number = (uint8_t)modes[index_zero]->number(); + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 35276b03a46d5..90c4881b2adf4 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -48,6 +48,10 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK uint32_t log_radio_bit() const override { return MASK_LOG_PM; } #endif + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: void handle_message(const mavlink_message_t &msg) override; diff --git a/Blimp/mode.h b/Blimp/mode.h index 8a990fbb31b30..8718d47fa144e 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -52,6 +52,9 @@ class Mode virtual const char *name() const = 0; virtual const char *name4() const = 0; + // returns a unique number specific to this mode + virtual Mode::Number number() const = 0; + virtual bool is_landing() const { return false; @@ -159,6 +162,8 @@ class ModeManual : public Mode return "MANU"; } + Mode::Number number() const override { return Mode::Number::MANUAL; } + private: }; @@ -201,6 +206,8 @@ class ModeVelocity : public Mode return "VELY"; } + Mode::Number number() const override { return Mode::Number::VELOCITY; } + private: }; @@ -244,6 +251,8 @@ class ModeLoiter : public Mode return "LOIT"; } + Mode::Number number() const override { return Mode::Number::LOITER; } + private: Vector3f target_pos; float target_yaw; @@ -286,6 +295,8 @@ class ModeLand : public Mode return "LAND"; } + Mode::Number number() const override { return Mode::Number::LAND; } + private: }; @@ -328,4 +339,7 @@ class ModeRTL : public Mode { return "RTL"; } + + Mode::Number number() const override { return Mode::Number::RTL; } + }; From b0f821a96f43ce1e196793b9690b8dbde588af2c Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 9 Nov 2024 12:41:50 +0000 Subject: [PATCH 095/117] Sub: add support for `AVAILABLE_MODES` msg --- ArduSub/GCS_Mavlink.cpp | 47 ++++++++++++++++++++++++++++++++++++++++- ArduSub/GCS_Mavlink.h | 4 ++++ ArduSub/mode.h | 15 +++++++++++++ 3 files changed, 65 insertions(+), 1 deletion(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index c2bbf8b5c42bb..f17901638d76a 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -443,7 +443,8 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #endif }; static const ap_message STREAM_PARAMS_msgs[] = { - MSG_NEXT_PARAM + MSG_NEXT_PARAM, + MSG_AVAILABLE_MODES }; const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { @@ -968,3 +969,47 @@ uint8_t GCS_MAVLINK_Sub::high_latency_tgt_airspeed() const return 0; } #endif // HAL_HIGH_LATENCY2_ENABLED + +// Send the mode with the given index (not mode number!) return the total number of modes +// Index starts at 1 +uint8_t GCS_MAVLINK_Sub::send_available_mode(uint8_t index) const +{ + const Mode* modes[] { + &sub.mode_manual, + &sub.mode_stabilize, + &sub.mode_acro, + &sub.mode_althold, + &sub.mode_surftrak, + &sub.mode_poshold, + &sub.mode_auto, + &sub.mode_guided, + &sub.mode_circle, + &sub.mode_surface, + &sub.mode_motordetect, + }; + + const uint8_t mode_count = ARRAY_SIZE(modes); + + // Convert to zero indexed + const uint8_t index_zero = index - 1; + if (index_zero >= mode_count) { + // Mode does not exist!? + return mode_count; + } + + // Ask the mode for its name and number + const char* name = modes[index_zero]->name(); + const uint8_t mode_number = (uint8_t)modes[index_zero]->number(); + + mavlink_msg_available_modes_send( + chan, + mode_count, + index, + MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD, + mode_number, + 0, // MAV_MODE_PROPERTY bitmask + name + ); + + return mode_count; +} diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index c38ec3f4abb6e..7dc2d0c1a2586 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -37,6 +37,10 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { uint64_t capabilities() const override; + // Send the mode with the given index (not mode number!) return the total number of modes + // Index starts at 1 + uint8_t send_available_mode(uint8_t index) const override; + private: void handle_message(const mavlink_message_t &msg) override; diff --git a/ArduSub/mode.h b/ArduSub/mode.h index 11a6447167511..35780cb9c04d0 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -72,6 +72,9 @@ class Mode virtual const char *name() const = 0; virtual const char *name4() const = 0; + // returns a unique number specific to this mode + virtual Mode::Number number() const = 0; + // functions for reporting to GCS virtual bool get_wp(Location &loc) { return false; } virtual int32_t wp_bearing() const { return 0; } @@ -202,6 +205,7 @@ class ModeManual : public Mode const char *name() const override { return "MANUAL"; } const char *name4() const override { return "MANU"; } + Mode::Number number() const override { return Mode::Number::MANUAL; } }; @@ -224,6 +228,7 @@ class ModeAcro : public Mode const char *name() const override { return "ACRO"; } const char *name4() const override { return "ACRO"; } + Mode::Number number() const override { return Mode::Number::ACRO; } }; @@ -246,6 +251,7 @@ class ModeStabilize : public Mode const char *name() const override { return "STABILIZE"; } const char *name4() const override { return "STAB"; } + Mode::Number number() const override { return Mode::Number::STABILIZE; } }; @@ -272,6 +278,7 @@ class ModeAlthold : public Mode const char *name() const override { return "ALT_HOLD"; } const char *name4() const override { return "ALTH"; } + Mode::Number number() const override { return Mode::Number::ALT_HOLD; } }; @@ -293,6 +300,7 @@ class ModeSurftrak : public ModeAlthold const char *name() const override { return "SURFTRAK"; } const char *name4() const override { return "STRK"; } + Mode::Number number() const override { return Mode::Number::SURFTRAK; } private: @@ -342,6 +350,8 @@ class ModeGuided : public Mode const char *name() const override { return "GUIDED"; } const char *name4() const override { return "GUID"; } + Mode::Number number() const override { return Mode::Number::GUIDED; } + autopilot_yaw_mode get_default_auto_yaw_mode(bool rtl) const; private: @@ -387,6 +397,7 @@ class ModeAuto : public ModeGuided const char *name() const override { return "AUTO"; } const char *name4() const override { return "AUTO"; } + Mode::Number number() const override { return Mode::Number::AUTO; } private: void auto_wp_run(); @@ -417,6 +428,7 @@ class ModePoshold : public ModeAlthold const char *name() const override { return "POSHOLD"; } const char *name4() const override { return "POSH"; } + Mode::Number number() const override { return Mode::Number::POSHOLD; } }; @@ -439,6 +451,7 @@ class ModeCircle : public Mode const char *name() const override { return "CIRCLE"; } const char *name4() const override { return "CIRC"; } + Mode::Number number() const override { return Mode::Number::CIRCLE; } }; class ModeSurface : public Mode @@ -460,6 +473,7 @@ class ModeSurface : public Mode const char *name() const override { return "SURFACE"; } const char *name4() const override { return "SURF"; } + Mode::Number number() const override { return Mode::Number::CIRCLE; } }; @@ -482,4 +496,5 @@ class ModeMotordetect : public Mode const char *name() const override { return "MOTORDETECT"; } const char *name4() const override { return "DETE"; } + Mode::Number number() const override { return Mode::Number::MOTOR_DETECT; } }; From f08a2120a8ce990e3939496dbe50ea1d4cae9fcb Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 12 Nov 2024 02:27:39 +0000 Subject: [PATCH 096/117] MAVLink: add required messages to support addition of modes at runtime --- modules/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/modules/mavlink b/modules/mavlink index 08da786b2d94c..4c64b9522f9bb 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 08da786b2d94ce0955a82f92ab2166c347259623 +Subproject commit 4c64b9522f9bb9815b089ab98cf2f33f11bded52 From b1acd6295b93c65da0c8dc965b1af07e59160ca5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 31 Dec 2019 09:52:36 +1100 Subject: [PATCH 097/117] HAL_ChibiOS: cleanup cube IMUs and compasses don't probe for old sensor set --- .../AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc | 29 ++----------------- .../AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat | 29 ++----------------- 2 files changed, 4 insertions(+), 54 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc index cb5769d8d514b..f7d85e808225c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc @@ -212,17 +212,8 @@ PE15 VDD_5V_PERIPH_nOC INPUT PULLUP SPIDEV ms5611 SPI1 DEVID3 BARO_CS MODE3 20*MHZ 20*MHZ SPIDEV ms5611_ext SPI4 DEVID2 BARO_EXT_CS MODE3 20*MHZ 20*MHZ -SPIDEV mpu6000 SPI1 DEVID4 MPU_CS MODE3 2*MHZ 8*MHZ -SPIDEV icm20608-am SPI1 DEVID2 ACCEL_EXT_CS MODE3 4*MHZ 8*MHZ -SPIDEV mpu9250 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ -SPIDEV mpu9250_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ SPIDEV icm20948 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ SPIDEV icm20948_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ -SPIDEV hmc5843 SPI1 DEVID5 MAG_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_g SPI1 DEVID1 GYRO_EXT_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_am SPI1 DEVID2 ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_ext_g SPI4 DEVID4 GYRO_EXT_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_ext_am SPI4 DEVID3 ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ SPIDEV icm20602_ext SPI4 DEVID4 GYRO_EXT_CS MODE3 4*MHZ 8*MHZ SPIDEV ramtron SPI2 DEVID10 FRAM_CS MODE3 8*MHZ 8*MHZ SPIDEV external0m0 SPI4 DEVID5 MPU_EXT_CS MODE0 2*MHZ 2*MHZ @@ -231,18 +222,7 @@ SPIDEV external0m2 SPI4 DEVID5 MPU_EXT_CS MODE2 2*MHZ 2*MHZ SPIDEV external0m3 SPI4 DEVID5 MPU_EXT_CS MODE3 2*MHZ 2*MHZ SPIDEV pixartPC15 SPI4 DEVID13 ACCEL_EXT_CS MODE3 2*MHZ 2*MHZ -# three IMUs, but allow for different variants. First two IMUs are -# isolated, 3rd isn't -IMU Invensense SPI:mpu9250_ext ROTATION_PITCH_180 - -# the 3 rotations for the LSM9DS0 driver are for the accel, the gyro -# and the H variant of the gyro -IMU LSM9DS0 SPI:lsm9ds0_ext_g SPI:lsm9ds0_ext_am ROTATION_ROLL_180_YAW_270 ROTATION_ROLL_180_YAW_90 ROTATION_ROLL_180_YAW_90 - -# 3rd non-isolated IMU -IMU Invensense SPI:mpu9250 ROTATION_YAW_270 - -# alternative IMU set for newer cubes +# IMU set for newer cubes IMU Invensense SPI:icm20602_ext ROTATION_ROLL_180_YAW_270 IMU Invensensev2 SPI:icm20948_ext ROTATION_PITCH_180 IMU Invensensev2 SPI:icm20948 ROTATION_YAW_270 @@ -270,12 +250,7 @@ define HAL_DEFAULT_INS_FAST_SAMPLE 7 BARO MS56XX SPI:ms5611_ext BARO MS56XX SPI:ms5611 -# two compasses. First is in the LSM303D -COMPASS LSM303D SPI:lsm9ds0_ext_am ROTATION_YAW_270 -# 2nd compass is part of the 2nd invensense IMU -COMPASS AK8963:probe_mpu9250 1 ROTATION_YAW_270 - -# compass as part of ICM20948 on newer cubes +# one internal compass COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180_YAW_90 # offset the internal compass for EM impact of the IMU heater diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat index afc44a107c605..55ac0d221db2c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat @@ -351,17 +351,8 @@ PE15 VDD_5V_PERIPH_nOC INPUT PULLUP SPIDEV ms5611 SPI1 DEVID3 BARO_CS MODE3 20*MHZ 20*MHZ SPIDEV ms5611_ext SPI4 DEVID2 BARO_EXT_CS MODE3 20*MHZ 20*MHZ -SPIDEV mpu6000 SPI1 DEVID4 MPU_CS MODE3 2*MHZ 8*MHZ -SPIDEV icm20608-am SPI1 DEVID2 ACCEL_EXT_CS MODE3 4*MHZ 8*MHZ -SPIDEV mpu9250 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ -SPIDEV mpu9250_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ SPIDEV icm20948 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ SPIDEV icm20948_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ -SPIDEV hmc5843 SPI1 DEVID5 MAG_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_g SPI1 DEVID1 GYRO_EXT_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_am SPI1 DEVID2 ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_ext_g SPI4 DEVID4 GYRO_EXT_CS MODE3 11*MHZ 11*MHZ -SPIDEV lsm9ds0_ext_am SPI4 DEVID3 ACCEL_EXT_CS MODE3 11*MHZ 11*MHZ SPIDEV icm20602_ext SPI4 DEVID4 GYRO_EXT_CS MODE3 4*MHZ 8*MHZ SPIDEV ramtron SPI2 DEVID10 FRAM_CS MODE3 8*MHZ 8*MHZ SPIDEV external0m0 SPI4 DEVID5 MPU_EXT_CS MODE0 2*MHZ 2*MHZ @@ -382,18 +373,7 @@ SPIDEV pixartPC15 SPI4 DEVID13 ACCEL_EXT_CS MODE3 2*MHZ 2*MHZ #SPIDEV clock8 SPI4 DEVID5 MPU_EXT_CS MODE0 8*MHZ 8*MHZ # gives 5.5MHz #SPIDEV clock16 SPI4 DEVID5 MPU_EXT_CS MODE0 16*MHZ 16*MHZ # gives 10.6MHz -# three IMUs, but allow for different variants. First two IMUs are -# isolated, 3rd isn't -IMU Invensense SPI:mpu9250_ext ROTATION_PITCH_180 - -# the 3 rotations for the LSM9DS0 driver are for the accel, the gyro -# and the H variant of the gyro -IMU LSM9DS0 SPI:lsm9ds0_ext_g SPI:lsm9ds0_ext_am ROTATION_ROLL_180_YAW_270 ROTATION_ROLL_180_YAW_90 ROTATION_ROLL_180_YAW_90 - -# 3rd non-isolated IMU -IMU Invensense SPI:mpu9250 ROTATION_YAW_270 - -# alternative IMU set for newer cubes +#IMU set for newer cubes IMU Invensense SPI:icm20602_ext ROTATION_ROLL_180_YAW_270 IMU Invensensev2 SPI:icm20948_ext ROTATION_PITCH_180 IMU Invensensev2 SPI:icm20948 ROTATION_YAW_270 @@ -404,12 +384,7 @@ define HAL_DEFAULT_INS_FAST_SAMPLE 5 BARO MS56XX SPI:ms5611_ext BARO MS56XX SPI:ms5611 -# two compasses. First is in the LSM303D -COMPASS LSM303D SPI:lsm9ds0_ext_am ROTATION_YAW_270 -# 2nd compass is part of the 2nd invensense IMU -COMPASS AK8963:probe_mpu9250 1 ROTATION_YAW_270 - -# compass as part of ICM20948 on newer cubes +# one compass COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180_YAW_90 # also probe for external compasses From 8354bedd5191bfef03d8a271e8ab307282988c7e Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Fri, 8 Nov 2024 13:02:11 -0700 Subject: [PATCH 098/117] Tools: Recommend what to do when astyle fails Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- Tools/scripts/build_ci.sh | 4 ++++ Tools/scripts/run_astyle.py | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/Tools/scripts/build_ci.sh b/Tools/scripts/build_ci.sh index 19d2912158a8f..8c7a3ba9c25eb 100755 --- a/Tools/scripts/build_ci.sh +++ b/Tools/scripts/build_ci.sh @@ -473,7 +473,11 @@ for t in $CI_BUILD_TARGET; do if [ "$t" == "astyle-cleanliness" ]; then echo "Checking AStyle code cleanliness" + ./Tools/scripts/run_astyle.py --dry-run + if [ $? -ne 0 ]; then + echo The code failed astyle cleanliness checks. Please run ./Tools/scripts/run_astyle.py + fi continue fi diff --git a/Tools/scripts/run_astyle.py b/Tools/scripts/run_astyle.py index 7f5a694f73dbe..23ebee79ac16f 100755 --- a/Tools/scripts/run_astyle.py +++ b/Tools/scripts/run_astyle.py @@ -60,7 +60,7 @@ def check(self): self.progress("astyle check failed: (%s)" % (ret.stdout)) self.retcode = 1 if "Formatted" in ret.stdout: - self.progress("Files needing formatting found") + self.progress("Files needing formatting found.") print(ret.stdout) self.retcode = 1 From 81d194534766ac989af22c3a6a53f7a779abd8f9 Mon Sep 17 00:00:00 2001 From: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Date: Fri, 8 Nov 2024 13:07:20 -0700 Subject: [PATCH 099/117] AP_DDS: Recommend run_astyle.py Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> --- libraries/AP_DDS/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 61aed38a6d5a3..03a1aa92938bf 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -364,10 +364,10 @@ for additional details. ### Development Requirements Astyle is used to format the C++ code in AP_DDS. This is required for CI to pass the build. -See [Tools/CodeStyle/ardupilot-astyle.sh](../../Tools/CodeStyle/ardupilot-astyle.sh). +To run the automated formatter, run: ```bash -./Tools/CodeStyle/ardupilot-astyle.sh libraries/AP_DDS/*.h libraries/AP_DDS/*.cpp +./Tools/scripts/run_astyle.py ``` Pre-commit is used for other things like formatting python and XML code. From 4f406f31f28fb4a02749c380f0801f048ee71040 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 6 Nov 2024 13:09:19 +0900 Subject: [PATCH 100/117] Plane: 4.6.0-beta1 release notes Co-authored-by: Bill Geyer Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com> Co-authored-by: Thomas Watson Co-authored-by: Andrew Tridgell --- ArduPlane/ReleaseNotes.txt | 424 +++++++++++++++++++++++++++++++++++++ 1 file changed, 424 insertions(+) diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index e8699fa5c9320..ed28f55e6d524 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,5 +1,429 @@ ArduPilot Plane Release Notes: ------------------------------------------------------------------ +Release 4.6.0-beta 13 Nov 2024 + +Changes from 4.5.7 + +1) Board specific changes + +- AnyLeaf H7 supports compass and onboard logging +- Blitz743Pro supports CAN +- BlueRobotics Navigator supports BMP390 baro +- Bootloader ECC failure check fixed on boards with >128k bootloader space (e.g CubeRed) +- CB Unmanned Stamp H743 support +- ClearSky CSKY405 support +- CUAV-7-Nano default batt monitor fixed +- CubeRed bootloader fixes including disabling 2nd core by default +- CubeRed supports PPP networking between primary and secondary MCU +- CubeRedPrimary supports external compasses +- ESP32 main loop rate improvements +- ESP32 RC input fixes and wifi connection reliability improved +- ESP32 safety switch and GPIO pin support +- FlyingMoon no longer support MAX7456 +- Flywoo F405HD-AIOv2 ELRS RX pin pulled high during boot +- Flywoo H743 Pro support +- Flywoo/Goku F405 HD 1-2S ELRS AIO v2 +- FlywooF745 supports DPS310 baro +- FPV boards lose SMBus battery support (to save flash) +- GEPRC F745BTHD support +- GEPRCF745BTHD loses parachute support, non-BMP280 baros (to save flash) +- Here4FC bootloader fix for mismatch between RAM0 and periph that could prevent firmware updates +- Holybro Kakute F4 Wing support +- iFlight 2RAW H743 supports onboard logging +- JFB110 supports measuring servo rail voltage +- JFB110 supports safety switch LED +- JHEM-JHEF405 bootloader supports firmware updates via serial +- JHEMCU GF30H743 HD support +- JHEMCU-GF16-F405 autopilot support +- JHEMCU-GSF405A becomes FPV board (to save flash) +- KakuteF7 only supports BMP280 baro (to save flash) +- KakuteH7Mini supports ICM42688 IMU +- Linux auto detection of GPS baud rate fixed +- Linux board scheduler jitter reduced +- Linux board shutdown fixes +- MakeFlyEasy PixPilot-V6Pro support +- MatekF405, Pixhawk1-1M-bdshot, revo-mini loses blended GPS (to save flash) +- MatekH7A3 support Bi-directional DShot +- MicoAir405v2 and MicoAir405Mini support optical flow and OSD +- MicoAir743 internal compass orientation fixed +- MicroAir405Mini, MicroAir743, NxtPX4v2 support +- MicroAir405v2 Bi-directional DShot and LED DMA fixes +- MicroAir405v2 defined as FPV board with reduced features (to save flash) +- ModalAI VOXL2 support including Starling 2 and Starling 2 max +- mRo Control Zero Classic supports servo rail analog input +- mRo KitCAN revC fixed +- Mugin MUPilot support +- OmnibusF7v2 loses quadplane support (to save flash) +- Pixhack-v3 board added (same as fmuv3) +- Pixhawk6C bootloader supports flashing firmware from SD card +- RadiolinkPIX6 imu orientation fixed +- RadiolinkPIX6 supports SPA06 baro +- ReaperF745 V4 FC supports MPU6000 IMU +- RPI5 support +- SDModelH7v2 SERIAL3/7/9_PROTOCOL param defaults changed +- Solo serial ports default to MAVLink1 +- SpeedyBeeF405Wing gets Bi-directional DShot +- SpeedyBeeF405WING loses landing gear support, some camera gimbals (to save flash) +- Spektreworks boom board support +- TrueNavPro-G4 SPI does not share DMA +- X-MAV AP-H743v2 support + +2) AHRS/EKF enhancements and fixes + +- AHRS_OPTION to disable fallback to DCM (affects Plane and Rover, Copter never falls back) +- AHRS_OPTION to disable innovation check for airspeed sensor +- Airspeed sensor health check fixed when using multiple sensors and AHRS affinity +- DCM support for MAV_CMD_EXTERNAL_WIND_ESTIMATE (Plane only) +- EK2 supports disabling external nav (see EK2_OPTIONS) +- EK3 External Nav position jump after switch from Optical flow removed (see EK3_SRC_OPTION=1) +- EK3 uses filtered velocity corrections for IMU position +- EKF2, EKF3, ExternalAHRS all use common origin +- EKF3 accepts set origin even when using GPS +- EKF3 allows earth-frame fields to be estimated with an origin but no GPS +- EKF3 copes better with GPS jamming +- EKF3 logs mag fusion selection to XKFS +- EKF3 wind estimation when using GPS-for-yaw fixed +- External AHRS improvements including handling variances, pre-arm origin check +- Inertial Labs External AHRS fixes +- VectorNav driver fix for handling of error from sensor +- VectorNav External AHRS enhancements including validation of config commands and logging improvements +- VectorNav support for sensors outside VN-100 and VN-300 + +3) Driver enhancements and bug fixes + +- ADSB fix to display last character in status text sent to GCS +- Ainstein LR-D1 radar support +- Airspeed ASP5033 whoami check fixed when autopilot rebooted independently of the sensor +- AIRSPEED message sent to GCS +- Analog temperature sensor extended to 5th order polynomial (see TEMP_A5) +- ARSPD_OPTIONS to report calibration offset to GCS +- Baro EAS2TAS conversions continuously calculated reducing shocks to TECS (Plane only) +- Baro provides improved atmospheric model for high altitude flight +- BARO_ALT_OFFSET slew slowed to keep EKF happy +- BATTx_ESC_MASK param supports flexible mapping of ESCs to batteries +- BATTx_OPTION to not send battery voltage, current, etc to GCS +- Benewake RDS02U radar support +- Bi-directional DShot on IOMCU supports reversible mask +- Bi-directional DShot telemetry support on F103 8Mhz IOMCUs +- BMM350 compass support +- CAN rangefinders and proximity sensors may share a CAN bus (allows NRA24 and MR72 on a single CAN bus) +- Compass calibration world magnetic model checks can use any position source (e.g. not just GPS) +- CRSF baro and vertical speeed fixed +- CRSF RX bind command support +- DroneCAN battery monitor check to avoid memory corruption when type changed +- DroneCAN DNA server fixes including removing use of invalid node IDs, faster ID allocation, elimination of rare inability to save info +- DroneCAN EFI health check fix +- DroneCAN ESC battery monitors calculate consumed mah +- DroneCAN ESCs forced to zero when disarmed +- DroneCAN RPM message support +- DroneCAN timeout fix for auxiliary frames +- DroneCAN to serial tunneling params accepts short-hand baud rates (e.g. '57' for '57600') +- F9P, F10-N and Zed-F9P support for GPSx_GNSS_MODE to turn on/off SBAS, Galileo, Beidou and Glonass +- FuelLevel battery monitor fix to report capacity +- GPS_xxx params renamed to GPS1_xxx, GPS_xxx2 renamed to GPS2_xxx +- Hirth EFI logging includes modified throttle +- Hirth ICEngine supports reversed crank direction (see ICE_OPTIONS parameter) +- Hott and LTM telemetry deprecated (still available through custom build server) +- i-BUS telemetry support +- ICE_PWM_IGN_ON, ICE_PWM_IGN_OFF, ICE_PWM_STRT_ON, ICE_PWM_STRT_OFF replaced with SERVOx_MIN/MAX/REVERSED (Plane only) +- ICE_START_CHAN replaced with RC aux function (Plane only) +- ICEngine retry max added (see ICE_STRT_MX_RTRY) +- IE 2400 generator error message severity to GCS improved +- INA2xx battery monitor support (reads temp, gets MAX_AMPS and SHUNT params) +- MCP9600 temperature sensor I2C address fixed +- MLX90614 temperature sensor support +- MSP GPS ground course scaling fixed +- MSP RC uses scaled RC inputs (fixes issue with RCx_REVERSED having no effect) +- Networking supports reconnection to TCP server or client +- OSD params for adjusting horizontal spacing and vertical extension (see OSD_SB_H_OFS, OSD_SB_V_EXT) +- Relay inverted output support (see RELAYx_INVERTED parameter) +- ROMFS efficiency improvements +- RS-485 driver enable RTS flow control +- Sagetech MXS ADSP altitude fix (now uses abs alt instead of terrain alt) +- Septentrio GPS sat count correctly drops to zero when 255 received +- Septentrio supports selecting constellations (see GPS_GNSS_MODE) +- Single LED for user notification supported +- SPA06 baro supported +- Sum battery monitor optionally reports minimum voltage instead of average +- Sum battery monitor reports average temp +- Torqeedo dual motor support (see TRQ1, TRQ2 params) +- Ublox GPS driver uses 64 bit time for PPS interrupt (avoids possible dropout at 1hour and 12 minutes) +- UBlox GPS time ignored until at least 2D fix +- VideoTX supports additional freq bands (RushFPV 3.3Ghz) +- Volz logs desired and actual position, voltage, current, motor and PCB temp +- Volz server feedback and logging fixed +- Volz servo output in its own thread resulting in smoother movements +- W25N02KV flash support + +4) Networking enhancements and fixes + +- Allow multiple UDP clients to connect/disconnect/reconnect +- Ethernet supports faster log downloading (raised SDMMC clock limit on H7) + +5) Camera and gimbal enhancements + +- Alexmos precision improved slightly +- CAMERA_CAPTURE_STATUS mavlink msg sent to GCS (reports when images taken or video recorded, used by QGC) +- CAMERA_FOV_STATUS mavlink reports lat,lon of what camera is pointing at +- DO_MOUNT_CONTROL yaw angle interpreted as body-frame (was incorrectly being interpreted as earth-frame) +- Dual serial camera gimbal mounts fixed +- Lua script bindings to send CAMERA_INFORMATION and VIDEO_STREAM_INFORMATION messages to GCS +- Retract Mount2 aux function added (see RCx_OPTION = 113) +- Servo gimbal reported angles respect roll, pitch and yaw limits +- Siyi driver sends autopilot location and speed (recorded in images via EXIF) +- Siyi picture and video download scripts +- Siyi ZT6 and ZT30 support sending min, max temperature (see CAMERA_THERMAL_RANGE msg) +- Siyi ZT6 and ZT30 thermal palette can be changed using camera-change-setting.lua script +- Siyi ZT6 hardware id and set-lens fixed +- Topotek gimbal support +- Trigger distance ignores GPS status and only uses EKF reported location + +6) Harmonic notch enhancements + +- Harmonic notch is active in forward flight on quadplanes +- Harmonic notch filter freq clamping and disabling reworked +- Harmonic notch handles negative ESC RPMs +- Harmonic notch supports per-motor throttle-based harmonic notch + +7) Copter specific enhancements and bug fixes + +- Attitude control fix to dt update order (reduces rate controller noise) +- Auto mode fix to avoid prematurely advancing to next waypoint if given enough time +- Auto mode small target position jump when takeoff completes removed +- Auto mode yaw drift when passing WP removed if CONDITION_YAW command used and WP_YAW_BEHAVIOR = 0/None +- Auto, Guided flight mode pause RC aux switch (see RCx_OPTION = 178) +- AutoRTL (e.g. DO_LAND_START) uses copter stopping point to decide nearest mission command +- AutoRTL mode supports DO_RETURN_PATH_START (Copter, Heli only) +- AutoTune fix to prevent spool up after landing +- AutoTune performance and safety enhancements (less chance of ESC desync, fails when vehicle likely can't be tuned well) +- Autotune test gains RC aux switch function allows testing gains in any mode (see RCx_OPTION = 180) +- Config error avoided if auto mode is paused very soon after poweron +- FLIGHT_OPTIONS bit added to require position estimate before arming +- Follow mode slowdown calcs fixed when target is moving +- Ground oscillation suppressed by reducing gains (see ATC_LAND_R/P/Y_MULT) +- Guided mode internal error fix when taking off using SET_ATTITUDE_CONTROL message +- Guided mode internal error resolved when switching between thrust or climb rate based altitude control +- Guided mode yaw fixed when WP_YAW_BEHAVIOR = 0/None and CONDITION_YAW command received containing relative angle +- Landing detector fixed when in stabilize mode at full throttle but aircraft is upside down +- Landing detector logging added to ease support (see LDET message) +- Loiter unlimited command accepts NaNs (QGC sends these) +- Mavlink SYSTEM_STATUS set to BOOT during initialisation +- MOT_PWM_TYPE of 9 (PWMAngle) respects SERVOx_MIN/MAX/TRIM/REVERSED param values +- Payload place bug fix when aborted because gripper is already released +- RC based tuning (aka CH6 tuning) can use any RC aux function channel (see RCx_OPTION = 219) +- RTL_ALT minimum reduced to 30cm +- SystemID position controller support (Copter and Heli) +- TriCopter motor test and slew-up fixed +- WPNAV_SPEED min reduced to 10 cm/s (Copter only) +- Loiter mode zeros desired accel when re-entering from Auto during RC failsafe + +8) TradHeli specific enhancements + +- Autorotation yaw behaviour fix +- Autotune improvements including using low frequency dwell for feedforward gain tuning and conducting sweep in attitude instead of rate +- Blade pitch angle logging added (see SWSH log message) +- Constrain cyclic roll for intermeshing +- ICE / turbine cool down fix +- Inverted flight extended to non manual throttle modes +- Inverted flight transitions smoothed and restricted to only Stabilize mode +- SWSH logging fix for reversed collectives + +9) Plane specific enhancements and bug fixes + +- AIRSPEED_STALL holds vehicle stall speed and is used for minimum speed scaling +- Allow for different orientations of landing rangefinder +- Assistance requirements evaluted on mode change +- FBWB/CRUISE climb/sink rate limited by TECS limits +- FLIGHT_OPTION to immediately climb in AUTO mode (not doing glide slope) +- Glider pullup support (available only through custom build server) +- Loiter breakout improved to better handle destinations inside loiter circle +- Manual mode throttle made consistent with other modes (e.g battery comp and watt limit is done if enabled) +- Mavlink GUIDED_CHANGE_ALTITUDE supports terrain altitudes +- Minimum throttle not applied during SLT VTOL airbrake (reduces increase in airspeed and alt during back transition) +- Q_APPROACH_DIST set minimum distance to use the fixed wing approach logic +- Quadplane assistance check enhancements +- Quadplane Deca frame support +- Quadplane gets smoother takeoff by input shaping target accel and velocity +- Servo wiggles in altitude wait staged to be one after another +- Set_position_target_global_int accepts MAV_FRAME_GLOBAL_RELATIVE_ALT and MAV_FRAME_GLOBAL_TERRAIN_ALT frames +- Takeoff airspeed control improved (see TKOFF_MODE, TKOFF_THR_MIN) +- Takeoff fixes for fence autoenable +- Takeoff improvements including less overshoot of TKOFF_ALT +- TECS reset along with other controllers (important if plane dropped from balloon) +- Tilt quadplane ramp of motors on back transition fixed +- Tiltrotor tilt angles logged +- TKOFF_THR_MIN applied to SLT transitions +- Twin motor planes with DroneCAN ESCs fix to avoid max throttle at arming due to misconfiguration +- VTOLs switch to QLAND if a LONG_FAILSAFE is triggered during takeoff + +10) Rover specific enhancements and bug fixes + +- Auto mode reversed state maintained if momentarily switched to Hold mode +- Circle mode tracks better and avoids getting stuck at circle edge +- Flight time stats fixed +- MAV_CMD_NAV_SET_YAW_SPEED deprecated +- Omni3Mecanum frame support +- Stopping point uses max deceleration (was incorrectly using acceleration) +- Wheel rate controller slew rate fix + +11) Antenna Tracker specific enhancements and bug fixes + +- Never track lat,lon of 0,0 + +12) Scripting enhancements + +- advance-wp.lua applet supports advancing Auto mode WP via RC switch +- AHRS_switch.lua supports switching between EKF2 and EKF3 via RC switch +- battery_internal_resistance_check.lua monitors battery resistance +- CAN:get_device returns nil for unconfigured CAN device +- copter_terrain_brake.lua script added to prevent impact with terrain in Loiter mode (Copter only) +- Copter:get_target_location, update_target_location support +- crosstrack_restore.lua example allows returning to previous track in Auto (Plane only) +- Display text on OLED display supported +- Docs improved for many bindings +- EFI get_last_update_ms binding +- EFI_SkyPower.lua driver accepts 2nd supply voltage +- ESC_slew_rate.lua example script supports testing ESCs +- Filesystem CRC32 check to allow scripts to check module versions +- forced arming support +- GPIO get/set mode bindings (see gpio:set_mode, get_mode) +- GPS-for-yaw angle binding (see gps:gps_yaw_deg) +- Halo6000 EFI driver can log all CAN packets for easier debugging +- handle_external_position_estimate binding allows sending position estimate from lua to EKF +- I2C:transfer support +- IMU gyros_consistent and accels_consistent bindings added +- INF_Inject.lua driver more robust to serial errors, improved logging, throttle and ignition control +- INS bindings for is calibrating, gyro and accel sensor values +- IPV4 address bindings (see SocketAPM_ipv4_addr_to_string) to allow UDP server that responds to individual clients +- Logging of booleans supported +- Lua language checks improved (finds more errors) +- MAVLink commands can be called from scripting +- MCU voltage binding (see analog:mcu_voltage) +- NMEA 2000 EFI driver (see EFI_NMEA2k.lua) +- "open directory failed" false warning when scripts in ROMFS fixed +- Param_Controller.lua supports quickly switching between 3 parameter sets via RC switch +- Pass by reference values are always initialized +- pelco_d_antennatracker.lua applet supports sending Pelco-D via RS-485 to PTZ cameras +- plane_aerobatics.lua minor enhancements +- REPL applet (read-evaluate-print-loop, see repl.lua) for interactive testing and experimentation +- "require" function failures in rare circumstances fixed +- "require" function works for modules in ROMFS (e.g. not on SD card) +- revert_param.lua supports more params (e.g ATC_RATE_R/P/Y, PTCH2SRV_TCONST, RLL2SRV_TCONST, TECS_) +- Scripts may receive mavlink messages which fail CRC (e.g messages which FC does not understand) +- SD card formatting supported +- Serial device simulation support (allows Lua to feed data to any supported serial protocol for e.g. sensor simulation) +- set_target_throttle_rate_rpy allows rate control from scripts (new for Copter) +- sitl_standby_sim.lua example shows how to switch between redundant flight controllers using an RC switch +- Slung payload oscillation suppression applet added (see copter-slung-payload.lua) +- Temperature sensor bindings added +- uint64 support +- Various performance and memory usage optimizations +- VTOL-quicktune.lua minor enhancements including increasing YAW_FLTE_MAX to 8 +- x-quad-cg-allocation.lua applet corrects for the CoM discrepancy in a quad-X frame + +13) GCS / mavlink related changes and fixes + +- BATTERY2 message deprecated (replaced by BATTERY_STATUS) +- CMD_MISSION_START/STOP rejected if first-item/last-item args provided +- Deny attempts to upload two missions simultaneously +- Fence and Rally points may be uploaded using FTP +- GPS_INPUT and HIL_GPS handles multiple GPSs +- HIGHRES_IMU mavlink message support (used by companion computers to receive IMU data from FC) +- MAV_CMD_COMPONENT_ARM_DISARM accepts force arm magic value of 21196 +- MAV_CMD_DO_SET_SAFETY_SWITCH_STATE support +- MAV_CMD_SET_HAGL support (Plane only) +- MAVFTP respects TX buffer flow control to improve FTP on low bandwidth links +- MAVLink receiver support (RADIO_RC_CHANNELS mavlink message) +- Message interval supports TERRAIN_REPORT msg +- Mission upload may be cancelled using MISSION_CLEAR_ALL +- MOUNT_CONFIGURE, MOUNT_CONTROL messages deprecated +- RC_CHANNELS_RAW deprecated +- Serial passthrough supports parity allowing STM32CubeProgrammer to be used to program FrSky R9 receivers +- SET_ATTITUDE_TARGET angular rate input frame fixed (Copter only) +- TIMESYNC and NAMED_VALUE_FLOAT messages not sent on high latency MAVLink ports + +14) Logging enhancements and fixes + +- AC_PID resets and I-term sets logged +- ANG provides attitude at high rate (equivalent to ATT) +- ATT logs angles as floats (better resolution than ints) +- CAND message gets driver index +- DCM log message includes roll/pitch and yaw error +- EDT2 log msg includes stress and status received via extended DShot Telemetry v2 +- EFI ECYL cylinder head and exhaust temp logs in degC (was Kelvin) +- ESCX log msg includes DroneCAN ESCs status, temp, duty cycle and power pct +- FMT messages written as required instead of all at beginning +- Logging restarted after download completes when LOG_DISARMED = 1 +- MISE msg logs active mission command (CMD logged when commands are uploaded) +- ORGN message logging fixed when set using SET_GPS_GLOBAL_ORIGIN +- RPM sensor logging gets instance field, quality and health fields +- Short filename support removed (e.g log1.BIN instead of 00000001.BIN) +- Temperature sensor logging option for only sensors with no source (see TEMP_LOG) +- UART data rates logged at 1hz (see UART message) + +15) ROS2 / DDS support + +- Airspeed published +- Battery topic reports all available batteries +- Compile-time configurable rates for each publisher +- DDS_TIMEOUT_MS and DDS_MAX_RETRY set timeout and num retries when client pings XRCE agent +- GPS global origin published at 1 Hz +- High frequency raw imu data transmission +- Joystick support +- Support sending waypoints to Copter and Rover +- Remove the XML refs file in favor of binary entity creation + +16) Safety related enhancements and fixes + +- Accel/Gyro inconsistent message fixed for using with only single IMU +- Battery monitor failure reported to GCS, triggers battery failsafe but does not take action +- Far from EKF origin pre-arm check removed (Copter only) +- Fence breach warning message slightly improved +- Fence enhancements incluiding alt min fence (Copter only, see FENCE_AUTOENABLE, FENCE_OPTIONS) +- Fences can be stored to SD Card (see BRD_SD_FENCE param) +- ICEngine stopped when in E-Stop or safety engaged (Plane only) +- LEDs flash green lights based on EKF location not GPS +- Parachute option to skip disarm before parachute release (see CHUTE_OPTIONS) +- Plane FENCE_AUTOENABLE of 1 or 2 deprecation warning added +- Pre-arm check if OpenDroneID is compiled in but disabled +- Pre-arm check of duplicate RC aux functions fixed (was skipping recently added aux functions) +- Pre-arm checks alert user more quickly on failure +- Prearm check for misconfigured EAHRS_SENSORS and GPS_TYPE +- Proximity sensor based avoidance keeps working even if one proximity sensor fails (Copter, Rover) +- RC aux functions for Arm, Disarm, E-Stop and Parachute ignored when RC is first turned on +- Warning of duplicate SERIALx_PROTOCOL = RCIN + +17) Other bug fixes and minor enhancements + +- Accel cal fixed for auxiliary IMUs (e.g. IMU4 and higher) +- Bootloader fix to reduce unnecessary mass erasing of flash when using STLink or Jlink tools +- Bootloader rejects allocation of broadcast node ID +- CAN forward registering/de-registering fix (affected Mission Planner DroneCAN UI) +- Dijkstras fix to correct use of uninitialised variable +- DShot rates are not limited by NeoPixel rates +- Ethernet and CAN bootloader fix to prevent getting stuck in bootloader mode +- Filesystem does not show entries for empty @ files +- Filesystem efficiency improvements when reading files +- Flight statistics only reset if user sets STAT_RESET to zero (avoids accidental reset) +- Flight time statistics updated on disarm (avoids issue if vehicle powered-down soon after disarm) +- Internal error thrown if we lose parameters due to saving queue being too small +- MAVLink via DroneCAN baud rate fix +- SPI pins may also be used as GPIOs +- Terrain cache size configurable (see TERRAIN_CACHE_SZ) + +18) Developer focused fixes and enhancements + +- AP_Camera gets example python code showing how to use GStreamer with UDP and RTSP video streams +- Cygwin build fix for non-SITL builds +- Cygwin build fixed with malloc wrap +- DroneCAN and scripting support FlexDebug messages (see CAN_Dn_UC_OPTION, FlexDebug.lua) +- EKF3 code generator documentation and cleanup +- GPS jamming simulator added +- MacOS compiler warnings reduced +- SFML joystick support +- SITL support for OpenBSD +- Text warning if older Fence or Rally point protocols are used +------------------------------------------------------------------ Release 4.5.7 08 Oct 2024 Changes from 4.5.7-beta1 From 1fe0273adffdc6ce432e8fac483d396fef3cb71c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 6 Nov 2024 13:10:10 +0900 Subject: [PATCH 101/117] Tracker: 4.6.0-beta1 release notes Co-authored-by: Bill Geyer Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com> Co-authored-by: Thomas Watson Co-authored-by: Andrew Tridgell --- AntennaTracker/ReleaseNotes.txt | 424 ++++++++++++++++++++++++++++++++ 1 file changed, 424 insertions(+) diff --git a/AntennaTracker/ReleaseNotes.txt b/AntennaTracker/ReleaseNotes.txt index 972964d76c3e7..c03c34c3ab4d8 100644 --- a/AntennaTracker/ReleaseNotes.txt +++ b/AntennaTracker/ReleaseNotes.txt @@ -1,5 +1,429 @@ Antenna Tracker Release Notes: ------------------------------------------------------------------ +Release 4.6.0-beta 13 Nov 2024 + +Changes from 4.5.7 + +1) Board specific changes + +- AnyLeaf H7 supports compass and onboard logging +- Blitz743Pro supports CAN +- BlueRobotics Navigator supports BMP390 baro +- Bootloader ECC failure check fixed on boards with >128k bootloader space (e.g CubeRed) +- CB Unmanned Stamp H743 support +- ClearSky CSKY405 support +- CUAV-7-Nano default batt monitor fixed +- CubeRed bootloader fixes including disabling 2nd core by default +- CubeRed supports PPP networking between primary and secondary MCU +- CubeRedPrimary supports external compasses +- ESP32 main loop rate improvements +- ESP32 RC input fixes and wifi connection reliability improved +- ESP32 safety switch and GPIO pin support +- FlyingMoon no longer support MAX7456 +- Flywoo F405HD-AIOv2 ELRS RX pin pulled high during boot +- Flywoo H743 Pro support +- Flywoo/Goku F405 HD 1-2S ELRS AIO v2 +- FlywooF745 supports DPS310 baro +- FPV boards lose SMBus battery support (to save flash) +- GEPRC F745BTHD support +- GEPRCF745BTHD loses parachute support, non-BMP280 baros (to save flash) +- Here4FC bootloader fix for mismatch between RAM0 and periph that could prevent firmware updates +- Holybro Kakute F4 Wing support +- iFlight 2RAW H743 supports onboard logging +- JFB110 supports measuring servo rail voltage +- JFB110 supports safety switch LED +- JHEM-JHEF405 bootloader supports firmware updates via serial +- JHEMCU GF30H743 HD support +- JHEMCU-GF16-F405 autopilot support +- JHEMCU-GSF405A becomes FPV board (to save flash) +- KakuteF7 only supports BMP280 baro (to save flash) +- KakuteH7Mini supports ICM42688 IMU +- Linux auto detection of GPS baud rate fixed +- Linux board scheduler jitter reduced +- Linux board shutdown fixes +- MakeFlyEasy PixPilot-V6Pro support +- MatekF405, Pixhawk1-1M-bdshot, revo-mini loses blended GPS (to save flash) +- MatekH7A3 support Bi-directional DShot +- MicoAir405v2 and MicoAir405Mini support optical flow and OSD +- MicoAir743 internal compass orientation fixed +- MicroAir405Mini, MicroAir743, NxtPX4v2 support +- MicroAir405v2 Bi-directional DShot and LED DMA fixes +- MicroAir405v2 defined as FPV board with reduced features (to save flash) +- ModalAI VOXL2 support including Starling 2 and Starling 2 max +- mRo Control Zero Classic supports servo rail analog input +- mRo KitCAN revC fixed +- Mugin MUPilot support +- OmnibusF7v2 loses quadplane support (to save flash) +- Pixhack-v3 board added (same as fmuv3) +- Pixhawk6C bootloader supports flashing firmware from SD card +- RadiolinkPIX6 imu orientation fixed +- RadiolinkPIX6 supports SPA06 baro +- ReaperF745 V4 FC supports MPU6000 IMU +- RPI5 support +- SDModelH7v2 SERIAL3/7/9_PROTOCOL param defaults changed +- Solo serial ports default to MAVLink1 +- SpeedyBeeF405Wing gets Bi-directional DShot +- SpeedyBeeF405WING loses landing gear support, some camera gimbals (to save flash) +- Spektreworks boom board support +- TrueNavPro-G4 SPI does not share DMA +- X-MAV AP-H743v2 support + +2) AHRS/EKF enhancements and fixes + +- AHRS_OPTION to disable fallback to DCM (affects Plane and Rover, Copter never falls back) +- AHRS_OPTION to disable innovation check for airspeed sensor +- Airspeed sensor health check fixed when using multiple sensors and AHRS affinity +- DCM support for MAV_CMD_EXTERNAL_WIND_ESTIMATE (Plane only) +- EK2 supports disabling external nav (see EK2_OPTIONS) +- EK3 External Nav position jump after switch from Optical flow removed (see EK3_SRC_OPTION=1) +- EK3 uses filtered velocity corrections for IMU position +- EKF2, EKF3, ExternalAHRS all use common origin +- EKF3 accepts set origin even when using GPS +- EKF3 allows earth-frame fields to be estimated with an origin but no GPS +- EKF3 copes better with GPS jamming +- EKF3 logs mag fusion selection to XKFS +- EKF3 wind estimation when using GPS-for-yaw fixed +- External AHRS improvements including handling variances, pre-arm origin check +- Inertial Labs External AHRS fixes +- VectorNav driver fix for handling of error from sensor +- VectorNav External AHRS enhancements including validation of config commands and logging improvements +- VectorNav support for sensors outside VN-100 and VN-300 + +3) Driver enhancements and bug fixes + +- ADSB fix to display last character in status text sent to GCS +- Ainstein LR-D1 radar support +- Airspeed ASP5033 whoami check fixed when autopilot rebooted independently of the sensor +- AIRSPEED message sent to GCS +- Analog temperature sensor extended to 5th order polynomial (see TEMP_A5) +- ARSPD_OPTIONS to report calibration offset to GCS +- Baro EAS2TAS conversions continuously calculated reducing shocks to TECS (Plane only) +- Baro provides improved atmospheric model for high altitude flight +- BARO_ALT_OFFSET slew slowed to keep EKF happy +- BATTx_ESC_MASK param supports flexible mapping of ESCs to batteries +- BATTx_OPTION to not send battery voltage, current, etc to GCS +- Benewake RDS02U radar support +- Bi-directional DShot on IOMCU supports reversible mask +- Bi-directional DShot telemetry support on F103 8Mhz IOMCUs +- BMM350 compass support +- CAN rangefinders and proximity sensors may share a CAN bus (allows NRA24 and MR72 on a single CAN bus) +- Compass calibration world magnetic model checks can use any position source (e.g. not just GPS) +- CRSF baro and vertical speeed fixed +- CRSF RX bind command support +- DroneCAN battery monitor check to avoid memory corruption when type changed +- DroneCAN DNA server fixes including removing use of invalid node IDs, faster ID allocation, elimination of rare inability to save info +- DroneCAN EFI health check fix +- DroneCAN ESC battery monitors calculate consumed mah +- DroneCAN ESCs forced to zero when disarmed +- DroneCAN RPM message support +- DroneCAN timeout fix for auxiliary frames +- DroneCAN to serial tunneling params accepts short-hand baud rates (e.g. '57' for '57600') +- F9P, F10-N and Zed-F9P support for GPSx_GNSS_MODE to turn on/off SBAS, Galileo, Beidou and Glonass +- FuelLevel battery monitor fix to report capacity +- GPS_xxx params renamed to GPS1_xxx, GPS_xxx2 renamed to GPS2_xxx +- Hirth EFI logging includes modified throttle +- Hirth ICEngine supports reversed crank direction (see ICE_OPTIONS parameter) +- Hott and LTM telemetry deprecated (still available through custom build server) +- i-BUS telemetry support +- ICE_PWM_IGN_ON, ICE_PWM_IGN_OFF, ICE_PWM_STRT_ON, ICE_PWM_STRT_OFF replaced with SERVOx_MIN/MAX/REVERSED (Plane only) +- ICE_START_CHAN replaced with RC aux function (Plane only) +- ICEngine retry max added (see ICE_STRT_MX_RTRY) +- IE 2400 generator error message severity to GCS improved +- INA2xx battery monitor support (reads temp, gets MAX_AMPS and SHUNT params) +- MCP9600 temperature sensor I2C address fixed +- MLX90614 temperature sensor support +- MSP GPS ground course scaling fixed +- MSP RC uses scaled RC inputs (fixes issue with RCx_REVERSED having no effect) +- Networking supports reconnection to TCP server or client +- OSD params for adjusting horizontal spacing and vertical extension (see OSD_SB_H_OFS, OSD_SB_V_EXT) +- Relay inverted output support (see RELAYx_INVERTED parameter) +- ROMFS efficiency improvements +- RS-485 driver enable RTS flow control +- Sagetech MXS ADSP altitude fix (now uses abs alt instead of terrain alt) +- Septentrio GPS sat count correctly drops to zero when 255 received +- Septentrio supports selecting constellations (see GPS_GNSS_MODE) +- Single LED for user notification supported +- SPA06 baro supported +- Sum battery monitor optionally reports minimum voltage instead of average +- Sum battery monitor reports average temp +- Torqeedo dual motor support (see TRQ1, TRQ2 params) +- Ublox GPS driver uses 64 bit time for PPS interrupt (avoids possible dropout at 1hour and 12 minutes) +- UBlox GPS time ignored until at least 2D fix +- VideoTX supports additional freq bands (RushFPV 3.3Ghz) +- Volz logs desired and actual position, voltage, current, motor and PCB temp +- Volz server feedback and logging fixed +- Volz servo output in its own thread resulting in smoother movements +- W25N02KV flash support + +4) Networking enhancements and fixes + +- Allow multiple UDP clients to connect/disconnect/reconnect +- Ethernet supports faster log downloading (raised SDMMC clock limit on H7) + +5) Camera and gimbal enhancements + +- Alexmos precision improved slightly +- CAMERA_CAPTURE_STATUS mavlink msg sent to GCS (reports when images taken or video recorded, used by QGC) +- CAMERA_FOV_STATUS mavlink reports lat,lon of what camera is pointing at +- DO_MOUNT_CONTROL yaw angle interpreted as body-frame (was incorrectly being interpreted as earth-frame) +- Dual serial camera gimbal mounts fixed +- Lua script bindings to send CAMERA_INFORMATION and VIDEO_STREAM_INFORMATION messages to GCS +- Retract Mount2 aux function added (see RCx_OPTION = 113) +- Servo gimbal reported angles respect roll, pitch and yaw limits +- Siyi driver sends autopilot location and speed (recorded in images via EXIF) +- Siyi picture and video download scripts +- Siyi ZT6 and ZT30 support sending min, max temperature (see CAMERA_THERMAL_RANGE msg) +- Siyi ZT6 and ZT30 thermal palette can be changed using camera-change-setting.lua script +- Siyi ZT6 hardware id and set-lens fixed +- Topotek gimbal support +- Trigger distance ignores GPS status and only uses EKF reported location + +6) Harmonic notch enhancements + +- Harmonic notch is active in forward flight on quadplanes +- Harmonic notch filter freq clamping and disabling reworked +- Harmonic notch handles negative ESC RPMs +- Harmonic notch supports per-motor throttle-based harmonic notch + +7) Copter specific enhancements and bug fixes + +- Attitude control fix to dt update order (reduces rate controller noise) +- Auto mode fix to avoid prematurely advancing to next waypoint if given enough time +- Auto mode small target position jump when takeoff completes removed +- Auto mode yaw drift when passing WP removed if CONDITION_YAW command used and WP_YAW_BEHAVIOR = 0/None +- Auto, Guided flight mode pause RC aux switch (see RCx_OPTION = 178) +- AutoRTL (e.g. DO_LAND_START) uses copter stopping point to decide nearest mission command +- AutoRTL mode supports DO_RETURN_PATH_START (Copter, Heli only) +- AutoTune fix to prevent spool up after landing +- AutoTune performance and safety enhancements (less chance of ESC desync, fails when vehicle likely can't be tuned well) +- Autotune test gains RC aux switch function allows testing gains in any mode (see RCx_OPTION = 180) +- Config error avoided if auto mode is paused very soon after poweron +- FLIGHT_OPTIONS bit added to require position estimate before arming +- Follow mode slowdown calcs fixed when target is moving +- Ground oscillation suppressed by reducing gains (see ATC_LAND_R/P/Y_MULT) +- Guided mode internal error fix when taking off using SET_ATTITUDE_CONTROL message +- Guided mode internal error resolved when switching between thrust or climb rate based altitude control +- Guided mode yaw fixed when WP_YAW_BEHAVIOR = 0/None and CONDITION_YAW command received containing relative angle +- Landing detector fixed when in stabilize mode at full throttle but aircraft is upside down +- Landing detector logging added to ease support (see LDET message) +- Loiter unlimited command accepts NaNs (QGC sends these) +- Mavlink SYSTEM_STATUS set to BOOT during initialisation +- MOT_PWM_TYPE of 9 (PWMAngle) respects SERVOx_MIN/MAX/TRIM/REVERSED param values +- Payload place bug fix when aborted because gripper is already released +- RC based tuning (aka CH6 tuning) can use any RC aux function channel (see RCx_OPTION = 219) +- RTL_ALT minimum reduced to 30cm +- SystemID position controller support (Copter and Heli) +- TriCopter motor test and slew-up fixed +- WPNAV_SPEED min reduced to 10 cm/s (Copter only) +- Loiter mode zeros desired accel when re-entering from Auto during RC failsafe + +8) TradHeli specific enhancements + +- Autorotation yaw behaviour fix +- Autotune improvements including using low frequency dwell for feedforward gain tuning and conducting sweep in attitude instead of rate +- Blade pitch angle logging added (see SWSH log message) +- Constrain cyclic roll for intermeshing +- ICE / turbine cool down fix +- Inverted flight extended to non manual throttle modes +- Inverted flight transitions smoothed and restricted to only Stabilize mode +- SWSH logging fix for reversed collectives + +9) Plane specific enhancements and bug fixes + +- AIRSPEED_STALL holds vehicle stall speed and is used for minimum speed scaling +- Allow for different orientations of landing rangefinder +- Assistance requirements evaluted on mode change +- FBWB/CRUISE climb/sink rate limited by TECS limits +- FLIGHT_OPTION to immediately climb in AUTO mode (not doing glide slope) +- Glider pullup support (available only through custom build server) +- Loiter breakout improved to better handle destinations inside loiter circle +- Manual mode throttle made consistent with other modes (e.g battery comp and watt limit is done if enabled) +- Mavlink GUIDED_CHANGE_ALTITUDE supports terrain altitudes +- Minimum throttle not applied during SLT VTOL airbrake (reduces increase in airspeed and alt during back transition) +- Q_APPROACH_DIST set minimum distance to use the fixed wing approach logic +- Quadplane assistance check enhancements +- Quadplane Deca frame support +- Quadplane gets smoother takeoff by input shaping target accel and velocity +- Servo wiggles in altitude wait staged to be one after another +- Set_position_target_global_int accepts MAV_FRAME_GLOBAL_RELATIVE_ALT and MAV_FRAME_GLOBAL_TERRAIN_ALT frames +- Takeoff airspeed control improved (see TKOFF_MODE, TKOFF_THR_MIN) +- Takeoff fixes for fence autoenable +- Takeoff improvements including less overshoot of TKOFF_ALT +- TECS reset along with other controllers (important if plane dropped from balloon) +- Tilt quadplane ramp of motors on back transition fixed +- Tiltrotor tilt angles logged +- TKOFF_THR_MIN applied to SLT transitions +- Twin motor planes with DroneCAN ESCs fix to avoid max throttle at arming due to misconfiguration +- VTOLs switch to QLAND if a LONG_FAILSAFE is triggered during takeoff + +10) Rover specific enhancements and bug fixes + +- Auto mode reversed state maintained if momentarily switched to Hold mode +- Circle mode tracks better and avoids getting stuck at circle edge +- Flight time stats fixed +- MAV_CMD_NAV_SET_YAW_SPEED deprecated +- Omni3Mecanum frame support +- Stopping point uses max deceleration (was incorrectly using acceleration) +- Wheel rate controller slew rate fix + +11) Antenna Tracker specific enhancements and bug fixes + +- Never track lat,lon of 0,0 + +12) Scripting enhancements + +- advance-wp.lua applet supports advancing Auto mode WP via RC switch +- AHRS_switch.lua supports switching between EKF2 and EKF3 via RC switch +- battery_internal_resistance_check.lua monitors battery resistance +- CAN:get_device returns nil for unconfigured CAN device +- copter_terrain_brake.lua script added to prevent impact with terrain in Loiter mode (Copter only) +- Copter:get_target_location, update_target_location support +- crosstrack_restore.lua example allows returning to previous track in Auto (Plane only) +- Display text on OLED display supported +- Docs improved for many bindings +- EFI get_last_update_ms binding +- EFI_SkyPower.lua driver accepts 2nd supply voltage +- ESC_slew_rate.lua example script supports testing ESCs +- Filesystem CRC32 check to allow scripts to check module versions +- forced arming support +- GPIO get/set mode bindings (see gpio:set_mode, get_mode) +- GPS-for-yaw angle binding (see gps:gps_yaw_deg) +- Halo6000 EFI driver can log all CAN packets for easier debugging +- handle_external_position_estimate binding allows sending position estimate from lua to EKF +- I2C:transfer support +- IMU gyros_consistent and accels_consistent bindings added +- INF_Inject.lua driver more robust to serial errors, improved logging, throttle and ignition control +- INS bindings for is calibrating, gyro and accel sensor values +- IPV4 address bindings (see SocketAPM_ipv4_addr_to_string) to allow UDP server that responds to individual clients +- Logging of booleans supported +- Lua language checks improved (finds more errors) +- MAVLink commands can be called from scripting +- MCU voltage binding (see analog:mcu_voltage) +- NMEA 2000 EFI driver (see EFI_NMEA2k.lua) +- "open directory failed" false warning when scripts in ROMFS fixed +- Param_Controller.lua supports quickly switching between 3 parameter sets via RC switch +- Pass by reference values are always initialized +- pelco_d_antennatracker.lua applet supports sending Pelco-D via RS-485 to PTZ cameras +- plane_aerobatics.lua minor enhancements +- REPL applet (read-evaluate-print-loop, see repl.lua) for interactive testing and experimentation +- "require" function failures in rare circumstances fixed +- "require" function works for modules in ROMFS (e.g. not on SD card) +- revert_param.lua supports more params (e.g ATC_RATE_R/P/Y, PTCH2SRV_TCONST, RLL2SRV_TCONST, TECS_) +- Scripts may receive mavlink messages which fail CRC (e.g messages which FC does not understand) +- SD card formatting supported +- Serial device simulation support (allows Lua to feed data to any supported serial protocol for e.g. sensor simulation) +- set_target_throttle_rate_rpy allows rate control from scripts (new for Copter) +- sitl_standby_sim.lua example shows how to switch between redundant flight controllers using an RC switch +- Slung payload oscillation suppression applet added (see copter-slung-payload.lua) +- Temperature sensor bindings added +- uint64 support +- Various performance and memory usage optimizations +- VTOL-quicktune.lua minor enhancements including increasing YAW_FLTE_MAX to 8 +- x-quad-cg-allocation.lua applet corrects for the CoM discrepancy in a quad-X frame + +13) GCS / mavlink related changes and fixes + +- BATTERY2 message deprecated (replaced by BATTERY_STATUS) +- CMD_MISSION_START/STOP rejected if first-item/last-item args provided +- Deny attempts to upload two missions simultaneously +- Fence and Rally points may be uploaded using FTP +- GPS_INPUT and HIL_GPS handles multiple GPSs +- HIGHRES_IMU mavlink message support (used by companion computers to receive IMU data from FC) +- MAV_CMD_COMPONENT_ARM_DISARM accepts force arm magic value of 21196 +- MAV_CMD_DO_SET_SAFETY_SWITCH_STATE support +- MAV_CMD_SET_HAGL support (Plane only) +- MAVFTP respects TX buffer flow control to improve FTP on low bandwidth links +- MAVLink receiver support (RADIO_RC_CHANNELS mavlink message) +- Message interval supports TERRAIN_REPORT msg +- Mission upload may be cancelled using MISSION_CLEAR_ALL +- MOUNT_CONFIGURE, MOUNT_CONTROL messages deprecated +- RC_CHANNELS_RAW deprecated +- Serial passthrough supports parity allowing STM32CubeProgrammer to be used to program FrSky R9 receivers +- SET_ATTITUDE_TARGET angular rate input frame fixed (Copter only) +- TIMESYNC and NAMED_VALUE_FLOAT messages not sent on high latency MAVLink ports + +14) Logging enhancements and fixes + +- AC_PID resets and I-term sets logged +- ANG provides attitude at high rate (equivalent to ATT) +- ATT logs angles as floats (better resolution than ints) +- CAND message gets driver index +- DCM log message includes roll/pitch and yaw error +- EDT2 log msg includes stress and status received via extended DShot Telemetry v2 +- EFI ECYL cylinder head and exhaust temp logs in degC (was Kelvin) +- ESCX log msg includes DroneCAN ESCs status, temp, duty cycle and power pct +- FMT messages written as required instead of all at beginning +- Logging restarted after download completes when LOG_DISARMED = 1 +- MISE msg logs active mission command (CMD logged when commands are uploaded) +- ORGN message logging fixed when set using SET_GPS_GLOBAL_ORIGIN +- RPM sensor logging gets instance field, quality and health fields +- Short filename support removed (e.g log1.BIN instead of 00000001.BIN) +- Temperature sensor logging option for only sensors with no source (see TEMP_LOG) +- UART data rates logged at 1hz (see UART message) + +15) ROS2 / DDS support + +- Airspeed published +- Battery topic reports all available batteries +- Compile-time configurable rates for each publisher +- DDS_TIMEOUT_MS and DDS_MAX_RETRY set timeout and num retries when client pings XRCE agent +- GPS global origin published at 1 Hz +- High frequency raw imu data transmission +- Joystick support +- Support sending waypoints to Copter and Rover +- Remove the XML refs file in favor of binary entity creation + +16) Safety related enhancements and fixes + +- Accel/Gyro inconsistent message fixed for using with only single IMU +- Battery monitor failure reported to GCS, triggers battery failsafe but does not take action +- Far from EKF origin pre-arm check removed (Copter only) +- Fence breach warning message slightly improved +- Fence enhancements incluiding alt min fence (Copter only, see FENCE_AUTOENABLE, FENCE_OPTIONS) +- Fences can be stored to SD Card (see BRD_SD_FENCE param) +- ICEngine stopped when in E-Stop or safety engaged (Plane only) +- LEDs flash green lights based on EKF location not GPS +- Parachute option to skip disarm before parachute release (see CHUTE_OPTIONS) +- Plane FENCE_AUTOENABLE of 1 or 2 deprecation warning added +- Pre-arm check if OpenDroneID is compiled in but disabled +- Pre-arm check of duplicate RC aux functions fixed (was skipping recently added aux functions) +- Pre-arm checks alert user more quickly on failure +- Prearm check for misconfigured EAHRS_SENSORS and GPS_TYPE +- Proximity sensor based avoidance keeps working even if one proximity sensor fails (Copter, Rover) +- RC aux functions for Arm, Disarm, E-Stop and Parachute ignored when RC is first turned on +- Warning of duplicate SERIALx_PROTOCOL = RCIN + +17) Other bug fixes and minor enhancements + +- Accel cal fixed for auxiliary IMUs (e.g. IMU4 and higher) +- Bootloader fix to reduce unnecessary mass erasing of flash when using STLink or Jlink tools +- Bootloader rejects allocation of broadcast node ID +- CAN forward registering/de-registering fix (affected Mission Planner DroneCAN UI) +- Dijkstras fix to correct use of uninitialised variable +- DShot rates are not limited by NeoPixel rates +- Ethernet and CAN bootloader fix to prevent getting stuck in bootloader mode +- Filesystem does not show entries for empty @ files +- Filesystem efficiency improvements when reading files +- Flight statistics only reset if user sets STAT_RESET to zero (avoids accidental reset) +- Flight time statistics updated on disarm (avoids issue if vehicle powered-down soon after disarm) +- Internal error thrown if we lose parameters due to saving queue being too small +- MAVLink via DroneCAN baud rate fix +- SPI pins may also be used as GPIOs +- Terrain cache size configurable (see TERRAIN_CACHE_SZ) + +18) Developer focused fixes and enhancements + +- AP_Camera gets example python code showing how to use GStreamer with UDP and RTSP video streams +- Cygwin build fix for non-SITL builds +- Cygwin build fixed with malloc wrap +- DroneCAN and scripting support FlexDebug messages (see CAN_Dn_UC_OPTION, FlexDebug.lua) +- EKF3 code generator documentation and cleanup +- GPS jamming simulator added +- MacOS compiler warnings reduced +- SFML joystick support +- SITL support for OpenBSD +- Text warning if older Fence or Rally point protocols are used +------------------------------------------------------------------ Release 4.5.7 08 Oct 2024 Changes from 4.5.7-beta1 From 1e261136102bc98131d6f62ac5514cc7c42be6c5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 6 Nov 2024 13:08:55 +0900 Subject: [PATCH 102/117] Rover: 4.6.0-beta1 release notes Co-authored-by: Bill Geyer Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com> Co-authored-by: Thomas Watson Co-authored-by: Andrew Tridgell --- Rover/ReleaseNotes.txt | 424 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 424 insertions(+) diff --git a/Rover/ReleaseNotes.txt b/Rover/ReleaseNotes.txt index 792f86077d3b0..9722121b97a89 100644 --- a/Rover/ReleaseNotes.txt +++ b/Rover/ReleaseNotes.txt @@ -1,5 +1,429 @@ Rover Release Notes: ------------------------------------------------------------------ +Release 4.6.0-beta 13 Nov 2024 + +Changes from 4.5.7 + +1) Board specific changes + +- AnyLeaf H7 supports compass and onboard logging +- Blitz743Pro supports CAN +- BlueRobotics Navigator supports BMP390 baro +- Bootloader ECC failure check fixed on boards with >128k bootloader space (e.g CubeRed) +- CB Unmanned Stamp H743 support +- ClearSky CSKY405 support +- CUAV-7-Nano default batt monitor fixed +- CubeRed bootloader fixes including disabling 2nd core by default +- CubeRed supports PPP networking between primary and secondary MCU +- CubeRedPrimary supports external compasses +- ESP32 main loop rate improvements +- ESP32 RC input fixes and wifi connection reliability improved +- ESP32 safety switch and GPIO pin support +- FlyingMoon no longer support MAX7456 +- Flywoo F405HD-AIOv2 ELRS RX pin pulled high during boot +- Flywoo H743 Pro support +- Flywoo/Goku F405 HD 1-2S ELRS AIO v2 +- FlywooF745 supports DPS310 baro +- FPV boards lose SMBus battery support (to save flash) +- GEPRC F745BTHD support +- GEPRCF745BTHD loses parachute support, non-BMP280 baros (to save flash) +- Here4FC bootloader fix for mismatch between RAM0 and periph that could prevent firmware updates +- Holybro Kakute F4 Wing support +- iFlight 2RAW H743 supports onboard logging +- JFB110 supports measuring servo rail voltage +- JFB110 supports safety switch LED +- JHEM-JHEF405 bootloader supports firmware updates via serial +- JHEMCU GF30H743 HD support +- JHEMCU-GF16-F405 autopilot support +- JHEMCU-GSF405A becomes FPV board (to save flash) +- KakuteF7 only supports BMP280 baro (to save flash) +- KakuteH7Mini supports ICM42688 IMU +- Linux auto detection of GPS baud rate fixed +- Linux board scheduler jitter reduced +- Linux board shutdown fixes +- MakeFlyEasy PixPilot-V6Pro support +- MatekF405, Pixhawk1-1M-bdshot, revo-mini loses blended GPS (to save flash) +- MatekH7A3 support Bi-directional DShot +- MicoAir405v2 and MicoAir405Mini support optical flow and OSD +- MicoAir743 internal compass orientation fixed +- MicroAir405Mini, MicroAir743, NxtPX4v2 support +- MicroAir405v2 Bi-directional DShot and LED DMA fixes +- MicroAir405v2 defined as FPV board with reduced features (to save flash) +- ModalAI VOXL2 support including Starling 2 and Starling 2 max +- mRo Control Zero Classic supports servo rail analog input +- mRo KitCAN revC fixed +- Mugin MUPilot support +- OmnibusF7v2 loses quadplane support (to save flash) +- Pixhack-v3 board added (same as fmuv3) +- Pixhawk6C bootloader supports flashing firmware from SD card +- RadiolinkPIX6 imu orientation fixed +- RadiolinkPIX6 supports SPA06 baro +- ReaperF745 V4 FC supports MPU6000 IMU +- RPI5 support +- SDModelH7v2 SERIAL3/7/9_PROTOCOL param defaults changed +- Solo serial ports default to MAVLink1 +- SpeedyBeeF405Wing gets Bi-directional DShot +- SpeedyBeeF405WING loses landing gear support, some camera gimbals (to save flash) +- Spektreworks boom board support +- TrueNavPro-G4 SPI does not share DMA +- X-MAV AP-H743v2 support + +2) AHRS/EKF enhancements and fixes + +- AHRS_OPTION to disable fallback to DCM (affects Plane and Rover, Copter never falls back) +- AHRS_OPTION to disable innovation check for airspeed sensor +- Airspeed sensor health check fixed when using multiple sensors and AHRS affinity +- DCM support for MAV_CMD_EXTERNAL_WIND_ESTIMATE (Plane only) +- EK2 supports disabling external nav (see EK2_OPTIONS) +- EK3 External Nav position jump after switch from Optical flow removed (see EK3_SRC_OPTION=1) +- EK3 uses filtered velocity corrections for IMU position +- EKF2, EKF3, ExternalAHRS all use common origin +- EKF3 accepts set origin even when using GPS +- EKF3 allows earth-frame fields to be estimated with an origin but no GPS +- EKF3 copes better with GPS jamming +- EKF3 logs mag fusion selection to XKFS +- EKF3 wind estimation when using GPS-for-yaw fixed +- External AHRS improvements including handling variances, pre-arm origin check +- Inertial Labs External AHRS fixes +- VectorNav driver fix for handling of error from sensor +- VectorNav External AHRS enhancements including validation of config commands and logging improvements +- VectorNav support for sensors outside VN-100 and VN-300 + +3) Driver enhancements and bug fixes + +- ADSB fix to display last character in status text sent to GCS +- Ainstein LR-D1 radar support +- Airspeed ASP5033 whoami check fixed when autopilot rebooted independently of the sensor +- AIRSPEED message sent to GCS +- Analog temperature sensor extended to 5th order polynomial (see TEMP_A5) +- ARSPD_OPTIONS to report calibration offset to GCS +- Baro EAS2TAS conversions continuously calculated reducing shocks to TECS (Plane only) +- Baro provides improved atmospheric model for high altitude flight +- BARO_ALT_OFFSET slew slowed to keep EKF happy +- BATTx_ESC_MASK param supports flexible mapping of ESCs to batteries +- BATTx_OPTION to not send battery voltage, current, etc to GCS +- Benewake RDS02U radar support +- Bi-directional DShot on IOMCU supports reversible mask +- Bi-directional DShot telemetry support on F103 8Mhz IOMCUs +- BMM350 compass support +- CAN rangefinders and proximity sensors may share a CAN bus (allows NRA24 and MR72 on a single CAN bus) +- Compass calibration world magnetic model checks can use any position source (e.g. not just GPS) +- CRSF baro and vertical speeed fixed +- CRSF RX bind command support +- DroneCAN battery monitor check to avoid memory corruption when type changed +- DroneCAN DNA server fixes including removing use of invalid node IDs, faster ID allocation, elimination of rare inability to save info +- DroneCAN EFI health check fix +- DroneCAN ESC battery monitors calculate consumed mah +- DroneCAN ESCs forced to zero when disarmed +- DroneCAN RPM message support +- DroneCAN timeout fix for auxiliary frames +- DroneCAN to serial tunneling params accepts short-hand baud rates (e.g. '57' for '57600') +- F9P, F10-N and Zed-F9P support for GPSx_GNSS_MODE to turn on/off SBAS, Galileo, Beidou and Glonass +- FuelLevel battery monitor fix to report capacity +- GPS_xxx params renamed to GPS1_xxx, GPS_xxx2 renamed to GPS2_xxx +- Hirth EFI logging includes modified throttle +- Hirth ICEngine supports reversed crank direction (see ICE_OPTIONS parameter) +- Hott and LTM telemetry deprecated (still available through custom build server) +- i-BUS telemetry support +- ICE_PWM_IGN_ON, ICE_PWM_IGN_OFF, ICE_PWM_STRT_ON, ICE_PWM_STRT_OFF replaced with SERVOx_MIN/MAX/REVERSED (Plane only) +- ICE_START_CHAN replaced with RC aux function (Plane only) +- ICEngine retry max added (see ICE_STRT_MX_RTRY) +- IE 2400 generator error message severity to GCS improved +- INA2xx battery monitor support (reads temp, gets MAX_AMPS and SHUNT params) +- MCP9600 temperature sensor I2C address fixed +- MLX90614 temperature sensor support +- MSP GPS ground course scaling fixed +- MSP RC uses scaled RC inputs (fixes issue with RCx_REVERSED having no effect) +- Networking supports reconnection to TCP server or client +- OSD params for adjusting horizontal spacing and vertical extension (see OSD_SB_H_OFS, OSD_SB_V_EXT) +- Relay inverted output support (see RELAYx_INVERTED parameter) +- ROMFS efficiency improvements +- RS-485 driver enable RTS flow control +- Sagetech MXS ADSP altitude fix (now uses abs alt instead of terrain alt) +- Septentrio GPS sat count correctly drops to zero when 255 received +- Septentrio supports selecting constellations (see GPS_GNSS_MODE) +- Single LED for user notification supported +- SPA06 baro supported +- Sum battery monitor optionally reports minimum voltage instead of average +- Sum battery monitor reports average temp +- Torqeedo dual motor support (see TRQ1, TRQ2 params) +- Ublox GPS driver uses 64 bit time for PPS interrupt (avoids possible dropout at 1hour and 12 minutes) +- UBlox GPS time ignored until at least 2D fix +- VideoTX supports additional freq bands (RushFPV 3.3Ghz) +- Volz logs desired and actual position, voltage, current, motor and PCB temp +- Volz server feedback and logging fixed +- Volz servo output in its own thread resulting in smoother movements +- W25N02KV flash support + +4) Networking enhancements and fixes + +- Allow multiple UDP clients to connect/disconnect/reconnect +- Ethernet supports faster log downloading (raised SDMMC clock limit on H7) + +5) Camera and gimbal enhancements + +- Alexmos precision improved slightly +- CAMERA_CAPTURE_STATUS mavlink msg sent to GCS (reports when images taken or video recorded, used by QGC) +- CAMERA_FOV_STATUS mavlink reports lat,lon of what camera is pointing at +- DO_MOUNT_CONTROL yaw angle interpreted as body-frame (was incorrectly being interpreted as earth-frame) +- Dual serial camera gimbal mounts fixed +- Lua script bindings to send CAMERA_INFORMATION and VIDEO_STREAM_INFORMATION messages to GCS +- Retract Mount2 aux function added (see RCx_OPTION = 113) +- Servo gimbal reported angles respect roll, pitch and yaw limits +- Siyi driver sends autopilot location and speed (recorded in images via EXIF) +- Siyi picture and video download scripts +- Siyi ZT6 and ZT30 support sending min, max temperature (see CAMERA_THERMAL_RANGE msg) +- Siyi ZT6 and ZT30 thermal palette can be changed using camera-change-setting.lua script +- Siyi ZT6 hardware id and set-lens fixed +- Topotek gimbal support +- Trigger distance ignores GPS status and only uses EKF reported location + +6) Harmonic notch enhancements + +- Harmonic notch is active in forward flight on quadplanes +- Harmonic notch filter freq clamping and disabling reworked +- Harmonic notch handles negative ESC RPMs +- Harmonic notch supports per-motor throttle-based harmonic notch + +7) Copter specific enhancements and bug fixes + +- Attitude control fix to dt update order (reduces rate controller noise) +- Auto mode fix to avoid prematurely advancing to next waypoint if given enough time +- Auto mode small target position jump when takeoff completes removed +- Auto mode yaw drift when passing WP removed if CONDITION_YAW command used and WP_YAW_BEHAVIOR = 0/None +- Auto, Guided flight mode pause RC aux switch (see RCx_OPTION = 178) +- AutoRTL (e.g. DO_LAND_START) uses copter stopping point to decide nearest mission command +- AutoRTL mode supports DO_RETURN_PATH_START (Copter, Heli only) +- AutoTune fix to prevent spool up after landing +- AutoTune performance and safety enhancements (less chance of ESC desync, fails when vehicle likely can't be tuned well) +- Autotune test gains RC aux switch function allows testing gains in any mode (see RCx_OPTION = 180) +- Config error avoided if auto mode is paused very soon after poweron +- FLIGHT_OPTIONS bit added to require position estimate before arming +- Follow mode slowdown calcs fixed when target is moving +- Ground oscillation suppressed by reducing gains (see ATC_LAND_R/P/Y_MULT) +- Guided mode internal error fix when taking off using SET_ATTITUDE_CONTROL message +- Guided mode internal error resolved when switching between thrust or climb rate based altitude control +- Guided mode yaw fixed when WP_YAW_BEHAVIOR = 0/None and CONDITION_YAW command received containing relative angle +- Landing detector fixed when in stabilize mode at full throttle but aircraft is upside down +- Landing detector logging added to ease support (see LDET message) +- Loiter unlimited command accepts NaNs (QGC sends these) +- Mavlink SYSTEM_STATUS set to BOOT during initialisation +- MOT_PWM_TYPE of 9 (PWMAngle) respects SERVOx_MIN/MAX/TRIM/REVERSED param values +- Payload place bug fix when aborted because gripper is already released +- RC based tuning (aka CH6 tuning) can use any RC aux function channel (see RCx_OPTION = 219) +- RTL_ALT minimum reduced to 30cm +- SystemID position controller support (Copter and Heli) +- TriCopter motor test and slew-up fixed +- WPNAV_SPEED min reduced to 10 cm/s (Copter only) +- Loiter mode zeros desired accel when re-entering from Auto during RC failsafe + +8) TradHeli specific enhancements + +- Autorotation yaw behaviour fix +- Autotune improvements including using low frequency dwell for feedforward gain tuning and conducting sweep in attitude instead of rate +- Blade pitch angle logging added (see SWSH log message) +- Constrain cyclic roll for intermeshing +- ICE / turbine cool down fix +- Inverted flight extended to non manual throttle modes +- Inverted flight transitions smoothed and restricted to only Stabilize mode +- SWSH logging fix for reversed collectives + +9) Plane specific enhancements and bug fixes + +- AIRSPEED_STALL holds vehicle stall speed and is used for minimum speed scaling +- Allow for different orientations of landing rangefinder +- Assistance requirements evaluted on mode change +- FBWB/CRUISE climb/sink rate limited by TECS limits +- FLIGHT_OPTION to immediately climb in AUTO mode (not doing glide slope) +- Glider pullup support (available only through custom build server) +- Loiter breakout improved to better handle destinations inside loiter circle +- Manual mode throttle made consistent with other modes (e.g battery comp and watt limit is done if enabled) +- Mavlink GUIDED_CHANGE_ALTITUDE supports terrain altitudes +- Minimum throttle not applied during SLT VTOL airbrake (reduces increase in airspeed and alt during back transition) +- Q_APPROACH_DIST set minimum distance to use the fixed wing approach logic +- Quadplane assistance check enhancements +- Quadplane Deca frame support +- Quadplane gets smoother takeoff by input shaping target accel and velocity +- Servo wiggles in altitude wait staged to be one after another +- Set_position_target_global_int accepts MAV_FRAME_GLOBAL_RELATIVE_ALT and MAV_FRAME_GLOBAL_TERRAIN_ALT frames +- Takeoff airspeed control improved (see TKOFF_MODE, TKOFF_THR_MIN) +- Takeoff fixes for fence autoenable +- Takeoff improvements including less overshoot of TKOFF_ALT +- TECS reset along with other controllers (important if plane dropped from balloon) +- Tilt quadplane ramp of motors on back transition fixed +- Tiltrotor tilt angles logged +- TKOFF_THR_MIN applied to SLT transitions +- Twin motor planes with DroneCAN ESCs fix to avoid max throttle at arming due to misconfiguration +- VTOLs switch to QLAND if a LONG_FAILSAFE is triggered during takeoff + +10) Rover specific enhancements and bug fixes + +- Auto mode reversed state maintained if momentarily switched to Hold mode +- Circle mode tracks better and avoids getting stuck at circle edge +- Flight time stats fixed +- MAV_CMD_NAV_SET_YAW_SPEED deprecated +- Omni3Mecanum frame support +- Stopping point uses max deceleration (was incorrectly using acceleration) +- Wheel rate controller slew rate fix + +11) Antenna Tracker specific enhancements and bug fixes + +- Never track lat,lon of 0,0 + +12) Scripting enhancements + +- advance-wp.lua applet supports advancing Auto mode WP via RC switch +- AHRS_switch.lua supports switching between EKF2 and EKF3 via RC switch +- battery_internal_resistance_check.lua monitors battery resistance +- CAN:get_device returns nil for unconfigured CAN device +- copter_terrain_brake.lua script added to prevent impact with terrain in Loiter mode (Copter only) +- Copter:get_target_location, update_target_location support +- crosstrack_restore.lua example allows returning to previous track in Auto (Plane only) +- Display text on OLED display supported +- Docs improved for many bindings +- EFI get_last_update_ms binding +- EFI_SkyPower.lua driver accepts 2nd supply voltage +- ESC_slew_rate.lua example script supports testing ESCs +- Filesystem CRC32 check to allow scripts to check module versions +- forced arming support +- GPIO get/set mode bindings (see gpio:set_mode, get_mode) +- GPS-for-yaw angle binding (see gps:gps_yaw_deg) +- Halo6000 EFI driver can log all CAN packets for easier debugging +- handle_external_position_estimate binding allows sending position estimate from lua to EKF +- I2C:transfer support +- IMU gyros_consistent and accels_consistent bindings added +- INF_Inject.lua driver more robust to serial errors, improved logging, throttle and ignition control +- INS bindings for is calibrating, gyro and accel sensor values +- IPV4 address bindings (see SocketAPM_ipv4_addr_to_string) to allow UDP server that responds to individual clients +- Logging of booleans supported +- Lua language checks improved (finds more errors) +- MAVLink commands can be called from scripting +- MCU voltage binding (see analog:mcu_voltage) +- NMEA 2000 EFI driver (see EFI_NMEA2k.lua) +- "open directory failed" false warning when scripts in ROMFS fixed +- Param_Controller.lua supports quickly switching between 3 parameter sets via RC switch +- Pass by reference values are always initialized +- pelco_d_antennatracker.lua applet supports sending Pelco-D via RS-485 to PTZ cameras +- plane_aerobatics.lua minor enhancements +- REPL applet (read-evaluate-print-loop, see repl.lua) for interactive testing and experimentation +- "require" function failures in rare circumstances fixed +- "require" function works for modules in ROMFS (e.g. not on SD card) +- revert_param.lua supports more params (e.g ATC_RATE_R/P/Y, PTCH2SRV_TCONST, RLL2SRV_TCONST, TECS_) +- Scripts may receive mavlink messages which fail CRC (e.g messages which FC does not understand) +- SD card formatting supported +- Serial device simulation support (allows Lua to feed data to any supported serial protocol for e.g. sensor simulation) +- set_target_throttle_rate_rpy allows rate control from scripts (new for Copter) +- sitl_standby_sim.lua example shows how to switch between redundant flight controllers using an RC switch +- Slung payload oscillation suppression applet added (see copter-slung-payload.lua) +- Temperature sensor bindings added +- uint64 support +- Various performance and memory usage optimizations +- VTOL-quicktune.lua minor enhancements including increasing YAW_FLTE_MAX to 8 +- x-quad-cg-allocation.lua applet corrects for the CoM discrepancy in a quad-X frame + +13) GCS / mavlink related changes and fixes + +- BATTERY2 message deprecated (replaced by BATTERY_STATUS) +- CMD_MISSION_START/STOP rejected if first-item/last-item args provided +- Deny attempts to upload two missions simultaneously +- Fence and Rally points may be uploaded using FTP +- GPS_INPUT and HIL_GPS handles multiple GPSs +- HIGHRES_IMU mavlink message support (used by companion computers to receive IMU data from FC) +- MAV_CMD_COMPONENT_ARM_DISARM accepts force arm magic value of 21196 +- MAV_CMD_DO_SET_SAFETY_SWITCH_STATE support +- MAV_CMD_SET_HAGL support (Plane only) +- MAVFTP respects TX buffer flow control to improve FTP on low bandwidth links +- MAVLink receiver support (RADIO_RC_CHANNELS mavlink message) +- Message interval supports TERRAIN_REPORT msg +- Mission upload may be cancelled using MISSION_CLEAR_ALL +- MOUNT_CONFIGURE, MOUNT_CONTROL messages deprecated +- RC_CHANNELS_RAW deprecated +- Serial passthrough supports parity allowing STM32CubeProgrammer to be used to program FrSky R9 receivers +- SET_ATTITUDE_TARGET angular rate input frame fixed (Copter only) +- TIMESYNC and NAMED_VALUE_FLOAT messages not sent on high latency MAVLink ports + +14) Logging enhancements and fixes + +- AC_PID resets and I-term sets logged +- ANG provides attitude at high rate (equivalent to ATT) +- ATT logs angles as floats (better resolution than ints) +- CAND message gets driver index +- DCM log message includes roll/pitch and yaw error +- EDT2 log msg includes stress and status received via extended DShot Telemetry v2 +- EFI ECYL cylinder head and exhaust temp logs in degC (was Kelvin) +- ESCX log msg includes DroneCAN ESCs status, temp, duty cycle and power pct +- FMT messages written as required instead of all at beginning +- Logging restarted after download completes when LOG_DISARMED = 1 +- MISE msg logs active mission command (CMD logged when commands are uploaded) +- ORGN message logging fixed when set using SET_GPS_GLOBAL_ORIGIN +- RPM sensor logging gets instance field, quality and health fields +- Short filename support removed (e.g log1.BIN instead of 00000001.BIN) +- Temperature sensor logging option for only sensors with no source (see TEMP_LOG) +- UART data rates logged at 1hz (see UART message) + +15) ROS2 / DDS support + +- Airspeed published +- Battery topic reports all available batteries +- Compile-time configurable rates for each publisher +- DDS_TIMEOUT_MS and DDS_MAX_RETRY set timeout and num retries when client pings XRCE agent +- GPS global origin published at 1 Hz +- High frequency raw imu data transmission +- Joystick support +- Support sending waypoints to Copter and Rover +- Remove the XML refs file in favor of binary entity creation + +16) Safety related enhancements and fixes + +- Accel/Gyro inconsistent message fixed for using with only single IMU +- Battery monitor failure reported to GCS, triggers battery failsafe but does not take action +- Far from EKF origin pre-arm check removed (Copter only) +- Fence breach warning message slightly improved +- Fence enhancements incluiding alt min fence (Copter only, see FENCE_AUTOENABLE, FENCE_OPTIONS) +- Fences can be stored to SD Card (see BRD_SD_FENCE param) +- ICEngine stopped when in E-Stop or safety engaged (Plane only) +- LEDs flash green lights based on EKF location not GPS +- Parachute option to skip disarm before parachute release (see CHUTE_OPTIONS) +- Plane FENCE_AUTOENABLE of 1 or 2 deprecation warning added +- Pre-arm check if OpenDroneID is compiled in but disabled +- Pre-arm check of duplicate RC aux functions fixed (was skipping recently added aux functions) +- Pre-arm checks alert user more quickly on failure +- Prearm check for misconfigured EAHRS_SENSORS and GPS_TYPE +- Proximity sensor based avoidance keeps working even if one proximity sensor fails (Copter, Rover) +- RC aux functions for Arm, Disarm, E-Stop and Parachute ignored when RC is first turned on +- Warning of duplicate SERIALx_PROTOCOL = RCIN + +17) Other bug fixes and minor enhancements + +- Accel cal fixed for auxiliary IMUs (e.g. IMU4 and higher) +- Bootloader fix to reduce unnecessary mass erasing of flash when using STLink or Jlink tools +- Bootloader rejects allocation of broadcast node ID +- CAN forward registering/de-registering fix (affected Mission Planner DroneCAN UI) +- Dijkstras fix to correct use of uninitialised variable +- DShot rates are not limited by NeoPixel rates +- Ethernet and CAN bootloader fix to prevent getting stuck in bootloader mode +- Filesystem does not show entries for empty @ files +- Filesystem efficiency improvements when reading files +- Flight statistics only reset if user sets STAT_RESET to zero (avoids accidental reset) +- Flight time statistics updated on disarm (avoids issue if vehicle powered-down soon after disarm) +- Internal error thrown if we lose parameters due to saving queue being too small +- MAVLink via DroneCAN baud rate fix +- SPI pins may also be used as GPIOs +- Terrain cache size configurable (see TERRAIN_CACHE_SZ) + +18) Developer focused fixes and enhancements + +- AP_Camera gets example python code showing how to use GStreamer with UDP and RTSP video streams +- Cygwin build fix for non-SITL builds +- Cygwin build fixed with malloc wrap +- DroneCAN and scripting support FlexDebug messages (see CAN_Dn_UC_OPTION, FlexDebug.lua) +- EKF3 code generator documentation and cleanup +- GPS jamming simulator added +- MacOS compiler warnings reduced +- SFML joystick support +- SITL support for OpenBSD +- Text warning if older Fence or Rally point protocols are used +------------------------------------------------------------------ Release 4.5.7 08 Oct 2024 Changes from 4.5.7-beta1 From c03960cba4f65b8b2f59019f6704146afe0341de Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 1 Nov 2024 09:02:25 +0900 Subject: [PATCH 103/117] Copter: 4.6.0-beta1 release notes Co-authored-by: Bill Geyer Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com> Co-authored-by: Thomas Watson Co-authored-by: Andrew Tridgell --- ArduCopter/ReleaseNotes.txt | 424 ++++++++++++++++++++++++++++++++++++ 1 file changed, 424 insertions(+) diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 510ed875e842b..26bda5c232ac0 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,429 @@ ArduPilot Copter Release Notes: ------------------------------------------------------------------ +Release 4.6.0-beta 13 Nov 2024 + +Changes from 4.5.7 + +1) Board specific changes + +- AnyLeaf H7 supports compass and onboard logging +- Blitz743Pro supports CAN +- BlueRobotics Navigator supports BMP390 baro +- Bootloader ECC failure check fixed on boards with >128k bootloader space (e.g CubeRed) +- CB Unmanned Stamp H743 support +- ClearSky CSKY405 support +- CUAV-7-Nano default batt monitor fixed +- CubeRed bootloader fixes including disabling 2nd core by default +- CubeRed supports PPP networking between primary and secondary MCU +- CubeRedPrimary supports external compasses +- ESP32 main loop rate improvements +- ESP32 RC input fixes and wifi connection reliability improved +- ESP32 safety switch and GPIO pin support +- FlyingMoon no longer support MAX7456 +- Flywoo F405HD-AIOv2 ELRS RX pin pulled high during boot +- Flywoo H743 Pro support +- Flywoo/Goku F405 HD 1-2S ELRS AIO v2 +- FlywooF745 supports DPS310 baro +- FPV boards lose SMBus battery support (to save flash) +- GEPRC F745BTHD support +- GEPRCF745BTHD loses parachute support, non-BMP280 baros (to save flash) +- Here4FC bootloader fix for mismatch between RAM0 and periph that could prevent firmware updates +- Holybro Kakute F4 Wing support +- iFlight 2RAW H743 supports onboard logging +- JFB110 supports measuring servo rail voltage +- JFB110 supports safety switch LED +- JHEM-JHEF405 bootloader supports firmware updates via serial +- JHEMCU GF30H743 HD support +- JHEMCU-GF16-F405 autopilot support +- JHEMCU-GSF405A becomes FPV board (to save flash) +- KakuteF7 only supports BMP280 baro (to save flash) +- KakuteH7Mini supports ICM42688 IMU +- Linux auto detection of GPS baud rate fixed +- Linux board scheduler jitter reduced +- Linux board shutdown fixes +- MakeFlyEasy PixPilot-V6Pro support +- MatekF405, Pixhawk1-1M-bdshot, revo-mini loses blended GPS (to save flash) +- MatekH7A3 support Bi-directional DShot +- MicoAir405v2 and MicoAir405Mini support optical flow and OSD +- MicoAir743 internal compass orientation fixed +- MicroAir405Mini, MicroAir743, NxtPX4v2 support +- MicroAir405v2 Bi-directional DShot and LED DMA fixes +- MicroAir405v2 defined as FPV board with reduced features (to save flash) +- ModalAI VOXL2 support including Starling 2 and Starling 2 max +- mRo Control Zero Classic supports servo rail analog input +- mRo KitCAN revC fixed +- Mugin MUPilot support +- OmnibusF7v2 loses quadplane support (to save flash) +- Pixhack-v3 board added (same as fmuv3) +- Pixhawk6C bootloader supports flashing firmware from SD card +- RadiolinkPIX6 imu orientation fixed +- RadiolinkPIX6 supports SPA06 baro +- ReaperF745 V4 FC supports MPU6000 IMU +- RPI5 support +- SDModelH7v2 SERIAL3/7/9_PROTOCOL param defaults changed +- Solo serial ports default to MAVLink1 +- SpeedyBeeF405Wing gets Bi-directional DShot +- SpeedyBeeF405WING loses landing gear support, some camera gimbals (to save flash) +- Spektreworks boom board support +- TrueNavPro-G4 SPI does not share DMA +- X-MAV AP-H743v2 support + +2) AHRS/EKF enhancements and fixes + +- AHRS_OPTION to disable fallback to DCM (affects Plane and Rover, Copter never falls back) +- AHRS_OPTION to disable innovation check for airspeed sensor +- Airspeed sensor health check fixed when using multiple sensors and AHRS affinity +- DCM support for MAV_CMD_EXTERNAL_WIND_ESTIMATE (Plane only) +- EK2 supports disabling external nav (see EK2_OPTIONS) +- EK3 External Nav position jump after switch from Optical flow removed (see EK3_SRC_OPTION=1) +- EK3 uses filtered velocity corrections for IMU position +- EKF2, EKF3, ExternalAHRS all use common origin +- EKF3 accepts set origin even when using GPS +- EKF3 allows earth-frame fields to be estimated with an origin but no GPS +- EKF3 copes better with GPS jamming +- EKF3 logs mag fusion selection to XKFS +- EKF3 wind estimation when using GPS-for-yaw fixed +- External AHRS improvements including handling variances, pre-arm origin check +- Inertial Labs External AHRS fixes +- VectorNav driver fix for handling of error from sensor +- VectorNav External AHRS enhancements including validation of config commands and logging improvements +- VectorNav support for sensors outside VN-100 and VN-300 + +3) Driver enhancements and bug fixes + +- ADSB fix to display last character in status text sent to GCS +- Ainstein LR-D1 radar support +- Airspeed ASP5033 whoami check fixed when autopilot rebooted independently of the sensor +- AIRSPEED message sent to GCS +- Analog temperature sensor extended to 5th order polynomial (see TEMP_A5) +- ARSPD_OPTIONS to report calibration offset to GCS +- Baro EAS2TAS conversions continuously calculated reducing shocks to TECS (Plane only) +- Baro provides improved atmospheric model for high altitude flight +- BARO_ALT_OFFSET slew slowed to keep EKF happy +- BATTx_ESC_MASK param supports flexible mapping of ESCs to batteries +- BATTx_OPTION to not send battery voltage, current, etc to GCS +- Benewake RDS02U radar support +- Bi-directional DShot on IOMCU supports reversible mask +- Bi-directional DShot telemetry support on F103 8Mhz IOMCUs +- BMM350 compass support +- CAN rangefinders and proximity sensors may share a CAN bus (allows NRA24 and MR72 on a single CAN bus) +- Compass calibration world magnetic model checks can use any position source (e.g. not just GPS) +- CRSF baro and vertical speeed fixed +- CRSF RX bind command support +- DroneCAN battery monitor check to avoid memory corruption when type changed +- DroneCAN DNA server fixes including removing use of invalid node IDs, faster ID allocation, elimination of rare inability to save info +- DroneCAN EFI health check fix +- DroneCAN ESC battery monitors calculate consumed mah +- DroneCAN ESCs forced to zero when disarmed +- DroneCAN RPM message support +- DroneCAN timeout fix for auxiliary frames +- DroneCAN to serial tunneling params accepts short-hand baud rates (e.g. '57' for '57600') +- F9P, F10-N and Zed-F9P support for GPSx_GNSS_MODE to turn on/off SBAS, Galileo, Beidou and Glonass +- FuelLevel battery monitor fix to report capacity +- GPS_xxx params renamed to GPS1_xxx, GPS_xxx2 renamed to GPS2_xxx +- Hirth EFI logging includes modified throttle +- Hirth ICEngine supports reversed crank direction (see ICE_OPTIONS parameter) +- Hott and LTM telemetry deprecated (still available through custom build server) +- i-BUS telemetry support +- ICE_PWM_IGN_ON, ICE_PWM_IGN_OFF, ICE_PWM_STRT_ON, ICE_PWM_STRT_OFF replaced with SERVOx_MIN/MAX/REVERSED (Plane only) +- ICE_START_CHAN replaced with RC aux function (Plane only) +- ICEngine retry max added (see ICE_STRT_MX_RTRY) +- IE 2400 generator error message severity to GCS improved +- INA2xx battery monitor support (reads temp, gets MAX_AMPS and SHUNT params) +- MCP9600 temperature sensor I2C address fixed +- MLX90614 temperature sensor support +- MSP GPS ground course scaling fixed +- MSP RC uses scaled RC inputs (fixes issue with RCx_REVERSED having no effect) +- Networking supports reconnection to TCP server or client +- OSD params for adjusting horizontal spacing and vertical extension (see OSD_SB_H_OFS, OSD_SB_V_EXT) +- Relay inverted output support (see RELAYx_INVERTED parameter) +- ROMFS efficiency improvements +- RS-485 driver enable RTS flow control +- Sagetech MXS ADSP altitude fix (now uses abs alt instead of terrain alt) +- Septentrio GPS sat count correctly drops to zero when 255 received +- Septentrio supports selecting constellations (see GPS_GNSS_MODE) +- Single LED for user notification supported +- SPA06 baro supported +- Sum battery monitor optionally reports minimum voltage instead of average +- Sum battery monitor reports average temp +- Torqeedo dual motor support (see TRQ1, TRQ2 params) +- Ublox GPS driver uses 64 bit time for PPS interrupt (avoids possible dropout at 1hour and 12 minutes) +- UBlox GPS time ignored until at least 2D fix +- VideoTX supports additional freq bands (RushFPV 3.3Ghz) +- Volz logs desired and actual position, voltage, current, motor and PCB temp +- Volz server feedback and logging fixed +- Volz servo output in its own thread resulting in smoother movements +- W25N02KV flash support + +4) Networking enhancements and fixes + +- Allow multiple UDP clients to connect/disconnect/reconnect +- Ethernet supports faster log downloading (raised SDMMC clock limit on H7) + +5) Camera and gimbal enhancements + +- Alexmos precision improved slightly +- CAMERA_CAPTURE_STATUS mavlink msg sent to GCS (reports when images taken or video recorded, used by QGC) +- CAMERA_FOV_STATUS mavlink reports lat,lon of what camera is pointing at +- DO_MOUNT_CONTROL yaw angle interpreted as body-frame (was incorrectly being interpreted as earth-frame) +- Dual serial camera gimbal mounts fixed +- Lua script bindings to send CAMERA_INFORMATION and VIDEO_STREAM_INFORMATION messages to GCS +- Retract Mount2 aux function added (see RCx_OPTION = 113) +- Servo gimbal reported angles respect roll, pitch and yaw limits +- Siyi driver sends autopilot location and speed (recorded in images via EXIF) +- Siyi picture and video download scripts +- Siyi ZT6 and ZT30 support sending min, max temperature (see CAMERA_THERMAL_RANGE msg) +- Siyi ZT6 and ZT30 thermal palette can be changed using camera-change-setting.lua script +- Siyi ZT6 hardware id and set-lens fixed +- Topotek gimbal support +- Trigger distance ignores GPS status and only uses EKF reported location + +6) Harmonic notch enhancements + +- Harmonic notch is active in forward flight on quadplanes +- Harmonic notch filter freq clamping and disabling reworked +- Harmonic notch handles negative ESC RPMs +- Harmonic notch supports per-motor throttle-based harmonic notch + +7) Copter specific enhancements and bug fixes + +- Attitude control fix to dt update order (reduces rate controller noise) +- Auto mode fix to avoid prematurely advancing to next waypoint if given enough time +- Auto mode small target position jump when takeoff completes removed +- Auto mode yaw drift when passing WP removed if CONDITION_YAW command used and WP_YAW_BEHAVIOR = 0/None +- Auto, Guided flight mode pause RC aux switch (see RCx_OPTION = 178) +- AutoRTL (e.g. DO_LAND_START) uses copter stopping point to decide nearest mission command +- AutoRTL mode supports DO_RETURN_PATH_START (Copter, Heli only) +- AutoTune fix to prevent spool up after landing +- AutoTune performance and safety enhancements (less chance of ESC desync, fails when vehicle likely can't be tuned well) +- Autotune test gains RC aux switch function allows testing gains in any mode (see RCx_OPTION = 180) +- Config error avoided if auto mode is paused very soon after poweron +- FLIGHT_OPTIONS bit added to require position estimate before arming +- Follow mode slowdown calcs fixed when target is moving +- Ground oscillation suppressed by reducing gains (see ATC_LAND_R/P/Y_MULT) +- Guided mode internal error fix when taking off using SET_ATTITUDE_CONTROL message +- Guided mode internal error resolved when switching between thrust or climb rate based altitude control +- Guided mode yaw fixed when WP_YAW_BEHAVIOR = 0/None and CONDITION_YAW command received containing relative angle +- Landing detector fixed when in stabilize mode at full throttle but aircraft is upside down +- Landing detector logging added to ease support (see LDET message) +- Loiter unlimited command accepts NaNs (QGC sends these) +- Mavlink SYSTEM_STATUS set to BOOT during initialisation +- MOT_PWM_TYPE of 9 (PWMAngle) respects SERVOx_MIN/MAX/TRIM/REVERSED param values +- Payload place bug fix when aborted because gripper is already released +- RC based tuning (aka CH6 tuning) can use any RC aux function channel (see RCx_OPTION = 219) +- RTL_ALT minimum reduced to 30cm +- SystemID position controller support (Copter and Heli) +- TriCopter motor test and slew-up fixed +- WPNAV_SPEED min reduced to 10 cm/s (Copter only) +- Loiter mode zeros desired accel when re-entering from Auto during RC failsafe + +8) TradHeli specific enhancements + +- Autorotation yaw behaviour fix +- Autotune improvements including using low frequency dwell for feedforward gain tuning and conducting sweep in attitude instead of rate +- Blade pitch angle logging added (see SWSH log message) +- Constrain cyclic roll for intermeshing +- ICE / turbine cool down fix +- Inverted flight extended to non manual throttle modes +- Inverted flight transitions smoothed and restricted to only Stabilize mode +- SWSH logging fix for reversed collectives + +9) Plane specific enhancements and bug fixes + +- AIRSPEED_STALL holds vehicle stall speed and is used for minimum speed scaling +- Allow for different orientations of landing rangefinder +- Assistance requirements evaluted on mode change +- FBWB/CRUISE climb/sink rate limited by TECS limits +- FLIGHT_OPTION to immediately climb in AUTO mode (not doing glide slope) +- Glider pullup support (available only through custom build server) +- Loiter breakout improved to better handle destinations inside loiter circle +- Manual mode throttle made consistent with other modes (e.g battery comp and watt limit is done if enabled) +- Mavlink GUIDED_CHANGE_ALTITUDE supports terrain altitudes +- Minimum throttle not applied during SLT VTOL airbrake (reduces increase in airspeed and alt during back transition) +- Q_APPROACH_DIST set minimum distance to use the fixed wing approach logic +- Quadplane assistance check enhancements +- Quadplane Deca frame support +- Quadplane gets smoother takeoff by input shaping target accel and velocity +- Servo wiggles in altitude wait staged to be one after another +- Set_position_target_global_int accepts MAV_FRAME_GLOBAL_RELATIVE_ALT and MAV_FRAME_GLOBAL_TERRAIN_ALT frames +- Takeoff airspeed control improved (see TKOFF_MODE, TKOFF_THR_MIN) +- Takeoff fixes for fence autoenable +- Takeoff improvements including less overshoot of TKOFF_ALT +- TECS reset along with other controllers (important if plane dropped from balloon) +- Tilt quadplane ramp of motors on back transition fixed +- Tiltrotor tilt angles logged +- TKOFF_THR_MIN applied to SLT transitions +- Twin motor planes with DroneCAN ESCs fix to avoid max throttle at arming due to misconfiguration +- VTOLs switch to QLAND if a LONG_FAILSAFE is triggered during takeoff + +10) Rover specific enhancements and bug fixes + +- Auto mode reversed state maintained if momentarily switched to Hold mode +- Circle mode tracks better and avoids getting stuck at circle edge +- Flight time stats fixed +- MAV_CMD_NAV_SET_YAW_SPEED deprecated +- Omni3Mecanum frame support +- Stopping point uses max deceleration (was incorrectly using acceleration) +- Wheel rate controller slew rate fix + +11) Antenna Tracker specific enhancements and bug fixes + +- Never track lat,lon of 0,0 + +12) Scripting enhancements + +- advance-wp.lua applet supports advancing Auto mode WP via RC switch +- AHRS_switch.lua supports switching between EKF2 and EKF3 via RC switch +- battery_internal_resistance_check.lua monitors battery resistance +- CAN:get_device returns nil for unconfigured CAN device +- copter_terrain_brake.lua script added to prevent impact with terrain in Loiter mode (Copter only) +- Copter:get_target_location, update_target_location support +- crosstrack_restore.lua example allows returning to previous track in Auto (Plane only) +- Display text on OLED display supported +- Docs improved for many bindings +- EFI get_last_update_ms binding +- EFI_SkyPower.lua driver accepts 2nd supply voltage +- ESC_slew_rate.lua example script supports testing ESCs +- Filesystem CRC32 check to allow scripts to check module versions +- forced arming support +- GPIO get/set mode bindings (see gpio:set_mode, get_mode) +- GPS-for-yaw angle binding (see gps:gps_yaw_deg) +- Halo6000 EFI driver can log all CAN packets for easier debugging +- handle_external_position_estimate binding allows sending position estimate from lua to EKF +- I2C:transfer support +- IMU gyros_consistent and accels_consistent bindings added +- INF_Inject.lua driver more robust to serial errors, improved logging, throttle and ignition control +- INS bindings for is calibrating, gyro and accel sensor values +- IPV4 address bindings (see SocketAPM_ipv4_addr_to_string) to allow UDP server that responds to individual clients +- Logging of booleans supported +- Lua language checks improved (finds more errors) +- MAVLink commands can be called from scripting +- MCU voltage binding (see analog:mcu_voltage) +- NMEA 2000 EFI driver (see EFI_NMEA2k.lua) +- "open directory failed" false warning when scripts in ROMFS fixed +- Param_Controller.lua supports quickly switching between 3 parameter sets via RC switch +- Pass by reference values are always initialized +- pelco_d_antennatracker.lua applet supports sending Pelco-D via RS-485 to PTZ cameras +- plane_aerobatics.lua minor enhancements +- REPL applet (read-evaluate-print-loop, see repl.lua) for interactive testing and experimentation +- "require" function failures in rare circumstances fixed +- "require" function works for modules in ROMFS (e.g. not on SD card) +- revert_param.lua supports more params (e.g ATC_RATE_R/P/Y, PTCH2SRV_TCONST, RLL2SRV_TCONST, TECS_) +- Scripts may receive mavlink messages which fail CRC (e.g messages which FC does not understand) +- SD card formatting supported +- Serial device simulation support (allows Lua to feed data to any supported serial protocol for e.g. sensor simulation) +- set_target_throttle_rate_rpy allows rate control from scripts (new for Copter) +- sitl_standby_sim.lua example shows how to switch between redundant flight controllers using an RC switch +- Slung payload oscillation suppression applet added (see copter-slung-payload.lua) +- Temperature sensor bindings added +- uint64 support +- Various performance and memory usage optimizations +- VTOL-quicktune.lua minor enhancements including increasing YAW_FLTE_MAX to 8 +- x-quad-cg-allocation.lua applet corrects for the CoM discrepancy in a quad-X frame + +13) GCS / mavlink related changes and fixes + +- BATTERY2 message deprecated (replaced by BATTERY_STATUS) +- CMD_MISSION_START/STOP rejected if first-item/last-item args provided +- Deny attempts to upload two missions simultaneously +- Fence and Rally points may be uploaded using FTP +- GPS_INPUT and HIL_GPS handles multiple GPSs +- HIGHRES_IMU mavlink message support (used by companion computers to receive IMU data from FC) +- MAV_CMD_COMPONENT_ARM_DISARM accepts force arm magic value of 21196 +- MAV_CMD_DO_SET_SAFETY_SWITCH_STATE support +- MAV_CMD_SET_HAGL support (Plane only) +- MAVFTP respects TX buffer flow control to improve FTP on low bandwidth links +- MAVLink receiver support (RADIO_RC_CHANNELS mavlink message) +- Message interval supports TERRAIN_REPORT msg +- Mission upload may be cancelled using MISSION_CLEAR_ALL +- MOUNT_CONFIGURE, MOUNT_CONTROL messages deprecated +- RC_CHANNELS_RAW deprecated +- Serial passthrough supports parity allowing STM32CubeProgrammer to be used to program FrSky R9 receivers +- SET_ATTITUDE_TARGET angular rate input frame fixed (Copter only) +- TIMESYNC and NAMED_VALUE_FLOAT messages not sent on high latency MAVLink ports + +14) Logging enhancements and fixes + +- AC_PID resets and I-term sets logged +- ANG provides attitude at high rate (equivalent to ATT) +- ATT logs angles as floats (better resolution than ints) +- CAND message gets driver index +- DCM log message includes roll/pitch and yaw error +- EDT2 log msg includes stress and status received via extended DShot Telemetry v2 +- EFI ECYL cylinder head and exhaust temp logs in degC (was Kelvin) +- ESCX log msg includes DroneCAN ESCs status, temp, duty cycle and power pct +- FMT messages written as required instead of all at beginning +- Logging restarted after download completes when LOG_DISARMED = 1 +- MISE msg logs active mission command (CMD logged when commands are uploaded) +- ORGN message logging fixed when set using SET_GPS_GLOBAL_ORIGIN +- RPM sensor logging gets instance field, quality and health fields +- Short filename support removed (e.g log1.BIN instead of 00000001.BIN) +- Temperature sensor logging option for only sensors with no source (see TEMP_LOG) +- UART data rates logged at 1hz (see UART message) + +15) ROS2 / DDS support + +- Airspeed published +- Battery topic reports all available batteries +- Compile-time configurable rates for each publisher +- DDS_TIMEOUT_MS and DDS_MAX_RETRY set timeout and num retries when client pings XRCE agent +- GPS global origin published at 1 Hz +- High frequency raw imu data transmission +- Joystick support +- Support sending waypoints to Copter and Rover +- Remove the XML refs file in favor of binary entity creation + +16) Safety related enhancements and fixes + +- Accel/Gyro inconsistent message fixed for using with only single IMU +- Battery monitor failure reported to GCS, triggers battery failsafe but does not take action +- Far from EKF origin pre-arm check removed (Copter only) +- Fence breach warning message slightly improved +- Fence enhancements incluiding alt min fence (Copter only, see FENCE_AUTOENABLE, FENCE_OPTIONS) +- Fences can be stored to SD Card (see BRD_SD_FENCE param) +- ICEngine stopped when in E-Stop or safety engaged (Plane only) +- LEDs flash green lights based on EKF location not GPS +- Parachute option to skip disarm before parachute release (see CHUTE_OPTIONS) +- Plane FENCE_AUTOENABLE of 1 or 2 deprecation warning added +- Pre-arm check if OpenDroneID is compiled in but disabled +- Pre-arm check of duplicate RC aux functions fixed (was skipping recently added aux functions) +- Pre-arm checks alert user more quickly on failure +- Prearm check for misconfigured EAHRS_SENSORS and GPS_TYPE +- Proximity sensor based avoidance keeps working even if one proximity sensor fails (Copter, Rover) +- RC aux functions for Arm, Disarm, E-Stop and Parachute ignored when RC is first turned on +- Warning of duplicate SERIALx_PROTOCOL = RCIN + +17) Other bug fixes and minor enhancements + +- Accel cal fixed for auxiliary IMUs (e.g. IMU4 and higher) +- Bootloader fix to reduce unnecessary mass erasing of flash when using STLink or Jlink tools +- Bootloader rejects allocation of broadcast node ID +- CAN forward registering/de-registering fix (affected Mission Planner DroneCAN UI) +- Dijkstras fix to correct use of uninitialised variable +- DShot rates are not limited by NeoPixel rates +- Ethernet and CAN bootloader fix to prevent getting stuck in bootloader mode +- Filesystem does not show entries for empty @ files +- Filesystem efficiency improvements when reading files +- Flight statistics only reset if user sets STAT_RESET to zero (avoids accidental reset) +- Flight time statistics updated on disarm (avoids issue if vehicle powered-down soon after disarm) +- Internal error thrown if we lose parameters due to saving queue being too small +- MAVLink via DroneCAN baud rate fix +- SPI pins may also be used as GPIOs +- Terrain cache size configurable (see TERRAIN_CACHE_SZ) + +18) Developer focused fixes and enhancements + +- AP_Camera gets example python code showing how to use GStreamer with UDP and RTSP video streams +- Cygwin build fix for non-SITL builds +- Cygwin build fixed with malloc wrap +- DroneCAN and scripting support FlexDebug messages (see CAN_Dn_UC_OPTION, FlexDebug.lua) +- EKF3 code generator documentation and cleanup +- GPS jamming simulator added +- MacOS compiler warnings reduced +- SFML joystick support +- SITL support for OpenBSD +- Text warning if older Fence or Rally point protocols are used +------------------------------------------------------------------ Release 4.5.7 08 Oct 2024 Changes from 4.5.7-beta1 From 86f216703d4bfd37c93ea58ca6b44cb663e23ecb Mon Sep 17 00:00:00 2001 From: Tiziano Fiorenzani Date: Wed, 13 Nov 2024 16:30:42 +0000 Subject: [PATCH 104/117] AP_DDS: status topic to report RC failsafe with callback function --- libraries/AP_DDS/AP_DDS_Client.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 05ba28f06dc7a..a39240143fb55 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -632,15 +632,17 @@ bool AP_DDS_Client::update_topic(ardupilot_msgs_msg_Status& msg) msg.external_control = true; // Always true for now. To be filled after PR#28429. uint8_t fs_iter = 0; msg.failsafe_size = 0; - if (AP_Notify::flags.failsafe_radio) { + if (rc().in_rc_failsafe()) { msg.failsafe[fs_iter++] = FS_RADIO; } if (battery.has_failsafed()) { msg.failsafe[fs_iter++] = FS_BATTERY; } + // TODO: replace flag with function. if (AP_Notify::flags.failsafe_gcs) { msg.failsafe[fs_iter++] = FS_GCS; } + // TODO: replace flag with function. if (AP_Notify::flags.failsafe_ekf) { msg.failsafe[fs_iter++] = FS_EKF; } From 3674eb0c49c6dc04010aedad332e7a01ee3dd635 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 14 Nov 2024 10:03:58 +1100 Subject: [PATCH 105/117] autotest: tidy GpsForYaw using new infrastructure --- Tools/autotest/arducopter.py | 25 ++++++------------------- 1 file changed, 6 insertions(+), 19 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 97792527db53d..1482c6f829ade 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -9832,28 +9832,15 @@ def RefindGPS(self): def GPSForYaw(self): '''Moving baseline GPS yaw''' - self.context_push() self.load_default_params_file("copter-gps-for-yaw.parm") self.reboot_sitl() - ex = None - try: - self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True) - m = self.assert_receive_message("GPS2_RAW") - self.progress(self.dump_message_verbose(m)) - want = 27000 - if abs(m.yaw - want) > 500: - raise NotAchievedException("Expected to get GPS-from-yaw (want %f got %f)" % (want, m.yaw)) - self.wait_ready_to_arm() - except Exception as e: - self.print_exception_caught(e) - ex = e - self.context_pop() - - self.reboot_sitl() - - if ex is not None: - raise ex + self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True) + m = self.assert_receive_message("GPS2_RAW", very_verbose=True) + want = 27000 + if abs(m.yaw - want) > 500: + raise NotAchievedException("Expected to get GPS-from-yaw (want %f got %f)" % (want, m.yaw)) + self.wait_ready_to_arm() def SMART_RTL_EnterLeave(self): '''check SmartRTL behaviour when entering/leaving''' From f07df393bea1e00a6df74bd0fa02ab909f301d77 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 2 Oct 2024 18:14:47 +0100 Subject: [PATCH 106/117] AP_HAL_ChibiOS: support BMP280 on FoxeerF405v2 --- libraries/AP_HAL_ChibiOS/hwdef/FoxeerF405v2/hwdef.dat | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FoxeerF405v2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/FoxeerF405v2/hwdef.dat index cd08660e4922e..8a7d80b127a8a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FoxeerF405v2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/FoxeerF405v2/hwdef.dat @@ -147,6 +147,7 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin # Barometer setup BARO DPS310 I2C:0:0x76 +BARO BMP280 I2C:0:0x76 # IMU setup From 1e99226fd96a4c90113c819e7d9db29679210085 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Wed, 13 Nov 2024 16:36:53 -0800 Subject: [PATCH 107/117] AP_HAL_QURT: Fix the SPI transfer special case where a send buffer is passed in even though it is a read transaction. --- libraries/AP_HAL_QURT/SPIDevice.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_HAL_QURT/SPIDevice.cpp b/libraries/AP_HAL_QURT/SPIDevice.cpp index 616cf6a76f0aa..a4fcdc6660202 100644 --- a/libraries/AP_HAL_QURT/SPIDevice.cpp +++ b/libraries/AP_HAL_QURT/SPIDevice.cpp @@ -67,6 +67,12 @@ bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len, return transfer_fullduplex(send, (uint8_t*) send, send_len); } + // Special case handling. This can happen when a send buffer is specified + // even though we are doing only a read. + if (send == recv && send_len == recv_len) { + return transfer_fullduplex(send, recv, send_len); + } + // This is a read transaction uint8_t buf[send_len+recv_len]; if (send_len > 0) { From 37cfe71257b9751a9a758c941277ad37c8deebce Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 14 Nov 2024 16:00:32 +1100 Subject: [PATCH 108/117] AP_Periph: add missing mandatory virtual method define for GCS_MAVLink --- Tools/AP_Periph/GCS_MAVLink.h | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/AP_Periph/GCS_MAVLink.h b/Tools/AP_Periph/GCS_MAVLink.h index fa3366ed11ffd..7472fc005f67b 100644 --- a/Tools/AP_Periph/GCS_MAVLink.h +++ b/Tools/AP_Periph/GCS_MAVLink.h @@ -41,6 +41,7 @@ class GCS_MAVLINK_Periph : public GCS_MAVLINK void send_nav_controller_output() const override {}; void send_pid_tuning() override {}; + virtual uint8_t send_available_mode(uint8_t index) const override { return 0; } }; /* From 44ef597647afc1033eb5322ad137f73e2f6577c5 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 14 Nov 2024 16:01:04 +1100 Subject: [PATCH 109/117] .github: add CubeNode-ETH build to CI --- Tools/scripts/build_ci.sh | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Tools/scripts/build_ci.sh b/Tools/scripts/build_ci.sh index 8c7a3ba9c25eb..bdf43bb8cf399 100755 --- a/Tools/scripts/build_ci.sh +++ b/Tools/scripts/build_ci.sh @@ -243,6 +243,10 @@ for t in $CI_BUILD_TARGET; do $waf configure --board FreeflyRTK $waf clean $waf AP_Periph + echo "Building CubeNode-ETH peripheral fw" + $waf configure --board CubeNode-ETH + $waf clean + $waf AP_Periph continue fi From a69b777d17e6709a8b74fb94f74881928d9fc0d8 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Thu, 14 Nov 2024 17:08:02 +1100 Subject: [PATCH 110/117] Plane: add missing AccZ bit from GCS_PID_MASK doc --- ArduPlane/Parameters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index b2db9099f8f51..5f746640e16e2 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -60,7 +60,7 @@ const AP_Param::Info Plane::var_info[] = { // @DisplayName: GCS PID tuning mask // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for // @User: Advanced - // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:Steering,4:Landing + // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:Steering,4:Landing,5:AccZ GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), // @Param: KFF_RDDRMIX From 2115ad9c972f6c58ba3eb4b2bc7e80a953b28163 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 13 Nov 2024 15:33:17 -0800 Subject: [PATCH 111/117] AP_Rely: allow RELAYn_DEFAULT values for DroneCAN Periphs --- libraries/AP_Relay/AP_Relay.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index fe3a73ab2d984..2a64e19f59b75 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -353,7 +353,11 @@ void AP_Relay::init() continue; } - if (function == AP_Relay_Params::FUNCTION::RELAY) { + bool use_default_param = (function == AP_Relay_Params::FUNCTION::RELAY); +#ifdef HAL_BUILD_AP_PERIPH + use_default_param |= (function >= AP_Relay_Params::FUNCTION::DroneCAN_HARDPOINT_0 && function <= AP_Relay_Params::FUNCTION::DroneCAN_HARDPOINT_15); +#endif + if (use_default_param) { // relay by instance number, set the state to match our output const AP_Relay_Params::DefaultState default_state = _params[instance].default_state; if ((default_state == AP_Relay_Params::DefaultState::OFF) || From 4c72ca6ddc364a5f8af6393ae7fe84d4e3499840 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 15 Nov 2024 16:50:43 +1100 Subject: [PATCH 112/117] AP_TECS: make _EAS a local variable not used outside this one function Co-authored-by: Michelle Rossouw --- libraries/AP_TECS/AP_TECS.cpp | 3 +++ libraries/AP_TECS/AP_TECS.h | 3 --- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index f25cf5294e6f9..49277cfce07ba 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -427,6 +427,9 @@ void AP_TECS::_update_speed(float DT) // Get measured airspeed or default to trim speed and constrain to range between min and max if // airspeed sensor data cannot be used + + // Equivalent airspeed + float _EAS; if (!use_airspeed || !_ahrs.airspeed_estimate(_EAS)) { // If no airspeed available use average of min and max _EAS = constrain_float(aparm.airspeed_cruise.get(), (float)aparm.airspeed_min.get(), (float)aparm.airspeed_max.get()); diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 5bc757c0b79a7..57748edba1a07 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -271,9 +271,6 @@ class AP_TECS { float _vel_dot; float _vel_dot_lpf; - // Equivalent airspeed - float _EAS; - // True airspeed limits float _TASmax; float _TASmin; From 4d75b4477550e3fccb6a1917dd8a49594d3d2c9b Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 14 Nov 2024 11:31:24 -0800 Subject: [PATCH 113/117] AP_GPS: Remove unused private member to get rid of compiler warning in Qurt build --- libraries/AP_GPS/AP_GPS_Blended.h | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/AP_GPS/AP_GPS_Blended.h b/libraries/AP_GPS/AP_GPS_Blended.h index b68eb0f9660ed..77fe25ffb8884 100644 --- a/libraries/AP_GPS/AP_GPS_Blended.h +++ b/libraries/AP_GPS/AP_GPS_Blended.h @@ -64,7 +64,6 @@ class AP_GPS_Blended : public AP_GPS_Backend Vector3f _blended_antenna_offset; // blended antenna offset float _blended_lag_sec; // blended receiver lag in seconds float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances. - bool _output_is_blended; // true when a blended GPS solution being output uint8_t _blend_health_counter; // 0 = perfectly health, 100 = very unhealthy AP_GPS::GPS_timing &timing; From a4f6853a7bcb3cfa76f5b0a5b8f4a6d15a71ab60 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 13 Oct 2024 21:56:01 -0500 Subject: [PATCH 114/117] AP_CANManager: use 32 bit timeout for write_frame Saves a few bytes. 71 minutes ought to be enough for anybody! --- libraries/AP_CANManager/AP_CANSensor.cpp | 6 +++--- libraries/AP_CANManager/AP_CANSensor.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_CANManager/AP_CANSensor.cpp b/libraries/AP_CANManager/AP_CANSensor.cpp index e86ef31992475..ffa9bf3cc6d9f 100644 --- a/libraries/AP_CANManager/AP_CANSensor.cpp +++ b/libraries/AP_CANManager/AP_CANSensor.cpp @@ -135,7 +135,7 @@ bool CANSensor::add_interface(AP_HAL::CANIface* can_iface) return true; } -bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) +bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) { if (!_initialized) { debug_can(AP_CANManager::LOG_ERROR, "Driver not initialized for write_frame"); @@ -171,12 +171,12 @@ void CANSensor::loop() #endif while (true) { - uint64_t timeout = AP_HAL::micros64() + LOOP_INTERVAL_US; + uint64_t deadline_us = AP_HAL::micros64() + LOOP_INTERVAL_US; // wait to receive frame bool read_select = true; bool write_select = false; - bool ret = _can_iface->select(read_select, write_select, nullptr, timeout); + bool ret = _can_iface->select(read_select, write_select, nullptr, deadline_us); if (ret && read_select) { uint64_t time; AP_HAL::CANIface::CanIOFlags flags {}; diff --git a/libraries/AP_CANManager/AP_CANSensor.h b/libraries/AP_CANManager/AP_CANSensor.h index 7afd1bbea0fa5..b5ac4ee3b0b0f 100644 --- a/libraries/AP_CANManager/AP_CANSensor.h +++ b/libraries/AP_CANManager/AP_CANSensor.h @@ -43,7 +43,7 @@ class CANSensor : public AP_CANDriver { virtual void handle_frame(AP_HAL::CANFrame &frame) = 0; // handler for outgoing frames - bool write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us); + bool write_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us); #ifdef HAL_BUILD_AP_PERIPH static void set_periph(const uint8_t i, const AP_CAN::Protocol protocol, AP_HAL::CANIface* iface) { From 3dd8aa53047769f9d434cbea3cd5caf479ed2d6b Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 13 Oct 2024 22:04:20 -0500 Subject: [PATCH 115/117] AP_CANManager: use 32 bit timeout for write_aux_frame Saves a handful of bytes. 71 minutes ought to be enough for anybody! --- libraries/AP_CANManager/AP_CANDriver.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_CANManager/AP_CANDriver.h b/libraries/AP_CANManager/AP_CANDriver.h index f93e96cec0e7a..5f1c4f74a2a76 100644 --- a/libraries/AP_CANManager/AP_CANDriver.h +++ b/libraries/AP_CANManager/AP_CANDriver.h @@ -40,5 +40,5 @@ class AP_CANDriver virtual bool add_11bit_driver(CANSensor *sensor) { return false; } // handler for outgoing frames for auxillary drivers - virtual bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) { return false; } + virtual bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) { return false; } }; From 7ca558f625fa7898a82e97e32c2e6031f759a8a1 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sun, 13 Oct 2024 22:04:20 -0500 Subject: [PATCH 116/117] AP_DroneCAN: use 32 bit timeout for write_aux_frame Saves a handful of bytes. 71 minutes ought to be enough for anybody! --- libraries/AP_DroneCAN/AP_Canard_iface.cpp | 2 +- libraries/AP_DroneCAN/AP_Canard_iface.h | 2 +- libraries/AP_DroneCAN/AP_DroneCAN.cpp | 2 +- libraries/AP_DroneCAN/AP_DroneCAN.h | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp index 7382f57033c9d..fd38e303c4253 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp +++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp @@ -456,7 +456,7 @@ bool CanardInterface::add_11bit_driver(CANSensor *sensor) } // handler for outgoing frames for auxillary drivers -bool CanardInterface::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) +bool CanardInterface::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) { const uint64_t tx_deadline_us = AP_HAL::micros64() + timeout_us; bool ret = false; diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.h b/libraries/AP_DroneCAN/AP_Canard_iface.h index 43d786639fdf0..671722028dffc 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.h +++ b/libraries/AP_DroneCAN/AP_Canard_iface.h @@ -54,7 +54,7 @@ class CanardInterface : public Canard::Interface { bool add_11bit_driver(CANSensor *sensor); // handler for outgoing frames for auxillary drivers - bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us); + bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us); #if AP_TEST_DRONECAN_DRIVERS static CanardInterface& get_test_iface() { return test_iface; } diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 21243e5d08c40..639e017e82cb6 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -1956,7 +1956,7 @@ bool AP_DroneCAN::add_11bit_driver(CANSensor *sensor) } // handler for outgoing frames for auxillary drivers -bool AP_DroneCAN::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) +bool AP_DroneCAN::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) { if (out_frame.isExtended()) { // don't allow extended frames to be sent by auxillary driver diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 2d6e5ae2b7f78..9a7ae54c6639c 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -95,7 +95,7 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { bool add_11bit_driver(CANSensor *sensor) override; // handler for outgoing frames for auxillary drivers - bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) override; + bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) override; uint8_t get_driver_index() const { return _driver_index; } From 62565e603445499aa0c6d8d2b78c6646caf75df0 Mon Sep 17 00:00:00 2001 From: Prashant Date: Sat, 16 Nov 2024 15:46:29 +0530 Subject: [PATCH 117/117] Tools: added name to GIT_Success.txt --- Tools/GIT_Test/GIT_Success.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/GIT_Test/GIT_Success.txt b/Tools/GIT_Test/GIT_Success.txt index 13d517d005c22..a475ba17f1bd8 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -205,3 +205,4 @@ Naveen Kumar Kilaparthi Amr Attia Alessandro Martini Eren Mert YİĞİT +Prashant Powar