Merge branch 'master' into pr-webots-heron-usv

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
 };
@@ -315,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[] = {
@@ -643,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;
+}

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;

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();

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 {};
 };

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);

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)

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
 

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
@@ -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

ArduCopter/Copter.h

@@ -137,7 +137,7 @@
  #include <AP_OSD/AP_OSD.h>
 #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
 

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,
@@ -583,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,
@@ -1519,7 +1526,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;
     }
@@ -1657,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;
+}

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

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),
@@ -1240,8 +1240,11 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = {
 /*
   constructor for g2 object
  */
-ParametersG2::ParametersG2(void)
+ParametersG2::ParametersG2(void) :
-    : command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f)
+    unused_integer{17}
+#if HAL_BUTTON_ENABLED
+    ,button_ptr(&copter.button)
+#endif
 #if AP_TEMPCALIBRATION_ENABLED
     , temp_calibration()
 #endif
@@ -1251,21 +1254,21 @@ ParametersG2::ParametersG2(void)
 #if HAL_PROXIMITY_ENABLED
     , proximity()
 #endif
-#if ADVANCED_FAILSAFE
+#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
     ,afs()
 #endif
 #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

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;
@@ -530,8 +535,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

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

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 <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
 
 /*
@@ -44,5 +46,5 @@ protected:
     void set_mode_auto(void) override;
 };
 
-#endif // ADVANCED_FAILSAFE
+#endif // AP_COPTER_ADVANCED_FAILSAFE_ENABLED
 

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);
-        SRV_Channels::push();
+        srv.push();
 
         // read some compass values
         compass.read();

ArduCopter/config.h

@@ -574,8 +574,8 @@
 // Developer Items
 //
 
-#ifndef ADVANCED_FAILSAFE
+#ifndef AP_COPTER_ADVANCED_FAILSAFE_ENABLED
-# define ADVANCED_FAILSAFE 0
+# define AP_COPTER_ADVANCED_FAILSAFE_ENABLED 0
 #endif
 
 #ifndef CH_MODE_DEFAULT

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);
-        SRV_Channels::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);
-    SRV_Channels::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);
-        SRV_Channels::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);
-        SRV_Channels::push();
+        srv.push();
         esc_calibration_notify();
         hal.scheduler->delay(3);
     }

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);

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
 */

ArduCopter/mode.h

@@ -4,7 +4,7 @@
 #include <AP_Math/chirp.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h> // 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

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 / (fabsf(velocity) + 60.0f));
-        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));
-    }
 
     // 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);
@@ -592,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;

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
@@ -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
-    SRV_Channels::push();
+    srv.push();
 }
 
 // check for pilot stick input to trigger lost vehicle alarm

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);

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
@@ -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);

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,
@@ -710,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,
@@ -863,6 +870,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 +983,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
+        switch(HEADING_TYPE(packet.param1)) {
-        if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int
+        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
+            break;
-        } else if (int(packet.param1) == HEADING_TYPE_HEADING) { // compare as nearest int
+        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;
         }
@@ -1530,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;
+}

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);

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. 

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,
     };
@@ -483,6 +484,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 +583,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;

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),

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)
 {
 }
 

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;

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;

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;

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();
     

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);
@@ -853,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
@@ -1012,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();
@@ -1050,7 +1056,7 @@ void Plane::servos_output(void)
 
     SRV_Channels::output_ch_all();
 
