From ce0babc8f15f100a47523ed422532e9dbbf830b4 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sat, 9 Nov 2024 15:29:57 +0000 Subject: [PATCH 01/95] 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 f3a08aabde..e59e3c9a50 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 e7fc346758..87be3e1160 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 02/95] 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 b2939557ad..a3da572611 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 03/95] 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 c06072c597..40a86a8cff 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 04/95] 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 40a86a8cff..5474aaa841 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 05/95] 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 ecd2c79b89..d5d6c324bf 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 06/95] 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 02b18fc4ea..f14df30230 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -140,8 +140,8 @@ private: 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 07/95] 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 43f1c17feb..4dac4792b3 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 08/95] 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 16082d58f4..a25bd4870a 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -249,6 +249,7 @@ protected: // 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 e2c727f4b4..d077c603ec 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 1fba00e92a..7c97656c12 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 09/95] 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 9f3f57cc4d..c02608dbb2 100644 --- a/Tools/autotest/helicopter.py +++ b/Tools/autotest/helicopter.py @@ -708,6 +708,48 @@ class AutoTestHelicopter(AutoTestCopter): 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 @@ class AutoTestHelicopter(AutoTestCopter): 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 3cab908b59..5885c21267 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -7939,7 +7939,7 @@ class TestSuite(ABC): (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 @@ class TestSuite(ABC): 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 10/95] 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 693d197860..7db333f6b7 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 9d064633b1..94f9e7e31b 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 a25bd4870a..67b673d656 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -253,6 +253,9 @@ protected: }; 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 259de8e90c..631acc8778 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 59e4da66bc..bd638fa42a 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 82387a0e93..9b003de0fe 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 c2b93304e6..e70c030036 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 7c97656c12..743708fa9d 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 d0e82cd503..1c91af6a43 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 02c8c3f247..251c859693 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 09682a8429..bfe93ec83a 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 f9b06f3b80..1de89fbc67 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 6353ce41d0..c5727f6c02 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 11/95] 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 1c3aa1b164..26a47e2c8c 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 12/95] 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 0bc298658f..3636f23649 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 13/95] 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 d9ab17f4b4..3f37a9837f 100755 --- a/Tools/autotest/logger_metadata/enum_parse.py +++ b/Tools/autotest/logger_metadata/enum_parse.py @@ -92,6 +92,11 @@ class EnumDocco(object): 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 @@ class EnumDocco(object): 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 @@ class EnumDocco(object): 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 @@ class EnumDocco(object): 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 14/95] 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 ae5197297c..432ce26376 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 15/95] 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 97e2b86c68..8f0f642cde 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 16/95] 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 bc03742320..ab69c46824 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 17/95] 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 024d1f56a2..fc0bc9ecad 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 18/95] 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 fc0bc9ecad..daf54ed64e 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 19/95] 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 0000000000..8e6e95601c --- /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 b7fcf460c3..f5aaa58839 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 92bdbf2a4b..ded3451f99 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 @@ private: #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 0e86496dd5..667149d6aa 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 b8f31e368c..9bc3a55126 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 0000000000..992213a287 --- /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 0000000000..e6a6bfcca7 --- /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 0000000000..3adbc5233c --- /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 0000000000..d1c68fe120 --- /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 0000000000..d6579f51b2 --- /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 0000000000..ade6df7994 --- /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 20/95] 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 d066a2d32b..f8af788e8b 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 0000000000..38ff02804d --- /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 f5aaa58839..ef4e14a351 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 ded3451f99..8453577ec1 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 @@ private: 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 @@ private: // 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 f578d2e8a4..b9bde199dc 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 9bc3a55126..5785ca23b1 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 0000000000..a06a0da8cb --- /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 21/95] 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 415d7424a4..6bfba2ee17 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -453,9 +453,12 @@ private: 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 9e1eab32eb..a7146dd06e 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 22/95] 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 e33abf569d..6ef06d4216 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 8ff983a692..ffa148d6b5 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 23/95] 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 83e75ffdb2..08da786b2d 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 24/95] 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 d2800c3676..9256fdfe01 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 25/95] 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 5b86ade1f5..7f1ecabb7f 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 675b3b07ea..417468160c 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 60c0818e63..48b6141c9a 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 @@ public: 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 @@ private: // 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 1e4e6da598..6c2d169db8 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 8f54254821..ef7f785bea 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 0eee405833..7ba292bc5f 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -530,8 +530,8 @@ public: // 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 a0b83b36ca..0d639e1276 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 29e2dfa7a3..4d133f6276 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 @@ protected: 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 3faf4609c6..0da560ea09 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 d49ce3d01a..3002e773c0 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 57864b01be..f143505977 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 faf22b0180..5579b51fb7 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 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: 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 45eccc3e84..e382cb5941 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 26/95] 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 12f5b8c394..a11b8c63b1 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -287,6 +287,7 @@ class TestBuildOptions(object): 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 3f5dd534ff..54be58a3bd 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -156,6 +156,7 @@ BUILD_OPTIONS = [ 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 eec1399c02..36a375522a 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -278,6 +278,7 @@ class ExtractFeatures(object): ('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 27/95] 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 5140f1130b..64d400165f 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 28/95] 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 3492718ea4..1f0188917d 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 29/95] 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 b92ea9c406..a7e6c3fc17 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 30/95] 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 1f0188917d..2b86e63f73 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 31/95] 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 7f5c075a82..d7b7d8c314 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 32/95] 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 fe7f29f7ea..8dc31bbfa3 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 7db333f6b7..79a8a5eaf0 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 a8765e1743..10225afa72 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 94f9e7e31b..531d01c30a 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 ec27051057..35fe60265b 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 631acc8778..9e88a32417 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 79bd42af77..581eda3690 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 bd638fa42a..71875ad159 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 33aef375c4..6ff1a1ad1a 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 9b003de0fe..ace8903663 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 895143e80b..07b3d3d165 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 e70c030036..165d0d1a58 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 ecf1f7cbd8..52537f171d 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 @@ private: 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 743708fa9d..f2446c2af7 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 8ead11561f..a1da63210b 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 1c91af6a43..e16a29586d 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 20285b0b5b..7f7f280b22 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 @@ private: 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 bfe93ec83a..dc1fae7007 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 1de89fbc67..aa443483e8 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 1e045c8fd1..1100586cd1 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 c5727f6c02..86296d814b 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 31a6684023..0057f4ae37 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 c7b063b20e..c6e0791c2a 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 eba243eaba..6e270de102 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 b599b4c251..b514706f15 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 60bf156acb..ff76f0583d 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 33/95] 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 77617cdb02..e1c6f5d863 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 1d03debb9a..da6d55a03d 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 @@ public: 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 34/95] 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 0181f392d8..d69e3a3842 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 35/95] 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 af55acaa1f..4850e40b02 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 36/95] 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 4ff1c0c046..b5452aceda 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -110,9 +110,6 @@ private: 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 37/95] 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 dc1fae7007..4e468a7ae5 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 3d52f0ff62..9c8882936c 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.h +++ b/libraries/AP_Mount/AP_Mount_Topotek.h @@ -272,7 +272,7 @@ private: // 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 38/95] 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 332f9814dc..d7c295a08c 100755 --- a/Tools/scripts/powr_change.py +++ b/Tools/scripts/powr_change.py @@ -70,8 +70,6 @@ class POWRChange(object): 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 39/95] 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 19dbbb5ee7..6899e55e99 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 @@ group.add_option("", "--map", 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 40/95] 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 00d149e149..817cb2e947 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -136,6 +136,7 @@ public: 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 daf54ed64e..d22a7a224f 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 b3946aa81d..6b4fc49a8c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -37,6 +37,7 @@ public: 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 41/95] 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 ee80f080ae..8841d06d87 100755 --- a/Tools/scripts/decode_devid.py +++ b/Tools/scripts/decode_devid.py @@ -104,6 +104,7 @@ imu_types = { 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 42/95] 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 154ef72f8b..7ccddaad10 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -361,10 +361,10 @@ class Board: '-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 43/95] 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 ef7f785bea..fd5eea3e86 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 7ba292bc5f..0cf0a8cb51 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -499,6 +499,11 @@ public: // 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 44/95] 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 377eb9ded8..d29243d1ef 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -483,6 +483,9 @@ public: // 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 @@ public: 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 e4f97acc2c..8bece6d418 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 230b372499..8260924afc 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -159,6 +159,9 @@ protected: // 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 45/95] 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 05487e62a0..7f027a31a3 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 46/95] 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 723e68f64f..a2bef78e47 100644 --- a/libraries/AP_HAL/HAL.h +++ b/libraries/AP_HAL/HAL.h @@ -67,9 +67,6 @@ public: 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 @@ public: _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 47/95] 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 e285763c4f..08939bccf9 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 48/95] 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 ed438d60f3..e7173bdca9 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 49/95] 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 ef4e14a351..b70c026b6a 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 50/95] 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 b70c026b6a..05ba28f06d 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 51/95] 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 32d76256c4..b4953565cc 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 52/95] 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 54f62a382f..afe6f07ac7 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 53/95] 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 d643d8b013..b2db9099f8 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 d29243d1ef..24645b3b88 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -359,6 +359,7 @@ public: 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 49e3e07b46..672631e373 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 54/95] 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 722c53dcaf..8135a7d160 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 55/95] 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 bc0d157258..48334ab8d7 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -4831,6 +4831,34 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): 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 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): 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 56/95] 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 070276dd45..79a69b88b2 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 08ebdac605..467c056ab4 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 eb55bd76e4..380730a9af 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 70f4cb36fc..2a70a1c571 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -71,7 +71,9 @@ public: #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 57/95] GCS_MAVLink: add specific defines for sending of GPS mavlink messages --- libraries/GCS_MAVLink/GCS_Common.cpp | 30 ++++++++++++++++++---------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 9a1528b9dc..69598bf695 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}, - { MAVLINK_MSG_ID_GPS_RTK, MSG_GPS_RTK}, -#if GPS_MAX_RECEIVERS > 1 - { MAVLINK_MSG_ID_GPS2_RAW, MSG_GPS2_RAW}, - { MAVLINK_MSG_ID_GPS2_RTK, MSG_GPS2_RTK}, #endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED + { MAVLINK_MSG_ID_GPS_RTK, MSG_GPS_RTK}, +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED + { MAVLINK_MSG_ID_GPS2_RAW, MSG_GPS2_RAW}, +#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 58/95] 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 31c1148db9..93e6638304 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 59/95] 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 6c2d169db8..c07b4807a2 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 60/95] 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 9256fdfe01..b733f7b7bf 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 61/95] 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 0d0bad47eb..c2bbf8b5c4 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 62/95] 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 b132d80160..4d91191a02 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 63/95] 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 6a39311ead..3704110ec9 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 a3df4631fe..f35ee3a580 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 64/95] 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 467c056ab4..550add0e1f 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 65/95] 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 b9100f5f17..2a059af35b 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 66/95] 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 69598bf695..806831a345 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 5f059cb9a1..2c8800a959 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 67/95] 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 a11b8c63b1..7cbc005c31 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -258,7 +258,6 @@ class TestBuildOptions(object): '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 54be58a3bd..f31edfaf87 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -364,7 +364,6 @@ BUILD_OPTIONS = [ 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 36a375522a..de6eec6589 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -273,7 +273,6 @@ class ExtractFeatures(object): ('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 68/95] 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 00988f1a0a..10f9deed01 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -618,9 +618,9 @@ public: // 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 69/95] 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 562ac60607..3005e40d3a 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 70/95] 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 e27ef29cdc..70f9754d62 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 71/95] 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 24631a448d..e19c52a4bf 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -473,7 +473,7 @@ public: 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 @@ public: 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 @@ public: 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 @@ private: #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 @@ private: // 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 c117915e8a..3fb4e92274 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 67a9af5b00..3ef0c7e343 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 72/95] 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 435134d618..a35a6f3251 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 23e1e0cab5..5ca8bc0b54 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 73/95] 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 417468160c..84e31f3ebb 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 703f5cf722..c0d26041c0 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 b4c5c11906..722127898e 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 e382cb5941..328eb83150 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 b92178d098..9873d1a284 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 74/95] 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 d69e3a3842..1e10df8c42 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 cb2048be6d..49835adcfd 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 672631e373..b8db6e14b8 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 75/95] 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 df4729ba01..002fbfbaad 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 5d45a2c00f..7bfe51c249 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 76/95] 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 83f8e62073..8ae8c0b704 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 35ba08b90c..c2993c99bd 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 4b0c5a9a70..47212c5012 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 77/95] 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 a8fc94074d..7460a561e0 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 6911b78b74..06405d5e31 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 78/95] 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 2dbfbe07f5..b4d9d410a5 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 79/95] 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 f28db5c924..336eea61e3 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 80/95] 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 70f9754d62..4b63101d5f 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 81/95] 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 e19c52a4bf..e4778b2773 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -538,9 +538,8 @@ public: } 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 3ef0c7e343..c1fda6226c 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 82/95] 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 c0d26041c0..28282c3539 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 722127898e..0c1d7ea70e 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 328eb83150..c37a4d55a2 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 83/95] 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 b8db6e14b8..aa3de533d3 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 84/95] 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 7bfe51c249..504c6651d9 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 85/95] 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 c2993c99bd..90fb3308a9 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 86/95] 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 b4d9d410a5..8feb3e0805 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 87/95] 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 4b4f30d79e..1d4c7cc77f 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 88/95] 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 8edbc60474..8bb1108de3 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -412,6 +412,10 @@ public: 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 @@ private: // 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 806831a345..9922a4f837 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 f12155294d..17e74ee0ba 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -35,6 +35,7 @@ protected: 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 a14be7cd53..b82e64611f 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 89/95] 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 c07b4807a2..b873753207 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 e38c9b6e7a..558804c05e 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -64,6 +64,10 @@ protected: 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 90/95] 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 b733f7b7bf..298b4e47a5 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 9e03940882..2fa88274e1 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -50,6 +50,10 @@ protected: 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 91/95] 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 3704110ec9..a903e591c6 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 b39a8cfb36..fd92cac3d5 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -40,6 +40,10 @@ protected: 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 92/95] 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 93e6638304..c9ec2050c7 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 a431f5217e..af98ee5a39 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -30,6 +30,10 @@ protected: 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 d6a10022dc..bfdcad9ac7 100644 --- a/AntennaTracker/mode.h +++ b/AntennaTracker/mode.h @@ -22,6 +22,7 @@ public: // 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 @@ protected: 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 @@ public: 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 @@ private: 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 @@ public: 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 @@ public: 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 @@ public: 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 @@ public: 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 93/95] 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 4d91191a02..1eb5985e81 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 35276b03a4..90c4881b2a 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -48,6 +48,10 @@ protected: 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 8a990fbb31..8718d47fa1 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -52,6 +52,9 @@ public: 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 @@ protected: return "MANU"; } + Mode::Number number() const override { return Mode::Number::MANUAL; } + private: }; @@ -201,6 +206,8 @@ protected: return "VELY"; } + Mode::Number number() const override { return Mode::Number::VELOCITY; } + private: }; @@ -244,6 +251,8 @@ protected: return "LOIT"; } + Mode::Number number() const override { return Mode::Number::LOITER; } + private: Vector3f target_pos; float target_yaw; @@ -286,6 +295,8 @@ protected: return "LAND"; } + Mode::Number number() const override { return Mode::Number::LAND; } + private: }; @@ -328,4 +339,7 @@ protected: { 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 94/95] 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 c2bbf8b5c4..f17901638d 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 c38ec3f4ab..7dc2d0c1a2 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -37,6 +37,10 @@ protected: 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 11a6447167..35780cb9c0 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -72,6 +72,9 @@ public: 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 @@ protected: 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 @@ protected: 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 @@ protected: 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 @@ protected: 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 @@ protected: 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 @@ protected: 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 @@ protected: 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 @@ protected: 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 @@ protected: 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 @@ protected: 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 @@ protected: 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 95/95] 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 08da786b2d..4c64b9522f 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 08da786b2d94ce0955a82f92ab2166c347259623 +Subproject commit 4c64b9522f9bb9815b089ab98cf2f33f11bded52