-    SRV_Channels::push();
+    srv.push();
 
     if (g2.servo_channels.auto_trim_enabled()) {
         servos_auto_trim();

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

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,
@@ -437,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[] = {
@@ -962,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;
+}

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;

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; }
 };

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();
-        SRV_Channels::push();
+        srv.push();
     }
 }
 

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;
 

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,
@@ -395,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,
@@ -608,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;
+}

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;

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; }
+
 };

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
-    SRV_Channels::push();
+    srv.push();
-}
+}

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();

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,
@@ -613,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,
@@ -1148,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;
+}

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;

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);

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();

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
 };

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);
 }
 

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<SERVO_OUT_MOTOR_MAX; i++) {
         servo_channels.set_esc_scaling_for(SRV_Channels::get_motor_function(i));
@@ -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();
-    SRV_Channels::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;

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',

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,

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
 

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:

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',

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
@@ -287,6 +286,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:

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
 

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"
 

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 <https://www.gnu.org/licenses/>.
+
+"""
+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

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

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.

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
 
@@ -363,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

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 = {

Tools/scripts/extract_features.py

@@ -273,11 +273,11 @@ 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'),
 
             ('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'),

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

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

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
 

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),    
 

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -17,6 +17,7 @@
 #include <AP_Arming/AP_Arming.h>
 # endif // AP_DDS_ARM_SERVER_ENABLED
 #include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_Common/AP_FWVersion.h>
 #include <AP_ExternalControl/AP_ExternalControl_config.h>
 
 #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,
@@ -81,6 +85,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
@@ -604,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<uint8_t>(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
  */
@@ -801,6 +866,198 @@ 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, &parameter_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
+        };
+
+        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);
+        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
+        };
+
+        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);
+        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 +1204,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<uint16_t>(domain_id), participant_name, UXR_REPLACE);
 
@@ -1257,6 +1514,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);
@@ -1336,6 +1610,14 @@ 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;
+    }
+#endif // AP_DDS_STATUS_PUB_ENABLED
 
     status_ok = uxr_run_session_time(&session, 1);
 }

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
@@ -46,6 +49,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 <AP_HAL/AP_HAL.h>
 #include <AP_HAL/Scheduler.h>
@@ -175,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;
@@ -201,6 +223,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};
@@ -255,6 +285,7 @@ private:
     // client key we present
     static constexpr uint32_t key = 0xAAAABBBB;
 
+
 public:
     ~AP_DDS_Client();
 

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
 };

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),

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
@@ -126,6 +134,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 +151,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

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<uint8> failsafe;
+    };
+  };
+};

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;
+    };
+  };
+};

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;
+    };
+  };
+};

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<octet> byte_array_value;
+
+      @verbatim (language="comment", text=
+        "An array of boolean values.")
+      sequence<boolean> bool_array_value;
+
+      @verbatim (language="comment", text=
+        "An array of 64-bit integer values.")
+      sequence<int64> integer_array_value;
+
+      @verbatim (language="comment", text=
+        "An array of 64-bit floating point values.")
+      sequence<double> double_array_value;
+
+      @verbatim (language="comment", text=
+        "An array of string values.")
+      sequence<string> string_array_value;
+    };
+  };
+};

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;
+    };
+  };
+};

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<string> 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<rcl_interfaces::msg::ParameterValue> values;
+    };
+  };
+};

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<rcl_interfaces::msg::Parameter> parameters;
+    };
+    @verbatim (language="comment", text=
+      "Indicates whether setting each parameter succeeded or not and why.")
+    struct SetParameters_Response {
+      sequence<rcl_interfaces::msg::SetParametersResult> results;
+    };
+  };
+};

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) {

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;

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 && (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 && (AP_GPS_SBF_ENABLED || AP_GPS_ERB_ENABLED)
+#endif

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
+
 
 
 /*

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
 

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

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) {

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

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*
 

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

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),

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)
 {

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);
         }
     }

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;

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
+    auto &xrc = rc();
-    MIX_UPDATE(mixing.rc_channel[0], rcmap->roll());
+    // note that if not all of these channels are specified correctly
-    MIX_UPDATE(mixing.rc_channel[1], rcmap->pitch());
+    // in parameters then these may be a "dummy" RC channel pointer.
-    MIX_UPDATE(mixing.rc_channel[2], rcmap->throttle());
+    // In that case c->ch() will be zero.
-    MIX_UPDATE(mixing.rc_channel[3], rcmap->yaw());
+    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);
+        const auto *c = channels[i];
-        if (!c) {
+        MIX_UPDATE(mixing.rc_channel[i], c->ch());
-            continue;
-        }
         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());

libraries/AP_IOMCU/AP_IOMCU.h

@@ -11,7 +11,6 @@
 #if HAL_WITH_IO_MCU
 
 #include "iofirmware/ioprotocol.h"
-#include <AP_RCMapper/AP_RCMapper.h>
 #include <AP_HAL/RCOutput.h>
 #include <AP_ESC_Telem/AP_ESC_Telem_Backend.h>
 
@@ -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