diff --git a/.github/workflows/test_environment.yml b/.github/workflows/test_environment.yml
index 1442f2baa80978..c84151418d9859 100644
--- a/.github/workflows/test_environment.yml
+++ b/.github/workflows/test_environment.yml
@@ -32,8 +32,6 @@ jobs:
             name: focal
           - os: ubuntu
             name: jammy
-          - os: ubuntu
-            name: mantic
           - os: ubuntu
             name: noble
           - os: archlinux
diff --git a/.github/workflows/test_sitl_blimp.yml b/.github/workflows/test_sitl_blimp.yml
index 7d9e55e51b2c85..ebd105943129db 100644
--- a/.github/workflows/test_sitl_blimp.yml
+++ b/.github/workflows/test_sitl_blimp.yml
@@ -229,7 +229,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v3
+        uses: actions/cache/restore@v4
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
diff --git a/.github/workflows/test_sitl_copter.yml b/.github/workflows/test_sitl_copter.yml
index 07efd7f4d143cf..86fd5ecebada2a 100644
--- a/.github/workflows/test_sitl_copter.yml
+++ b/.github/workflows/test_sitl_copter.yml
@@ -247,7 +247,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v3
+        uses: actions/cache/restore@v4
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -343,7 +343,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v3
+        uses: actions/cache/restore@v4
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
diff --git a/.github/workflows/test_sitl_plane.yml b/.github/workflows/test_sitl_plane.yml
index 813560a94109bd..3b1e41bd4c77bf 100644
--- a/.github/workflows/test_sitl_plane.yml
+++ b/.github/workflows/test_sitl_plane.yml
@@ -230,7 +230,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v3
+        uses: actions/cache/restore@v4
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
diff --git a/.github/workflows/test_sitl_rover.yml b/.github/workflows/test_sitl_rover.yml
index 3c29c931be908f..6adeb20060c1c8 100644
--- a/.github/workflows/test_sitl_rover.yml
+++ b/.github/workflows/test_sitl_rover.yml
@@ -244,7 +244,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v3
+        uses: actions/cache/restore@v4
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
diff --git a/.github/workflows/test_sitl_sub.yml b/.github/workflows/test_sitl_sub.yml
index 0a8555c114c9c5..427ba6c1f90b7b 100644
--- a/.github/workflows/test_sitl_sub.yml
+++ b/.github/workflows/test_sitl_sub.yml
@@ -244,7 +244,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v3
+        uses: actions/cache/restore@v4
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
diff --git a/.github/workflows/test_sitl_tracker.yml b/.github/workflows/test_sitl_tracker.yml
index 518d648b7f8777..278a2d446a86be 100644
--- a/.github/workflows/test_sitl_tracker.yml
+++ b/.github/workflows/test_sitl_tracker.yml
@@ -230,7 +230,7 @@ jobs:
           NOW=$(date -u +"%F-%T")
           echo "timestamp=${NOW}" >> $GITHUB_OUTPUT
       - name: ccache cache files
-        uses: actions/cache/restore@v3
+        uses: actions/cache/restore@v4
         with:
           path: ~/.ccache
           key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}}
diff --git a/.github/workflows/test_size.yml b/.github/workflows/test_size.yml
index 30e7f4bba2a4dc..fe83756a3a7599 100644
--- a/.github/workflows/test_size.yml
+++ b/.github/workflows/test_size.yml
@@ -268,7 +268,7 @@ jobs:
         shell: bash
         run: |
           cd pr/
-          Tools/scripts/pretty_diff_size.py -m $GITHUB_WORKSPACE/base_branch_bin -s $GITHUB_WORKSPACE/pr_bin
+          Tools/scripts/build_tests/pretty_diff_size.py -m $GITHUB_WORKSPACE/base_branch_bin -s $GITHUB_WORKSPACE/pr_bin
 
       - name: Feature compare with  ${{ github.event.pull_request.base.ref }}
         shell: bash
@@ -301,8 +301,26 @@ jobs:
           Tools/scripts/extract_features.py "$EF_BASE_BRANCH_BINARY" -nm "${BIN_PREFIX}nm" >features-base_branch.txt
           Tools/scripts/extract_features.py "$EF_PR_BRANCH_BINARY" -nm "${BIN_PREFIX}nm" >features-pr.txt
           diff -u features-base_branch.txt features-pr.txt || true
+          diff_output=$(diff -u features-base_branch.txt features-pr.txt || true)
+          echo "### Features Diff Output" >> $GITHUB_STEP_SUMMARY
+          if [ -n "$diff_output" ]; then
+            echo '```diff' >> $GITHUB_STEP_SUMMARY
+            echo "$diff_output" >> $GITHUB_STEP_SUMMARY
+            echo '```' >> $GITHUB_STEP_SUMMARY
+          else
+            echo "No differences found." >> $GITHUB_STEP_SUMMARY
+          fi
 
       - name: Checksum compare with ${{ github.event.pull_request.base.ref }}
         shell: bash
         run: |
           diff -r $GITHUB_WORKSPACE/base_branch_bin_no_versions $GITHUB_WORKSPACE/pr_bin_no_versions --exclude=*.elf --exclude=*.apj || true
+          diff_output=$(diff -r $GITHUB_WORKSPACE/base_branch_bin_no_versions $GITHUB_WORKSPACE/pr_bin_no_versions --exclude=*.elf --exclude=*.apj || true || true)
+          echo "### Checksum Diff Output" >> $GITHUB_STEP_SUMMARY
+          if [ -n "$diff_output" ]; then
+            echo '```diff' >> $GITHUB_STEP_SUMMARY
+            echo "$diff_output" >> $GITHUB_STEP_SUMMARY
+            echo '```' >> $GITHUB_STEP_SUMMARY
+          else
+            echo "No differences found." >> $GITHUB_STEP_SUMMARY
+          fi
diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
index 84e31f3ebb326e..0f263b89c0621b 100644
--- a/ArduCopter/Copter.cpp
+++ b/ArduCopter/Copter.cpp
@@ -285,13 +285,9 @@ bool Copter::set_target_location(const Location& target_loc)
 
     return mode_guided.set_destination(target_loc);
 }
-#endif //MODE_GUIDED_ENABLED
-#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 
-#if AP_SCRIPTING_ENABLED
-#if MODE_GUIDED_ENABLED
 // start takeoff to given altitude (for use by scripting)
-bool Copter::start_takeoff(float alt)
+bool Copter::start_takeoff(const float alt)
 {
     // exit if vehicle is not in Guided mode or Auto-Guided mode
     if (!flightmode->in_guided_mode()) {
@@ -304,7 +300,11 @@ bool Copter::start_takeoff(float alt)
     }
     return false;
 }
+#endif //MODE_GUIDED_ENABLED
+#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 
+#if AP_SCRIPTING_ENABLED
+#if MODE_GUIDED_ENABLED
 // set target position (for use by scripting)
 bool Copter::set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt)
 {
@@ -411,7 +411,55 @@ bool Copter::set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_
     mode_guided.set_angle(q, ang_vel_body, throttle, true);
     return true;
 }
-#endif
+
+// Register a custom mode with given number and names
+AP_Vehicle::custom_mode_state* Copter::register_custom_mode(const uint8_t num, const char* full_name, const char* short_name)
+{
+    const Mode::Number number = (Mode::Number)num;
+
+    // See if this mode has been registered already, if it has return the state for it
+    // This allows scripting restarts
+    for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
+        if (mode_guided_custom[i] == nullptr) {
+            break;
+        }
+        if ((mode_guided_custom[i]->mode_number() == number) &&
+            (strcmp(mode_guided_custom[i]->name(), full_name) == 0) &&
+            (strncmp(mode_guided_custom[i]->name4(), short_name, 4) == 0)) {
+            return &mode_guided_custom[i]->state;
+        }
+    }
+
+    // Number already registered to existing mode
+    if (mode_from_mode_num(number) != nullptr) {
+        return nullptr;
+    }
+
+    // Find free slot
+    for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
+        if (mode_guided_custom[i] == nullptr) {
+            // Duplicate strings so were not pointing to unknown memory
+            const char* full_name_copy = strdup(full_name);
+            const char* short_name_copy = strndup(short_name, 4);
+            if ((full_name_copy != nullptr) && (short_name_copy != nullptr)) {
+                mode_guided_custom[i] = NEW_NOTHROW ModeGuidedCustom(number, full_name_copy, short_name_copy);
+            }
+            if (mode_guided_custom[i] == nullptr) {
+                // Allocation failure
+                return nullptr;
+            }
+
+            // Registration sucsessful, notify the GCS that it should re-request the avalable modes
+            gcs().available_modes_changed();
+
+            return &mode_guided_custom[i]->state;
+        }
+    }
+
+    // No free slots
+    return nullptr;
+}
+#endif // MODE_GUIDED_ENABLED
 
 #if MODE_CIRCLE_ENABLED
 // circle mode controls
diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 48b6141c9a84db..db74c31dc412c1 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -668,12 +668,12 @@ class Copter : public AP_Vehicle {
 #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 #if MODE_GUIDED_ENABLED
     bool set_target_location(const Location& target_loc) override;
+    bool start_takeoff(const float alt) override;
 #endif // MODE_GUIDED_ENABLED
 #endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
 
 #if AP_SCRIPTING_ENABLED
 #if MODE_GUIDED_ENABLED
-    bool start_takeoff(float alt) override;
     bool get_target_location(Location& target_loc) override;
     bool update_target_location(const Location &old_loc, const Location &new_loc) override;
     bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) override;
@@ -684,6 +684,8 @@ class Copter : public AP_Vehicle {
     bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override;
     bool set_target_rate_and_throttle(float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps, float throttle) override;
 
+    // Register a custom mode with given number and names
+    AP_Vehicle::custom_mode_state* register_custom_mode(const uint8_t number, const char* full_name, const char* short_name) override;
 #endif
 #if MODE_CIRCLE_ENABLED
     bool get_circle_radius(float &radius_m) override;
@@ -1029,6 +1031,10 @@ class Copter : public AP_Vehicle {
 #endif
 #if MODE_GUIDED_ENABLED
     ModeGuided mode_guided;
+#if AP_SCRIPTING_ENABLED
+    // Custom modes registered at runtime
+    ModeGuidedCustom *mode_guided_custom[5];
+#endif
 #endif
     ModeLand mode_land;
 #if MODE_LOITER_ENABLED
diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
index b87375320753a8..1b5833ec75824d 100644
--- a/ArduCopter/GCS_Mavlink.cpp
+++ b/ArduCopter/GCS_Mavlink.cpp
@@ -1742,7 +1742,16 @@ uint8_t GCS_MAVLINK_Copter::send_available_mode(uint8_t index) const
 #endif
     };
 
-    const uint8_t mode_count = ARRAY_SIZE(modes);
+    const uint8_t base_mode_count = ARRAY_SIZE(modes);
+    uint8_t mode_count = base_mode_count;
+
+#if AP_SCRIPTING_ENABLED
+    for (uint8_t i = 0; i < ARRAY_SIZE(copter.mode_guided_custom); i++) {
+        if (copter.mode_guided_custom[i] != nullptr) {
+            mode_count += 1;
+        }
+    }
+#endif
 
     // Convert to zero indexed
     const uint8_t index_zero = index - 1;
@@ -1752,8 +1761,27 @@ uint8_t GCS_MAVLINK_Copter::send_available_mode(uint8_t index) const
     }
 
     // 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();
+    const char* name;
+    uint8_t mode_number;
+
+    if (index_zero < base_mode_count) {
+        name = modes[index_zero]->name();
+        mode_number = (uint8_t)modes[index_zero]->mode_number();
+
+    } else {
+#if AP_SCRIPTING_ENABLED
+        const uint8_t custom_index = index_zero - base_mode_count;
+        if (copter.mode_guided_custom[custom_index] == nullptr) {
+            // Invalid index, should not happen
+            return mode_count;
+        }
+        name = copter.mode_guided_custom[custom_index]->name();
+        mode_number = (uint8_t)copter.mode_guided_custom[custom_index]->mode_number();
+#else
+        // Should not endup here
+        return mode_count;
+#endif
+    }
 
 #if MODE_AUTO_ENABLED
     // Auto RTL is odd
diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp
index 5d8fc5a9e40bbb..b5900f88304791 100644
--- a/ArduCopter/mode.cpp
+++ b/ArduCopter/mode.cpp
@@ -32,158 +32,141 @@ PayloadPlace Mode::payload_place;
 // return the static controller object corresponding to supplied mode
 Mode *Copter::mode_from_mode_num(const Mode::Number mode)
 {
-    Mode *ret = nullptr;
 
     switch (mode) {
 #if MODE_ACRO_ENABLED
         case Mode::Number::ACRO:
-            ret = &mode_acro;
-            break;
+            return &mode_acro;
 #endif
 
         case Mode::Number::STABILIZE:
-            ret = &mode_stabilize;
-            break;
+            return &mode_stabilize;
 
         case Mode::Number::ALT_HOLD:
-            ret = &mode_althold;
-            break;
+            return &mode_althold;
 
 #if MODE_AUTO_ENABLED
         case Mode::Number::AUTO:
-            ret = &mode_auto;
-            break;
+            return &mode_auto;
 #endif
 
 #if MODE_CIRCLE_ENABLED
         case Mode::Number::CIRCLE:
-            ret = &mode_circle;
-            break;
+            return &mode_circle;
 #endif
 
 #if MODE_LOITER_ENABLED
         case Mode::Number::LOITER:
-            ret = &mode_loiter;
-            break;
+            return &mode_loiter;
 #endif
 
 #if MODE_GUIDED_ENABLED
         case Mode::Number::GUIDED:
-            ret = &mode_guided;
-            break;
+            return &mode_guided;
 #endif
 
         case Mode::Number::LAND:
-            ret = &mode_land;
-            break;
+            return &mode_land;
 
 #if MODE_RTL_ENABLED
         case Mode::Number::RTL:
-            ret = &mode_rtl;
-            break;
+            return &mode_rtl;
 #endif
 
 #if MODE_DRIFT_ENABLED
         case Mode::Number::DRIFT:
-            ret = &mode_drift;
-            break;
+            return &mode_drift;
 #endif
 
 #if MODE_SPORT_ENABLED
         case Mode::Number::SPORT:
-            ret = &mode_sport;
-            break;
+            return &mode_sport;
 #endif
 
 #if MODE_FLIP_ENABLED
         case Mode::Number::FLIP:
-            ret = &mode_flip;
-            break;
+            return &mode_flip;
 #endif
 
 #if AUTOTUNE_ENABLED
         case Mode::Number::AUTOTUNE:
-            ret = &mode_autotune;
-            break;
+            return &mode_autotune;
 #endif
 
 #if MODE_POSHOLD_ENABLED
         case Mode::Number::POSHOLD:
-            ret = &mode_poshold;
-            break;
+            return &mode_poshold;
 #endif
 
 #if MODE_BRAKE_ENABLED
         case Mode::Number::BRAKE:
-            ret = &mode_brake;
-            break;
+            return &mode_brake;
 #endif
 
 #if MODE_THROW_ENABLED
         case Mode::Number::THROW:
-            ret = &mode_throw;
-            break;
+            return &mode_throw;
 #endif
 
 #if HAL_ADSB_ENABLED
         case Mode::Number::AVOID_ADSB:
-            ret = &mode_avoid_adsb;
-            break;
+            return &mode_avoid_adsb;
 #endif
 
 #if MODE_GUIDED_NOGPS_ENABLED
         case Mode::Number::GUIDED_NOGPS:
-            ret = &mode_guided_nogps;
-            break;
+            return &mode_guided_nogps;
 #endif
 
 #if MODE_SMARTRTL_ENABLED
         case Mode::Number::SMART_RTL:
-            ret = &mode_smartrtl;
-            break;
+            return &mode_smartrtl;
 #endif
 
 #if MODE_FLOWHOLD_ENABLED
         case Mode::Number::FLOWHOLD:
-            ret = (Mode *)g2.mode_flowhold_ptr;
-            break;
+            return (Mode *)g2.mode_flowhold_ptr;
 #endif
 
 #if MODE_FOLLOW_ENABLED
         case Mode::Number::FOLLOW:
-            ret = &mode_follow;
-            break;
+            return &mode_follow;
 #endif
 
 #if MODE_ZIGZAG_ENABLED
         case Mode::Number::ZIGZAG:
-            ret = &mode_zigzag;
-            break;
+            return &mode_zigzag;
 #endif
 
 #if MODE_SYSTEMID_ENABLED
         case Mode::Number::SYSTEMID:
-            ret = (Mode *)g2.mode_systemid_ptr;
-            break;
+            return (Mode *)g2.mode_systemid_ptr;
 #endif
 
 #if MODE_AUTOROTATE_ENABLED
         case Mode::Number::AUTOROTATE:
-            ret = &mode_autorotate;
-            break;
+            return &mode_autorotate;
 #endif
 
 #if MODE_TURTLE_ENABLED
         case Mode::Number::TURTLE:
-            ret = &mode_turtle;
-            break;
+            return &mode_turtle;
 #endif
 
         default:
             break;
     }
 
-    return ret;
+#if MODE_GUIDED_ENABLED && AP_SCRIPTING_ENABLED
+    // Check registered custom modes
+    for (uint8_t i = 0; i < ARRAY_SIZE(mode_guided_custom); i++) {
+        if ((mode_guided_custom[i] != nullptr) && (mode_guided_custom[i]->mode_number() == mode)) {
+            return mode_guided_custom[i];
+        }
+    }
+#endif
+
+    return nullptr;
 }
 
 
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
index 5579b51fb786ee..4b142409bc80cd 100644
--- a/ArduCopter/mode.h
+++ b/ArduCopter/mode.h
@@ -1193,14 +1193,37 @@ class ModeGuided : public Mode {
     void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
 
     // controls which controller is run (pos or vel):
-    SubMode guided_mode = SubMode::TakeOff;
-    bool send_notification;     // used to send one time notification to ground station
-    bool takeoff_complete;      // true once takeoff has completed (used to trigger retracting of landing gear)
+    static SubMode guided_mode;
+    static bool send_notification;     // used to send one time notification to ground station
+    static bool takeoff_complete;      // true once takeoff has completed (used to trigger retracting of landing gear)
 
     // guided mode is paused or not
-    bool _paused;
+    static bool _paused;
 };
 
+#if AP_SCRIPTING_ENABLED
+// Mode which behaves as guided with custom mode number and name
+class ModeGuidedCustom : public ModeGuided {
+public:
+    // constructor registers custom number and names
+    ModeGuidedCustom(const Number _number, const char* _full_name, const char* _short_name);
+
+    bool init(bool ignore_checks) override;
+
+    Number mode_number() const override { return number; }
+
+    const char *name() const override { return full_name; }
+    const char *name4() const override { return short_name; }
+
+    // State object which can be edited by scripting
+    AP_Vehicle::custom_mode_state state;
+
+private:
+    const Number number;
+    const char* full_name;
+    const char* short_name;
+};
+#endif
 
 class ModeGuidedNoGPS : public ModeGuided {
 
diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index f897b660c2335b..1f8460fad4a749 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/ArduCopter/mode_auto.cpp
@@ -824,7 +824,7 @@ void ModeAuto::exit_mission()
 bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
 {
     // only process guided waypoint if we are in guided mode
-    if (copter.flightmode->mode_number() != Mode::Number::GUIDED && !(copter.flightmode->mode_number() == Mode::Number::AUTO && _mode == SubMode::NAVGUIDED)) {
+    if (!copter.flightmode->in_guided_mode()) {
         return false;
     }
 
diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp
index f7ce7642a09815..ba0321a1444ecd 100644
--- a/ArduCopter/mode_guided.cpp
+++ b/ArduCopter/mode_guided.cpp
@@ -7,7 +7,7 @@
  */
 
 static Vector3p guided_pos_target_cm;       // position target (used by posvel controller only)
-bool guided_pos_terrain_alt;                // true if guided_pos_target_cm.z is an alt above terrain
+static bool guided_pos_terrain_alt;         // true if guided_pos_target_cm.z is an alt above terrain
 static Vector3f guided_vel_target_cms;      // velocity target (used by pos_vel_accel controller and vel_accel controller)
 static Vector3f guided_accel_target_cmss;   // acceleration target (used by pos_vel_accel controller vel_accel controller and accel controller)
 static uint32_t update_time_ms;             // system time of last target update to pos_vel_accel, vel_accel or accel controller
@@ -30,7 +30,15 @@ struct Guided_Limit {
     float horiz_max_cm; // horizontal position limit in cm from where guided mode was initiated (0 = no limit)
     uint32_t start_time;// system time in milliseconds that control was handed to the external computer
     Vector3f start_pos; // start position as a distance from home in cm.  used for checking horiz_max limit
-} guided_limit;
+} static guided_limit;
+
+// controls which controller is run (pos or vel):
+ModeGuided::SubMode ModeGuided::guided_mode = SubMode::TakeOff;
+bool ModeGuided::send_notification;     // used to send one time notification to ground station
+bool ModeGuided::takeoff_complete;      // true once takeoff has completed (used to trigger retracting of landing gear)
+
+// guided mode is paused or not
+bool ModeGuided::_paused;
 
 // init - initialise guided controller
 bool ModeGuided::init(bool ignore_checks)
diff --git a/ArduCopter/mode_guided_custom.cpp b/ArduCopter/mode_guided_custom.cpp
new file mode 100644
index 00000000000000..322ed74a1f0085
--- /dev/null
+++ b/ArduCopter/mode_guided_custom.cpp
@@ -0,0 +1,23 @@
+#include "Copter.h"
+
+#if MODE_GUIDED_ENABLED && AP_SCRIPTING_ENABLED
+// constructor registers custom number and names
+ModeGuidedCustom::ModeGuidedCustom(const Number _number, const char* _full_name, const char* _short_name):
+    number(_number),
+    full_name(_full_name),
+    short_name(_short_name)
+{
+}
+
+bool ModeGuidedCustom::init(bool ignore_checks)
+{
+    // Stript can block entry
+    if (!state.allow_entry) {
+        return false;
+    }
+
+    // Guided entry checks must also pass
+    return ModeGuided::init(ignore_checks);
+}
+
+#endif
diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 1e10df8c42784d..7d27791de5571d 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -139,6 +139,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
 #if AC_PRECLAND_ENABLED
     SCHED_TASK(precland_update, 400, 50, 160),
 #endif
+#if AP_QUICKTUNE_ENABLED
+    SCHED_TASK(update_quicktune, 40, 100, 163),
+#endif
 };
 
 void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
@@ -1025,4 +1028,16 @@ void Plane::precland_update(void)
 }
 #endif
 
+#if AP_QUICKTUNE_ENABLED
+/*
+  update AP_Quicktune object. We pass the supports_quicktune() method
+  in so that quicktune can detect if the user changes to a
+  non-quicktune capable mode while tuning and the gains can be reverted
+ */
+void Plane::update_quicktune(void)
+{
+    quicktune.update(control_mode->supports_quicktune());
+}
+#endif
+
 AP_HAL_MAIN_CALLBACKS(&plane);
diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
index 5f746640e16e21..04a5ba3a595258 100644
--- a/ArduPlane/Parameters.cpp
+++ b/ArduPlane/Parameters.cpp
@@ -1038,6 +1038,12 @@ const AP_Param::Info Plane::var_info[] = {
     // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp
     PARAM_VEHICLE_INFO,
 
+#if AP_QUICKTUNE_ENABLED
+    // @Group: QWIK_
+    // @Path: ../libraries/AP_Quicktune/AP_Quicktune.cpp
+    GOBJECT(quicktune, "QWIK_",  AP_Quicktune),
+#endif
+    
     AP_VAREND
 };
 
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 24645b3b88471d..e7cc5326f4280f 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -362,6 +362,7 @@ class Parameters {
         k_param_takeoff_throttle_idle,
 
         k_param_pullup = 270,
+        k_param_quicktune,
     };
 
     AP_Int16 format_version;
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 64b5ba6d956471..d2d35a7b14e0a6 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -330,6 +330,10 @@ class Plane : public AP_Vehicle {
     ModeThermal mode_thermal;
 #endif
 
+#if AP_QUICKTUNE_ENABLED
+    AP_Quicktune quicktune;
+#endif
+    
     // This is the state of the flight control system
     // There are multiple states defined such as MANUAL, FBW-A, AUTO
     Mode *control_mode = &mode_initializing;
@@ -875,6 +879,10 @@ class Plane : public AP_Vehicle {
     static const TerrainLookupTable Terrain_lookup[];
 #endif
 
+#if AP_QUICKTUNE_ENABLED
+    void update_quicktune(void);
+#endif
+
     // Attitude.cpp
     void adjust_nav_pitch_throttle(void);
     void update_load_factor(void);
@@ -1159,7 +1167,7 @@ class Plane : public AP_Vehicle {
     void servos_twin_engine_mix();
     void force_flare();
     void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle);
-    void throttle_slew_limit(SRV_Channel::Aux_servo_function_t func);
+    void throttle_slew_limit();
     bool suppress_throttle(void);
     void update_throttle_hover();
     void channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in,
diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp
index ac05d1174af0a0..2dc6f1edaab017 100644
--- a/ArduPlane/RC_Channel.cpp
+++ b/ArduPlane/RC_Channel.cpp
@@ -1,6 +1,7 @@
 #include "Plane.h"
 
 #include "RC_Channel.h"
+#include "qautotune.h"
 
 // defining these two macros and including the RC_Channels_VarInfo
 // header defines the parameter information common to all vehicle
@@ -176,6 +177,9 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option,
 #endif
 #if QAUTOTUNE_ENABLED
     case AUX_FUNC::AUTOTUNE_TEST_GAINS:
+#endif
+#if AP_QUICKTUNE_ENABLED
+    case AUX_FUNC::QUICKTUNE:
 #endif
         break;
 
@@ -458,6 +462,12 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch
         break;
 #endif
 
+#if AP_QUICKTUNE_ENABLED
+    case AUX_FUNC::QUICKTUNE:
+        plane.quicktune.update_switch_pos(ch_flag);
+        break;
+#endif
+
     default:
         return RC_Channel::do_aux_function(ch_option, ch_flag);
     }
diff --git a/ArduPlane/RC_Channel.h b/ArduPlane/RC_Channel.h
index 858034478c9bf1..e9b4804aac5454 100644
--- a/ArduPlane/RC_Channel.h
+++ b/ArduPlane/RC_Channel.h
@@ -30,7 +30,6 @@ class RC_Channel_Plane : public RC_Channel
     void do_aux_function_soaring_3pos(AuxSwitchPos ch_flag);
 
     void do_aux_function_flare(AuxSwitchPos ch_flag);
-
 };
 
 class RC_Channels_Plane : public RC_Channels
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index 8260924afca893..213e547c6a6682 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -11,6 +11,12 @@
 #include <AP_Mission/AP_Mission.h>
 #include "pullup.h"
 
+#ifndef AP_QUICKTUNE_ENABLED
+#define AP_QUICKTUNE_ENABLED HAL_QUADPLANE_ENABLED
+#endif
+
+#include <AP_Quicktune/AP_Quicktune.h>
+
 class AC_PosControl;
 class AC_AttitudeControl_Multi;
 class AC_Loiter;
@@ -142,6 +148,11 @@ class Mode
     // true if voltage correction should be applied to throttle
     virtual bool use_battery_compensation() const;
 
+#if AP_QUICKTUNE_ENABLED
+    // does this mode support VTOL quicktune?
+    virtual bool supports_quicktune() const { return false; }
+#endif
+
 protected:
 
     // subclasses override this to perform checks before entering the mode
@@ -325,6 +336,9 @@ class ModeGuided : public Mode
 
     bool _enter() override;
     bool _pre_arm_checks(size_t buflen, char *buffer) const override { return true; }
+#if AP_QUICKTUNE_ENABLED
+    bool supports_quicktune() const override { return true; }
+#endif
 
 private:
     float active_radius_m;
@@ -662,6 +676,9 @@ class ModeQHover : public Mode
 protected:
 
     bool _enter() override;
+#if AP_QUICKTUNE_ENABLED
+    bool supports_quicktune() const override { return true; }
+#endif
 };
 
 class ModeQLoiter : public Mode
@@ -688,6 +705,10 @@ friend class Plane;
 
     bool _enter() override;
     uint32_t last_target_loc_set_ms;
+
+#if AP_QUICKTUNE_ENABLED
+    bool supports_quicktune() const override { return true; }
+#endif
 };
 
 class ModeQLand : public Mode
diff --git a/ArduPlane/qautotune.h b/ArduPlane/qautotune.h
index 938c8e47e43323..684fe5e54783df 100644
--- a/ArduPlane/qautotune.h
+++ b/ArduPlane/qautotune.h
@@ -8,7 +8,7 @@
 
 #include "quadplane.h"
 #ifndef QAUTOTUNE_ENABLED
-  #define QAUTOTUNE_ENABLED HAL_QUADPLANE_ENABLED
+#define QAUTOTUNE_ENABLED (HAL_QUADPLANE_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif
 
 #if QAUTOTUNE_ENABLED
diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
index aa3de533d3cf60..2ded238cf2f5ba 100644
--- a/ArduPlane/servos.cpp
+++ b/ArduPlane/servos.cpp
@@ -22,7 +22,7 @@
 /*****************************************
 * Throttle slew limit
 *****************************************/
-void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func)
+void Plane::throttle_slew_limit()
 {
 #if HAL_QUADPLANE_ENABLED
     const bool do_throttle_slew = (control_mode->does_auto_throttle() || quadplane.in_assisted_flight() || quadplane.in_vtol_mode());
@@ -32,7 +32,9 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func)
 
     if (!do_throttle_slew) {
         // only do throttle slew limiting in modes where throttle control is automatic
-        SRV_Channels::set_slew_rate(func, 0.0, 100, G_Dt);
+        SRV_Channels::set_slew_rate(SRV_Channel::k_throttle,      0.0, 100, G_Dt);
+        SRV_Channels::set_slew_rate(SRV_Channel::k_throttleLeft,  0.0, 100, G_Dt);
+        SRV_Channels::set_slew_rate(SRV_Channel::k_throttleRight, 0.0, 100, G_Dt);
         return;
     }
 
@@ -55,7 +57,9 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func)
         slewrate = g.takeoff_throttle_slewrate;
     }
 #endif
-    SRV_Channels::set_slew_rate(func, slewrate, 100, G_Dt);
+    SRV_Channels::set_slew_rate(SRV_Channel::k_throttle,      slewrate, 100, G_Dt);
+    SRV_Channels::set_slew_rate(SRV_Channel::k_throttleLeft,  slewrate, 100, G_Dt);
+    SRV_Channels::set_slew_rate(SRV_Channel::k_throttleRight, slewrate, 100, G_Dt);
 }
 
 /* We want to suppress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
@@ -793,8 +797,6 @@ void Plane::servos_twin_engine_mix(void)
     } else {
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle_left);
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle_right);
-        throttle_slew_limit(SRV_Channel::k_throttleLeft);
-        throttle_slew_limit(SRV_Channel::k_throttleRight);
     }
 }
 
@@ -913,7 +915,7 @@ void Plane::set_servos(void)
     airbrake_update();
 
     // slew rate limit throttle
-    throttle_slew_limit(SRV_Channel::k_throttle);
+    throttle_slew_limit();
 
     int8_t min_throttle = 0;
 #if AP_ICENGINE_ENABLED
diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp
index 33d111db3d0667..759fa3d1a44684 100644
--- a/ArduPlane/tailsitter.cpp
+++ b/ArduPlane/tailsitter.cpp
@@ -289,9 +289,6 @@ void Tailsitter::output(void)
         return;
     }
 
-    float tilt_left = 0.0f;
-    float tilt_right = 0.0f;
-
     // throttle 0 to 1
     float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01;
 
@@ -341,6 +338,10 @@ void Tailsitter::output(void)
             // set AP_MotorsMatrix throttles for forward flight
             motors->output_motor_mask(throttle, motor_mask, plane.rudder_dt);
 
+            // No tilt output unless forward gain is set
+            float tilt_left = 0.0;
+            float tilt_right = 0.0;
+
             // in forward flight: set motor tilt servos and throttles using FW controller
             if (vectored_forward_gain > 0) {
                 // remove scaling from surface speed scaling and apply throttle scaling
@@ -398,8 +399,11 @@ void Tailsitter::output(void)
             }
 
             // output tilt motors
-            tilt_left = 0.0f;
-            tilt_right = 0.0f;
+
+            // No output unless hover gain is set
+            float tilt_left = 0.0;
+            float tilt_right = 0.0;
+
             if (vectored_hover_gain > 0) {
                 const float hover_throttle = motors->get_throttle_hover();
                 const float output_throttle = motors->get_throttle();
@@ -438,8 +442,10 @@ void Tailsitter::output(void)
         tailsitter_motors->set_min_throttle(0.0);
     }
 
-    tilt_left = 0.0f;
-    tilt_right = 0.0f;
+    // No tilt output unless hover gain is set
+    float tilt_left = 0.0;
+    float tilt_right = 0.0;
+
     if (vectored_hover_gain > 0) {
         // thrust vectoring VTOL modes
         tilt_left = SRV_Channels::get_output_scaled(SRV_Channel::k_tiltMotorLeft);
diff --git a/ArduPlane/wscript b/ArduPlane/wscript
index 78fad7650e3779..930fec0b537634 100644
--- a/ArduPlane/wscript
+++ b/ArduPlane/wscript
@@ -27,6 +27,7 @@ def build(bld):
             'AP_Follow',
             'AC_PrecLand',
             'AP_IRLock',
+            'AP_Quicktune',
         ],
     )
 
diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt
index 2fbc625cec688a..209e26f657287f 100644
--- a/Tools/AP_Bootloader/board_types.txt
+++ b/Tools/AP_Bootloader/board_types.txt
@@ -292,13 +292,15 @@ AP_HW_2RAWH743                       1173
 AP_HW_X-MAV-AP-H743V2                1174
 AP_HW_BETAFPV_F4_2-3S_20A            1175
 AP_HW_MicoAir743AIOv1                1176
-
+AP_HW_CrazyF405                      1177
 AP_HW_FlywooF405HD_AIOv2             1180
 AP_HW_FlywooH743Pro                  1181
 
 AP_HW_ESP32_PERIPH                   1205
 AP_HW_ESP32S3_PERIPH                 1206
 
+AP_HW_CSKY-PMU                       1212
+
 AP_HW_MUPilot                        1222
 
 AP_HW_CBUnmanned-CM405-FC            1301
@@ -333,8 +335,12 @@ AP_HW_GEPRC_TAKER_H743               1502
 
 AP_HW_HGLRCF405V4                    1524
 
+AP_HW_F4BY_F427                      1530
+
 AP_HW_MFT-SEMA100                    2000
 
+AP_HW_AET-H743-Basic                 2024
+
 AP_HW_SakuraRC-H743                  2714
 
 # IDs 4000-4009 reserved for Karshak Drones
@@ -361,6 +367,9 @@ AP_HW_UAV-DEV-F9P-G4                 5223
 AP_HW_UAV-DEV-UM982-G4               5224
 AP_HW_UAV-DEV-M20D-G4                5225
 AP_HW_UAV-DEV-Sensorboard-G4         5226
+AP_HW_UAV-DEV-PWM-G4                 5227
+AP_HW_UAV-DEV-AUAV-H7                5228
+AP_HW_UAV-DEV-FC-H7                  5229
 
 # IDs 5240-5249 reserved for TM IT-Systemhaus
 AP_HW_TM-SYS-BeastFC                 5240
@@ -406,6 +415,12 @@ AP_HW_DroneBuild_G2                  5701
 
 # IDs 6000-6099 reserved for SpektreWorks
 
+# IDs 6100-6109 reserved for MFE
+AP_HW_MFE_PDB_CAN                            6100
+AP_HW_MFE_POS3_CAN                          6101
+AP_HW_MFE_RTK_CAN                             6102
+AP_HW_MFE_AirSpeed_CAN                     6103
+
 # IDs 6600-6699 reserved for Eagle Eye Drones
 
 # IDs 6900-6909 reserved for Easy Aerial
diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index 2ab0e4437a6163..f9479d285a1eca 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -627,6 +627,7 @@ void AP_Periph_FW::prepare_reboot()
  */
 void AP_Periph_FW::reboot(bool hold_in_bootloader)
 {
+    prepare_reboot();
     hal.scheduler->reboot(hold_in_bootloader);
 }
 
diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript
index 826e9762751a25..a65b269c32f32d 100644
--- a/Tools/AP_Periph/wscript
+++ b/Tools/AP_Periph/wscript
@@ -82,7 +82,8 @@ def build(bld):
                      'AP_RobotisServo',
                      'AP_FETtecOneWire',
                      'GCS_MAVLink',
-                     'AP_Relay'
+                     'AP_Relay',
+                     'AP_MultiHeap',
                      ]
 
     bld.ap_stlib(
diff --git a/Tools/GIT_Test/GIT_Success.txt b/Tools/GIT_Test/GIT_Success.txt
index 13d517d005c22b..a475ba17f1bd86 100644
--- a/Tools/GIT_Test/GIT_Success.txt
+++ b/Tools/GIT_Test/GIT_Success.txt
@@ -205,3 +205,4 @@ Naveen Kumar Kilaparthi
 Amr Attia
 Alessandro Martini
 Eren Mert YİĞİT
+Prashant Powar
diff --git a/Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin b/Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin
new file mode 100755
index 00000000000000..d9cad7f0ca76bf
Binary files /dev/null and b/Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin differ
diff --git a/Tools/IO_Firmware/iofirmware_cube_dshot_lowpolh.bin b/Tools/IO_Firmware/iofirmware_cube_dshot_lowpolh.bin
new file mode 100755
index 00000000000000..2e181269c0e302
Binary files /dev/null and b/Tools/IO_Firmware/iofirmware_cube_dshot_lowpolh.bin differ
diff --git a/Tools/IO_Firmware/iofirmware_cube_highpolh.bin b/Tools/IO_Firmware/iofirmware_cube_highpolh.bin
new file mode 100755
index 00000000000000..f23e2095b76294
Binary files /dev/null and b/Tools/IO_Firmware/iofirmware_cube_highpolh.bin differ
diff --git a/Tools/IO_Firmware/iofirmware_cube_lowpolh.bin b/Tools/IO_Firmware/iofirmware_cube_lowpolh.bin
new file mode 100755
index 00000000000000..82aabcbe572886
Binary files /dev/null and b/Tools/IO_Firmware/iofirmware_cube_lowpolh.bin differ
diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py
index 1c28dedc5fb2f5..880d1978dff090 100644
--- a/Tools/ardupilotwaf/ardupilotwaf.py
+++ b/Tools/ardupilotwaf/ardupilotwaf.py
@@ -128,6 +128,7 @@
     'AP_Beacon',
     'AP_Arming',
     'AP_RCMapper',
+    'AP_MultiHeap',
 ]
 
 def get_legacy_defines(sketch_name, bld):
diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py
index 7ccddaad109e0c..0e692be45a6fec 100644
--- a/Tools/ardupilotwaf/boards.py
+++ b/Tools/ardupilotwaf/boards.py
@@ -177,7 +177,10 @@ def srcpath(path):
 
         if cfg.options.enable_networking_tests:
             env.CXXFLAGS += ['-DAP_NETWORKING_TESTS_ENABLED=1']
-            
+
+        if cfg.options.enable_iomcu_profiled_support:
+            env.CXXFLAGS += ['-DAP_IOMCU_PROFILED_SUPPORT_ENABLED=1']
+
         d = env.get_merged_dict()
         # Always prepend so that arguments passed in the command line get
         # the priority.
@@ -696,6 +699,12 @@ def configure_env(self, cfg, env):
         cfg.define('AP_NOTIFY_LP5562_BUS', 2)
         cfg.define('AP_NOTIFY_LP5562_ADDR', 0x30)
 
+        # turn on fencepoint and rallypoint protocols so they're still tested:
+        env.CXXFLAGS.extend([
+            '-DAP_MAVLINK_RALLY_POINT_PROTOCOL_ENABLED=HAL_GCS_ENABLED&&HAL_RALLY_ENABLED',
+            '-DAC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT=HAL_GCS_ENABLED&&AP_FENCE_ENABLED'
+        ])
+
         try:
             env.CXXFLAGS.remove('-DHAL_NAVEKF2_AVAILABLE=0')
         except ValueError:
diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 1482c6f829ade8..f1dccfcf1b139d 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -552,13 +552,13 @@ def ThrottleFailsafe(self, side=60, timeout=360):
         self.start_subtest("Radio failsafe RTL fails into land mode due to bad position.")
         self.set_parameter('FS_THR_ENABLE', 1)
         self.takeoffAndMoveAway()
-        self.set_parameter('SIM_GPS_DISABLE', 1)
+        self.set_parameter('SIM_GPS1_ENABLE', 0)
         self.delay_sim_time(5)
         self.set_parameter("SIM_RC_FAIL", 1)
         self.wait_mode("LAND")
         self.wait_landed_and_disarmed()
         self.set_parameter("SIM_RC_FAIL", 0)
-        self.set_parameter('SIM_GPS_DISABLE', 0)
+        self.set_parameter('SIM_GPS1_ENABLE', 1)
         self.wait_ekf_happy()
         self.end_subtest("Completed Radio failsafe RTL fails into land mode due to bad position.")
 
@@ -567,13 +567,13 @@ def ThrottleFailsafe(self, side=60, timeout=360):
         self.start_subtest("Radio failsafe SmartRTL->RTL fails into land mode due to bad position.")
         self.set_parameter('FS_THR_ENABLE', 4)
         self.takeoffAndMoveAway()
-        self.set_parameter('SIM_GPS_DISABLE', 1)
+        self.set_parameter('SIM_GPS1_ENABLE', 0)
         self.delay_sim_time(5)
         self.set_parameter("SIM_RC_FAIL", 1)
         self.wait_mode("LAND")
         self.wait_landed_and_disarmed()
         self.set_parameter("SIM_RC_FAIL", 0)
-        self.set_parameter('SIM_GPS_DISABLE', 0)
+        self.set_parameter('SIM_GPS1_ENABLE', 1)
         self.wait_ekf_happy()
         self.end_subtest("Completed Radio failsafe SmartRTL->RTL fails into land mode due to bad position.")
 
@@ -582,13 +582,13 @@ def ThrottleFailsafe(self, side=60, timeout=360):
         self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to bad position.")
         self.set_parameter('FS_THR_ENABLE', 5)
         self.takeoffAndMoveAway()
-        self.set_parameter('SIM_GPS_DISABLE', 1)
+        self.set_parameter('SIM_GPS1_ENABLE', 0)
         self.delay_sim_time(5)
         self.set_parameter("SIM_RC_FAIL", 1)
         self.wait_mode("LAND")
         self.wait_landed_and_disarmed()
         self.set_parameter("SIM_RC_FAIL", 0)
-        self.set_parameter('SIM_GPS_DISABLE', 0)
+        self.set_parameter('SIM_GPS1_ENABLE', 1)
         self.wait_ekf_happy()
         self.end_subtest("Completed Radio failsafe SmartRTL->LAND fails into land mode due to bad position.")
 
@@ -597,9 +597,9 @@ def ThrottleFailsafe(self, side=60, timeout=360):
         self.start_subtest("Radio failsafe SmartRTL->RTL fails into RTL mode due to no path.")
         self.set_parameter('FS_THR_ENABLE', 4)
         self.takeoffAndMoveAway()
-        self.set_parameter('SIM_GPS_DISABLE', 1)
+        self.set_parameter('SIM_GPS1_ENABLE', 0)
         self.wait_statustext("SmartRTL deactivated: bad position", timeout=60)
-        self.set_parameter('SIM_GPS_DISABLE', 0)
+        self.set_parameter('SIM_GPS1_ENABLE', 1)
         self.wait_ekf_happy()
         self.delay_sim_time(5)
         self.set_parameter("SIM_RC_FAIL", 1)
@@ -613,9 +613,9 @@ def ThrottleFailsafe(self, side=60, timeout=360):
         self.start_subtest("Radio failsafe SmartRTL->LAND fails into land mode due to no path.")
         self.set_parameter('FS_THR_ENABLE', 5)
         self.takeoffAndMoveAway()
-        self.set_parameter('SIM_GPS_DISABLE', 1)
+        self.set_parameter('SIM_GPS1_ENABLE', 0)
         self.wait_statustext("SmartRTL deactivated: bad position", timeout=60)
-        self.set_parameter('SIM_GPS_DISABLE', 0)
+        self.set_parameter('SIM_GPS1_ENABLE', 1)
         self.wait_ekf_happy()
         self.delay_sim_time(5)
         self.set_parameter("SIM_RC_FAIL", 1)
@@ -1958,8 +1958,8 @@ def GPSGlitchLoiter(self, timeout=30, max_distance=20):
         glitch_current = 0
         self.progress("Apply first glitch")
         self.set_parameters({
-            "SIM_GPS_GLITCH_X": glitch_lat[glitch_current],
-            "SIM_GPS_GLITCH_Y": glitch_lon[glitch_current],
+            "SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
+            "SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
         })
 
         # record position for 30 seconds
@@ -1973,15 +1973,15 @@ def GPSGlitchLoiter(self, timeout=30, max_distance=20):
                     glitch_current = -1
                     self.progress("Completed Glitches")
                     self.set_parameters({
-                        "SIM_GPS_GLITCH_X": 0,
-                        "SIM_GPS_GLITCH_Y": 0,
+                        "SIM_GPS1_GLTCH_X": 0,
+                        "SIM_GPS1_GLTCH_Y": 0,
                     })
                 else:
                     self.progress("Applying glitch %u" % glitch_current)
                     # move onto the next glitch
                     self.set_parameters({
-                        "SIM_GPS_GLITCH_X": glitch_lat[glitch_current],
-                        "SIM_GPS_GLITCH_Y": glitch_lon[glitch_current],
+                        "SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
+                        "SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
                     })
 
             # start displaying distance moved after all glitches applied
@@ -2002,8 +2002,8 @@ def GPSGlitchLoiter(self, timeout=30, max_distance=20):
         # disable gps glitch
         if glitch_current != -1:
             self.set_parameters({
-                "SIM_GPS_GLITCH_X": 0,
-                "SIM_GPS_GLITCH_Y": 0,
+                "SIM_GPS1_GLTCH_X": 0,
+                "SIM_GPS1_GLTCH_Y": 0,
             })
         if self.use_map:
             self.show_gps_and_sim_positions(False)
@@ -2024,7 +2024,7 @@ def GPSGlitchLoiter2(self):
         self.wait_attitude(desroll=0, despitch=0, timeout=10, tolerance=1)
 
         # apply glitch
-        self.set_parameter("SIM_GPS_GLITCH_X", 0.001)
+        self.set_parameter("SIM_GPS1_GLTCH_X", 0.001)
 
         # check lean angles remain stable for 20 seconds
         tstart = self.get_sim_time()
@@ -2098,11 +2098,11 @@ def GPSGlitchAuto(self, timeout=180):
         # stop and test loss of GPS for a short time - it should resume GPS use without falling back into a non aiding mode
         self.change_mode("LOITER")
         self.set_parameters({
-            "SIM_GPS_DISABLE": 1,
+            "SIM_GPS1_ENABLE": 0,
         })
         self.delay_sim_time(2)
         self.set_parameters({
-            "SIM_GPS_DISABLE": 0,
+            "SIM_GPS1_ENABLE": 1,
         })
         # regaining GPS should not result in it falling back to a non-navigation mode
         self.wait_ekf_flags(mavutil.mavlink.ESTIMATOR_POS_HORIZ_ABS, 0, timeout=1)
@@ -2118,8 +2118,8 @@ def GPSGlitchAuto(self, timeout=180):
         glitch_current = 0
         self.progress("Apply first glitch")
         self.set_parameters({
-            "SIM_GPS_GLITCH_X": glitch_lat[glitch_current],
-            "SIM_GPS_GLITCH_Y": glitch_lon[glitch_current],
+            "SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
+            "SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
         })
 
         # record position for 30 seconds
@@ -2132,15 +2132,15 @@ def GPSGlitchAuto(self, timeout=180):
                 if glitch_current < glitch_num:
                     self.progress("Applying glitch %u" % glitch_current)
                     self.set_parameters({
-                        "SIM_GPS_GLITCH_X": glitch_lat[glitch_current],
-                        "SIM_GPS_GLITCH_Y": glitch_lon[glitch_current],
+                        "SIM_GPS1_GLTCH_X": glitch_lat[glitch_current],
+                        "SIM_GPS1_GLTCH_Y": glitch_lon[glitch_current],
                     })
 
         # turn off glitching
         self.progress("Completed Glitches")
         self.set_parameters({
-            "SIM_GPS_GLITCH_X": 0,
-            "SIM_GPS_GLITCH_Y": 0,
+            "SIM_GPS1_GLTCH_X": 0,
+            "SIM_GPS1_GLTCH_Y": 0,
         })
 
         # continue with the mission
@@ -2526,7 +2526,7 @@ def OpticalFlowLimits(self):
         self.set_parameters({
             "SIM_FLOW_ENABLE": 1,
             "FLOW_TYPE": 10,
-            "SIM_GPS_DISABLE": 1,
+            "SIM_GPS1_ENABLE": 0,
             "SIM_TERRAIN": 0,
         })
 
@@ -2980,13 +2980,13 @@ def FarOrigin(self):
         '''fly a mission far from the vehicle origin'''
         # Fly mission #1
         self.set_parameters({
-            "SIM_GPS_DISABLE": 1,
+            "SIM_GPS1_ENABLE": 0,
         })
         self.reboot_sitl()
         nz = mavutil.location(-43.730171, 169.983118, 1466.3, 270)
         self.set_origin(nz)
         self.set_parameters({
-            "SIM_GPS_DISABLE": 0,
+            "SIM_GPS1_ENABLE": 1,
         })
         self.progress("# Load copter_mission")
         # load the waypoint count
@@ -3028,8 +3028,8 @@ def CANGPSCopterMission(self):
             "GPS1_TYPE": 9,
             "GPS2_TYPE": 9,
             # disable simulated GPS, so only via DroneCAN
-            "SIM_GPS_DISABLE": 1,
-            "SIM_GPS2_DISABLE": 1,
+            "SIM_GPS1_ENABLE": 0,
+            "SIM_GPS2_ENABLE": 0,
             # this ensures we use DroneCAN baro and compass
             "SIM_BARO_COUNT" : 0,
             "SIM_MAG1_DEVID" : 0,
@@ -3206,7 +3206,7 @@ def GuidedEKFLaneChange(self):
             "EK3_SRC1_POSZ": 3,
             "EK3_AFFINITY" : 1,
             "GPS2_TYPE" : 1,
-            "SIM_GPS2_DISABLE" : 0,
+            "SIM_GPS2_ENABLE" : 1,
             "SIM_GPS2_GLTCH_Z" : -30
             })
         self.reboot_sitl()
@@ -7245,7 +7245,7 @@ def loiter_requires_position(self):
         self.context_push()
         self.set_parameters({
             "GPS1_TYPE": 2,
-            "SIM_GPS_DISABLE": 1,
+            "SIM_GPS1_ENABLE": 0,
         })
         # if there is no GPS at all then we must direct EK3 to not use
         # it at all.  Otherwise it will never initialise, as it wants
@@ -8986,7 +8986,7 @@ def test_replay_gps_bit(self):
             "RNGFND1_POS_Y": -0.07,
             "RNGFND1_POS_Z": -0.005,
             "SIM_SONAR_SCALE": 30,
-            "SIM_GPS2_DISABLE": 0,
+            "SIM_GPS2_ENABLE": 1,
         })
         self.reboot_sitl()
 
@@ -9045,7 +9045,7 @@ def GPSBlendingLog(self):
             self.set_parameters({
                 "GPS2_TYPE": 1,
                 "SIM_GPS2_TYPE": 1,
-                "SIM_GPS2_DISABLE": 0,
+                "SIM_GPS2_ENABLE": 1,
                 "GPS_AUTO_SWITCH": 2,
             })
             self.reboot_sitl()
@@ -9111,16 +9111,14 @@ def GPSBlending(self):
         '''Test GPS Blending'''
         '''ensure we get dataflash log messages for blended instance'''
 
-        self.context_push()
-
         # configure:
         self.set_parameters({
             "WP_YAW_BEHAVIOR": 0,  # do not yaw
             "GPS2_TYPE": 1,
             "SIM_GPS2_TYPE": 1,
-            "SIM_GPS2_DISABLE": 0,
-            "SIM_GPS_POS_X": 1.0,
-            "SIM_GPS_POS_Y": -1.0,
+            "SIM_GPS2_ENABLE": 1,
+            "SIM_GPS1_POS_X": 1.0,
+            "SIM_GPS1_POS_Y": -1.0,
             "SIM_GPS2_POS_X": -1.0,
             "SIM_GPS2_POS_Y": 1.0,
             "GPS_AUTO_SWITCH": 2,
@@ -9165,8 +9163,8 @@ def GPSBlending(self):
                         raise NotAchievedException("Blended diverged")
                 current_ts = None
 
-        self.context_pop()
-        self.reboot_sitl()
+        if len(measurements) != 3:
+            raise NotAchievedException("Did not see three GPS measurements!")
 
     def GPSWeightedBlending(self):
         '''Test GPS Weighted Blending'''
@@ -9178,9 +9176,9 @@ def GPSWeightedBlending(self):
             "WP_YAW_BEHAVIOR": 0,  # do not yaw
             "GPS2_TYPE": 1,
             "SIM_GPS2_TYPE": 1,
-            "SIM_GPS2_DISABLE": 0,
-            "SIM_GPS_POS_X": 1.0,
-            "SIM_GPS_POS_Y": -1.0,
+            "SIM_GPS2_ENABLE": 1,
+            "SIM_GPS1_POS_X": 1.0,
+            "SIM_GPS1_POS_Y": -1.0,
             "SIM_GPS2_POS_X": -1.0,
             "SIM_GPS2_POS_Y": 1.0,
             "GPS_AUTO_SWITCH": 2,
@@ -9188,8 +9186,8 @@ def GPSWeightedBlending(self):
         # configure velocity errors such that the 1st GPS should be
         # 4/5, second GPS 1/5 of result (0.5**2)/((0.5**2)+(1.0**2))
         self.set_parameters({
-            "SIM_GPS_VERR_X": 0.3,  # m/s
-            "SIM_GPS_VERR_Y": 0.4,
+            "SIM_GPS1_VERR_X": 0.3,  # m/s
+            "SIM_GPS1_VERR_Y": 0.4,
             "SIM_GPS2_VERR_X": 0.6,  # m/s
             "SIM_GPS2_VERR_Y": 0.8,
             "GPS_BLEND_MASK": 4,  # use only speed for blend calculations
@@ -9245,9 +9243,9 @@ def GPSBlendingAffinity(self):
             "WP_YAW_BEHAVIOR": 0,  # do not yaw
             "GPS2_TYPE": 1,
             "SIM_GPS2_TYPE": 1,
-            "SIM_GPS2_DISABLE": 0,
-            "SIM_GPS_POS_X": 1.0,
-            "SIM_GPS_POS_Y": -1.0,
+            "SIM_GPS2_ENABLE": 1,
+            "SIM_GPS1_POS_X": 1.0,
+            "SIM_GPS1_POS_Y": -1.0,
             "SIM_GPS2_POS_X": -1.0,
             "SIM_GPS2_POS_Y": 1.0,
             "GPS_AUTO_SWITCH": 2,
@@ -11741,7 +11739,7 @@ def GuidedForceArm(self):
         '''ensure Guided acts appropriately when force-armed'''
         self.set_parameters({
             "EK3_SRC2_VELXY": 5,
-            "SIM_GPS_DISABLE": 1,
+            "SIM_GPS1_ENABLE": 0,
         })
         self.load_default_params_file("copter-optflow.parm")
         self.reboot_sitl()
@@ -11928,7 +11926,7 @@ def REQUIRE_POSITION_FOR_ARMING(self):
         '''check FlightOption::REQUIRE_POSITION_FOR_ARMING works'''
         self.context_push()
         self.set_parameters({
-            "SIM_GPS_NUMSATS": 3,  # EKF does not like < 6
+            "SIM_GPS1_NUMSATS": 3,  # EKF does not like < 6
         })
         self.reboot_sitl()
         self.change_mode('STABILIZE')
@@ -12129,7 +12127,7 @@ def CommonOrigin(self):
 
         # switch to EKF3
         self.set_parameters({
-            'SIM_GPS_GLITCH_X' : 0.001, # about 100m
+            'SIM_GPS1_GLTCH_X' : 0.001, # about 100m
             'EK3_CHECK_SCALE' : 100,
             'AHRS_EKF_TYPE'   : 3})
 
diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 48334ab8d7c2e5..fa4a0949133ff0 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -185,7 +185,7 @@ def fly_RTL(self):
     def NeedEKFToArm(self):
         """Ensure the EKF must be healthy for the vehicle to arm."""
         self.progress("Ensuring we need EKF to be healthy to arm")
-        self.set_parameter("SIM_GPS_DISABLE", 1)
+        self.set_parameter("SIM_GPS1_ENABLE", 0)
         self.context_collect("STATUSTEXT")
         tstart = self.get_sim_time()
         success = False
@@ -201,7 +201,7 @@ def NeedEKFToArm(self):
                 except AutoTestTimeoutException:
                     pass
 
-        self.set_parameter("SIM_GPS_DISABLE", 0)
+        self.set_parameter("SIM_GPS1_ENABLE", 1)
         self.wait_ready_to_arm()
 
     def fly_LOITER(self, num_circles=4):
@@ -2046,14 +2046,14 @@ def deadreckoning_main(self, disable_airspeed_sensor=False):
                 self.delay_sim_time(60)
             else:
                 self.delay_sim_time(20)
-            self.set_parameter("SIM_GPS_DISABLE", 1)
+            self.set_parameter("SIM_GPS1_ENABLE", 0)
             self.progress("Continue orbit without GPS")
             self.delay_sim_time(20)
             self.change_mode("RTL")
             self.wait_distance_to_home(100, 200, timeout=200)
             # go into LOITER to create additonal time for a GPS re-enable test
             self.change_mode("LOITER")
-            self.set_parameter("SIM_GPS_DISABLE", 0)
+            self.set_parameter("SIM_GPS1_ENABLE", 1)
             t_enabled = self.get_sim_time()
             # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning
             # to prevent bad GPS being used when coming back after loss of lock due to interence.
@@ -2064,9 +2064,9 @@ def deadreckoning_main(self, disable_airspeed_sensor=False):
             self.delay_sim_time(20)
 
             self.set_parameter("AHRS_OPTIONS", 1)
-            self.set_parameter("SIM_GPS_JAM", 1)
+            self.set_parameter("SIM_GPS1_JAM", 1)
             self.delay_sim_time(10)
-            self.set_parameter("SIM_GPS_JAM", 0)
+            self.set_parameter("SIM_GPS1_JAM", 0)
             t_enabled = self.get_sim_time()
             # The EKF should wait for GPS checks to pass when we are still able to navigate using dead reckoning
             # to prevent bad GPS being used when coming back after loss of lock due to interence.
@@ -3325,7 +3325,7 @@ def EKFlaneswitch(self):
             "EK3_AFFINITY": 15, # enable affinity for all sensors
             "EK3_IMU_MASK": 3, # use only 2 IMUs
             "GPS2_TYPE": 1,
-            "SIM_GPS2_DISABLE": 0,
+            "SIM_GPS2_ENABLE": 1,
             "SIM_BARO_COUNT": 2,
             "SIM_BAR2_DISABLE": 0,
             "ARSPD2_TYPE": 2,
@@ -3400,9 +3400,9 @@ def statustext_hook(mav, message):
         # noise on each axis
         def sim_gps_verr():
             self.set_parameters({
-                "SIM_GPS_VERR_X": self.get_parameter("SIM_GPS_VERR_X") + 2,
-                "SIM_GPS_VERR_Y": self.get_parameter("SIM_GPS_VERR_Y") + 2,
-                "SIM_GPS_VERR_Z": self.get_parameter("SIM_GPS_VERR_Z") + 2,
+                "SIM_GPS1_VERR_X": self.get_parameter("SIM_GPS1_VERR_X") + 2,
+                "SIM_GPS1_VERR_Y": self.get_parameter("SIM_GPS1_VERR_Y") + 2,
+                "SIM_GPS1_VERR_Z": self.get_parameter("SIM_GPS1_VERR_Z") + 2,
             })
         self.wait_statustext(text="EKF3 lane switch", timeout=30, the_function=sim_gps_verr, check_context=True)
         if self.lane_switches != [1, 0, 1]:
@@ -4870,8 +4870,8 @@ def DCMFallback(self):
         self.context_collect('STATUSTEXT')
         self.set_parameters({
             "EK3_POS_I_GATE": 0,
-            "SIM_GPS_HZ": 1,
-            "SIM_GPS_LAG_MS": 1000,
+            "SIM_GPS1_HZ": 1,
+            "SIM_GPS1_LAG_MS": 1000,
         })
         self.wait_statustext("DCM Active", check_context=True, timeout=60)
         self.wait_statustext("EKF3 Active", check_context=True)
@@ -5530,8 +5530,8 @@ def MissionJumpTags(self):
     def AltResetBadGPS(self):
         '''Tests the handling of poor GPS lock pre-arm alt resets'''
         self.set_parameters({
-            "SIM_GPS_GLITCH_Z": 0,
-            "SIM_GPS_ACC": 0.3,
+            "SIM_GPS1_GLTCH_Z": 0,
+            "SIM_GPS1_ACC": 0.3,
         })
         self.wait_ready_to_arm()
 
@@ -5541,8 +5541,8 @@ def AltResetBadGPS(self):
             raise NotAchievedException("Bad relative alt %.1f" % relalt)
 
         self.progress("Setting low accuracy, glitching GPS")
-        self.set_parameter("SIM_GPS_ACC", 40)
-        self.set_parameter("SIM_GPS_GLITCH_Z", -47)
+        self.set_parameter("SIM_GPS1_ACC", 40)
+        self.set_parameter("SIM_GPS1_GLTCH_Z", -47)
 
         self.progress("Waiting 10s for height update")
         self.delay_sim_time(10)
@@ -6119,7 +6119,7 @@ def GuidedAttitudeNoGPS(self):
         self.takeoff(50)
         self.change_mode('GUIDED')
         self.context_push()
-        self.set_parameter('SIM_GPS_DISABLE', 1)
+        self.set_parameter('SIM_GPS1_ENABLE', 0)
         self.delay_sim_time(30)
         self.set_attitude_target()
         self.context_pop()
@@ -6196,7 +6196,7 @@ def SetHomeAltChange(self):
 
     def ForceArm(self):
         '''check force-arming functionality'''
-        self.set_parameter("SIM_GPS_DISABLE", 1)
+        self.set_parameter("SIM_GPS1_ENABLE", 0)
         # 21196 is the mavlink standard, 2989 is legacy
         for magic_value in 21196, 2989:
             self.wait_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_PREARM_CHECK,
diff --git a/Tools/autotest/default_params/airsim-quadX.parm b/Tools/autotest/default_params/airsim-quadX.parm
index a8987896f8a847..c0636c5205c37a 100644
--- a/Tools/autotest/default_params/airsim-quadX.parm
+++ b/Tools/autotest/default_params/airsim-quadX.parm
@@ -18,7 +18,7 @@ WPNAV_SPEED_DN   300
 WPNAV_SPEED     2000
 RTL_ALT         2500
 ANGLE_MAX       3000
-SIM_GPS_LAG_MS  0
+SIM_GPS1_LAG_MS  0
 GPS_DELAY_MS    20
 INS_GYRO_FILTER 50
 INS_ACCEL_FILTER 50
diff --git a/Tools/autotest/default_params/copter-gps-for-yaw-nmea.parm b/Tools/autotest/default_params/copter-gps-for-yaw-nmea.parm
index 41af6fa868b915..6ec6add79623ff 100644
--- a/Tools/autotest/default_params/copter-gps-for-yaw-nmea.parm
+++ b/Tools/autotest/default_params/copter-gps-for-yaw-nmea.parm
@@ -1,5 +1,5 @@
 # SITL GPS-for-yaw using a single simulated NMEA GPS
 EK3_SRC1_YAW 2
 GPS1_TYPE 5
-SIM_GPS_TYPE 5
-SIM_GPS_HDG 1
+SIM_GPS1_TYPE 5
+SIM_GPS1_HDG 1
diff --git a/Tools/autotest/default_params/copter-gps-for-yaw.parm b/Tools/autotest/default_params/copter-gps-for-yaw.parm
index 231fb84e8ecece..1e54532f4f6b13 100644
--- a/Tools/autotest/default_params/copter-gps-for-yaw.parm
+++ b/Tools/autotest/default_params/copter-gps-for-yaw.parm
@@ -5,7 +5,8 @@ GPS1_TYPE 17
 GPS2_TYPE 18
 GPS1_POS_Y -0.2
 GPS2_POS_Y 0.2
-SIM_GPS_POS_Y -0.2
+SIM_GPS1_POS_Y -0.2
 SIM_GPS2_POS_Y 0.2
-SIM_GPS2_DISABLE 0
+SIM_GPS2_ENABLE 1
 SIM_GPS2_HDG 1
+SIM_GPS1_HDG 4
diff --git a/Tools/autotest/default_params/quadplane.parm b/Tools/autotest/default_params/quadplane.parm
index 62c5225d052d52..9da3368889f2f4 100644
--- a/Tools/autotest/default_params/quadplane.parm
+++ b/Tools/autotest/default_params/quadplane.parm
@@ -70,10 +70,16 @@ Q_M_PWM_MIN      1000
 Q_M_PWM_MAX      2000
 Q_M_BAT_VOLT_MAX 12.8
 Q_M_BAT_VOLT_MIN 9.6
-Q_A_RAT_RLL_P 0.15
-Q_A_RAT_PIT_P 0.15
-Q_A_RAT_RLL_D 0.002
-Q_A_RAT_PIT_D 0.002
+Q_A_RAT_RLL_P 0.108
+Q_A_RAT_RLL_I 0.108
+Q_A_RAT_RLL_D 0.001
+Q_A_RAT_PIT_P 0.103
+Q_A_RAT_PIT_I 0.103
+Q_A_RAT_PIT_D 0.001
+Q_A_RAT_YAW_P 0.2
+Q_A_RAT_YAW_P 0.02
+Q_A_ANG_RLL_P 6
+Q_A_ANG_PIT_P 6
 RTL_ALTITUDE 20.00
 PTCH_RATE_FF 1.407055
 RLL_RATE_FF 0.552741
diff --git a/Tools/autotest/default_params/tracker-gps-for-yaw.parm b/Tools/autotest/default_params/tracker-gps-for-yaw.parm
index 231fb84e8ecece..1e54532f4f6b13 100644
--- a/Tools/autotest/default_params/tracker-gps-for-yaw.parm
+++ b/Tools/autotest/default_params/tracker-gps-for-yaw.parm
@@ -5,7 +5,8 @@ GPS1_TYPE 17
 GPS2_TYPE 18
 GPS1_POS_Y -0.2
 GPS2_POS_Y 0.2
-SIM_GPS_POS_Y -0.2
+SIM_GPS1_POS_Y -0.2
 SIM_GPS2_POS_Y 0.2
-SIM_GPS2_DISABLE 0
+SIM_GPS2_ENABLE 1
 SIM_GPS2_HDG 1
+SIM_GPS1_HDG 4
diff --git a/Tools/autotest/default_params/vee-gull_005.param b/Tools/autotest/default_params/vee-gull_005.param
index 7a2e5bf48474dc..ab340c7eaf020c 100644
--- a/Tools/autotest/default_params/vee-gull_005.param
+++ b/Tools/autotest/default_params/vee-gull_005.param
@@ -628,19 +628,12 @@ SIM_FLOW_POS_X,0
 SIM_FLOW_POS_Y,0
 SIM_FLOW_POS_Z,0
 SIM_FLOW_RATE,10
-SIM_GP2_GLITCH_X,0
-SIM_GP2_GLITCH_Y,0
-SIM_GP2_GLITCH_Z,0
-SIM_GPS_BYTELOSS,0
-SIM_GPS_DISABLE,0
-SIM_GPS_DRIFTALT,0
-SIM_GPS_GLITCH_X,0
-SIM_GPS_GLITCH_Y,0
-SIM_GPS_GLITCH_Z,0
-SIM_GPS_HZ,5
-SIM_GPS_NOISE,0
-SIM_GPS_NUMSATS,10
-SIM_GPS_TYPE,1
+SIM_GPS1_BYTELOSS,0
+SIM_GPS1_DRIFTALT,0
+SIM_GPS1_HZ,5
+SIM_GPS1_NOISE,0
+SIM_GPS1_NUMSATS,10
+SIM_GPS1_TYPE,1
 SIM_GPS2_ENABLE,0
 SIM_IMU_POS_X,0
 SIM_IMU_POS_Y,0
diff --git a/Tools/autotest/logger_metadata/parse.py b/Tools/autotest/logger_metadata/parse.py
index 20bac7be3c0132..b56b7ad2662cd0 100755
--- a/Tools/autotest/logger_metadata/parse.py
+++ b/Tools/autotest/logger_metadata/parse.py
@@ -182,7 +182,7 @@ def set_fmts(self, fmts):
                 return
             # Make sure lengths match up
             if len(fmts) != len(self.fields_order):
-                print(f"Number of fmts don't match fields: msg={self.name} fmts={fmts} num_fields={len(self.fields_order)}")
+                print(f"Number of fmts don't match fields: msg={self.name} fmts={fmts} num_fields={len(self.fields_order)} {self.fields_order}")  # noqa:E501
                 return
             # Loop through the list
             for idx in range(0, len(fmts)):
diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py
index 0e6ef03d3f6c27..721752a5c337d6 100644
--- a/Tools/autotest/quadplane.py
+++ b/Tools/autotest/quadplane.py
@@ -363,6 +363,20 @@ def EXTENDED_SYS_STATE(self):
     def QAUTOTUNE(self):
         '''test Plane QAutoTune mode'''
 
+        # adjust tune so QAUTOTUNE can cope
+        self.set_parameters({
+            "Q_A_RAT_RLL_P" : 0.15,
+            "Q_A_RAT_RLL_I" : 0.25,
+            "Q_A_RAT_RLL_D" : 0.002,
+            "Q_A_RAT_PIT_P" : 0.15,
+            "Q_A_RAT_PIT_I" : 0.25,
+            "Q_A_RAT_PIT_D" : 0.002,
+            "Q_A_RAT_YAW_P" : 0.18,
+            "Q_A_RAT_YAW_I" : 0.018,
+            "Q_A_ANG_RLL_P" : 4.5,
+            "Q_A_ANG_PIT_P" : 4.5,
+            })
+
         # this is a list of all parameters modified by QAUTOTUNE.  Set
         # them so that when the context is popped we get the original
         # values back:
@@ -1411,6 +1425,81 @@ def VTOLQuicktune(self):
 
         self.wait_disarmed(timeout=120)
 
+    def VTOLQuicktune_CPP(self):
+        '''VTOL Quicktune in C++'''
+        self.set_parameters({
+            "RC7_OPTION": 181,
+            "QWIK_ENABLE" : 1,
+            "QWIK_DOUBLE_TIME" : 5, # run faster for autotest
+        })
+
+        self.context_push()
+        self.context_collect('STATUSTEXT')
+
+        # reduce roll/pitch gains by 2
+        gain_mul = 0.5
+        soften_params = ['Q_A_RAT_RLL_P', 'Q_A_RAT_RLL_I', 'Q_A_RAT_RLL_D',
+                         'Q_A_RAT_PIT_P', 'Q_A_RAT_PIT_I', 'Q_A_RAT_PIT_D',
+                         'Q_A_RAT_YAW_P', 'Q_A_RAT_YAW_I']
+
+        original_values = self.get_parameters(soften_params)
+
+        softened_values = {}
+        for p in original_values.keys():
+            softened_values[p] = original_values[p] * gain_mul
+        self.set_parameters(softened_values)
+
+        self.wait_ready_to_arm()
+        self.change_mode("QLOITER")
+        self.set_rc(7, 1000)
+        self.arm_vehicle()
+        self.takeoff(20, 'QLOITER')
+
+        # use rc switch to start tune
+        self.set_rc(7, 1500)
+
+        self.wait_text("Quicktune: starting tune", check_context=True)
+        for axis in ['Roll', 'Pitch', 'Yaw']:
+            self.wait_text("Starting %s tune" % axis, check_context=True)
+            self.wait_text("Quicktune: %s D done" % axis, check_context=True, timeout=120)
+            self.wait_text("Quicktune: %s P done" % axis, check_context=True, timeout=120)
+            self.wait_text("Quicktune: %s done" % axis, check_context=True, timeout=120)
+
+        new_values = self.get_parameters(soften_params)
+        for p in original_values.keys():
+            threshold = 0.8 * original_values[p]
+            self.progress("tuned param %s %.4f need %.4f" % (p, new_values[p], threshold))
+            if new_values[p] < threshold:
+                raise NotAchievedException(
+                    "parameter %s %.4f not increased over %.4f" %
+                    (p, new_values[p], threshold))
+
+        self.progress("ensure we are not overtuned")
+        self.set_parameter('SIM_ENGINE_MUL', 0.9)
+
+        self.delay_sim_time(5)
+
+        # and restore it
+        self.set_parameter('SIM_ENGINE_MUL', 1)
+
+        for i in range(5):
+            self.wait_heartbeat()
+
+        if self.statustext_in_collections("ABORTING"):
+            raise NotAchievedException("tune has aborted, overtuned")
+
+        self.progress("using aux fn for save tune")
+
+        # to test aux function method, use aux fn for save
+        self.run_auxfunc(181, 2)
+        self.wait_text("Quicktune: saved", check_context=True)
+        self.change_mode("QLAND")
+
+        self.wait_disarmed(timeout=120)
+        self.set_parameter("QWIK_ENABLE", 0)
+        self.context_pop()
+        self.reboot_sitl()
+
     def PrecisionLanding(self):
         '''VTOL precision landing'''
 
@@ -1764,8 +1853,8 @@ def DCMClimbRate(self):
 
         # Kill any GPSs
         self.set_parameters({
-            'SIM_GPS_DISABLE': 1,
-            'SIM_GPS2_DISABLE': 1,
+            'SIM_GPS1_ENABLE': 0,
+            'SIM_GPS2_ENABLE': 0,
         })
         self.delay_sim_time(5)
 
@@ -2104,6 +2193,7 @@ def tests(self):
             self.LoiterAltQLand,
             self.VTOLLandSpiral,
             self.VTOLQuicktune,
+            self.VTOLQuicktune_CPP,
             self.PrecisionLanding,
             self.ShipLanding,
             Test(self.MotorTest, kwargs={  # tests motors 4 and 2
diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py
index 46ec91abf99c99..af876bdece7c80 100644
--- a/Tools/autotest/rover.py
+++ b/Tools/autotest/rover.py
@@ -6807,7 +6807,7 @@ def JammingSimulation(self):
         '''Test jamming simulation works'''
         self.wait_ready_to_arm()
         start_loc = self.assert_receive_message('GPS_RAW_INT')
-        self.set_parameter("SIM_GPS_JAM", 1)
+        self.set_parameter("SIM_GPS1_JAM", 1)
 
         class Requirement():
             def __init__(self, field, min_value):
@@ -6876,6 +6876,75 @@ def BatteryInvalid(self):
         self.set_parameter("BATT_MONITOR", 0)
         self.wait_ready_to_arm()
 
+    # this method modified from cmd_addpoly in the MAVProxy code:
+    def generate_polyfence(self, centre_loc, command, radius, count, rotation=0):
+        '''adds a number of waypoints equally spaced around a circle
+
+        '''
+        if count < 3:
+            raise ValueError("Invalid count (%s)" % str(count))
+        if radius <= 0:
+            raise ValueError("Invalid radius (%s)" % str(radius))
+
+        latlon = (centre_loc.lat, centre_loc.lng)
+
+        items = []
+        for i in range(0, count):
+            (lat, lon) = mavextra.gps_newpos(latlon[0],
+                                             latlon[1],
+                                             360/float(count)*i + rotation,
+                                             radius)
+
+            m = mavutil.mavlink.MAVLink_mission_item_int_message(
+                1,  # target system
+                1,  # target component
+                0,    # seq
+                mavutil.mavlink.MAV_FRAME_GLOBAL,    # frame
+                command,    # command
+                0,    # current
+                0,    # autocontinue
+                count, # param1,
+                0.0,  # param2,
+                0.0,  # param3
+                0.0,  # param4
+                int(lat*1e7),  # x (latitude)
+                int(lon*1e7),  # y (longitude)
+                0,                     # z (altitude)
+                mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
+            )
+            items.append(m)
+
+        return items
+
+    def SDPolyFence(self):
+        '''test storage of fence on SD card'''
+        self.set_parameters({
+            'BRD_SD_FENCE': 32767,
+        })
+        self.reboot_sitl()
+
+        home = self.home_position_as_mav_location()
+        fence = self.generate_polyfence(
+            home,
+            mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION,
+            radius=100,
+            count=100,
+        )
+
+        for bearing in range(0, 359, 60):
+            x = self.offset_location_heading_distance(home, bearing, 100)
+            fence.extend(self.generate_polyfence(
+                x,
+                mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION,
+                radius=100,
+                count=100,
+            ))
+
+        self.correct_wp_seq_numbers(fence)
+        self.check_fence_upload_download(fence)
+
+        self.delay_sim_time(1000)
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestRover, self).tests()
@@ -6927,6 +6996,7 @@ def tests(self):
             self.DataFlash,
             self.SkidSteer,
             self.PolyFence,
+            self.SDPolyFence,
             self.PolyFenceAvoidance,
             self.PolyFenceObjectAvoidanceAuto,
             self.PolyFenceObjectAvoidanceGuided,
diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py
index 5885c21267fc9f..3c08f0939652b0 100644
--- a/Tools/autotest/vehicle_test_suite.py
+++ b/Tools/autotest/vehicle_test_suite.py
@@ -2711,8 +2711,6 @@ def get_sim_parameter_documentation_get_whitelist(self):
             "SIM_MAG1_OFS_Z",
             "SIM_PARA_ENABLE",
             "SIM_PARA_PIN",
-            "SIM_PLD_ALT_LMT",
-            "SIM_PLD_DIST_LMT",
             "SIM_RICH_CTRL",
             "SIM_RICH_ENABLE",
             "SIM_SHIP_DSIZE",
@@ -3320,7 +3318,7 @@ def try_symlink_tlog(self):
             os.link(self.logfile, self.buildlog)
         except OSError as error:
             self.progress("OSError [%d]: %s" % (error.errno, error.strerror))
-            self.progress("WARN: Failed to create link: %s => %s, "
+            self.progress("Problem: Failed to create link: %s => %s, "
                           "will copy tlog manually to target location" %
                           (self.logfile, self.buildlog))
             self.copy_tlog = True
@@ -3665,7 +3663,7 @@ def HIGH_LATENCY2(self):
         if (m.failure_flags & mavutil.mavlink.HL_FAILURE_FLAG_GPS) != 0:
             raise NotAchievedException("Expected GPS to be OK")
         self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_GPS, True, True, True)
-        self.set_parameter("SIM_GPS_TYPE", 0)
+        self.set_parameter("SIM_GPS1_TYPE", 0)
         self.delay_sim_time(10)
         self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_GPS, False, False, False)
         m = self.poll_message("HIGH_LATENCY2")
@@ -3674,7 +3672,7 @@ def HIGH_LATENCY2(self):
             raise NotAchievedException("Expected GPS to be failed")
 
         self.start_subtest("HIGH_LATENCY2 location")
-        self.set_parameter("SIM_GPS_TYPE", 1)
+        self.set_parameter("SIM_GPS1_TYPE", 1)
         self.delay_sim_time(10)
         m = self.poll_message("HIGH_LATENCY2")
         self.progress(self.dump_message_verbose(m))
@@ -8471,7 +8469,7 @@ def wait_ekf_flags(self, required_value, error_bits, **kwargs):
 
     def wait_gps_disable(self, position_horizontal=True, position_vertical=False, timeout=30):
         """Disable GPS and wait for EKF to report the end of assistance from GPS."""
-        self.set_parameter("SIM_GPS_DISABLE", 1)
+        self.set_parameter("SIM_GPS1_ENABLE", 0)
         tstart = self.get_sim_time()
 
         """ if using SITL estimates directly """
@@ -9430,6 +9428,20 @@ def offset_location_ne(self, location, metres_north, metres_east):
                                 location.alt,
                                 location.heading)
 
+    def offset_location_heading_distance(self, location, bearing, distance):
+        (target_lat, target_lng) = mavextra.gps_newpos(
+            location.lat,
+            location.lng,
+            bearing,
+            distance
+        )
+        return mavutil.location(
+            target_lat,
+            target_lng,
+            location.alt,
+            location.heading
+        )
+
     def monitor_groundspeed(self, want, tolerance=0.5, timeout=5):
         tstart = self.get_sim_time()
         while True:
@@ -10683,7 +10695,7 @@ def ArmFeatures(self):
                     p1=1,  # ARM
                     want_result=mavutil.mavlink.MAV_RESULT_FAILED,
                 )
-                self.set_parameter("SIM_GPS_DISABLE", 0)
+                self.set_parameter("SIM_GPS1_ENABLE", 1)
                 self.wait_ekf_happy() # EKF may stay unhappy for a while
                 self.progress("PASS not able to arm without Position in mode : %s" % mode)
             if mode in self.get_no_position_not_settable_modes_list():
@@ -10693,10 +10705,10 @@ def ArmFeatures(self):
                 try:
                     self.change_mode(mode, timeout=15)
                 except AutoTestTimeoutException:
-                    self.set_parameter("SIM_GPS_DISABLE", 0)
+                    self.set_parameter("SIM_GPS1_ENABLE", 1)
                     self.progress("PASS not able to set mode without Position : %s" % mode)
                 except ValueError:
-                    self.set_parameter("SIM_GPS_DISABLE", 0)
+                    self.set_parameter("SIM_GPS1_ENABLE", 1)
                     self.progress("PASS not able to set mode without Position : %s" % mode)
             if mode == "FOLLOW":
                 self.set_parameter("FOLL_ENABLE", 0)
@@ -12313,6 +12325,7 @@ def download_parameters(self, target_system, target_component):
         expected_count = 0
         seen_ids = {}
         self.progress("Downloading parameters")
+        debug = False
         while True:
             now = self.get_sim_time_cached()
             if not start_done or now - last_parameter_received > 10:
@@ -12323,6 +12336,7 @@ def download_parameters(self, target_system, target_component):
                 elif attempt_count != 0:
                     self.progress("Download failed; retrying")
                     self.delay_sim_time(1)
+                    debug = True
                 self.drain_mav()
                 self.mav.mav.param_request_list_send(target_system, target_component)
                 attempt_count += 1
@@ -12337,8 +12351,8 @@ def download_parameters(self, target_system, target_component):
             if m.param_index == 65535:
                 self.progress("volunteered parameter: %s" % str(m))
                 continue
-            if False:
-                self.progress("  received (%4u/%4u %s=%f" %
+            if debug:
+                self.progress("  received id=%4u param_count=%4u %s=%f" %
                               (m.param_index, m.param_count, m.param_id, m.param_value))
             if m.param_index >= m.param_count:
                 raise ValueError("parameter index (%u) gte parameter count (%u)" %
@@ -12531,7 +12545,7 @@ def AdvancedFailsafe(self):
             self.context_collect("STATUSTEXT")
             self.set_parameters({
                 "AFS_MAX_GPS_LOSS": 1,
-                "SIM_GPS_DISABLE": 1,
+                "SIM_GPS1_ENABLE": 0,
             })
             self.wait_statustext("AFS State: GPS_LOSS", check_context=True)
             self.context_pop()
@@ -14160,7 +14174,7 @@ def GPSTypes(self):
         self.context_collect("STATUSTEXT")
         for (sim_gps_type, name, gps_type, detect_name, serial_protocol, detect_prefix) in sim_gps:
             self.start_subtest("Checking GPS type %s" % name)
-            self.set_parameter("SIM_GPS_TYPE", sim_gps_type)
+            self.set_parameter("SIM_GPS1_TYPE", sim_gps_type)
             self.set_parameter("SERIAL3_PROTOCOL", serial_protocol)
             if gps_type is None:
                 gps_type = 1  # auto-detect
@@ -14265,7 +14279,7 @@ def MultipleGPS(self):
 
         self.start_subtest("Ensure detection when sim gps connected")
         self.set_parameter("SIM_GPS2_TYPE", 1)
-        self.set_parameter("SIM_GPS2_DISABLE", 0)
+        self.set_parameter("SIM_GPS2_ENABLE", 1)
         # a reboot is required after setting GPS2_TYPE.  We start
         # sending GPS2_RAW out, once the parameter is set, but a
         # reboot is required because _port[1] is only set in
@@ -14281,9 +14295,9 @@ def MultipleGPS(self):
             raise NotAchievedException("Incorrect fix type")
 
         self.start_subtest("Check parameters are per-GPS")
-        self.assert_parameter_value("SIM_GPS_NUMSATS", 10)
+        self.assert_parameter_value("SIM_GPS1_NUMSATS", 10)
         self.assert_gps_satellite_count("GPS_RAW_INT", 10)
-        self.set_parameter("SIM_GPS_NUMSATS", 13)
+        self.set_parameter("SIM_GPS1_NUMSATS", 13)
         self.assert_gps_satellite_count("GPS_RAW_INT", 13)
 
         self.assert_parameter_value("SIM_GPS2_NUMSATS", 10)
@@ -14311,7 +14325,7 @@ def MultipleGPS(self):
         if abs(gpi_alt - new_gpi_alt) > 100:
             raise NotAchievedException("alt moved unexpectedly")
         self.progress("Killing first GPS")
-        self.set_parameter("SIM_GPS_DISABLE", 1)
+        self.set_parameter("SIM_GPS1_ENABLE", 0)
         self.delay_sim_time(1)
         self.progress("Checking altitude now matches second GPS")
         m = self.assert_receive_message("GLOBAL_POSITION_INT")
diff --git a/Tools/bootloaders/AET-H743-Basic_bl.bin b/Tools/bootloaders/AET-H743-Basic_bl.bin
new file mode 100644
index 00000000000000..8afdc716014802
Binary files /dev/null and b/Tools/bootloaders/AET-H743-Basic_bl.bin differ
diff --git a/Tools/bootloaders/AET-H743-Basic_bl.hex b/Tools/bootloaders/AET-H743-Basic_bl.hex
new file mode 100644
index 00000000000000..e06821cdab430f
--- /dev/null
+++ b/Tools/bootloaders/AET-H743-Basic_bl.hex
@@ -0,0 +1,1276 @@
+:020000040800F2
+:1000000000060020E1020008E3020008E302000805
+:10001000E3020008E3020008E3020008E30200082C
+:10002000E3020008E3020008E30200089D30000834
+:10003000E3020008E3020008E3020008E30200080C
+:10004000E3020008E3020008E3020008E3020008FC
+:10005000E3020008E3020008FD4700082948000801
+:100060005548000881480008AD48000831110008D3
+:100070005911000885110008B1110008DD110008B0
+:100080000512000831120008E3020008E30200082C
+:10009000E3020008E3020008E3020008D948000870
+:1000A000E3020008E3020008E3020008E30200089C
+:1000B000E3020008E3020008E3020008E30200088C
+:1000C000E3020008E3020008E3020008E30200087C
+:1000D000E3020008E3020008E3020008E30200086C
+:1000E0003D490008E3020008E3020008E3020008BB
+:1000F000E3020008E3020008E30200085D120008C2
+:10010000E3020008E3020008C5490008E302000812
+:10011000E3020008E3020008E3020008E30200082B
+:1001200089120008B1120008DD1200080913000846
+:1001300035130008E3020008E3020008E3020008A8
+:10014000E3020008E3020008E3020008E3020008FB
+:100150005D13000889130008B5130008E3020008C6
+:10016000E3020008E3020008E3020008E3020008DB
+:10017000E3020008893C0008E3020008E3020008EB
+:10018000E3020008E3020008B1490008E3020008A6
+:10019000E3020008E3020008E3020008E3020008AB
+:1001A000E3020008E3020008E3020008E30200089B
+:1001B000E3020008E3020008E3020008E30200088B
+:1001C000E3020008E3020008E3020008E30200087B
+:1001D000E3020008753C0008E3020008E30200089F
+:1001E000E3020008E3020008E3020008E30200085B
+:1001F000E3020008E3020008E3020008E30200084B
+:10020000E3020008E3020008E3020008E30200083A
+:10021000E3020008E3020008E3020008E30200082A
+:10022000E3020008E3020008E3020008E30200081A
+:10023000E3020008E3020008E3020008E30200080A
+:10024000E3020008E3020008E3020008E3020008FA
+:10025000E3020008E3020008E3020008E3020008EA
+:10026000E3020008E3020008E3020008E3020008DA
+:10027000E3020008E3020008E3020008E3020008CA
+:10028000E3020008E3020008E3020008E3020008BA
+:10029000E3020008E3020008E3020008E3020008AA
+:1002A000E3020008E3020008E3020008E30200089A
+:1002B000E3020008E3020008E3020008E30200088A
+:1002C000E3020008E3020008E3020008E30200087A
+:1002D000E3020008E3020008E3020008E30200086A
+:1002E00002E000F000F8FEE772B6374880F30888B5
+:1002F000364880F3098836483649086040F20000E5
+:10030000CCF200004EF63471CEF200010860BFF36B
+:100310004F8FBFF36F8F40F20000C0F2F0004EF637
+:100320008851CEF200010860BFF34F8FBFF36F8F8B
+:100330004FF00000E1EE100A4EF63C71CEF20001E3
+:100340000860062080F31488BFF36F8F02F008FF67
+:1003500004F00EF84FF055301F491B4A91423CBF44
+:1003600041F8040BFAE71D49184A91423CBF41F895
+:10037000040BFAE71A491B4A1B4B9A423EBF51F83D
+:10038000040B42F8040BF8E700201849184A914280
+:100390003CBF41F8040BFAE702F020FF04F06CF8D0
+:1003A000144C154DAC4203DA54F8041B8847F9E7A6
+:1003B00000F042F8114C124DAC4203DA54F8041B21
+:1003C0008847F9E702F008BF00060020002200205D
+:1003D0000000000808ED00E00000002000060020FA
+:1003E000384F0008002200206422002068220020EC
+:1003F0000C490020E0020008E0020008E0020008CA
+:10040000E00200082DE9F04F2DED108AC1F80CD064
+:10041000D0F80CD0BDEC108ABDE8F08F002383F338
+:1004200011882846A047002002F000F9FEE702F0FC
+:100430008BF800DFFEE7000038B500F045FC00F067
+:1004400001FE02F0EBFD054602F01EFE0446C0B9B7
+:100450000E4B9D4217D001339D4241F2883512BFA9
+:10046000044600250124002002F0E2FD0CB100F05A
+:1004700075F800F085FD284600F01EF900F06EF8D2
+:10048000F9E70025EFE70546EDE700BF010007B0FB
+:1004900008B500F0E5FBA0F120035842584108BD23
+:1004A00007B541F21203022101A8ADF8043000F0B3
+:1004B000F5FB03B05DF804FB38B5302383F31188F6
+:1004C000174803680BB102F07DF90023154A4FF479
+:1004D0007A71134802F06CF9002383F31188124CEF
+:1004E000236813B12368013B2360636813B1636819
+:1004F000013B63600D4D2B7833B963687BB90220F3
+:1005000000F0A8FC322363602B78032B07D16368CB
+:100510002BB9022000F09EFC4FF47A73636038BD63
+:1005200068220020B90400088823002080220020CF
+:10053000084B187003280CD8DFE800F00805020803
+:10054000022000F07FBC022000F072BC024B0022AF
+:100550005A6070478022002088230020F8B5504B55
+:10056000504A1C461968013100F0998004339342C7
+:10057000F8D162684C4B9A4240F291804B4B9B6899
+:1005800003F1006303F500339A4280F08880002075
+:1005900000F0C0FB0220FFF7CBFF454B0021D3F852
+:1005A000E820C3F8E810D3F81021C3F81011D3F8ED
+:1005B0001021D3F8EC20C3F8EC10D3F81421C3F8C1
+:1005C0001411D3F81421D3F8F020C3F8F010D3F8A5
+:1005D0001821C3F81811D3F81821D3F8802042F05D
+:1005E0000062C3F88020D3F8802022F00062C3F8B4
+:1005F0008020D3F88020D3F8802042F00072C3F826
+:100600008020D3F8802022F00072C3F88020D3F835
+:10061000803072B64FF0E023C3F8084DD4E90004EF
+:10062000BFF34F8FBFF36F8F224AC2F88410BFF31E
+:100630004F8F536923F480335361BFF34F8FD2F848
+:10064000803043F6E076C3F3C905C3F34E335B0154
+:1006500003EA060C29464CEA81770139C2F8747224
+:10066000F9D2203B13F1200FF2D1BFF34F8FBFF32C
+:100670006F8FBFF34F8FBFF36F8F536923F4003336
+:1006800053610023C2F85032BFF34F8FBFF36F8F17
+:10069000302383F31188854680F308882047F8BD0E
+:1006A0000000020820000208FFFF010800220020CD
+:1006B0000044025800ED00E02DE9F04F93B0B44B38
+:1006C0002022FF2100900AA89D6800F003FCB14A97
+:1006D0001378A3B90121B04811700360302383F36C
+:1006E000118803680BB102F06DF80023AB4A4FF498
+:1006F0007A71A94802F05CF8002383F31188009B0B
+:1007000013B1A74B009A1A60A64A1378032B03D0A3
+:1007100000231370A24A53604FF0000A009CD34696
+:100720005646D146012000F08DFB24B19C4B1B683E
+:10073000002B00F02682002000F092FA0390039B29
+:10074000002BF2DB012000F073FB039B213B1F2BEE
+:10075000E8D801A252F823F0D907000801080008E0
+:100760009508000825070008250700082507000848
+:1007700027090008F70A0008110A0008730A000890
+:100780009B0A0008C10A000825070008D30A0008D0
+:1007900025070008450B0008790800082507000810
+:1007A000890B0008E50700087908000825070008FC
+:1007B000730A000825070008250700082507000818
+:1007C0002507000825070008250700082507000859
+:1007D00025070008950800080220FFF759FE0028A9
+:1007E00040F0F981009B022105A8BAF1000F08BF73
+:1007F0001C4641F21233ADF8143000F04FFA91E785
+:100800004FF47A7000F02CFA071EEBDB0220FFF7A2
+:100810003FFE0028E6D0013F052F00F2DE81DFE831
+:1008200007F0030A0D1013360523042105A80593CC
+:1008300000F034FA17E004215548F9E704215A483A
+:10084000F6E704215948F3E74FF01C08404608F149
+:10085000040800F061FA0421059005A800F01EFAD2
+:10086000B8F12C0FF2D101204FF0000900FA07F780
+:1008700047EA0B0B5FFA8BFB00F078FB26B10BF01D
+:100880000B030B2B08BF0024FFF70AFE4AE70421E5
+:100890004748CDE7002EA5D00BF00B030B2BA1D1C1
+:1008A0000220FFF7F5FD074600289BD00120002617
+:1008B00000F030FA0220FFF73BFE1FFA86F84046B0
+:1008C00000F038FA0446B0B1039940460136A1F170
+:1008D00040025142514100F03DFA0028EDD1BA46A4
+:1008E000044641F21213022105A83E46ADF8143029
+:1008F00000F0D4F916E725460120FFF719FE244B36
+:100900009B68AB4207D9284600F006FA013040F058
+:1009100067810435F3E70025224BBA463E461D7039
+:100920001F4B5D60A8E7002E3FF45CAF0BF00B039C
+:100930000B2B7FF457AF0220FFF7FAFD322000F0B7
+:100940008FF9B0F10008FFF64DAF18F003077FF400
+:1009500049AF0F4A08EB0503926893423FF642AF56
+:10096000B8F5807F3FF73EAF124BB845019323DDCA
+:100970004FF47A7000F074F90390039A002AFFF69E
+:1009800031AF039A0137019B03F8012BEDE700BF5C
+:10099000002200208423002068220020B9040008DF
+:1009A000882300208022002004220020082200202A
+:1009B0000C22002084220020C820FFF769FD074692
+:1009C00000283FF40FAF1F2D11D8C5F120020AAB4C
+:1009D00025F0030084494245184428BF424601924D
+:1009E00000F052FA019AFF217F4800F073FA4FEAB3
+:1009F000A803C8F387027C492846019300F072FAE5
+:100A0000064600283FF46DAF019B05EB830533E7F5
+:100A10000220FFF73DFD00283FF4E4AE00F0B8F9F6
+:100A200000283FF4DFAE0027B846704B9B68BB42FE
+:100A300018D91F2F11D80A9B01330ED027F00303BA
+:100A400012AA134453F8203C05934046042205A9FA
+:100A5000043700F057FB8046E7E7384600F05CF9C2
+:100A60000590F2E7CDF81480042105A800F016F9EE
+:100A700002E70023642104A8049300F005F900288C
+:100A80007FF4B0AE0220FFF703FD00283FF4AAAECA
+:100A9000049800F073F90590E6E70023642104A8A8
+:100AA000049300F0F1F800287FF49CAE0220FFF7D9
+:100AB000EFFC00283FF496AE049800F061F9EAE7F5
+:100AC0000220FFF7E5FC00283FF48CAE00F070F93F
+:100AD000E1E70220FFF7DCFC00283FF483AE05A924
+:100AE000142000F06BF907460421049004A800F0DC
+:100AF000D5F83946B9E7322000F0B2F8071EFFF604
+:100B000071AEBB077FF46EAE384A07EB09039268FB
+:100B100093423FF667AE0220FFF7BAFC00283FF48D
+:100B200061AE27F003074F44B9453FF4A5AE4846F0
+:100B300009F1040900F0F0F80421059005A800F07F
+:100B4000ADF8F1E74FF47A70FFF7A2FC00283FF40C
+:100B500049AE00F01DF9002844D00A9B01330BD0A8
+:100B600008220AA9002000F0BDF900283AD020226E
+:100B7000FF210AA800F0AEF9FFF792FC1C4801F033
+:100B80005BFD13B0BDE8F08F002E3FF42BAE0BF0F1
+:100B90000B030B2B7FF426AE0023642105A80593DD
+:100BA00000F072F8074600287FF41CAE0220FFF721
+:100BB0006FFC804600283FF415AEFFF771FC41F250
+:100BC000883001F039FD059800F01AFA46463C4697
+:100BD00000F0CCF9A6E506464EE64FF0000901E626
+:100BE000BA467EE637467CE68422002000220020BA
+:100BF000A08601002DE9F84F4FF47A7306460D46A2
+:100C0000002402FB03F7DFF85080DFF8509098F9DA
+:100C100000305FFA84FA5A1C01D0A34212D159F86D
+:100C200024002A4631460368D3F820B03B46D84713
+:100C3000854207D1074B012083F800A0BDE8F88F5B
+:100C40000124E4E7002CFBD04FF4FA7001F0F4FC2F
+:100C50000020F3E7D42300201022002014220020DB
+:100C6000002307B5024601210DF107008DF807307A
+:100C7000FFF7C0FF20B19DF8070003B05DF804FB4B
+:100C80004FF0FF30F9E700000A46042108B5FFF7EE
+:100C9000B1FF80F00100C0B2404208BD074B0A46D8
+:100CA00030B41978064B53F821400146236820469A
+:100CB000DD69044BAC4630BC604700BFD423002044
+:100CC00014220020A086010070B5104C0025104EA3
+:100CD00001F0CEFF20803068238883420CD80025A5
+:100CE0002088013801F0C0FF23880544013BB5F599
+:100CF000802F2380F4D370BD01F0B6FF3368054424
+:100D00000133B5F5003F3360E5D3E8E7D623002093
+:100D10009023002002F07AB800F1006000F5003066
+:100D20000068704700F10060920000F5003001F0AB
+:100D3000FBBF0000054B1A68054B1B889B1A8342BA
+:100D400002D9104401F090BF00207047902300208A
+:100D5000D623002038B50446074D29B12868204421
+:100D6000BDE8384001F098BF2868204401F082FFB8
+:100D70000028F3D038BD00BF90230020002070472A
+:100D800000F1FF5000F58F10D0F800087047000008
+:100D9000064991F8243033B100230822086A81F80B
+:100DA0002430FFF7BFBF0120704700BF942300200D
+:100DB000014B1868704700BF0010005C194B0138E8
+:100DC0000322084470B51D68174BC5F30B042D0CA6
+:100DD0001E88A6420BD15C680A46013C824213463B
+:100DE0000FD214F9016F4EB102F8016BF6E7013A28
+:100DF00003F10803ECD181420B4602D22C2203F806
+:100E0000012B0424094A1688AE4204D1984284BFBB
+:100E1000967803F8016B013C02F10402F3D1581AF1
+:100E200070BD00BF0010005C1C220020C84A0008F2
+:100E3000022803D1024B4FF400229A61704700BF91
+:100E400000100258022802D1014B08229A61704713
+:100E500000100258022804D1024A536983F00803A3
+:100E6000536170470010025870B504464FF47A760B
+:100E70004CB1412C254628BF412506FB05F0641BDB
+:100E800001F0DAFBF4E770BD002310B5934203D004
+:100E9000CC5CC4540133F9E710BD0000013810B533
+:100EA00010F9013F3BB191F900409C4203D11AB1C6
+:100EB0000131013AF4E71AB191F90020981A10BDF6
+:100EC0001046FCE703460246D01A12F9011B00291E
+:100ED000FAD1704702440346934202D003F8011B43
+:100EE000FAE770472DE9F8431F4D14460746884638
+:100EF00095F8242052BBDFF870909CB395F824300D
+:100F00002BB92022FF2148462F62FFF7E3FF95F817
+:100F100024004146C0F1080205EB8000A24228BF30
+:100F20002246D6B29200FFF7AFFF95F82430A41BFB
+:100F300017441E449044E4B2F6B2082E85F82460AB
+:100F4000DBD1FFF725FF0028D7D108E02B6A03EBA0
+:100F500082038342CFD0FFF71BFF0028CBD10020B4
+:100F6000BDE8F8830120FBE794230020024B1A78A8
+:100F7000024B1A70704700BFD423002010220020BB
+:100F800038B51A4C1A4D204600F086FE29462046F8
+:100F900000F0AEFE2D684FF47A70D5F89020D2F8AC
+:100FA000043843F00203C2F80438FFF75DFF11492B
+:100FB000284600F0ABFFD5F890200F4DD2F804384A
+:100FC000286823F002030D49A042C2F804384FF408
+:100FD000E1330B6001D000F0BDFD6868A04204D091
+:100FE0000649BDE8384000F0B5BD38BD382B0020BB
+:100FF000C44C0008CC4C000814220020BC23002064
+:101000000C4B70B50C4D04461E780C4B55F8262041
+:101010009A420DD00A4B002118221846FFF75AFFBA
+:101020000460014655F82600BDE8704000F092BD0E
+:1010300070BD00BFD423002014220020382B0020D4
+:10104000BC230020F8B571B6002301201A461946CA
+:1010500000F058FA0446802001F076FF002849D0BD
+:101060000025254A80274FF4D06C3D26136913F0E4
+:10107000C06F26D1D2F8103113F0C06F21D1236890
+:1010800005F1006199602368D86023685F60236878
+:10109000C3F800C021680B6843F001030B602168AE
+:1010A0000B6823F01E030B6021680B68DB07FCD480
+:1010B000237B8035616806FA03F3B5F5001F0B60EA
+:1010C000D4D1204600F05EFAB5F5001F11D00024FF
+:1010D0000A4E0B4D012001F099FE3388A34205D939
+:1010E00028682044013401F0D7FDF6E7002001F024
+:1010F0008DFE61B6F8BD00BF00200052D62300204F
+:101100009023002030B50A44084D91420DD011F8CB
+:10111000013B5840082340F30004013B2C4013F0EE
+:10112000FF0384EA5000F6D1EFE730BD2083B8ED2D
+:1011300008B5074B074A196801F03D0199605368EB
+:101140000BB190689847BDE8084001F0DFBF00BFD1
+:1011500000000240D823002008B5084B196889090F
+:1011600001F03D018A019A60054AD3680BB110690C
+:101170009847BDE8084001F0C9BF00BF0000024029
+:10118000D823002008B5084B1968090C01F03D016F
+:101190000A049A60054A53690BB190699847BDE803
+:1011A000084001F0B3BF00BF00000240D823002078
+:1011B00008B5084B1968890D01F03D018A059A6050
+:1011C000054AD3690BB1106A9847BDE8084001F0A1
+:1011D0009DBF00BF00000240D823002008B5074B88
+:1011E000074A596801F03D01D960536A0BB1906A12
+:1011F0009847BDE8084001F089BF00BF00000240E9
+:10120000D823002008B5084B5968890901F03D0131
+:101210008A01DA60054AD36A0BB1106B9847BDE8C2
+:10122000084001F073BF00BF00000240D823002037
+:1012300008B5084B5968090C01F03D010A04DA6051
+:10124000054A536B0BB1906B9847BDE8084001F01D
+:101250005DBF00BF00000240D823002008B5084B46
+:101260005968890D01F03D018A05DA60054AD36BA2
+:101270000BB1106C9847BDE8084001F047BF00BFB4
+:1012800000000240D823002008B5074B074A196820
+:1012900001F03D019960536C0BB1906C9847BDE82B
+:1012A000084001F033BF00BF00040240D8230020F3
+:1012B00008B5084B1968890901F03D018A019A6057
+:1012C000054AD36C0BB1106D9847BDE8084001F09A
+:1012D0001DBF00BF00040240D823002008B5084B02
+:1012E0001968090C01F03D010A049A60054A536D22
+:1012F0000BB1906D9847BDE8084001F007BF00BFF3
+:1013000000040240D823002008B5084B1968890D55
+:1013100001F03D018A059A60054AD36D0BB1106E4C
+:101320009847BDE8084001F0F1BE00BF000402404C
+:10133000D823002008B5074B074A596801F03D0142
+:10134000D960536E0BB1906E9847BDE8084001F02C
+:10135000DDBE00BF00040240D823002008B5084BC2
+:101360005968890901F03D018A01DA60054AD36EA6
+:101370000BB1106F9847BDE8084001F0C7BE00BF31
+:1013800000040240D823002008B5084B5968090C16
+:1013900001F03D010A04DA60054A536F0BB1906F0A
+:1013A0009847BDE8084001F0B1BE00BF000402400C
+:1013B000D823002008B5084B5968890D01F03D017C
+:1013C0008A05DA60054AD36F13B1D2F880009847D6
+:1013D000BDE8084001F09ABE00040240D823002076
+:1013E00000230C4910B51A460B4C0B6054F823002F
+:1013F000026001EB430004334260402BF6D1074A00
+:101400004FF0FF339360D360C2F80834C2F80C3455
+:1014100010BD00BFD8230020D84A000800000240B9
+:101420000F28F8B510D9102810D0112811D0122883
+:1014300008D10F240720DFF8C8E00126DEF80050AD
+:10144000A04208D9002653E00446F4E70F24002008
+:10145000F1E70724FBE706FA00F73D424AD1264CA4
+:101460004FEA001C3D4304EB00160EEBC000CEF823
+:101470000050C0E90123FBB273B12048D0F8D83046
+:1014800043F00103C0F8D830D0F8003143F0010335
+:10149000C0F80031D0F8003117F47F4F0ED0174854
+:1014A000D0F8D83043F00203C0F8D830D0F800317B
+:1014B00043F00203C0F80031D0F8003154F80C00BA
+:1014C000036823F01F030360056815F00105FBD1D5
+:1014D00004EB0C033D2493F80CC05F6804FA0CF491
+:1014E0003C6021240560446112B1987B00F066F9EC
+:1014F0003046F8BD0130A3E7D84A0008004402583E
+:10150000D823002010B5302484F31188FFF788FF1A
+:10151000002383F3118810BD10B50446807B00F0D2
+:1015200063F901231549627B03FA02F20B6823EA8F
+:101530000203DAB20B6072B9114AD2F8D81021F066
+:101540000101C2F8D810D2F8001121F00101C2F84F
+:101550000011D2F8002113F47F4F0ED1084BD3F8BD
+:10156000D82022F00202C3F8D820D3F8002122F0BC
+:101570000202C3F80021D3F8003110BDD8230020A7
+:101580000044025808B5302383F31188FFF7C4FFE5
+:10159000002383F3118808BD02684368114301608A
+:1015A00003B1184770470000024A136843F0C003B4
+:1015B000136070470078004013B50E4C204600F0D1
+:1015C000BDFA04F1140000234FF400720A4900949C
+:1015D00000F07AF9094B4FF40072094904F1380020
+:1015E000009400F0F3F9074A074BC4E9172302B04F
+:1015F00010BD00BF5C240020C8240020A9150008ED
+:10160000C82600200078004000E1F505037C30B5D5
+:10161000244C002918BF0C46012B11D1224B9842B3
+:101620000ED1224BD3F8E82042F08042C3F8E820E4
+:10163000D3F8102142F08042C3F81021D3F81031C2
+:101640002268036EC16D03EB52038466B3FBF2F3B1
+:101650006268150442BF23F0070503F0070343EA5D
+:101660004503CB60A36843F040034B60E36843F05D
+:1016700001038B6042F4967343F001030B604FF05B
+:10168000FF330B62510505D512F0102205D0B2F1DF
+:10169000805F04D080F8643030BD7F23FAE73F23B9
+:1016A000F8E700BFD84B00085C2400200044025833
+:1016B0002DE9F047C66D05463768F46921073462A5
+:1016C0001AD014F0080118BF4FF48071E20748BF28
+:1016D00041F02001A3074FF0300348BF41F0400123
+:1016E000600748BF41F0800183F31188281DFFF790
+:1016F00053FF002383F31188E2050AD5302383F3D7
+:1017000011884FF48061281DFFF746FF002383F303
+:1017100011884FF030094FF0000A14F0200838D13A
+:101720003B0616D54FF0300905F1380A200610D5D2
+:1017300089F31188504600F07DF9002836DA082137
+:10174000281DFFF729FF27F080033360002383F370
+:101750001188790614D5620612D5302383F31188D7
+:10176000D5E913239A4208D12B6C33B127F04007F7
+:101770001021281DFFF710FF3760002383F3118825
+:10178000E30618D5AA6E1369ABB15069BDE8F047FE
+:10179000184789F31188736A284695F86410194030
+:1017A00000F0E6F98AF31188F469B6E7B06288F3CD
+:1017B0001188F469BAE7BDE8F0870000090100F17B
+:1017C0006043012203F56143C9B283F8001300F0BE
+:1017D0001F039A4043099B0003F1604303F56143F3
+:1017E000C3F880211A60704700F01F0301229A405D
+:1017F000430900F160409B0000F5614003F1604344
+:1018000003F56143C3F88020C3F88021002380F8EA
+:1018100000337047F8B51546826804460B46AA4265
+:1018200000D28568A1692669761AB5420BD218469E
+:101830002A46FFF729FBA3692B44A3612846A36826
+:101840005B1BA360F8BD0CD9AF1B18463246FFF7EF
+:101850001BFB3A46E1683044FFF716FBE3683B4464
+:10186000EBE718462A46FFF70FFBE368E5E70000C1
+:1018700083689342F7B50446154600D28568D4E9DB
+:101880000460361AB5420BD22A46FFF7FDFA6369A7
+:101890002B4463612846A3685B1BA36003B0F0BDC3
+:1018A0000DD93246AF1B0191FFF7EEFA01993A4686
+:1018B000E0683144FFF7E8FAE3683B44E9E72A4689
+:1018C000FFF7E2FAE368E4E710B50A440024C361D5
+:1018D000029B8460C16002610362C0E90000C0E94C
+:1018E000051110BD08B5D0E90532934201D18268D7
+:1018F00082B98268013282605A1C426119700021EB
+:10190000D0E904329A4224BFC368436100F0B0FEBC
+:10191000002008BD4FF0FF30FBE7000070B530231A
+:1019200004460E4683F31188A568A5B1A368A26991
+:10193000013BA360531CA36115782269934224BF25
+:10194000E368A361E3690BB120469847002383F362
+:101950001188284607E03146204600F079FE00282D
+:10196000E2DA85F3118870BD2DE9F74F04460E4683
+:1019700017469846D0F81C904FF0300A8AF3118829
+:101980004FF0000B154665B12A4631462046FFF759
+:1019900041FF034660B94146204600F059FE002849
+:1019A000F1D0002383F31188781B03B0BDE8F08FDA
+:1019B000B9F1000F03D001902046C847019B8BF37B
+:1019C0001188ED1A1E448AF31188DCE7C160C361F7
+:1019D000009B82600362C0E905111144C0E9000068
+:1019E00001617047F8B504460D461646302383F36F
+:1019F0001188A768A7B1A368013BA36063695A1C5B
+:101A000062611D70D4E904329A4224BFE3686361C5
+:101A1000E3690BB120469847002080F3118807E066
+:101A20003146204600F014FE0028E2DA87F31188E0
+:101A3000F8BD0000D0E9052310B59A4201D18268B3
+:101A40007AB982680021013282605A1C82611C7856
+:101A500003699A4224BFC368836100F009FE2046EF
+:101A600010BD4FF0FF30FBE72DE9F74F04460E465F
+:101A700017469846D0F81C904FF0300A8AF3118828
+:101A80004FF0000B154665B12A4631462046FFF758
+:101A9000EFFE034660B94146204600F0D9FD00281C
+:101AA000F1D0002383F31188781B03B0BDE8F08FD9
+:101AB000B9F1000F03D001902046C847019B8BF37A
+:101AC0001188ED1A1E448AF31188DCE70268436826
+:101AD0001143016003B11847704700001430FFF74D
+:101AE00043BF00004FF0FF331430FFF73DBF00004D
+:101AF0003830FFF7B9BF00004FF0FF333830FFF741
+:101B0000B3BF00001430FFF709BF00004FF0FF31F2
+:101B10001430FFF703BF00003830FFF763BF000049
+:101B20004FF0FF323830FFF75DBF0000012914BFCE
+:101B30006FF0130000207047FFF73EBD044B0360B9
+:101B400000234360C0E9023301230374704700BFE0
+:101B5000F04B000810B53023044683F31188FFF7DB
+:101B600055FD02230020237480F3118810BD00006E
+:101B700038B5C36904460D461BB904210844FFF774
+:101B8000A5FF294604F11400FFF7ACFE002806DA91
+:101B9000201D4FF40061BDE83840FFF797BF38BD06
+:101BA000026843681143016003B1184770470000A1
+:101BB00013B5406B00F58054D4F8A4381A68117836
+:101BC000042914D1017C022911D119790123128928
+:101BD0008B4013420BD101A94C3002F087F9D4F8A5
+:101BE000A4480246019B2179206800F0DFF902B089
+:101BF00010BD0000143002F009B900004FF0FF33AF
+:101C0000143002F003B900004C3002F0DBB90000E0
+:101C10004FF0FF334C3002F0D5B90000143002F021
+:101C2000D7B800004FF0FF31143002F0D1B80000F7
+:101C30004C3002F0A7B900004FF0FF324C3002F0F8
+:101C4000A1B900000020704710B500F58054D4F809
+:101C5000A4381A681178042917D1017C022914D1FB
+:101C60005979012352898B4013420ED1143002F06E
+:101C700069F8024648B1D4F8A4484FF4407361793A
+:101C80002068BDE8104000F07FB910BD406BFFF741
+:101C9000DBBF0000704700007FB5124B0125042612
+:101CA000044603600023057400F184024360294662
+:101CB000C0E902330C4B0290143001934FF440738F
+:101CC000009602F01BF8094B04F69442294604F1F1
+:101CD0004C000294CDE900634FF4407302F0E2F847
+:101CE00004B070BD184C00088D1C0008B11B000822
+:101CF0000A68302383F311880B790B3342F82300F1
+:101D00004B79133342F823008B7913B10B3342F82C
+:101D1000230000F58053C3F8A418022303740020A5
+:101D200080F311887047000038B5037F044613B173
+:101D300090F85430ABB90125201D0221FFF730FF88
+:101D400004F114006FF00101257700F09DFC04F10F
+:101D50004C0084F854506FF00101BDE8384000F0A9
+:101D600093BC38BD10B5012104460430FFF718FFBD
+:101D70000023237784F8543010BD000038B50446A2
+:101D80000025143001F0D2FF04F14C00257702F059
+:101D9000A1F8201D84F854500121FFF701FF2046CF
+:101DA000BDE83840FFF750BF90F8803003F0600383
+:101DB000202B06D190F881200023212A03D81F2A46
+:101DC00006D800207047222AFBD1C0E91D3303E06A
+:101DD000034A426707228267C3670120704700BF3A
+:101DE0003422002037B500F58055D5F8A4381A689C
+:101DF000117804291AD1017C022917D119790123FC
+:101E000012898B40134211D100F14C04204602F09C
+:101E100021F958B101A9204602F068F8D5F8A44884
+:101E20000246019B2179206800F0C0F803B030BD64
+:101E300001F10B03F0B550F8236085B004460D4660
+:101E4000FEB1302383F3118804EB8507301D082190
+:101E5000FFF7A6FEFB6806F14C005B691B681BB12F
+:101E6000019002F051F8019803A902F03FF80246F0
+:101E700048B1039B2946204600F098F8002383F3DD
+:101E8000118805B0F0BDFB685A691268002AF5D0C8
+:101E90001B8A013B1340F1D104F18002EAE7000004
+:101EA000133138B550F82140ECB1302383F3118859
+:101EB00004F58053D3F8A4281368527903EB820306
+:101EC000DB689B695D6845B104216018FFF768FE17
+:101ED000294604F1140001F03FFF2046FFF7B4FE4D
+:101EE000002383F3118838BD7047000001F00CBA5D
+:101EF00001234022002110B5044600F8303BFEF7D4
+:101F0000E9FF0023C4E9013310BD000010B5302300
+:101F1000044683F311882422416000210C30FEF72F
+:101F2000D9FF204601F012FA02230020237080F32B
+:101F3000118810BD70B500EB8103054650690E464F
+:101F40001446DA6018B110220021FEF7C3FFA06921
+:101F500018B110220021FEF7BDFF31462846BDE82A
+:101F6000704001F0F9BA000083682022002103F0DC
+:101F7000011310B5044683601030FEF7ABFF204616
+:101F8000BDE8104001F074BBF0B4012500EB810402
+:101F900047898D40E4683D43A4694581236000235F
+:101FA000A2606360F0BC01F091BB0000F0B40125B9
+:101FB00000EB810407898D40E4683D436469058135
+:101FC00023600023A2606360F0BC01F007BC000046
+:101FD00070B5022300250446242203702946C0F868
+:101FE00088500C3040F8045CFEF774FF204684F8FB
+:101FF000705001F045FA63681B6823B129462046FA
+:10200000BDE87040184770BD0378052B10B5044635
+:102010000AD080F88C300523037043681B680BB12D
+:10202000042198470023A36010BD00000178052912
+:1020300006D190F88C20436802701B6803B11847E2
+:102040007047000070B590F87030044613B100235B
+:1020500080F8703004F18002204601F02DFB6368A7
+:102060009B68B3B994F8803013F0600535D0002137
+:10207000204601F01FFE0021204601F00FFE63689C
+:102080001B6813B1062120469847062384F8703058
+:1020900070BD204698470028E4D0B4F88630A26F7F
+:1020A0009A4288BFA36794F98030A56F002B4FF048
+:1020B000300380F20381002D00F0F280092284F8C1
+:1020C000702083F3118800212046D4E91D23FFF7F7
+:1020D0006DFF002383F31188DAE794F8812003F081
+:1020E0007F0343EA022340F20232934200F0C580AC
+:1020F00021D8B3F5807F48D00DD8012B3FD0022BDB
+:1021000000F09380002BB2D104F1880262670222B2
+:10211000A267E367C1E7B3F5817F00F09B80B3F569
+:10212000407FA4D194F88230012BA0D1B4F888303C
+:1021300043F0020332E0B3F5006F4DD017D8B3F58A
+:10214000A06F31D0A3F5C063012B90D863682046FF
+:1021500094F882205E6894F88310B4F88430B04715
+:10216000002884D0436863670368A3671AE0B3F567
+:10217000106F36D040F6024293427FF478AF5C4B4A
+:1021800063670223A3670023C3E794F88230012B1F
+:102190007FF46DAFB4F8883023F00203A4F88830E0
+:1021A000C4E91D55E56778E7B4F88030B3F5A06F52
+:1021B0000ED194F88230204684F88A3001F0BEF9BE
+:1021C00063681B6813B101212046984703232370DD
+:1021D0000023C4E91D339CE704F18B0363670123EB
+:1021E000C3E72378042B10D1302383F311882046D2
+:1021F000FFF7BAFE85F311880321636884F88B50DA
+:1022000021701B680BB12046984794F88230002B50
+:10221000DED084F88B300423237063681B68002BA6
+:10222000D6D0022120469847D2E794F88430204641
+:102230001D0603F00F010AD501F030FA012804D081
+:1022400002287FF414AF2B4B9AE72B4B98E701F051
+:1022500017FAF3E794F88230002B7FF408AF94F874
+:10226000843013F00F01B3D01A06204602D501F0D6
+:1022700039FDADE701F02AFDAAE794F88230002B82
+:102280007FF4F5AE94F8843013F00F01A0D01B0654
+:10229000204602D501F00EFD9AE701F0FFFC97E71A
+:1022A000142284F8702083F311882B462A4629468D
+:1022B0002046FFF769FE85F31188E9E65DB1152236
+:1022C00084F8702083F3118800212046D4E91D236F
+:1022D000FFF75AFEFDE60B2284F8702083F3118885
+:1022E0002B462A4629462046FFF760FEE3E700BF5B
+:1022F000484C0008404C0008444C000838B590F8A1
+:1023000070300446002B3ED0063BDAB20F2A34D898
+:102310000F2B32D8DFE803F0373131082232313168
+:102320003131313131313737856FB0F886309D42E8
+:1023300014D2C3681B8AB5FBF3F203FB12556DB9C7
+:10234000302383F311882B462A462946FFF72EFEB9
+:1023500085F311880A2384F870300EE0142384F882
+:102360007030302383F31188002320461A46194623
+:10237000FFF70AFE002383F3118838BDC36F03B152
+:1023800098470023E7E70021204601F093FC002155
+:10239000204601F083FC63681B6813B106212046C8
+:1023A00098470623D7E7000010B590F87030044630
+:1023B000142B29D017D8062B05D001D81BB110BD7E
+:1023C000093B022BFBD80021204601F073FC0021C1
+:1023D000204601F063FC63681B6813B106212046A8
+:1023E0009847062319E0152BE9D10B2380F87030AC
+:1023F000302383F3118800231A461946FFF7D6FDD0
+:10240000002383F31188DAE7C3689B695B68002BBC
+:10241000D5D1C36F03B19847002384F87030CEE75D
+:1024200000238268037503691B6899689142FBD297
+:102430005A680360426010605860704700238268E9
+:10244000037503691B6899689142FBD85A68036059
+:10245000426010605860704708B50846302383F327
+:1024600011880A7D0023052A06D8DFE802F00B0553
+:102470000503120E826913604FF0FF338361FFF78B
+:10248000CFFF002383F3118808BD8269936801336D
+:102490009360D0E9003213605A60EDE7FFF7C0BFE8
+:1024A000054BD96808751868026853601A600122E4
+:1024B000D8600275FDF7A6BFC82800200C4B30B5C8
+:1024C000DD684B1C87B004460FD02B46094A68468E
+:1024D00000F06EF92046FFF7E3FF009B13B168465A
+:1024E00000F070F9A86907B030BDFFF7D9FFF9E730
+:1024F000C82800205924000838B50C4D04468161D5
+:10250000EB6881689A68914203D8BDE83840FFF7CC
+:1025100087BF1846FFF792FF01230146EC60204673
+:102520002375BDE83840FDF76DBF00BFC828002007
+:10253000044B1A68DB6890689B68984294BF00203F
+:1025400001207047C8280020084B10B51C68D868C7
+:10255000226853601A600122DC602275FFF76EFF6B
+:1025600001462046BDE81040FDF74CBFC8280020BA
+:1025700038B50123084C00252370656001F0A6FDE5
+:1025800001F0CCFD0549064801F05EFE02232370F0
+:1025900085F3118838BD00BF302B0020504C000857
+:1025A000C828002000F044B9034A516853685B1AF8
+:1025B0009842FBD8704700BF001000E08B600223F8
+:1025C000086108468B8270478368A3F1840243F850
+:1025D000142C026943F8442C426943F8402C094A00
+:1025E00043F8242CC268A3F1200043F8182C0222DF
+:1025F00003F80C2C002203F80B2C034A43F8102C90
+:10260000704700BF1D040008C828002008B5FFF768
+:10261000DBFFBDE80840FFF741BF0000024BDB686D
+:1026200098610F20FFF73CBFC8280020302383F3B8
+:102630001188FFF7F3BF000008B50146302383F38C
+:1026400011880820FFF73AFF002383F3118808BDA3
+:10265000064BDB6839B1426818605A60136043600A
+:102660000420FFF72BBF4FF0FF307047C828002031
+:102670000368984206D01A68026050601846996153
+:10268000FFF70CBF7047000038B504460D462068C0
+:10269000844200D138BD036823605C608561FFF728
+:1026A000FDFEF4E7036810B59C68A2420CD85C6894
+:1026B0008A600B604C602160596099688A1A9A6040
+:1026C0004FF0FF33836010BD121B1B68ECE7000066
+:1026D0000A2938BF0A2170B504460D460A2660193A
+:1026E00001F0EEFC01F0D6FC041BA54203D8751CDA
+:1026F00004462E46F3E70A2E04D90120BDE87040B7
+:1027000001F0E2BD70BD0000F8B5144B0D460A2A79
+:102710004FF00A07D96103F11001826038BF0A2225
+:10272000416019691446016048601861A81801F0F9
+:10273000B7FC01F0AFFC431B0646A34206D37C1C4A
+:1027400028192746354601F0BBFCF2E70A2F04D9C9
+:102750000120BDE8F84001F0B7BDF8BDC828002051
+:10276000F8B506460D4601F095FC0F4A134653F89E
+:10277000107F9F4206D12A4601463046BDE8F84008
+:10278000FFF7C2BFD169BB68441A2C1928BF2C4679
+:10279000A34202D92946FFF79BFF22463146034850
+:1027A000BDE8F840FFF77EBFC8280020D8280020E9
+:1027B000C0E90323002310B45DF8044B4361FFF725
+:1027C000CFBF000010B5194C236998420DD0816825
+:1027D000D0E9003213605A609A680A449A60002374
+:1027E00003604FF0FF33A36110BD0268234643F836
+:1027F000102F53600022026022699A4203D1BDE883
+:10280000104001F057BC936881680B44936001F05D
+:1028100041FC2269E1699268441AA242E4D9114458
+:10282000BDE81040091AFFF753BF00BFC8280020B9
+:102830002DE9F047DFF8BC8008F110072C4ED8F8DE
+:10284000105001F027FCD8F81C40AA68031B9A42DC
+:102850003ED814444FF00009D5E90032C8F81C40B6
+:1028600013605A60C5F80090D8F81030B34201D117
+:1028700001F020FC89F31188D5E9033128469847F7
+:10288000302383F311886B69002BD8D001F002FC50
+:102890006A69A0EB040982464A450DD2022001F084
+:1028A00013FD0022D8F81030B34208D15146284613
+:1028B000BDE8F047FFF728BF121A2244F2E712EBF7
+:1028C00009092946384638BF4A46FFF7EBFEB5E707
+:1028D000D8F81030B34208D01444C8F81C00211AAC
+:1028E000A960BDE8F047FFF7F3BEBDE8F08700BF81
+:1028F000D8280020C828002000207047FEE70000EC
+:10290000704700004FF0FF307047000002290CD0E4
+:10291000032904D00129074818BF00207047032A63
+:1029200005D8054800EBC2007047044870470020F6
+:10293000704700BF284D000844220020DC4C0008EE
+:1029400070B59AB005460846144601A900F0C2F8D1
+:1029500001A8FEF7B7FA431C0022C6B25B0010467E
+:10296000C5E9003423700323023404F8013C01ABB1
+:10297000D1B202348E4201D81AB070BD13F8011BD7
+:10298000013204F8010C04F8021CF1E708B5302309
+:1029900083F311880348FFF725FA002383F3118896
+:1029A00008BD00BF382B002090F8803003F01F02D4
+:1029B000012A07D190F881200B2A03D10023C0E916
+:1029C0001D3315E003F06003202B08D1B0F88430EC
+:1029D0002BB990F88120212A03D81F2A04D8FFF7A9
+:1029E000E3B9222AEBD0FAE7034A4267072282675B
+:1029F000C3670120704700BF3B22002007B50529AF
+:102A000017D8DFE801F0191603191920302383F3D2
+:102A10001188104A01210190FFF78CFA01980221D8
+:102A20000D4AFFF787FA0D48FFF7A8F9002383F353
+:102A3000118803B05DF804FB302383F31188074845
+:102A4000FFF772F9F2E7302383F311880348FFF7A9
+:102A500089F9EBE77C4C0008A04C0008382B0020DB
+:102A600038B50C4D0C4C2A460C4904F10800FFF710
+:102A700067FF05F1CA0204F110000949FFF760FF82
+:102A800005F5CA7204F118000649BDE83840FFF7A1
+:102A900057BF00BF10440020442200205C4C0008B7
+:102AA000664C0008714C000870B5044608460D4697
+:102AB000FEF708FAC6B22046013403780BB918466F
+:102AC00070BD32462946FEF7E9F90028F3D101200E
+:102AD000F6E700002DE9F04705460C46FEF7F2F94F
+:102AE0002B49C6B22846FFF7DFFF08B10736F6B21A
+:102AF00028492846FFF7D8FF08B11036F6B2632EF2
+:102B00000BD8DFF88C80DFF88C90234FDFF894A08F
+:102B10002E7846B92670BDE8F08729462046BDE8E4
+:102B2000F04701F0B3BF252E2ED10722414628469B
+:102B3000FEF7B4F970B9194B224603F10C0153F8B2
+:102B4000040B8B4242F8040BF9D11B8807350E3475
+:102B50001380DDE7082249462846FEF79FF998B919
+:102B6000A21C0F4B197802320909C95D02F8041C36
+:102B700013F8011B01F00F015345C95D02F8031C56
+:102B8000F0D118340835C3E7013504F8016BBFE70D
+:102B9000484D0008714C00085F4D0008504D00087A
+:102BA00000E8F11F0CE8F11FBFF34F8F044B1A69C7
+:102BB0005107FCD1D3F810215207F8D1704700BF5C
+:102BC0000020005208B50D4B1B78ABB9FFF7ECFFA6
+:102BD0000B4BDA68D10704D50A4A5A6002F18832F1
+:102BE0005A60D3F80C21D20706D5064AC3F804214F
+:102BF00002F18832C3F8042108BD00BF6E460020F0
+:102C0000002000522301674508B5114B1B78F3B92A
+:102C1000104B1A69510703D5DA6842F04002DA60B6
+:102C2000D3F81021520705D5D3F80C2142F0400209
+:102C3000C3F80C21FFF7B8FF064BDA6842F0010237
+:102C4000DA60D3F80C2142F00102C3F80C2108BD70
+:102C50006E460020002000520F289ABF00F58060C9
+:102C600040040020704700004FF40030704700001F
+:102C7000102070470F2808B50BD8FFF7EDFF00F5BF
+:102C800000330268013204D104308342F9D10120BB
+:102C900008BD0020FCE700000F2838B505463FD8E6
+:102CA000FFF782FF1F4CFFF78DFF4FF0FF33072820
+:102CB0006361C4F814311DD82361FFF775FF030267
+:102CC00043F02403E360E36843F08003E360236997
+:102CD0005A07FCD42846FFF767FFFFF7BDFF4FF404
+:102CE000003100F0B3F92846FFF78EFFBDE8384009
+:102CF000FFF7C0BFC4F81031FFF756FFA0F108037B
+:102D00001B0243F02403C4F80C31D4F80C3143F017
+:102D10008003C4F80C31D4F810315B07FBD4D9E739
+:102D2000002038BD002000522DE9F84F05460C4622
+:102D3000104645EA0203DE0602D00020BDE8F88F07
+:102D400020F01F00DFF8BCB0DFF8BCA0FFF73AFFAF
+:102D500004EB0008444503D10120FFF755FFEDE7E0
+:102D600020222946204601F081FE10B9203520346A
+:102D7000F0E72B4605F120021F68791CDDD10433F2
+:102D80009A42F9D105F178431B481C4EB3F5801FD8
+:102D90001B4B38BF184603F1F80332BFD946D14662
+:102DA0001E46FFF701FF0760A5EB040C336804F132
+:102DB0001C0143F002033360231FD9F8007017F0A1
+:102DC0000507FAD153F8042F8B424CF80320F4D1B5
+:102DD000BFF34F8FFFF7E8FE4FF0FF33202221466D
+:102DE00003602846336823F00203336001F03EFE9F
+:102DF0000028BBD03846B0E7142100520C20005206
+:102E000014200052102000521021005210B5084C1E
+:102E1000237828B11BB9FFF7D5FE0123237010BD1D
+:102E2000002BFCD02070BDE81040FFF7EDBE00BFC6
+:102E30006E4600202DE9F04F0D4685B0814658B111
+:102E400011F00D0614BF2022082211F0080301938F
+:102E500004D0431E03426AD0002435E0002E37D050
+:102E600009F11F0121F01F094FF00108324F05F051
+:102E70000403DFF8D0A005EA080BBBF1000F32D045
+:102E80007869C0072FD408F101080C37B8F1060F94
+:102E9000F3D19EB9294D4946A819019201F060F974
+:102EA000044600283AD11836019A782EF3D14946C3
+:102EB00001F056F90446002830D1019A49462048CD
+:102EC00001F04EF9044668BB204605B0BDE8F08F1E
+:102ED0000029C9D101462846029201F041F9044671
+:102EE000E0B9029AC0E713B178694107CBD5AC07C6
+:102EF00002D578698007C6D5019911B178690107B3
+:102F0000C1D51820494600FB08A0CDE9022301F0F5
+:102F100027F90446DDE902230028B4D04A460021FF
+:102F2000204601E04A460021FDF7D4FFCCE70246E7
+:102F3000002E95D198E700BF704D0008A0460020F4
+:102F400070460020884600200121FFF773BF000073
+:102F5000F8B5144D01241827134E40F2FF3200211A
+:102F60000120FDF7B7FF07FB046001342A6955F81B
+:102F70000C1F01F0E1F8062CF5D137254FF4C054B1
+:102F80002046FFF7E1FF014628B122460748BDE889
+:102F9000F84001F0D1B8C4EBC404013D4FEAD404B9
+:102FA000EED1F8BD704D0008884600207046002024
+:102FB0000244074BD2B210B5904200D110BD441C60
+:102FC00000B253F8200041F8040BE0B2F4E700BF70
+:102FD000504000580E4B30B51C6F240405D41C6FB4
+:102FE0001C671C6F44F400441C670A4C02442368AD
+:102FF000D2B243F480732360074B904200D130BDBE
+:10300000441C51F8045B00B243F82050E0B2F4E7EE
+:1030100000440258004802585040005807B50122A9
+:1030200001A90020FFF7C4FF019803B05DF804FB7D
+:1030300013B50446FFF7F2FFA04205D0012201A913
+:1030400000200194FFF7C6FF02B010BD0144BFF39A
+:103050004F8F064B884204D3BFF34F8FBFF36F8F60
+:103060007047C3F85C022030F4E700BF00ED00E0D9
+:10307000034B1A681AB9034AD2F8D0241A60704771
+:10308000184700200040025808B5FFF7F1FF024B37
+:103090001868C0F3806008BD18470020EFF309836B
+:1030A000054968334A6B22F001024A6383F30988B9
+:1030B000002383F31188704700EF00E0302080F395
+:1030C000118862B60D4B0E4AD96821F4E0610904FB
+:1030D000090C0A430B49DA60D3F8FC2042F08072F5
+:1030E000C3F8FC20084AC2F8B01F116841F0010182
+:1030F00011602022DA7783F82200704700ED00E0AB
+:103100000003FA0555CEACC5001000E0302310B521
+:1031100083F311880E4B5B6813F4006314D0F1EE57
+:10312000103AEFF309844FF08073683CE361094B78
+:10313000DB6B236684F30988FFF7FAF910B1064BBD
+:10314000A36110BD054BFBE783F31188F9E700BFCE
+:1031500000ED00E000EF00E02F040008320400085A
+:1031600070B5BFF34F8FBFF36F8F1A4A0021C2F8BB
+:103170005012BFF34F8FBFF36F8F536943F4003387
+:103180005361BFF34F8FBFF36F8FC2F88410BFF34B
+:103190004F8FD2F8803043F6E074C3F3C900C3F315
+:1031A0004E335B0103EA0406014646EA81750139A4
+:1031B000C2F86052F9D2203B13F1200FF2D1BFF3D5
+:1031C0004F8F536943F480335361BFF34F8FBFF385
+:1031D0006F8F70BD00ED00E0FEE70000214B22483C
+:1031E000224A70B5904237D3214BC11EDA1C121A05
+:1031F00022F003028B4238BF00220021FDF76AFE55
+:103200001C4A0023C2F88430BFF34F8FD2F88030BD
+:1032100043F6E074C3F3C900C3F34E335B0103EA22
+:103220000406014646EA81750139C2F86C52F9D2AA
+:10323000203B13F1200FF2D1BFF34F8FBFF36F8FFD
+:10324000BFF34F8FBFF36F8F0023C2F85032BFF32D
+:103250004F8FBFF36F8F70BD53F8041B40F8041BF2
+:10326000C0E700BF9C4F00080C4900200C4900201B
+:103270000C49002000ED00E0074BD3F8D81021EAFC
+:103280000001C3F8D810D3F8002122EA0002C3F8E5
+:103290000021D3F8003170470044025870B5D0E9DE
+:1032A000244300224FF0FF359E6804EB4213510186
+:1032B000D3F80009002805DAD3F8000940F080406F
+:1032C000C3F80009D3F8000B002805DAD3F8000B87
+:1032D00040F08040C3F8000B013263189642C3F8F7
+:1032E0000859C3F8085BE0D24FF00113C4F81C384A
+:1032F00070BD0000890141F02001016103699B0656
+:10330000FCD41220FFF750B910B50A4C2046FEF746
+:10331000EFFD094BC4F89030084BC4F89430084CCA
+:103320002046FEF7E5FD074BC4F89030064BC4F885
+:10333000943010BD1C47002000000840DC4D000800
+:10334000B847002000000440E84D000870B503783D
+:103350000546012B5CD1434BD0F89040984258D1A0
+:10336000414B0E216520D3F8D82042F00062C3F80B
+:10337000D820D3F8002142F00062C3F80021D3F82E
+:103380000021D3F8802042F00062C3F88020D3F8F7
+:10339000802022F00062C3F88020D3F88030FEF74E
+:1033A0000DFA324BE360324BC4F800380023D5F8F5
+:1033B0009060C4F8003EC02323604FF40413A3635D
+:1033C0003369002BFCDA01230C203361FFF7ECF8A2
+:1033D0003369DB07FCD41220FFF7E6F83369002BD2
+:1033E000FCDA00262846A660FFF758FF6B68C4F891
+:1033F0001068DB68C4F81468C4F81C6883BB1D4BF4
+:10340000A3614FF0FF336361A36843F00103A3603E
+:1034100070BD194B9842C9D1134B4FF08060D3F85F
+:10342000D82042F00072C3F8D820D3F8002142F02F
+:103430000072C3F80021D3F80021D3F8802042F0B5
+:103440000072C3F88020D3F8802022F00072C3F805
+:103450008020D3F88030FFF70FFF0E214D209EE72C
+:10346000064BCDE71C4700200044025840140040A2
+:1034700003002002003C30C0B8470020083C30C0A8
+:10348000F8B5D0F89040054600214FF00066204680
+:10349000FFF730FFD5F8941000234FF001128F682A
+:1034A0004FF0FF30C4F83438C4F81C2804EB431242
+:1034B00001339F42C2F80069C2F8006BC2F80809E4
+:1034C000C2F8080BF2D20B68D5F89020C5F89830F6
+:1034D000636210231361166916F01006FBD11220E7
+:1034E000FFF762F8D4F8003823F4FE63C4F800381C
+:1034F000A36943F4402343F01003A3610923C4F8F4
+:103500001038C4F814380B4BEB604FF0C043C4F8CC
+:10351000103B094BC4F8003BC4F81069C4F80039EB
+:10352000D5F8983003F1100243F48013C5F89820C1
+:10353000A362F8BDB84D000840800010D0F890207C
+:1035400090F88A10D2F8003823F4FE6343EA01139E
+:10355000C2F80038704700002DE9F84300EB810302
+:10356000D0F890500C468046DA680FFA81F948018D
+:10357000166806F00306731E022B05EB41134FF08D
+:10358000000194BFB604384EC3F8101B4FF0010180
+:1035900004F1100398BF06F1805601FA03F3916914
+:1035A00098BF06F5004600293AD0578A04F1580121
+:1035B000374349016F50D5F81C180B430021C5F85B
+:1035C0001C382B180127C3F81019A7405369611E36
+:1035D0009BB3138A928B9B08012A88BF5343D8F868
+:1035E0009820981842EA034301F140022146C8F8A6
+:1035F0009800284605EB82025360FFF77BFE08EB3C
+:103600008900C3681B8A43EA845348341E4364011B
+:103610002E51D5F81C381F43C5F81C78BDE8F88337
+:1036200005EB4917D7F8001B21F40041C7F8001B30
+:10363000D5F81C1821EA0303C0E704F13F030B4A45
+:103640002846214605EB83035A60FFF753FE05EB3E
+:103650004910D0F8003923F40043C0F80039D5F8F8
+:103660001C3823EA0707D7E7008000100004000297
+:10367000D0F894201268C0F89820FFF70FBE000021
+:103680005831D0F8903049015B5813F4004004D011
+:1036900013F4001F0CBF0220012070474831D0F8FE
+:1036A000903049015B5813F4004004D013F4001F1C
+:1036B0000CBF02200120704700EB8101CB68196A22
+:1036C0000B6813604B6853607047000000EB810388
+:1036D00030B5DD68AA691368D36019B9402B84BF7F
+:1036E000402313606B8A1468D0F890201C4402EBCE
+:1036F0004110013C09B2B4FBF3F46343033323F0FC
+:10370000030343EAC44343F0C043C0F8103B2B68B3
+:1037100003F00303012B0ED1D2F8083802EB41105D
+:1037200013F4807FD0F8003B14BF43F0805343F084
+:103730000053C0F8003B02EB4112D2F8003B43F0CB
+:103740000443C2F8003B30BD2DE9F041D0F8906051
+:1037500005460C4606EB4113D3F8087B3A07C3F83D
+:10376000087B08D5D6F814381B0704D500EB810375
+:10377000DB685B689847FA071FD5D6F81438DB0773
+:103780001BD505EB8403D968CCB98B69488A5A6884
+:10379000B2FBF0F600FB16228AB91868DA6890428C
+:1037A0000DD2121AC3E90024302383F31188214675
+:1037B0002846FFF78BFF84F31188BDE8F0810123D1
+:1037C00003FA04F26B8923EA02036B81CB68002BB6
+:1037D000F3D021462846BDE8F041184700EB8103AD
+:1037E0004A0170B5DD68D0F890306C692668E660F3
+:1037F00056BB1A444FF40020C2F810092A6802F0A0
+:103800000302012A0AB20ED1D3F8080803EB4214CE
+:1038100010F4807FD4F8000914BF40F0805040F0CD
+:103820000050C4F8000903EB4212D2F8000940F03E
+:103830000440C2F800090122D3F8340802FA01F169
+:103840000143C3F8341870BD19B9402E84BF40201D
+:10385000206020681A442E8A8419013CB4FBF6F4D7
+:1038600040EAC44040F00050C6E700002DE9F843AC
+:10387000D0F8906005460C464F0106EB4113D3F893
+:10388000088918F0010FC3F808891CD0D6F8103841
+:10389000DB0718D500EB8103D3F80CC0DCF814303B
+:1038A000D3F800E0DA68964530D2A2EB0E024FF072
+:1038B00000091A60C3F80490302383F31188FFF7DE
+:1038C0008DFF89F3118818F0800F1DD0D6F8343899
+:1038D0000126A640334217D005EB84030134D5F806
+:1038E0009050D3F80CC0E4B22F44DCF8142005EB60
+:1038F0000434D2F800E05168714514D3D5F8343857
+:1039000023EA0606C5F83468BDE8F883012303FA04
+:1039100001F2038923EA02030381DCF80830002B5B
+:10392000D1D09847CFE7AEEB0103BCF8100083423B
+:1039300028BF0346D7F8180980B2B3EB800FE3D84D
+:103940009068A0F1040959F8048FC4F80080A0EB36
+:1039500009089844B8F1040FF5D818440B44906056
+:103960005360C8E72DE9F84FD0F8905004466E69CF
+:10397000AB691E4016F480586E6103D0BDE8F84F65
+:10398000FEF726BB002E12DAD5F8003E9B0705D0C5
+:10399000D5F8003E23F00303C5F8003ED5F80438FF
+:1039A000204623F00103C5F80438FEF73FFB370536
+:1039B00005D52046FFF772FC2046FEF725FBB00434
+:1039C0000CD5D5F8083813F0060FEB6823F47053C4
+:1039D0000CBF43F4105343F4A053EB6031071BD5E5
+:1039E0006368DB681BB9AB6923F00803AB6123781C
+:1039F000052B0CD1D5F8003E9A0705D0D5F8003E2E
+:103A000023F00303C5F8003E2046FEF70FFB636872
+:103A1000DB680BB120469847F30200F1BA80B70289
+:103A200026D5D4F8909000274FF0010A09EB4712F1
+:103A3000D2F8003B03F44023B3F5802F11D1D2F824
+:103A4000003B002B0DDA62890AFA07F322EA03032E
+:103A5000638104EB8703DB68DB6813B139462046DA
+:103A600098470137D4F89430FFB29B689F42DDD964
+:103A7000F00619D5D4F89000026AC2F30A1702F0D2
+:103A80000F0302F4F012B2F5802F00F0CA80B2F5F5
+:103A9000402F09D104EB8303002200F58050DB683E
+:103AA0001B6A974240F0B0803003D5F8185835D5DE
+:103AB000E90303D500212046FFF746FEAA0303D5FC
+:103AC00001212046FFF740FE6B0303D5022120466B
+:103AD000FFF73AFE2F0303D503212046FFF734FEFC
+:103AE000E80203D504212046FFF72EFEA90203D5E4
+:103AF00005212046FFF728FE6A0203D5062120464D
+:103B0000FFF722FE2B0203D507212046FFF71CFEFC
+:103B1000EF0103D508212046FFF716FE700340F1A0
+:103B2000A780E90703D500212046FFF79FFEAA07DB
+:103B300003D501212046FFF799FE6B0703D502212B
+:103B40002046FFF793FE2F0703D503212046FFF7FA
+:103B50008DFEEE0603D504212046FFF787FEA8065A
+:103B600003D505212046FFF781FE690603D506210E
+:103B70002046FFF77BFE2A0603D507212046FFF7E4
+:103B800075FEEB0574D520460821BDE8F84FFFF718
+:103B90006DBED4F890904FF0000B4FF0010AD4F8AE
+:103BA00094305FFA8BF79B689F423FF638AF09EB82
+:103BB0004713D3F8002902F44022B2F5802F20D118
+:103BC000D3F80029002A1CDAD3F8002942F09042E9
+:103BD000C3F80029D3F80029002AFBDB3946D4F8C2
+:103BE0009000FFF787FB22890AFA07F322EA030312
+:103BF000238104EB8703DB689B6813B139462046B9
+:103C000098470BF1010BCAE7910701D1D0F800806A
+:103C1000072A02F101029CBF03F8018B4FEA182822
+:103C20003FE704EB830300F58050DA68D2F818C050
+:103C3000DCF80820DCE9001CA1EB0C0C00218F4211
+:103C400008D1DB689B699A683A449A605A683A449A
+:103C50005A6029E711F0030F01D1D0F800808C459C
+:103C600001F1010184BF02F8018B4FEA1828E6E751
+:103C7000BDE8F88F08B50348FFF774FEBDE80840BB
+:103C8000FFF744BA1C47002008B50348FFF76AFE57
+:103C9000BDE80840FFF73ABAB8470020D0F89030A6
+:103CA00003EB4111D1F8003B43F40013C1F8003B92
+:103CB00070470000D0F8903003EB4111D1F8003983
+:103CC00043F40013C1F8003970470000D0F8903079
+:103CD00003EB4111D1F8003B23F40013C1F8003B82
+:103CE00070470000D0F8903003EB4111D1F8003953
+:103CF00023F40013C1F800397047000030B50433D5
+:103D0000039C0172002104FB0325C160C0E9065336
+:103D1000049B0363059BC0E90000C0E90422C0E9DD
+:103D20000842C0E90A11436330BD00000022416A25
+:103D3000C260C0E90411C0E90A226FF00101FEF778
+:103D4000A3BC0000D0E90432934201D1C2680AB991
+:103D5000181D704700207047036919600021C26870
+:103D60000132C260C269134482699342036124BF75
+:103D7000436A0361FEF77CBC38B504460D46E36830
+:103D80003BB162690020131D1268A3621344E36211
+:103D900007E0237A33B929462046FEF759FC00286C
+:103DA000EDDA38BD6FF00100FBE70000C368C269BF
+:103DB000013BC3604369134482699342436124BF5A
+:103DC000436A436100238362036B03B11847704762
+:103DD00070B53023044683F31188866A3EB9FFF735
+:103DE000CBFF054618B186F31188284670BDA36A3B
+:103DF000E26A13F8015B9342A36202D32046FFF705
+:103E0000D5FF002383F31188EFE700002DE9F84F79
+:103E100004460E46174698464FF0300989F311883C
+:103E20000025AA46D4F828B0BBF1000F09D14146BD
+:103E30002046FFF7A1FF20B18BF311882846BDE88B
+:103E4000F88FD4E90A12A7EB050B521A934528BF45
+:103E50009346BBF1400F1BD9334601F1400251F8A4
+:103E6000040B914243F8040BF9D1A36A4036403564
+:103E70004033A362D4E90A239A4202D32046FFF7D3
+:103E800095FF8AF31188BD42D8D289F31188C9E71A
+:103E900030465A46FCF7F8FFA36A5E445D445B4433
+:103EA000A362E7E710B5029C0433017203FB04210F
+:103EB000C460C0E906130023C0E90A33039B03630F
+:103EC000049BC0E90000C0E90422C0E90842436342
+:103ED00010BD0000026A6FF00101C260426AC0E9D1
+:103EE00004220022C0E90A22FEF7CEBBD0E9042357
+:103EF0009A4201D1C26822B9184650F8043B0B60BF
+:103F0000704700231846FAE7C3680021C2690133ED
+:103F1000C3604369134482699342436124BF436A87
+:103F20004361FEF7A5BB000038B504460D46E368C3
+:103F30003BB1236900201A1DA262E2691344E362C7
+:103F400007E0237A33B929462046FEF781FB002893
+:103F5000EDDA38BD6FF00100FBE70000036919607E
+:103F6000C268013AC260C269134482699342036124
+:103F700024BF436A036100238362036B03B11847C4
+:103F80007047000070B530230D460446114683F398
+:103F90001188866A2EB9FFF7C7FF10B186F3118822
+:103FA00070BDA36A1D70A36AE26A01339342A362E3
+:103FB00004D3E16920460439FFF7D0FF002080F3E5
+:103FC0001188EDE72DE9F84F04460D4690469946D5
+:103FD0004FF0300A8AF311880026B346A76A4FB91A
+:103FE00049462046FFF7A0FF20B187F311883046ED
+:103FF000BDE8F88FD4E90A073A1AA8EB06079742FA
+:1040000028BF1746402F1BD905F1400355F8042B54
+:104010009D4240F8042BF9D1A36A40364033A36295
+:10402000D4E90A239A4204D3E16920460439FFF710
+:1040300095FF8BF311884645D9D28AF31188CDE7D5
+:1040400029463A46FCF720FFA36A3D443E443B44E0
+:10405000A362E5E7D0E904239A4217D1C3689BB174
+:10406000836A8BB1043B9B1A0ED01360C368013B7B
+:10407000C360C3691A4483699A42026124BF436AD8
+:104080000361002383620123184670470023FBE786
+:1040900000F01EBA014B586A704700BF000C004088
+:1040A000034B002258631A610222DA60704700BF96
+:1040B000000C0040014B0022DA607047000C004009
+:1040C000014B5863704700BF000C0040024B034A8D
+:1040D0001A60034A5A6070476C480020104900205B
+:1040E00000000220074B494210B55C68201A0840C6
+:1040F0001968821A8A4203D3A24201D85A6010BDBD
+:104100000020FCE76C48002008B5302383F31188B9
+:10411000FFF7E8FF002383F3118808BD044801215D
+:10412000044B03600023C0E901330C3000F0D4B825
+:104130007448002009410008CB1D083A23F007030A
+:10414000591A521A012110B4D2080024C0E90043C0
+:1041500084600C301C605A605DF8044B00F0BCB801
+:104160002DE9F84F364ECD1D0F46002818BF0646E4
+:10417000082A4FEAD50538BF082206F10C08341D7D
+:104180009146404600F0C4F809F10701C9F1000E5C
+:10419000224624686CB9404600F0C4F83368CBB3BB
+:1041A00008224946E8009847044698B340E9026768
+:1041B00030E004EB010CD4F804A00CEA0E0C0AF178
+:1041C0000100ACF1080304EBC0009842E0D9A0EB79
+:1041D0000C0CB5EBEC0F4FEAEC0BD9D89C421CD27F
+:1041E00004F10802AB45A3EB02024FEAE20262606F
+:1041F00009D9691CED43206803EBC1025D44556099
+:1042000043F8310022601C465F60404644F8086B6A
+:1042100000F088F82046BDE8F88FAA45216802D151
+:1042200011602346EFE7013504EBC50344F8351070
+:1042300003F10801401AC01058601360F1E700BF95
+:1042400074480020FEE7000070B51B4B00250446B3
+:1042500086B058600E4685620163FEF72FFF04F1B9
+:104260001003A560E562C4E904334FF0FF33C4E9ED
+:104270000044C4E90635FFF70DFF2B46024604F162
+:1042800034012046C4E9082380230C4A2565FEF743
+:1042900095F901230A4AE060009203756846726846
+:1042A0000192B268CDE90223064BCDE90435FEF751
+:1042B000ADF906B070BD00BF302B0020F44D0008F2
+:1042C000F94D000845420008024AD36A1843D062FB
+:1042D000704700BFC8280020C0E900008160704717
+:1042E0008368013B002B10B583600CDA074BDC6858
+:1042F0004368A061206063601C6044600520FEF795
+:10430000CFF8A06910BD0020FCE700BFC82800203E
+:1043100008B5302383F31188FFF7E2FF002383F30E
+:10432000118808BD08B5302383F3118883680133F1
+:10433000002B836007DC036800211A68026050606C
+:104340001846FEF7D9F8002383F3118808BD000052
+:104350004B6843608B688360CB68C3600B694361C3
+:104360004B6903628B6943620B680360704700000E
+:1043700008B53C4B40F2FF713B48D3F888200A4314
+:10438000C3F88820D3F8882022F4FF6222F00702C5
+:10439000C3F88820D3F88820D3F8E0200A43C3F874
+:1043A000E020D3F808210A43C3F808212F4AD3F8A4
+:1043B00008311146FFF7CCFF00F5806002F11C01C7
+:1043C000FFF7C6FF00F5806002F13801FFF7C0FF7C
+:1043D00000F5806002F15401FFF7BAFF00F580603C
+:1043E00002F17001FFF7B4FF00F5806002F18C016B
+:1043F000FFF7AEFF00F5806002F1A801FFF7A8FF0C
+:1044000000F5806002F1C401FFF7A2FF00F58060B3
+:1044100002F1E001FFF79CFF00F5806002F1FC0172
+:10442000FFF796FF02F58C7100F58060FFF790FFB3
+:1044300000F01AF90E4BD3F8902242F00102C3F8B3
+:104440009022D3F8942242F00102C3F8942205226C
+:10445000C3F898204FF06052C3F89C20054AC3F877
+:10446000A02008BD0044025800000258004E000879
+:1044700000ED00E01F00080308B500F0D7FAFEF7D2
+:1044800077F8104BD3F8DC2042F04002C3F8DC2070
+:10449000D3F8042122F04002C3F80421D3F80431F8
+:1044A000094B1A6842F008021A601A6842F00402C6
+:1044B0001A60FEF7DDFDFEF74BFDBDE80840FEF794
+:1044C000CFBA00BF004402580018024870470000ED
+:1044D000114BD3F8E82042F00802C3F8E820D3F8E3
+:1044E000102142F00802C3F810210C4AD3F8103111
+:1044F000D36B43F00803D363C722094B9A624FF092
+:10450000FF32DA6200229A615A63DA605A6001224D
+:104510005A611A60704700BF004402580010005CE6
+:10452000000C0040094A08B51169D3680B40D9B2A4
+:104530009B076FEA0101116107D5302383F31188CE
+:10454000FEF730F8002383F3118808BD000C00400B
+:10455000064BD3F8DC200243C3F8DC20D3F8042157
+:104560001043C3F80401D3F80431704700440258E3
+:1045700008B53C4B4FF0FF31D3F8802062F0004289
+:10458000C3F88020D3F8802002F00042C3F88020D6
+:10459000D3F88020D3F88420C3F88410D3F8842083
+:1045A0000022C3F88420D3F88400D86F40F0FF4085
+:1045B00040F4FF0040F4DF4040F07F00D867D86F40
+:1045C00020F0FF4020F4FF0020F4DF4020F07F00C7
+:1045D000D867D86FD3F888006FEA40506FEA505020
+:1045E000C3F88800D3F88800C0F30A00C3F8880035
+:1045F000D3F88800D3F89000C3F89010D3F8900057
+:10460000C3F89020D3F89000D3F89400C3F8941026
+:10461000D3F89400C3F89420D3F89400D3F898000A
+:10462000C3F89810D3F89800C3F89820D3F89800EE
+:10463000D3F88C00C3F88C10D3F88C00C3F88C200E
+:10464000D3F88C00D3F89C00C3F89C10D3F89C10CE
+:10465000C3F89C20D3F89C30FCF7C2FEBDE80840AC
+:1046600000F0B8B90044025808B50122534BC3F812
+:104670000821534BD3F8F42042F00202C3F8F4208F
+:10468000D3F81C2142F00202C3F81C210222D3F805
+:104690001C314C4BDA605A689104FCD54A4A1A60C6
+:1046A00001229A60494ADA6000221A614FF44042BE
+:1046B0009A61444B9A699204FCD51A6842F480725C
+:1046C0001A603F4B1A6F12F4407F04D04FF48032CF
+:1046D0001A6700221A671A6842F001021A60384B02
+:1046E0001A685007FCD500221A611A6912F03802C4
+:1046F000FBD1012119604FF0804159605A67344A5B
+:10470000DA62344A1A611A6842F480321A602C4B19
+:104710001A689103FCD51A6842F480521A601A682C
+:104720009204FCD52C4A2D499A6200225A631963DF
+:1047300001F57C01DA6301F2E71199635A64284AB2
+:104740001A64284ADA621A6842F0A8521A601C4BAE
+:104750001A6802F02852B2F1285FF9D148229A6112
+:104760004FF48862DA6140221A621F4ADA641F4AF3
+:104770001A651F4A5A651F4A9A6532231E4A1360FA
+:10478000136803F00F03022BFAD10D4A136943F0AB
+:1047900003031361136903F03803182BFAD14FF0A8
+:1047A0000050FFF7D5FE4FF08040FFF7D1FE4FF0ED
+:1047B0000040BDE80840FFF7CBBE00BF00800051BD
+:1047C000004402580048025800C000F002000001F6
+:1047D0000000FF0100889008121020006302090108
+:1047E000470E0508DD0BBF0120000020000001106E
+:1047F0000910E00000010110002000524FF0B0420B
+:1048000008B5D2F8883003F00103C2F8883023B12C
+:10481000044A13680BB150689847BDE80840FEF79A
+:1048200075BC00BF8C4800204FF0B04208B5D2F8EC
+:10483000883003F00203C2F8883023B1044A936839
+:104840000BB1D0689847BDE80840FEF75FBC00BFD9
+:104850008C4800204FF0B04208B5D2F8883003F001
+:104860000403C2F8883023B1044A13690BB15069BC
+:104870009847BDE80840FEF749BC00BF8C480020BF
+:104880004FF0B04208B5D2F8883003F00803C2F800
+:10489000883023B1044A93690BB1D0699847BDE8C9
+:1048A0000840FEF733BC00BF8C4800204FF0B042F8
+:1048B00008B5D2F8883003F01003C2F8883023B16D
+:1048C000044A136A0BB1506A9847BDE80840FEF7E6
+:1048D0001DBC00BF8C4800204FF0B04310B5D3F88A
+:1048E000884004F47872C3F88820A30604D5124ADD
+:1048F000936A0BB1D06A9847600604D50E4A136BD1
+:104900000BB1506B9847210604D50B4A936B0BB142
+:10491000D06B9847E20504D5074A136C0BB1506C75
+:104920009847A30504D5044A936C0BB1D06C984703
+:10493000BDE81040FEF7EABB8C4800204FF0B043C2
+:1049400010B5D3F8884004F47C42C3F8882062058F
+:1049500004D5164A136D0BB1506D9847230504D545
+:10496000124A936D0BB1D06D9847E00404D50F4AFD
+:10497000136E0BB1506E9847A10404D50B4A936E89
+:104980000BB1D06E9847620404D5084A136F0BB17F
+:10499000506F9847230404D5044A936F0BB1D06F2E
+:1049A0009847BDE81040FEF7B1BB00BF8C4800201F
+:1049B00008B50348FCF77CFEBDE80840FEF7A6BB3F
+:1049C0005C24002008B5FFF7ADFDBDE80840FEF708
+:1049D0009DBB0000062108B50846FCF7EFFE062146
+:1049E0000720FCF7EBFE06210820FCF7E7FE062176
+:1049F0000920FCF7E3FE06210A20FCF7DFFE062172
+:104A00001720FCF7DBFE06212820FCF7D7FE092142
+:104A10007A20FCF7D3FE07213220FCF7CFFE0C21D1
+:104A20005220BDE80840FCF7C9BE000008B5FFF7FA
+:104A30009FFD00F00DF8FDF77FF8FDF757FAFDF741
+:104A400029F9FFF743FDBDE80840FFF721BB00004F
+:104A50000023054A19460133102BC2E9001102F167
+:104A60000802F8D1704700BF8C48002010B501390A
+:104A70000244904201D1002005E0037811F8014F73
+:104A8000A34201D0181B10BD0130F2E7034611F814
+:104A9000012B03F8012B002AF9D1704753544D33F1
+:104AA0003248373F3F3F0053544D33324837337815
+:104AB0002F3732780053544D3332483734332F3741
+:104AC00035332F373530000001105A0003105900DC
+:104AD0000120580003205600100002400800024048
+:104AE0000008024000000B002800024008000240BD
+:104AF0000408024006010C00400002400800024089
+:104B00000808024010020D00580002400800024050
+:104B10000C08024016030E00700002400C00024018
+:104B20001008024000040F00880002400C00024000
+:104B30001408024006051000A00002400C000240CC
+:104B40001808024010061100B80002400C00024094
+:104B50001C08024016072F001004024008040240FF
+:104B600020080240000838002804024008040240DF
+:104B700024080240060939004004024008040240AB
+:104B800028080240100A3A00580402400804024073
+:104B90002C080240160B3B00700402400C0402403B
+:104BA00030080240000C3C00880402400C04024023
+:104BB00034080240060D4400A00402400C040240E8
+:104BC00038080240100E4500B80402400C040240B0
+:104BD0003C080240160F460000960000000000004E
+:104BE00000000000000000000000000000000000C5
+:104BF00000000000F91A0008E51A0008211B00084F
+:104C00000D1B0008191B0008051B0008F11A0008FD
+:104C1000DD1A00082D1B000800000000111C000810
+:104C2000FD1B0008391C0008251C0008311C000869
+:104C30001D1C0008091C0008F51B0008451C000885
+:104C400000000000010000000000000063300000D0
+:104C50004C4C000820290020302B00204172647544
+:104C600050696C6F740025424F415244252D424CCF
+:104C7000002553455249414C250000000200000028
+:104C800000000000311E0008A11E00084000400086
+:104C9000E0430020F043002002000000000000007C
+:104CA0000300000000000000E91E000800000000F2
+:104CB000100000000044002000000000010000007F
+:104CC000000000001C47002001010200FD2900082F
+:104CD0000D290008A92900088D29000843000000BB
+:104CE000E44C000809024300020100C0320904003C
+:104CF0000001020201000524001001052401000149
+:104D0000042402020524060001070582030800FFAF
+:104D100009040100020A000000070501024000002A
+:104D2000070581024000000012000000304D00081D
+:104D30001201100102000040091241570002010255
+:104D4000030100000403090425424F415244250099
+:104D50004145542D483734332D4261736963003027
+:104D600031323334353637383941424344454600D1
+:104D700000000020000002000200000000000030DF
+:104D800000000400080000000000002400000800EB
+:104D9000040000000004000000FC0000020000000D
+:104DA000000004300080000008000000000000380F
+:104DB0000000010001000000000000004520000884
+:104DC000FD220008A92300084000400054480020AC
+:104DD00054480020010000006448002080000000CA
+:104DE0004001000008000000000100000010000069
+:104DF000080000006D61696E0069646C6500000068
+:104E00000000802A00000000AAAAAAAA000000242C
+:104E1000FFFF00000000000000A00A0000000001E9
+:104E200000000000AAAAAAAA00000001FFFF0000DB
+:104E3000000000000000000000000044000000002E
+:104E4000AAAAAAAA00000044FFFF00000000000078
+:104E5000000000000001100000000000AAAAAAAA99
+:104E600000010000FFFB0000000000000000000047
+:104E70005080420000000000AAAAAAAA10404100E7
+:104E8000F7FF0000000000700700000000000000B5
+:104E900000000000AAAAAAAA00000000FFFF00006C
+:104EA0000000000000000000000000000000000002
+:104EB000AAAAAAAA00000000FFFF0000000000004C
+:104EC000000000000000000000000000AAAAAAAA3A
+:104ED00000000000FFFF00000000000000000000D4
+:104EE0000000000000000000AAAAAAAA000000001A
+:104EF000FFFF0000000000000000000000000000B4
+:104F000000000000AAAAAAAA00000000FFFF0000FB
+:104F10000000000000000000000000000000000091
+:104F2000AAAAAAAA00000000FFFF000000000000DB
+:104F30000000000000000000E80700000000000082
+:104F400000001A0000000000FF000000382B0020C5
+:104F50005C240020000000009C4A0008830400003C
+:104F6000A74A000850040000B54A00080096000057
+:104F70000000080096000000000800000400000087
+:104F8000444D000800000000000000000000000088
+:0C4F900000000000000000000000000015
+:00000001FF
diff --git a/Tools/bootloaders/F4BY_F427_bl.bin b/Tools/bootloaders/F4BY_F427_bl.bin
new file mode 100755
index 00000000000000..92ed4fbdb6564e
Binary files /dev/null and b/Tools/bootloaders/F4BY_F427_bl.bin differ
diff --git a/Tools/bootloaders/MFE_POS3_CAN_bl.bin b/Tools/bootloaders/MFE_POS3_CAN_bl.bin
new file mode 100644
index 00000000000000..524a52a4c89cfb
Binary files /dev/null and b/Tools/bootloaders/MFE_POS3_CAN_bl.bin differ
diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh
index d7b7d8c31440d5..5c205577ba73a6 100755
--- a/Tools/environment_install/install-prereqs-ubuntu.sh
+++ b/Tools/environment_install/install-prereqs-ubuntu.sh
@@ -164,7 +164,7 @@ fi
 # Lists of packages to install
 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"
+PYTHON_PKGS="$PYTHON_PKGS flake8 junitparser wsproto tabulate"
 
 # add some Python packages required for commonly-used MAVProxy modules and hex file generation:
 if [[ $SKIP_AP_EXT_ENV -ne 1 ]]; then
diff --git a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py
new file mode 100755
index 00000000000000..4443c17ab24fcb
--- /dev/null
+++ b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/copter_takeoff.py
@@ -0,0 +1,180 @@
+#!/usr/bin/env python3
+# 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/>.
+
+"""
+Run takeoff test on Copter.
+
+Warning - This is NOT production code; it's a simple demo of capability.
+"""
+
+import math
+import rclpy
+import time
+import errno
+
+from rclpy.node import Node
+from builtin_interfaces.msg import Time
+from ardupilot_msgs.msg import GlobalPosition
+from geographic_msgs.msg import GeoPoseStamped
+from geopy import distance
+from geopy import point
+from ardupilot_msgs.srv import ArmMotors
+from ardupilot_msgs.srv import ModeSwitch
+from ardupilot_msgs.srv import Takeoff
+
+
+
+COPTER_MODE_GUIDED = 4
+
+TAKEOFF_ALT = 10.5
+
+class CopterTakeoff(Node):
+    """Copter takeoff using guided control."""
+
+    def __init__(self):
+        """Initialise the node."""
+        super().__init__("copter_takeoff")
+
+        self.declare_parameter("arm_topic", "/ap/arm_motors")
+        self._arm_topic = self.get_parameter("arm_topic").get_parameter_value().string_value
+        self._client_arm = self.create_client(ArmMotors, self._arm_topic)
+        while not self._client_arm.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('arm service not available, waiting again...')
+
+        self.declare_parameter("mode_topic", "/ap/mode_switch")
+        self._mode_topic = self.get_parameter("mode_topic").get_parameter_value().string_value
+        self._client_mode_switch = self.create_client(ModeSwitch, self._mode_topic)
+        while not self._client_mode_switch.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('mode switch service not available, waiting again...')
+
+        self.declare_parameter("takeoff_service", "/ap/experimental/takeoff")
+        self._takeoff_topic = self.get_parameter("takeoff_service").get_parameter_value().string_value
+        self._client_takeoff = self.create_client(Takeoff, self._takeoff_topic)
+        while not self._client_takeoff.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('takeoff service not available, waiting again...')
+        
+        self.declare_parameter("geopose_topic", "/ap/geopose/filtered")
+        self._geopose_topic = self.get_parameter("geopose_topic").get_parameter_value().string_value
+        qos = rclpy.qos.QoSProfile(
+            reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, durability=rclpy.qos.DurabilityPolicy.VOLATILE, depth=1
+        )
+
+        self._subscription_geopose = self.create_subscription(GeoPoseStamped, self._geopose_topic, self.geopose_cb, qos)
+        self._cur_geopose = GeoPoseStamped()
+
+    def geopose_cb(self, msg: GeoPoseStamped):
+        """Process a GeoPose message."""
+        stamp = msg.header.stamp
+        if stamp.sec:
+            self.get_logger().info("From AP : Geopose [sec:{}, nsec: {}]".format(stamp.sec, stamp.nanosec))
+
+            # Store current state
+            self._cur_geopose = msg
+
+
+    def arm(self):
+        req = ArmMotors.Request()
+        req.arm = True
+        future = self._client_arm.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result()
+
+    def arm_with_timeout(self, timeout: rclpy.duration.Duration):
+        """Try to arm. Returns true on success, or false if arming fails or times out."""
+        armed = False
+        start = self.get_clock().now()
+        while not armed and self.get_clock().now() - start < timeout:
+            armed = self.arm().result
+            time.sleep(1)
+        return armed
+
+    def switch_mode(self, mode):
+        req = ModeSwitch.Request()
+        assert mode in [COPTER_MODE_GUIDED]
+        req.mode = mode
+        future = self._client_mode_switch.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result()
+
+    def switch_mode_with_timeout(self, desired_mode: int, timeout: rclpy.duration.Duration):
+        """Try to switch mode. Returns true on success, or false if mode switch fails or times out."""
+        is_in_desired_mode = False
+        start = self.get_clock().now()
+        while not is_in_desired_mode:
+            result = self.switch_mode(desired_mode)
+            # Handle successful switch or the case that the vehicle is already in expected mode
+            is_in_desired_mode = result.status or result.curr_mode == desired_mode
+            time.sleep(1)
+
+        return is_in_desired_mode
+    
+    def takeoff(self, alt):
+        req = Takeoff.Request()
+        req.alt = alt
+        future = self._client_takeoff.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result()
+
+    def takeoff_with_timeout(self, alt: int, timeout: rclpy.duration.Duration):
+        """Try to takeoff. Returns true on success, or false if takeoff fails or times out."""
+        takeoff_success = False
+        start = self.get_clock().now()
+        while not takeoff_success:
+            result = self.takeoff(alt)
+            takeoff_success = result.status
+            time.sleep(1)
+
+        return takeoff_success
+    
+    def get_cur_geopose(self):
+        """Return latest geopose."""
+        return self._cur_geopose
+
+def main(args=None):
+    """Node entry point."""
+    rclpy.init(args=args)
+    node = CopterTakeoff()
+    try:
+        if not node.switch_mode_with_timeout(COPTER_MODE_GUIDED, rclpy.duration.Duration(seconds=20)):
+            raise RuntimeError("Unable to switch to guided mode")
+        # Block till armed, which will wait for EKF3 to initialize
+        if not node.arm_with_timeout(rclpy.duration.Duration(seconds=30)):
+            raise RuntimeError("Unable to arm")
+
+        # Block till in takeoff
+        if not node.takeoff_with_timeout(TAKEOFF_ALT, rclpy.duration.Duration(seconds=20)):
+            raise RuntimeError("Unable to takeoff")
+
+        is_ascending_to_takeoff_alt = True
+        while is_ascending_to_takeoff_alt:
+            rclpy.spin_once(node)
+            time.sleep(1.0)
+
+            is_ascending_to_takeoff_alt = node.get_cur_geopose().pose.position.altitude < TAKEOFF_ALT
+
+        if is_ascending_to_takeoff_alt:
+            raise RuntimeError("Failed to reach takeoff altitude")
+
+    except KeyboardInterrupt:
+        pass
+
+    # Destroy the node explicitly.
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py
index 27de6f272f2ed8..7db9aa9be5eee1 100755
--- a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py
+++ b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py
@@ -33,6 +33,7 @@
 from geopy import point
 from ardupilot_msgs.srv import ArmMotors
 from ardupilot_msgs.srv import ModeSwitch
+from geographic_msgs.msg import GeoPointStamped
 
 
 PLANE_MODE_TAKEOFF = 13
@@ -78,6 +79,15 @@ def __init__(self):
 
         self._subscription_geopose = self.create_subscription(GeoPoseStamped, self._geopose_topic, self.geopose_cb, qos)
         self._cur_geopose = GeoPoseStamped()
+        
+        self.declare_parameter("goal_topic", "/ap/goal_lla")
+        self._goal_topic = self.get_parameter("goal_topic").get_parameter_value().string_value
+        qos = rclpy.qos.QoSProfile(
+            reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, depth=1
+        )
+
+        self._subscription_goal = self.create_subscription(GeoPointStamped, self._goal_topic, self.goal_cb, qos)
+        self._cur_goal = GeoPointStamped()        
 
     def geopose_cb(self, msg: GeoPoseStamped):
         """Process a GeoPose message."""
@@ -87,6 +97,15 @@ def geopose_cb(self, msg: GeoPoseStamped):
 
             # Store current state
             self._cur_geopose = msg
+            
+    def goal_cb(self, msg: GeoPointStamped):
+        """Process a Goal message."""
+        stamp = msg.header.stamp
+        self.get_logger().info("From AP : Goal [sec:{}, nsec: {}, lat:{} lon:{}]"
+                               .format(stamp.sec, stamp.nanosec,msg.position.latitude, msg.position.longitude))
+
+        # Store current state
+        self._cur_goal = msg
 
     def arm(self):
         req = ArmMotors.Request()
@@ -127,6 +146,10 @@ def switch_mode_with_timeout(self, desired_mode: int, timeout: rclpy.duration.Du
     def get_cur_geopose(self):
         """Return latest geopose."""
         return self._cur_geopose
+    
+    def get_cur_goal(self):
+        """Return latest goal."""
+        return self._cur_goal 
 
     def send_goal_position(self, goal_global_pos):
         """Send goal position. Must be in guided for this to work."""
@@ -148,6 +171,15 @@ def achieved_goal(goal_global_pos, cur_geopose):
     print(f"Goal is {euclidian_distance} meters away")
     return euclidian_distance < 150
 
+def going_to_goal(goal_global_pos, cur_goal):
+    p1 = (goal_global_pos.latitude, goal_global_pos.longitude, goal_global_pos.altitude)
+    cur_pos_lla = cur_goal.position
+    p2 = (cur_pos_lla.latitude, cur_pos_lla.longitude, cur_pos_lla.altitude)
+
+    flat_distance = distance.distance(p1[:2], p2[:2]).m
+    euclidian_distance = math.sqrt(flat_distance**2 + (p2[2] - p1[2]) ** 2)
+    print(f"Commanded and received goal are {euclidian_distance} meters away")
+    return euclidian_distance < 1    
 
 def main(args=None):
     """Node entry point."""
@@ -191,11 +223,15 @@ def main(args=None):
 
         start = node.get_clock().now()
         has_achieved_goal = False
+        is_going_to_goal = False
         while not has_achieved_goal and node.get_clock().now() - start < rclpy.duration.Duration(seconds=120):
             rclpy.spin_once(node)
+            is_going_to_goal = going_to_goal(goal_pos, node.get_cur_goal())
             has_achieved_goal = achieved_goal(goal_pos, node.get_cur_geopose())
             time.sleep(1.0)
 
+        if not is_going_to_goal:
+            raise RuntimeError("Unable to go to goal location")
         if not has_achieved_goal:
             raise RuntimeError("Unable to achieve goal location")
 
diff --git a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/pre_arm_check_service.py b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/pre_arm_check_service.py
new file mode 100644
index 00000000000000..e835209015fa43
--- /dev/null
+++ b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/pre_arm_check_service.py
@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+# 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/>.
+
+"""
+Run pre_arm check test on Copter.
+
+Warning - This is NOT production code; it's a simple demo of capability.
+"""
+
+import math
+import rclpy
+import time
+import errno
+
+from rclpy.node import Node
+from builtin_interfaces.msg import Time
+from std_srvs.srv import Trigger
+
+
+class CopterPreArm(Node):
+    """Check Prearm Service."""
+
+    def __init__(self):
+        """Initialise the node."""
+        super().__init__("copter_prearm")
+
+        self.declare_parameter("pre_arm_service", "/ap/prearm_check")
+        self._prearm_service = self.get_parameter("pre_arm_service").get_parameter_value().string_value
+        self._client_prearm = self.create_client(Trigger, self._prearm_service)
+        while not self._client_prearm.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('prearm service not available, waiting again...')
+
+    def prearm(self):
+        req = Trigger.Request()
+        future = self._client_prearm.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result()
+
+    def prearm_with_timeout(self, timeout: rclpy.duration.Duration):
+        """Check if armable. Returns true on success, or false if arming will fail or times out."""
+        armable = False
+        start = self.get_clock().now()
+        while not armable and self.get_clock().now() - start < timeout:
+            armable = self.prearm().success
+            time.sleep(1)
+        return armable
+
+def main(args=None):
+    """Node entry point."""
+    rclpy.init(args=args)
+    node = CopterPreArm()
+    try:
+        # Block till armed, which will wait for EKF3 to initialize
+        if not node.prearm_with_timeout(rclpy.duration.Duration(seconds=30)):
+            raise RuntimeError("Vehicle not armable")
+    except KeyboardInterrupt:
+        pass
+
+    # Destroy the node explicitly.
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/Tools/ros2/ardupilot_dds_tests/setup.py b/Tools/ros2/ardupilot_dds_tests/setup.py
index bbe11e6a5c8123..bf9e0577af6a92 100644
--- a/Tools/ros2/ardupilot_dds_tests/setup.py
+++ b/Tools/ros2/ardupilot_dds_tests/setup.py
@@ -26,6 +26,8 @@
         'console_scripts': [
             "time_listener = ardupilot_dds_tests.time_listener:main",
             "plane_waypoint_follower = ardupilot_dds_tests.plane_waypoint_follower:main",
+            "pre_arm_check = ardupilot_dds_tests.pre_arm_check_service:main",
+            "copter_takeoff = ardupilot_dds_tests.copter_takeoff:main",
         ],
     },
 )
diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_prearm_service.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_prearm_service.py
new file mode 100644
index 00000000000000..f081d5cc1cfe6f
--- /dev/null
+++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_prearm_service.py
@@ -0,0 +1,166 @@
+# 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 whether the vehicle can be armed.
+
+colcon test --executor sequential --parallel-workers 0 --base-paths src/ardupilot \
+    --event-handlers=console_cohesion+ --packages-select ardupilot_dds_tests \
+    --pytest-args -k test_prearm_service
+
+"""
+
+import launch_pytest
+import math
+import time
+import pytest
+import rclpy
+import rclpy.node
+import threading
+
+from launch import LaunchDescription
+
+from launch_pytest.tools import process as process_tools
+from std_srvs.srv import Trigger
+
+
+SERVICE = "/ap/prearm_check"
+
+class PreamService(rclpy.node.Node):
+    def __init__(self):
+        """Initialise the node."""
+        super().__init__("prearm_client")
+        self.service_available_object = threading.Event()
+        self.is_armable_object = threading.Event()
+        self._client_prearm = self.create_client(Trigger, "/ap/prearm_check")
+
+    def start_node(self):
+        # Add a spin thread.
+        self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,))
+        self.ros_spin_thread.start()
+
+    def prearm_check(self):
+        req = Trigger.Request()
+        future = self._client_prearm.call_async(req)
+        time.sleep(0.2)
+        try:
+            return future.result().success
+        except Exception  as e:
+            print(e)
+            return False
+
+    def prearm_with_timeout(self, timeout: rclpy.duration.Duration):
+        result = False
+        start = self.get_clock().now()
+        while not result and self.get_clock().now() - start < timeout:
+            result = self.prearm_check()
+            time.sleep(1)
+        return result
+
+    def process_prearm(self):
+        if self.prearm_with_timeout(rclpy.duration.Duration(seconds=30)):
+            self.is_armable_object.set()
+
+    def start_prearm(self):
+        try:
+            self.prearm_thread.stop()
+        except:
+            print("start_prearm not started yet")
+        self.prearm_thread = threading.Thread(target=self.process_prearm)
+        self.prearm_thread.start()
+        
+        
+
+
+@launch_pytest.fixture
+def launch_sitl_copter_dds_serial(sitl_copter_dds_serial):
+    """Fixture to create the launch description."""
+    sitl_ld, sitl_actions = sitl_copter_dds_serial
+
+    ld = LaunchDescription(
+        [
+            sitl_ld,
+            launch_pytest.actions.ReadyToTest(),
+        ]
+    )
+    actions = sitl_actions
+    yield ld, actions
+
+
+@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_serial)
+def test_dds_serial_prearm_service_call(launch_context, launch_sitl_copter_dds_serial):
+    """Test prearm service AP_DDS."""
+    _, actions = launch_sitl_copter_dds_serial
+    virtual_ports = actions["virtual_ports"].action
+    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, virtual_ports, timeout=2)
+    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 = PreamService()
+        node.start_node()
+        node.start_prearm()
+        is_armable_flag = node.is_armable_object.wait(timeout=25.0)
+        assert is_armable_flag, f"Vehicle not armable."
+    finally:
+        rclpy.shutdown()
+    yield
+
+
+@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp)
+def test_dds_udp_prearm_service_call(launch_context, launch_sitl_copter_dds_udp):
+    """Test prearm service 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 = PreamService()
+        node.start_node()
+        node.start_prearm()
+        is_armable_flag = node.is_armable_object.wait(timeout=25.0)
+        assert is_armable_flag, f"Vehicle not armable."
+    finally:
+        rclpy.shutdown()
+    yield
diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt
index f8af788e8b81ec..767b89a89b7691 100644
--- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt
+++ b/Tools/ros2/ardupilot_msgs/CMakeLists.txt
@@ -17,6 +17,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Status.msg"
   "srv/ArmMotors.srv"
   "srv/ModeSwitch.srv"
+  "srv/Takeoff.srv"
   DEPENDENCIES geometry_msgs std_msgs
   ADD_LINTER_TESTS
 )
diff --git a/Tools/ros2/ardupilot_msgs/srv/Takeoff.srv b/Tools/ros2/ardupilot_msgs/srv/Takeoff.srv
new file mode 100644
index 00000000000000..05e5de8bd5a34e
--- /dev/null
+++ b/Tools/ros2/ardupilot_msgs/srv/Takeoff.srv
@@ -0,0 +1,10 @@
+
+# This service requests the vehicle to takeoff
+
+# alt : Set the takeoff altitude above home or above terrain(in case of rangefinder)
+
+float32 alt
+---
+# status    : True if the request for mode switch was successful, False otherwise
+
+bool status
diff --git a/Tools/scripts/build_ci.sh b/Tools/scripts/build_ci.sh
index bdf43bb8cf3998..460d0db00f0f3e 100755
--- a/Tools/scripts/build_ci.sh
+++ b/Tools/scripts/build_ci.sh
@@ -5,7 +5,13 @@
 
 XOLDPWD=$PWD  # profile changes directory :-(
 
-. ~/.profile
+if [ -z "$GITHUB_ACTIONS" ] || [ "$GITHUB_ACTIONS" != "true" ]; then
+  . ~/.profile
+fi
+
+if [ "$CI" = "true" ]; then
+  export PIP_ROOT_USER_ACTION=ignore
+fi
 
 cd $XOLDPWD
 
@@ -40,7 +46,7 @@ function install_pymavlink() {
     if [ $pymavlink_installed -eq 0 ]; then
         echo "Installing pymavlink"
         git submodule update --init --recursive --depth 1
-        (cd modules/mavlink/pymavlink && python3 -m pip install --user .)
+        (cd modules/mavlink/pymavlink && python3 -m pip install --progress-bar off --cache-dir /tmp/pip-cache --user .)
         pymavlink_installed=1
     fi
 }
@@ -51,12 +57,12 @@ function install_mavproxy() {
         pushd /tmp
           git clone https://github.com/ardupilot/MAVProxy --depth 1
           pushd MAVProxy
-            python3 -m pip install --user --force .
+            python3 -m pip install --progress-bar off --cache-dir /tmp/pip-cache --user --force .
           popd
         popd
         mavproxy_installed=1
         # now uninstall the version of pymavlink pulled in by MAVProxy deps:
-        python3 -m pip uninstall -y pymavlink
+        python3 -m pip uninstall -y pymavlink --cache-dir /tmp/pip-cache
     fi
 }
 
@@ -458,7 +464,7 @@ for t in $CI_BUILD_TARGET; do
         echo "Building signed firmwares"
         sudo apt-get update
         sudo apt-get install -y python3-dev
-        python3 -m pip install pymonocypher==3.1.3.2
+        python3 -m pip install pymonocypher==3.1.3.2 --progress-bar off --cache-dir /tmp/pip-cache
         ./Tools/scripts/signing/generate_keys.py testkey
         $waf configure --board CubeOrange-ODID --signed-fw --private-key testkey_private_key.dat
         $waf copter
diff --git a/Tools/scripts/build_iofirmware.py b/Tools/scripts/build_iofirmware.py
index c2169d3ed9dd43..c2f67096a82fce 100755
--- a/Tools/scripts/build_iofirmware.py
+++ b/Tools/scripts/build_iofirmware.py
@@ -29,12 +29,24 @@ def run_program(cmd_list):
 shutil.copy('build/iomcu/bin/iofirmware_lowpolh.bin', 'Tools/IO_Firmware/iofirmware_lowpolh.bin')
 shutil.copy('build/iomcu/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_highpolh.bin')
 
+run_program(["./waf", "configure", "--board", 'iomcu', '--enable-iomcu-profiled-support'])
+run_program(["./waf", "clean"])
+run_program(["./waf", "iofirmware"])
+shutil.copy('build/iomcu/bin/iofirmware_lowpolh.bin', 'Tools/IO_Firmware/iofirmware_cube_lowpolh.bin')
+shutil.copy('build/iomcu/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_cube_highpolh.bin')
+
 run_program(["./waf", "configure", "--board", 'iomcu-dshot'])
 run_program(["./waf", "clean"])
 run_program(["./waf", "iofirmware"])
 shutil.copy('build/iomcu-dshot/bin/iofirmware_lowpolh.bin', 'Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin')
 shutil.copy('build/iomcu-dshot/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_dshot_highpolh.bin')
 
+run_program(["./waf", "configure", "--board", 'iomcu-dshot', '--enable-iomcu-profiled-support'])
+run_program(["./waf", "clean"])
+run_program(["./waf", "iofirmware"])
+shutil.copy('build/iomcu-dshot/bin/iofirmware_lowpolh.bin', 'Tools/IO_Firmware/iofirmware_cube_dshot_lowpolh.bin')
+shutil.copy('build/iomcu-dshot/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin')
+
 run_program(["./waf", "configure", "--board", 'iomcu-f103'])
 run_program(["./waf", "clean"])
 run_program(["./waf", "iofirmware"])
diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py
index f31edfaf87ff3a..1c259adeae2114 100644
--- a/Tools/scripts/build_options.py
+++ b/Tools/scripts/build_options.py
@@ -224,6 +224,7 @@ def config_option(self):
     Feature('Plane', 'AP_TX_TUNING', 'AP_TUNING_ENABLED', 'Enable TX-based tuning parameter adjustments', 0, None),
     Feature('Plane', 'PLANE_GUIDED_SLEW', 'AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED', 'Enable Offboard-guided slew commands', 0, None),  # noqa:401
     Feature('Plane', 'PLANE_GLIDER_PULLUP', 'AP_PLANE_GLIDER_PULLUP_ENABLED', 'Enable Glider pullup support', 0, None),
+    Feature('Plane', 'QUICKTUNE', 'AP_QUICKTUNE_ENABLED', 'Enable VTOL quicktune', 0, None),
 
     Feature('RC', 'RC_Protocol', 'AP_RCPROTOCOL_ENABLED', "Enable Serial RC Protocols", 0, None),   # NOQA: E501
     Feature('RC', 'RC_CRSF', 'AP_RCPROTOCOL_CRSF_ENABLED', "Enable CRSF", 0, "RC_Protocol"),   # NOQA: E501
diff --git a/Tools/scripts/pretty_diff_size.py b/Tools/scripts/build_tests/pretty_diff_size.py
similarity index 89%
rename from Tools/scripts/pretty_diff_size.py
rename to Tools/scripts/build_tests/pretty_diff_size.py
index 79598d706fb221..59d0c1eadeacb5 100755
--- a/Tools/scripts/pretty_diff_size.py
+++ b/Tools/scripts/build_tests/pretty_diff_size.py
@@ -90,6 +90,21 @@ def print_table(summary_data_list_second, summary_data_list_master):
                 print_data.append(col_data)
     print(tabulate(print_data, headers=["Binary Name", "Text [B]", "Data [B]", "BSS (B)",
                                         "Total Flash Change [B] (%)", "Flash Free After PR (B)"]))
+    # Get the GitHub Actions summary file path
+    summary_file = os.getenv('GITHUB_STEP_SUMMARY')
+    if summary_file:
+        # Append the output to the summary file
+        with open(summary_file, 'a') as f:
+            f.write("### Diff summary\n")
+            f.write(tabulate(print_data, headers=[
+                "Binary Name",
+                "Text [B]",
+                "Data [B]",
+                "BSS (B)",
+                "Total Flash Change [B] (%)",
+                "Flash Free After PR (B)"
+            ], tablefmt="github"))
+            f.write("\n")
 
 
 def extract_binaries_size(path):
diff --git a/Tools/scripts/build_tests/test_ccache.py b/Tools/scripts/build_tests/test_ccache.py
index c0d5543d1ebd0b..9abdb22badef6c 100755
--- a/Tools/scripts/build_tests/test_ccache.py
+++ b/Tools/scripts/build_tests/test_ccache.py
@@ -65,11 +65,22 @@ def build_board(boardname):
 build_board(boards[0])
 subprocess.run(["ccache", "-z"])
 build_board(boards[1])
-subprocess.run(["ccache", "-s"])
+result = subprocess.run(["ccache", "-s"], capture_output=True, text=True)
+print(result.stdout)
+
+# Get the GitHub Actions summary file path
+summary_file = os.getenv('GITHUB_STEP_SUMMARY')
 
 post = ccache_stats()
 hit_pct = 100 * post[0] / float(post[0]+post[1])
 print("ccache hit percentage: %.1f%%  %s" % (hit_pct, post))
+if summary_file:
+    # Append the output to the summary file
+    with open(summary_file, 'a') as f:
+        f.write(f"### ccache -s Output with {boards}\n")
+        f.write(f"```\n{result.stdout}\n```\n")
+        f.write(f"### ccache hit percentage (min {args.min_cache_pct})\n")
+        f.write("ccache hit percentage: %.1f%%  %s\n" % (hit_pct, post))
 if hit_pct < args.min_cache_pct:
     print("ccache hits too low, need %d%%" % args.min_cache_pct)
     sys.exit(1)
diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py
index de6eec65895238..c4a554e8059723 100755
--- a/Tools/scripts/extract_features.py
+++ b/Tools/scripts/extract_features.py
@@ -281,6 +281,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
 
             ('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'),
+            ('AP_QUICKTUNE_ENABLED', r'AP_Quicktune::update'),
         ]
 
     def progress(self, msg):
diff --git a/Tools/vagrant/mavinit.scr b/Tools/vagrant/mavinit.scr
index 4b56c421c538ba..fe7027ba0def73 100644
--- a/Tools/vagrant/mavinit.scr
+++ b/Tools/vagrant/mavinit.scr
@@ -17,7 +17,6 @@ module load graph
 @alias add odroidpower relay set 0
 @alias add neutral2 servo set 12 1500
 @alias add ekf param set AHRS_EKF_USE
-@alias add gpsdisable param set SIM_GPS_DISABLE
 @alias add bb status gps* scaled* raw*
 @alias add g graph
 @alias add grc3 g RC_CHANNELS.chan3_raw SERVO_OUTPUT_RAW.servo3_raw
diff --git a/libraries/AC_Fence/AC_Fence_config.h b/libraries/AC_Fence/AC_Fence_config.h
index 119616c98194b9..3e10480711ab85 100644
--- a/libraries/AC_Fence/AC_Fence_config.h
+++ b/libraries/AC_Fence/AC_Fence_config.h
@@ -17,5 +17,5 @@
 // ArduPilot 4.7 stops compiling them in
 // ArduPilot 4.8 removes the code entirely
 #ifndef AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT
-#define AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT HAL_GCS_ENABLED && AP_FENCE_ENABLED
+#define AC_POLYFENCE_FENCE_POINT_PROTOCOL_SUPPORT 0
 #endif
diff --git a/libraries/AC_Fence/AC_PolyFence_loader.cpp b/libraries/AC_Fence/AC_PolyFence_loader.cpp
index b97334476e0533..1ed5ecce921d32 100644
--- a/libraries/AC_Fence/AC_PolyFence_loader.cpp
+++ b/libraries/AC_Fence/AC_PolyFence_loader.cpp
@@ -306,7 +306,7 @@ bool AC_PolyFence_loader::formatted() const
 uint16_t AC_PolyFence_loader::max_items() const
 {
     // this is 84 items on PixHawk
-    return MIN(255U, fence_storage.size() / sizeof(Vector2l));
+    return fence_storage.size() / sizeof(Vector2l);
 }
 
 bool AC_PolyFence_loader::format()
@@ -650,7 +650,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
     // FIXME: find some way of factoring out all of these allocation routines.
 
     { // allocate storage for inclusion polyfences:
-        const uint8_t count = index_fence_count(AC_PolyFenceType::POLYGON_INCLUSION);
+        const auto count = index_fence_count(AC_PolyFenceType::POLYGON_INCLUSION);
         Debug("Fence: Allocating %u bytes for inc. fences",
               (unsigned)(count * sizeof(InclusionBoundary)));
         _loaded_inclusion_boundary = NEW_NOTHROW InclusionBoundary[count];
@@ -662,7 +662,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
     }
 
     { // allocate storage for exclusion polyfences:
-        const uint8_t count = index_fence_count(AC_PolyFenceType::POLYGON_EXCLUSION);
+        const auto count = index_fence_count(AC_PolyFenceType::POLYGON_EXCLUSION);
         Debug("Fence: Allocating %u bytes for exc. fences",
               (unsigned)(count * sizeof(ExclusionBoundary)));
         _loaded_exclusion_boundary = NEW_NOTHROW ExclusionBoundary[count];
@@ -674,7 +674,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
     }
 
     { // allocate storage for circular inclusion fences:
-        uint8_t count = index_fence_count(AC_PolyFenceType::CIRCLE_INCLUSION);
+        uint32_t count = index_fence_count(AC_PolyFenceType::CIRCLE_INCLUSION);
         count += index_fence_count(AC_PolyFenceType::CIRCLE_INCLUSION_INT)
         Debug("Fence: Allocating %u bytes for circ. inc. fences",
               (unsigned)(count * sizeof(InclusionCircle)));
@@ -687,7 +687,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
     }
 
     { // allocate storage for circular exclusion fences:
-        uint8_t count = index_fence_count(AC_PolyFenceType::CIRCLE_EXCLUSION);
+        uint32_t count = index_fence_count(AC_PolyFenceType::CIRCLE_EXCLUSION);
         count += index_fence_count(AC_PolyFenceType::CIRCLE_EXCLUSION_INT)
         Debug("Fence: Allocating %u bytes for circ. exc. fences",
               (unsigned)(count * sizeof(ExclusionCircle)));
@@ -1060,7 +1060,7 @@ bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_
         return false;
     }
 
-    uint8_t total_vertex_count = 0;
+    uint16_t total_vertex_count = 0;
     uint16_t offset = 4; // skipping magic
     uint8_t vertex_count = 0;
     for (uint16_t i=0; i<count; i++) {
diff --git a/libraries/AC_Fence/AC_PolyFence_loader.h b/libraries/AC_Fence/AC_PolyFence_loader.h
index 4470cfa4fb6cc5..1625c87793f7ba 100644
--- a/libraries/AC_Fence/AC_PolyFence_loader.h
+++ b/libraries/AC_Fence/AC_PolyFence_loader.h
@@ -9,7 +9,7 @@
 // radius looks like an integer as a backwards-compatibility measure.
 // For 4.2 we might consider only loading _INT and always saving as
 // float, and in 4.3 considering _INT invalid
-enum class AC_PolyFenceType {
+enum class AC_PolyFenceType : uint8_t {
     END_OF_STORAGE        = 99,
     POLYGON_INCLUSION     = 98,
     POLYGON_EXCLUSION     = 97,
diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp
index 397c5d1d6e91bc..e8991ca98fe879 100644
--- a/libraries/AP_Airspeed/AP_Airspeed.cpp
+++ b/libraries/AP_Airspeed/AP_Airspeed.cpp
@@ -157,7 +157,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     
     // @Param: _OFF_PCNT
     // @DisplayName: Maximum offset cal speed error 
-    // @Description: The maximum percentage speed change in airspeed reports that is allowed due to offset changes between calibrations before a warning is issued. This potential speed error is in percent of ASPD_FBW_MIN. 0 disables. Helps warn of calibrations without pitot being covered.
+    // @Description: The maximum percentage speed change in airspeed reports that is allowed due to offset changes between calibrations before a warning is issued. This potential speed error is in percent of AIRSPEED_MIN. 0 disables. Helps warn of calibrations without pitot being covered.
     // @Range: 0.0 10.0
     // @Units: %
     // @User: Advanced
diff --git a/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.cpp b/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.cpp
index 4f0b3da42f89ac..8d3dff897ae543 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.cpp
+++ b/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.cpp
@@ -13,21 +13,15 @@ extern const AP_HAL::HAL& hal;
 AP_Airspeed_DroneCAN::DetectedModules AP_Airspeed_DroneCAN::_detected_modules[];
 HAL_Semaphore AP_Airspeed_DroneCAN::_sem_registry;
 
-void AP_Airspeed_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_Airspeed_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_airspeed, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("airspeed_sub");
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_airspeed, driver_index) != nullptr)
 #if AP_AIRSPEED_HYGROMETER_ENABLE
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_hygrometer, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("hygrometer_sub");
-    }
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_hygrometer, driver_index) != nullptr)
 #endif
+    ;
 }
 
 AP_Airspeed_Backend* AP_Airspeed_DroneCAN::probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid)
diff --git a/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h b/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h
index 2167f9f061521a..7d92c27f9555ed 100644
--- a/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h
+++ b/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h
@@ -26,7 +26,7 @@ class AP_Airspeed_DroneCAN : public AP_Airspeed_Backend {
     bool get_hygrometer(uint32_t &last_sample_ms, float &temperature, float &humidity) override;
 #endif
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
 
     static AP_Airspeed_Backend* probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid);
 
diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
index ffa148d6b5af1e..4b058abe53605e 100644
--- a/libraries/AP_Arming/AP_Arming.cpp
+++ b/libraries/AP_Arming/AP_Arming.cpp
@@ -582,11 +582,11 @@ bool AP_Arming::compass_checks(bool report)
                              (double)MAX(fabsf(diff_mgauss.x), (double)fabsf(diff_mgauss.y)), (int)magfield_error_threshold);
                 return false;
             }
-            if (fabsf(diff_mgauss.x) > magfield_error_threshold*2.0) {
+            if (fabsf(diff_mgauss.z) > magfield_error_threshold*2.0) {
                 check_failed(ARMING_CHECK_COMPASS, report, "Check mag field (z diff:%.0f>%d)",
                              (double)fabsf(diff_mgauss.z), (int)magfield_error_threshold*2);
                 return false;
-            }           
+            }
         }
 #endif  // AP_AHRS_ENABLED
     }
diff --git a/libraries/AP_Baro/AP_Baro_DroneCAN.cpp b/libraries/AP_Baro/AP_Baro_DroneCAN.cpp
index e171aab4a67e79..07975e10823230 100644
--- a/libraries/AP_Baro/AP_Baro_DroneCAN.cpp
+++ b/libraries/AP_Baro/AP_Baro_DroneCAN.cpp
@@ -21,18 +21,13 @@ AP_Baro_DroneCAN::AP_Baro_DroneCAN(AP_Baro &baro) :
     AP_Baro_Backend(baro)
 {}
 
-void AP_Baro_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_Baro_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_pressure, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("pressure_sub");
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("temperature_sub");
-    }
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_pressure, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, driver_index) != nullptr)
+    ;
 }
 
 AP_Baro_Backend* AP_Baro_DroneCAN::probe(AP_Baro &baro)
diff --git a/libraries/AP_Baro/AP_Baro_DroneCAN.h b/libraries/AP_Baro/AP_Baro_DroneCAN.h
index 3040b97ab6b36b..5aa3a1fa4c4e69 100644
--- a/libraries/AP_Baro/AP_Baro_DroneCAN.h
+++ b/libraries/AP_Baro/AP_Baro_DroneCAN.h
@@ -15,7 +15,7 @@ class AP_Baro_DroneCAN : public AP_Baro_Backend {
 
     void update() override;
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
     static AP_Baro_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, bool create_new);
     static AP_Baro_Backend* probe(AP_Baro &baro);
 
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp
index 24bb0b69f5f088..d505dcc20d93ac 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp
@@ -42,23 +42,14 @@ AP_BattMonitor_DroneCAN::AP_BattMonitor_DroneCAN(AP_BattMonitor &mon, AP_BattMon
     _state.healthy = false;
 }
 
-void AP_BattMonitor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_BattMonitor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("battinfo_sub");
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_aux_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("battinfo_aux_sub");
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_mppt_stream_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("mppt_stream_sub");
-    }
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_trampoline, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_aux_trampoline, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_mppt_stream_trampoline, driver_index) != nullptr)
+    ;
 }
 
 /*
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h
index a25ef9667855c8..7d34b91acf1faa 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h
@@ -47,7 +47,7 @@ class AP_BattMonitor_DroneCAN : public AP_BattMonitor_Backend
     // return mavlink fault bitmask (see MAV_BATTERY_FAULT enum)
     uint32_t get_mavlink_fault_bitmask() const override;
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
     static AP_BattMonitor_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t battery_id);
     static void handle_battery_info_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_power_BatteryInfo &msg);
     static void handle_battery_info_aux_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg);
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
index 285481d38b7e8a..aceee7c05b373e 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
@@ -150,14 +150,16 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
     AP_GROUPINFO("ARM_MAH", 19, AP_BattMonitor_Params, _arming_minimum_capacity, 0),
 
     // 20 was BUS
+#endif // HAL_BUILD_AP_PERIPH
 
+#if AP_BATTERY_OPTIONS_PARAM_ENABLED
     // @Param: OPTIONS
     // @DisplayName: Battery monitor options
     // @Description: This sets options to change the behaviour of the battery monitor
     // @Bitmask: 0:Ignore DroneCAN SoC, 1:MPPT reports input voltage and current, 2:MPPT Powered off when disarmed, 3:MPPT Powered on when armed, 4:MPPT Powered off at boot, 5:MPPT Powered on at boot, 6:Send resistance compensated voltage to GCS, 7:Allow DroneCAN InfoAux to be from a different CAN node, 9:Sum monitor measures minimum voltage instead of average
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 21, AP_BattMonitor_Params, _options, 0),
-#endif // HAL_BUILD_AP_PERIPH
+#endif // AP_BATTERY_OPTIONS_PARAM_ENABLED
 
 #if AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED
     // @Param: ESC_INDEX
diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_config.h b/libraries/AP_BattMonitor/AP_BattMonitor_config.h
index 14a1fa7d281e12..3159b075d6b1c5 100644
--- a/libraries/AP_BattMonitor/AP_BattMonitor_config.h
+++ b/libraries/AP_BattMonitor/AP_BattMonitor_config.h
@@ -119,3 +119,7 @@
 #ifndef AP_BATTERY_SCRIPTING_ENABLED
 #define AP_BATTERY_SCRIPTING_ENABLED (AP_SCRIPTING_ENABLED && AP_BATTERY_BACKEND_DEFAULT_ENABLED)
 #endif
+
+#ifndef AP_BATTERY_OPTIONS_PARAM_ENABLED
+#define AP_BATTERY_OPTIONS_PARAM_ENABLED (!defined(HAL_BUILD_AP_PERIPH) || AP_BATTERY_SUM_ENABLED)
+#endif
diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp
index a802f9e13897a5..b3af55da949d32 100644
--- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp
+++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp
@@ -281,7 +281,7 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: Board options
     // @Description: Board specific option flags
-    // @Bitmask: 0:Enable hardware watchdog, 1:Disable MAVftp, 2:Enable set of internal parameters, 3:Enable Debug Pins, 4:Unlock flash on reboot, 5:Write protect firmware flash on reboot, 6:Write protect bootloader flash on reboot, 7:Skip board validation, 8:Disable board arming gpio output change on arm/disarm
+    // @Bitmask: 0:Enable hardware watchdog, 1:Disable MAVftp, 2:Enable set of internal parameters, 3:Enable Debug Pins, 4:Unlock flash on reboot, 5:Write protect firmware flash on reboot, 6:Write protect bootloader flash on reboot, 7:Skip board validation, 8:Disable board arming gpio output change on arm/disarm, 9:Use safety pins as profiled
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 19, AP_BoardConfig, _options, HAL_BRD_OPTIONS_DEFAULT),
 
diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h
index 3f6401d6249f97..1ff4e2038e7ce5 100644
--- a/libraries/AP_BoardConfig/AP_BoardConfig.h
+++ b/libraries/AP_BoardConfig/AP_BoardConfig.h
@@ -158,7 +158,8 @@ class AP_BoardConfig {
         WRITE_PROTECT_FLASH = (1<<5),
         WRITE_PROTECT_BOOTLOADER = (1<<6),
         SKIP_BOARD_VALIDATION = (1<<7),
-        DISABLE_ARMING_GPIO = (1<<8)
+        DISABLE_ARMING_GPIO = (1<<8),
+        IO_SAFETY_PINS_AS_PROFILED = (1<<9),
     };
 
     //return true if arming gpio output is disabled
@@ -200,6 +201,12 @@ class AP_BoardConfig {
         return _singleton?(_singleton->_options & ALLOW_SET_INTERNAL_PARM)!=0:false;
     }
     
+#if HAL_WITH_IO_MCU
+    static bool use_safety_as_led(void) {
+        return _singleton?(_singleton->_options & IO_SAFETY_PINS_AS_PROFILED)!=0:false;
+    }
+#endif
+
     // handle press of safety button. Return true if safety state
     // should be toggled
     bool safety_button_handle_pressed(uint8_t press_count);
diff --git a/libraries/AP_CANManager/AP_CANDriver.h b/libraries/AP_CANManager/AP_CANDriver.h
index f93e96cec0e7a8..5f1c4f74a2a76c 100644
--- a/libraries/AP_CANManager/AP_CANDriver.h
+++ b/libraries/AP_CANManager/AP_CANDriver.h
@@ -40,5 +40,5 @@ class AP_CANDriver
     virtual bool add_11bit_driver(CANSensor *sensor) { return false; }
 
     // handler for outgoing frames for auxillary drivers
-    virtual bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) { return false; }
+    virtual bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) { return false; }
 };
diff --git a/libraries/AP_CANManager/AP_CANSensor.cpp b/libraries/AP_CANManager/AP_CANSensor.cpp
index e86ef319924753..ffa9bf3cc6d9ff 100644
--- a/libraries/AP_CANManager/AP_CANSensor.cpp
+++ b/libraries/AP_CANManager/AP_CANSensor.cpp
@@ -135,7 +135,7 @@ bool CANSensor::add_interface(AP_HAL::CANIface* can_iface)
     return true;
 }
 
-bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us)
+bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us)
 {
     if (!_initialized) {
         debug_can(AP_CANManager::LOG_ERROR, "Driver not initialized for write_frame");
@@ -171,12 +171,12 @@ void CANSensor::loop()
 #endif
 
     while (true) {
-        uint64_t timeout = AP_HAL::micros64() + LOOP_INTERVAL_US;
+        uint64_t deadline_us = AP_HAL::micros64() + LOOP_INTERVAL_US;
 
         // wait to receive frame
         bool read_select = true;
         bool write_select = false;
-        bool ret = _can_iface->select(read_select, write_select, nullptr, timeout);
+        bool ret = _can_iface->select(read_select, write_select, nullptr, deadline_us);
         if (ret && read_select) {
             uint64_t time;
             AP_HAL::CANIface::CanIOFlags flags {};
diff --git a/libraries/AP_CANManager/AP_CANSensor.h b/libraries/AP_CANManager/AP_CANSensor.h
index 7afd1bbea0fa57..b5ac4ee3b0b0ff 100644
--- a/libraries/AP_CANManager/AP_CANSensor.h
+++ b/libraries/AP_CANManager/AP_CANSensor.h
@@ -43,7 +43,7 @@ class CANSensor : public AP_CANDriver {
     virtual void handle_frame(AP_HAL::CANFrame &frame) = 0;
 
     // handler for outgoing frames
-    bool write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us);
+    bool write_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us);
 
 #ifdef HAL_BUILD_AP_PERIPH
     static void set_periph(const uint8_t i, const AP_CAN::Protocol protocol, AP_HAL::CANIface* iface) {
diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp
index 27803566d0bfd2..dc08944af3d3e0 100644
--- a/libraries/AP_Common/Location.cpp
+++ b/libraries/AP_Common/Location.cpp
@@ -9,12 +9,6 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Terrain/AP_Terrain.h>
 
-/// constructors
-Location::Location()
-{
-    zero();
-}
-
 const Location definitely_zero{};
 bool Location::is_zero(void) const
 {
diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h
index 14cb3b36a25996..72558024a0a876 100644
--- a/libraries/AP_Common/Location.h
+++ b/libraries/AP_Common/Location.h
@@ -28,7 +28,7 @@ class Location
     };
 
     /// constructors
-    Location();
+    Location() { zero(); }
     Location(int32_t latitude, int32_t longitude, int32_t alt_in_cm, AltFrame frame);
     Location(const Vector3f &ekf_offset_neu, AltFrame frame);
     Location(const Vector3d &ekf_offset_neu, AltFrame frame);
diff --git a/libraries/AP_Common/MultiHeap.cpp b/libraries/AP_Common/MultiHeap.cpp
deleted file mode 100644
index 7912e9d4008cac..00000000000000
--- a/libraries/AP_Common/MultiHeap.cpp
+++ /dev/null
@@ -1,148 +0,0 @@
-/*
-  multiple heap interface, allowing for an allocator that uses
-  multiple underlying heaps to cope with multiple memory regions on
-  STM32 boards
- */
-
-#include <AP_HAL/AP_HAL.h>
-#include <AP_HAL/Util.h>
-#include <AP_Math/AP_Math.h>
-
-#include "MultiHeap.h"
-
-#ifndef HAL_BOOTLOADER_BUILD
-
-/*
-  on ChibiOS allow up to 4 heaps. On other HALs only allow a single
-  heap. This is needed as hal.util->heap_realloc() needs to have the
-  property that heap_realloc(heap, ptr, 0) must not care if ptr comes
-  from the given heap. This is true on ChibiOS, but is not true on
-  other HALs
- */
-#ifndef MAX_HEAPS
-#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
-#define MAX_HEAPS 4
-#else
-#define MAX_HEAPS 1
-#endif
-#endif
-
-extern const AP_HAL::HAL &hal;
-
-/*
-  create heaps with a total memory size, splitting over at most
-  max_heaps
- */
-bool MultiHeap::create(uint32_t total_size, uint8_t max_heaps)
-{
-    max_heaps = MIN(MAX_HEAPS, max_heaps);
-    if (heaps != nullptr) {
-        // don't allow double allocation
-        return false;
-    }
-    heaps = NEW_NOTHROW void*[max_heaps];
-    if (heaps == nullptr) {
-        return false;
-    }
-    num_heaps = max_heaps;
-    for (uint8_t i=0; i<max_heaps; i++) {
-        uint32_t alloc_size = total_size;
-        while (alloc_size > 0) {
-            heaps[i] = hal.util->allocate_heap_memory(alloc_size);
-            if (heaps[i] != nullptr) {
-                total_size -= alloc_size;
-                break;
-            }
-            alloc_size *= 0.9;
-        }
-        if (total_size == 0) {
-            break;
-        }
-    }
-    if (total_size != 0) {
-        destroy();
-        return false;
-    }
-    return true;
-}
-
-// destroy heap
-void MultiHeap::destroy(void)
-{
-    if (!available()) {
-        return;
-    }
-    for (uint8_t i=0; i<num_heaps; i++) {
-        if (heaps[i] != nullptr) {
-            free(heaps[i]);
-            heaps[i] = nullptr;
-        }
-    }
-    delete[] heaps;
-    heaps = nullptr;
-    num_heaps = 0;
-}
-
-// return true if heap is available for operations
-bool MultiHeap::available(void) const
-{
-    return heaps != nullptr && heaps[0] != nullptr;
-}
-
-/*
-  allocate memory from a heap
- */
-void *MultiHeap::allocate(uint32_t size)
-{
-    if (!available()) {
-        return nullptr;
-    }
-    for (uint8_t i=0; i<num_heaps; i++) {
-        if (heaps[i] == nullptr) {
-            break;
-        }
-        void *newptr = hal.util->heap_realloc(heaps[i], nullptr, 0, size);
-        if (newptr != nullptr) {
-            return newptr;
-        }
-    }
-    return nullptr;
-}
-
-/*
-  free memory from a heap
- */
-void MultiHeap::deallocate(void *ptr)
-{
-    if (!available()) {
-        return;
-    }
-    // NOTE! this relies on either there being a single heap or heap_realloc()
-    // not using the first argument when size is zero.
-    hal.util->heap_realloc(heaps[0], ptr, 0, 0);
-}
-
-/*
-  change size of an allocation
- */
-void *MultiHeap::change_size(void *ptr, uint32_t old_size, uint32_t new_size)
-{
-    if (new_size == 0) {
-        deallocate(ptr);
-        return nullptr;
-    }
-    if (old_size == 0 || ptr == nullptr) {
-        return allocate(new_size);
-    }
-    /*
-      we cannot know which heap this came from, so we rely on the fact
-      that on ChibiOS the underlying call doesn't use the heap
-      argument and on other HALs we only have one heap. We pass
-      through old_size and new_size so that we can validate the lua
-      old_size value
-    */
-    return hal.util->heap_realloc(heaps[0], ptr, old_size, new_size);
-}
-
-#endif // HAL_BOOTLOADER_BUILD
-
diff --git a/libraries/AP_Common/MultiHeap.h b/libraries/AP_Common/MultiHeap.h
deleted file mode 100644
index e7aaff15b65394..00000000000000
--- a/libraries/AP_Common/MultiHeap.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
-  multiple heap interface, allowing for an allocator that uses
-  multiple underlying heaps to cope with multiple memory regions on
-  STM32 boards
- */
-
-class MultiHeap {
-public:
-    /*
-      allocate/deallocate heaps
-     */
-    bool create(uint32_t total_size, uint8_t max_heaps);
-    void destroy(void);
-
-    // return true if the heap is available for operations
-    bool available(void) const;
-
-    // allocate memory within heaps
-    void *allocate(uint32_t size);
-    void deallocate(void *ptr);
-
-    // change allocated size of a pointer - this requires the old
-    // size, unlike realloc()
-    void *change_size(void *ptr, uint32_t old_size, uint32_t new_size);
-
-private:
-    uint8_t num_heaps;
-    void **heaps;
-};
diff --git a/libraries/AP_Compass/AP_Compass_DroneCAN.cpp b/libraries/AP_Compass/AP_Compass_DroneCAN.cpp
index 920a47a429d569..50008d8424cb3e 100644
--- a/libraries/AP_Compass/AP_Compass_DroneCAN.cpp
+++ b/libraries/AP_Compass/AP_Compass_DroneCAN.cpp
@@ -37,24 +37,16 @@ AP_Compass_DroneCAN::AP_Compass_DroneCAN(AP_DroneCAN* ap_dronecan, uint32_t devi
 {
 }
 
-void AP_Compass_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_Compass_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("mag_sub");
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_2, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("mag2_sub");
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_2, driver_index) != nullptr)
 #if AP_COMPASS_DRONECAN_HIRES_ENABLED
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_hires, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("mag3_sub");
-    }
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_hires, driver_index) != nullptr)
 #endif
+    ;
 }
 
 AP_Compass_Backend* AP_Compass_DroneCAN::probe(uint8_t index)
diff --git a/libraries/AP_Compass/AP_Compass_DroneCAN.h b/libraries/AP_Compass/AP_Compass_DroneCAN.h
index 7a50f41406c14d..927821e5ef1fc5 100644
--- a/libraries/AP_Compass/AP_Compass_DroneCAN.h
+++ b/libraries/AP_Compass/AP_Compass_DroneCAN.h
@@ -14,7 +14,7 @@ class AP_Compass_DroneCAN : public AP_Compass_Backend {
 
     void        read(void) override;
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
     static AP_Compass_Backend* probe(uint8_t index);
     static uint32_t get_detected_devid(uint8_t index) { return _detected_modules[index].devid; }
     static void handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg);
diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp
index a39240143fb559..0a7a81689b802a 100644
--- a/libraries/AP_DDS/AP_DDS_Client.cpp
+++ b/libraries/AP_DDS/AP_DDS_Client.cpp
@@ -26,6 +26,12 @@
 #if AP_DDS_MODE_SWITCH_SERVER_ENABLED
 #include "ardupilot_msgs/srv/ModeSwitch.h"
 #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
+#if AP_DDS_ARM_CHECK_SERVER_ENABLED
+#include "std_srvs/srv/Trigger.h"
+#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED
+#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
+#include "ardupilot_msgs/srv/Takeoff.h"
+#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
 
 #if AP_EXTERNAL_CONTROL_ENABLED
 #include "AP_DDS_ExternalControl.h"
@@ -37,6 +43,8 @@
 #include "AP_DDS_Service_Table.h"
 #include "AP_DDS_External_Odom.h"
 
+#define STRCPY(D,S) strncpy(D, S, ARRAY_SIZE(D))
+
 // Enable DDS at runtime by default
 static constexpr uint8_t ENABLED_BY_DEFAULT = 1;
 #if AP_DDS_TIME_PUB_ENABLED
@@ -60,6 +68,9 @@ static constexpr uint16_t DELAY_AIRSPEED_TOPIC_MS = AP_DDS_DELAY_AIRSPEED_TOPIC_
 #if AP_DDS_GEOPOSE_PUB_ENABLED
 static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = AP_DDS_DELAY_GEO_POSE_TOPIC_MS;
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
+#if AP_DDS_GOAL_PUB_ENABLED
+static constexpr uint16_t DELAY_GOAL_TOPIC_MS = AP_DDS_DELAY_GOAL_TOPIC_MS ;
+#endif // AP_DDS_GOAL_PUB_ENABLED
 #if AP_DDS_CLOCK_PUB_ENABLED
 static constexpr uint16_t DELAY_CLOCK_TOPIC_MS =AP_DDS_DELAY_CLOCK_TOPIC_MS;
 #endif // AP_DDS_CLOCK_PUB_ENABLED
@@ -289,8 +300,8 @@ void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg)
         char gps_frame_id[16];
         //! @todo should GPS frame ID's be 0 or 1 indexed in ROS?
         hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i);
-        strcpy(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID);
-        strcpy(msg.transforms[i].child_frame_id, gps_frame_id);
+        STRCPY(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID);
+        STRCPY(msg.transforms[i].child_frame_id, gps_frame_id);
         // The body-frame offsets
         // X - Forward
         // Y - Right
@@ -392,7 +403,7 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_
 void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
 
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
@@ -443,7 +454,7 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg)
 void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
 
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
@@ -486,7 +497,7 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg)
 bool AP_DDS_Client::update_topic(geometry_msgs_msg_Vector3Stamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
     // In ROS REP 103, axis orientation uses the following convention:
@@ -515,7 +526,7 @@ bool AP_DDS_Client::update_topic(geometry_msgs_msg_Vector3Stamped& msg)
 void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
 
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
@@ -552,11 +563,44 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
 }
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
 
+#if AP_DDS_GOAL_PUB_ENABLED
+bool AP_DDS_Client::update_topic_goal(geographic_msgs_msg_GeoPointStamped& msg)
+{
+    const auto &vehicle = AP::vehicle();
+    update_topic(msg.header.stamp);
+    Location target_loc;
+    // Exit if no target is available.
+    if (!vehicle->get_target_location(target_loc)) {
+        return false;
+    }
+    target_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
+    msg.position.latitude = target_loc.lat * 1e-7;
+    msg.position.longitude = target_loc.lng * 1e-7;
+    msg.position.altitude = target_loc.alt * 1e-2;
+
+    // Check whether the goal has changed or if the topic has never been published.
+    const double tolerance_lat_lon = 1e-8; // One order of magnitude smaller than the target's resolution.
+    const double distance_alt = 1e-3;
+    if (abs(msg.position.latitude - prev_goal_msg.position.latitude) >  tolerance_lat_lon ||
+        abs(msg.position.longitude - prev_goal_msg.position.longitude) >  tolerance_lat_lon ||
+        abs(msg.position.altitude - prev_goal_msg.position.altitude) > distance_alt ||
+        prev_goal_msg.header.stamp.sec == 0 ) {
+        update_topic(prev_goal_msg.header.stamp);
+        prev_goal_msg.position.latitude = msg.position.latitude;
+        prev_goal_msg.position.longitude = msg.position.longitude;
+        prev_goal_msg.position.altitude = msg.position.altitude;
+        return true;
+    } else {
+        return false;
+    }
+}
+#endif // AP_DDS_GOAL_PUB_ENABLED
+
 #if AP_DDS_IMU_PUB_ENABLED
 void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_NED_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_NED_FRAME_ID);
 
     auto &imu = AP::ins();
     auto &ahrs = AP::ahrs();
@@ -603,7 +647,7 @@ void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg)
 void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
 
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
@@ -817,7 +861,13 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
         }
 
         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Request for %sing received", msg_prefix, arm_motors_request.arm ? "arm" : "disarm");
-        arm_motors_response.result = arm_motors_request.arm ? AP::arming().arm(AP_Arming::Method::DDS) : AP::arming().disarm(AP_Arming::Method::DDS);
+#if AP_EXTERNAL_CONTROL_ENABLED
+        const bool do_checks = true;
+        arm_motors_response.result = arm_motors_request.arm ? AP_DDS_External_Control::arm(AP_Arming::Method::DDS, do_checks) : AP_DDS_External_Control::disarm(AP_Arming::Method::DDS, do_checks);
+        if (!arm_motors_response.result) {
+            // TODO #23430 handle arm failure through rosout, throttled.
+        }
+#endif // AP_EXTERNAL_CONTROL_ENABLED
 
         const uxrObjectId replier_id = {
             .id = services[to_underlying(ServiceIndex::ARMING_MOTORS)].rep_id,
@@ -868,6 +918,64 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
         break;
     }
 #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
+#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
+    case services[to_underlying(ServiceIndex::TAKEOFF)].rep_id: {
+        ardupilot_msgs_srv_Takeoff_Request takeoff_request;
+        ardupilot_msgs_srv_Takeoff_Response takeoff_response;
+        const bool deserialize_success = ardupilot_msgs_srv_Takeoff_Request_deserialize_topic(ub, &takeoff_request);
+        if (deserialize_success == false) {
+            break;
+        }
+        takeoff_response.status = AP::vehicle()->start_takeoff(takeoff_request.alt);
+
+        const uxrObjectId replier_id = {
+            .id = services[to_underlying(ServiceIndex::TAKEOFF)].rep_id,
+            .type = UXR_REPLIER_ID
+        };
+
+        uint8_t reply_buffer[8] {};
+        ucdrBuffer reply_ub;
+
+        ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
+        const bool serialize_success = ardupilot_msgs_srv_Takeoff_Response_serialize_topic(&reply_ub, &takeoff_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(MAV_SEVERITY_INFO, "%s Request for Takeoff : %s", msg_prefix, takeoff_response.status ? "SUCCESS" : "FAIL");
+        break;
+    }
+#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
+#if AP_DDS_ARM_CHECK_SERVER_ENABLED
+    case services[to_underlying(ServiceIndex::PREARM_CHECK)].rep_id: {
+        std_srvs_srv_Trigger_Request prearm_check_request;
+        std_srvs_srv_Trigger_Response prearm_check_response;
+        const bool deserialize_success = std_srvs_srv_Trigger_Request_deserialize_topic(ub, &prearm_check_request);
+        if (deserialize_success == false) {
+            break;
+        }
+        prearm_check_response.success = AP::arming().pre_arm_checks(false);
+        STRCPY(prearm_check_response.message, prearm_check_response.success ? "Vehicle is Armable" : "Vehicle is Not Armable");
+
+        const uxrObjectId replier_id = {
+            .id = services[to_underlying(ServiceIndex::PREARM_CHECK)].rep_id,
+            .type = UXR_REPLIER_ID
+        };
+
+        uint8_t reply_buffer[sizeof(prearm_check_response.message) + 1] {};
+        ucdrBuffer reply_ub;
+
+        ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
+        const bool serialize_success = std_srvs_srv_Trigger_Response_serialize_topic(&reply_ub, &prearm_check_response);
+        if (serialize_success == false) {
+            break;
+        }
+
+        uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
+        break;
+    }
+#endif //AP_DDS_ARM_CHECK_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);
@@ -1516,6 +1624,22 @@ void AP_DDS_Client::write_gps_global_origin_topic()
 }
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
 
+#if AP_DDS_GOAL_PUB_ENABLED
+void AP_DDS_Client::write_goal_topic()
+{
+    WITH_SEMAPHORE(csem);
+    if (connected) {
+        ucdrBuffer ub {};
+        const uint32_t topic_size = geographic_msgs_msg_GeoPointStamped_size_of_topic(&goal_topic, 0);
+        uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GOAL_PUB)].dw_id, &ub, topic_size);
+        const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &goal_topic);
+        if (!success) {
+            // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
+        }
+    }
+}
+#endif // AP_DDS_GOAL_PUB_ENABLED
+
 #if AP_DDS_STATUS_PUB_ENABLED
 void AP_DDS_Client::write_status_topic()
 {
@@ -1612,6 +1736,14 @@ void AP_DDS_Client::update()
         write_gps_global_origin_topic();
     }
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
+#if AP_DDS_GOAL_PUB_ENABLED
+    if (cur_time_ms - last_goal_time_ms > DELAY_GOAL_TOPIC_MS) {
+        if (update_topic_goal(goal_topic)) {
+            write_goal_topic();
+        }
+        last_goal_time_ms = cur_time_ms;
+    }
+#endif // AP_DDS_GOAL_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)) {
diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h
index 8453577ec1edde..d5581c4ed57b2b 100644
--- a/libraries/AP_DDS/AP_DDS_Client.h
+++ b/libraries/AP_DDS/AP_DDS_Client.h
@@ -114,6 +114,16 @@ class AP_DDS_Client
     static void update_topic(geographic_msgs_msg_GeoPointStamped& msg);
 # endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
 
+#if AP_DDS_GOAL_PUB_ENABLED
+    geographic_msgs_msg_GeoPointStamped goal_topic;
+    // The last ms timestamp AP_DDS wrote a goal message
+    uint64_t last_goal_time_ms;
+    //! @brief Serialize the current goal and publish to the IO stream(s)
+    void write_goal_topic();
+    bool update_topic_goal(geographic_msgs_msg_GeoPointStamped& msg);
+    geographic_msgs_msg_GeoPointStamped prev_goal_msg;
+# endif // AP_DDS_GOAL_PUB_ENABLED
+
 #if AP_DDS_GEOPOSE_PUB_ENABLED
     geographic_msgs_msg_GeoPoseStamped geo_pose_topic;
     // The last ms timestamp AP_DDS wrote a GeoPose message
diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp
index 490ce519defe90..259dcb51601c9a 100644
--- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp
+++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp
@@ -85,6 +85,26 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta
     return false;
 }
 
+bool AP_DDS_External_Control::arm(AP_Arming::Method method, bool do_arming_checks)
+{
+    auto *external_control = AP::externalcontrol();
+    if (external_control == nullptr) {
+        return false;
+    }
+
+    return external_control->arm(method, do_arming_checks);
+}
+
+bool AP_DDS_External_Control::disarm(AP_Arming::Method method, bool do_disarm_checks)
+{
+    auto *external_control = AP::externalcontrol();
+    if (external_control == nullptr) {
+        return false;
+    }
+
+    return external_control->disarm(method, do_disarm_checks);
+}
+
 bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in,  Location::AltFrame& frame_out)
 {
 
diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.h b/libraries/AP_DDS/AP_DDS_ExternalControl.h
index 3986ef63774aa6..eeb86e5e9c1325 100644
--- a/libraries/AP_DDS/AP_DDS_ExternalControl.h
+++ b/libraries/AP_DDS/AP_DDS_ExternalControl.h
@@ -3,7 +3,7 @@
 #if AP_DDS_ENABLED
 #include "ardupilot_msgs/msg/GlobalPosition.h"
 #include "geometry_msgs/msg/TwistStamped.h"
-
+#include <AP_Arming/AP_Arming.h>
 #include <AP_Common/Location.h>
 
 class AP_DDS_External_Control
@@ -13,6 +13,9 @@ class AP_DDS_External_Control
     // https://ros.org/reps/rep-0147.html#goal-interface
     static bool handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos);
     static bool handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel);
+    static bool arm(AP_Arming::Method method, bool do_arming_checks);
+    static bool disarm(AP_Arming::Method method, bool do_disarm_checks);
+
 private:
     static bool convert_alt_frame(const uint8_t frame_in,  Location::AltFrame& frame_out);
 };
diff --git a/libraries/AP_DDS/AP_DDS_Service_Table.h b/libraries/AP_DDS/AP_DDS_Service_Table.h
index 667149d6aa7566..7f04ab324845d3 100644
--- a/libraries/AP_DDS/AP_DDS_Service_Table.h
+++ b/libraries/AP_DDS/AP_DDS_Service_Table.h
@@ -8,6 +8,12 @@ enum class ServiceIndex: uint8_t {
 #if AP_DDS_MODE_SWITCH_SERVER_ENABLED
     MODE_SWITCH,
 #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
+#if AP_DDS_ARM_CHECK_SERVER_ENABLED
+    PREARM_CHECK,
+#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED
+#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
+    TAKEOFF,
+#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
 #if AP_DDS_PARAMETER_SERVER_ENABLED
     SET_PARAMETERS,
     GET_PARAMETERS
@@ -51,6 +57,30 @@ 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_ARM_CHECK_SERVER_ENABLED
+    {
+        .req_id = to_underlying(ServiceIndex::PREARM_CHECK),
+        .rep_id = to_underlying(ServiceIndex::PREARM_CHECK),
+        .service_rr = Service_rr::Replier,
+        .service_name = "rs/ap/prearm_checkService",
+        .request_type = "std_srvs::srv::dds_::Trigger_Request_",
+        .reply_type = "std_srvs::srv::dds_::Trigger_Response_",
+        .request_topic_name = "rq/ap/prearm_checkRequest",
+        .reply_topic_name = "rr/ap/prearm_checkReply",
+    },
+#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED    
+#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
+    {
+        .req_id = to_underlying(ServiceIndex::TAKEOFF),
+        .rep_id = to_underlying(ServiceIndex::TAKEOFF),
+        .service_rr = Service_rr::Replier,
+        .service_name = "rs/ap/experimental/takeoffService",
+        .request_type = "ardupilot_msgs::srv::dds_::Takeoff_Request_",
+        .reply_type = "ardupilot_msgs::srv::dds_::Takeoff_Response_",
+        .request_topic_name = "rq/ap/experimental/takeoffRequest",
+        .reply_topic_name = "rr/ap/experimental/takeoffReply",
+    },
+#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
 #if AP_DDS_PARAMETER_SERVER_ENABLED
     {
         .req_id = to_underlying(ServiceIndex::SET_PARAMETERS),
diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h
index b9bde199dc0505..5212a42c902c5f 100644
--- a/libraries/AP_DDS/AP_DDS_Topic_Table.h
+++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h
@@ -42,6 +42,9 @@ enum class TopicIndex: uint8_t {
 #if AP_DDS_GEOPOSE_PUB_ENABLED
     GEOPOSE_PUB,
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
+#ifdef AP_DDS_GOAL_PUB_ENABLED
+    GOAL_PUB,
+#endif // AP_DDS_GOAL_PUB_ENABLED
 #if AP_DDS_CLOCK_PUB_ENABLED
     CLOCK_PUB,
 #endif // AP_DDS_CLOCK_PUB_ENABLED
@@ -235,6 +238,24 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         },
     },
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
+#if AP_DDS_GOAL_PUB_ENABLED
+    {
+        .topic_id = to_underlying(TopicIndex::GOAL_PUB),
+        .pub_id = to_underlying(TopicIndex::GOAL_PUB),
+        .sub_id = to_underlying(TopicIndex::GOAL_PUB),
+        .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GOAL_PUB), .type=UXR_DATAWRITER_ID},
+        .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GOAL_PUB), .type=UXR_DATAREADER_ID},
+        .topic_rw = Topic_rw::DataWriter,
+        .topic_name = "rt/ap/goal_lla",
+        .type_name = "geographic_msgs::msg::dds_::GeoPointStamped_",
+        .qos = {
+            .durability = UXR_DURABILITY_TRANSIENT_LOCAL,
+            .reliability = UXR_RELIABILITY_RELIABLE,
+            .history = UXR_HISTORY_KEEP_LAST,
+            .depth = 1,
+        },
+    },
+#endif // AP_DDS_GOAL_PUB_ENABLED
 #if AP_DDS_CLOCK_PUB_ENABLED
     {
         .topic_id = to_underlying(TopicIndex::CLOCK_PUB),
diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h
index 5785ca23b12127..022bce3d39dc69 100644
--- a/libraries/AP_DDS/AP_DDS_config.h
+++ b/libraries/AP_DDS/AP_DDS_config.h
@@ -106,6 +106,13 @@
 #define AP_DDS_DELAY_CLOCK_TOPIC_MS 10
 #endif
 
+#ifndef AP_DDS_GOAL_PUB_ENABLED
+#define AP_DDS_GOAL_PUB_ENABLED 1
+#endif
+
+#ifndef AP_DDS_DELAY_GOAL_TOPIC_MS
+#define AP_DDS_DELAY_GOAL_TOPIC_MS  200
+#endif
 #ifndef AP_DDS_STATUS_PUB_ENABLED
 #define AP_DDS_STATUS_PUB_ENABLED 1
 #endif
@@ -134,10 +141,18 @@
 #define AP_DDS_MODE_SWITCH_SERVER_ENABLED 1
 #endif
 
+#ifndef AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
+#define AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED 1
+#endif
+
 #ifndef AP_DDS_PARAMETER_SERVER_ENABLED
 #define AP_DDS_PARAMETER_SERVER_ENABLED 1
 #endif
 
+#ifndef AP_DDS_ARM_CHECK_SERVER_ENABLED
+#define AP_DDS_ARM_CHECK_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
 
diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl
new file mode 100644
index 00000000000000..2e16f5cd627145
--- /dev/null
+++ b/libraries/AP_DDS/Idl/ardupilot_msgs/srv/Takeoff.idl
@@ -0,0 +1,20 @@
+// generated from rosidl_adapter/resource/srv.idl.em
+// with input from ardupilot_msgs/srv/Takeoff.srv
+// generated code does not contain a copyright notice
+
+
+module ardupilot_msgs {
+  module srv {
+    struct Takeoff_Request {
+      @verbatim (language="comment", text=
+        "This service requests the vehicle to takeoff" "\n"
+        "float : Set the takeoff altitude [m] above home, or above terrain if rangefinder is healthy")
+      float alt;
+    };
+    @verbatim (language="comment", text=
+      "status    : True if the request for takeoff was successful, False otherwise")
+    struct Takeoff_Response {
+      boolean status;
+    };
+  };
+};
diff --git a/libraries/AP_DDS/Idl/std_srvs/srv/Trigger.idl b/libraries/AP_DDS/Idl/std_srvs/srv/Trigger.idl
new file mode 100644
index 00000000000000..6c763b5e7aab0a
--- /dev/null
+++ b/libraries/AP_DDS/Idl/std_srvs/srv/Trigger.idl
@@ -0,0 +1,21 @@
+// generated from rosidl_adapter/resource/srv.idl.em
+// with input from std_srvs/srv/Trigger.srv
+// generated code does not contain a copyright notice
+
+
+module std_srvs {
+  module srv {
+    struct Trigger_Request {
+      uint8 structure_needs_at_least_one_member;
+    };
+    struct Trigger_Response {
+      @verbatim (language="comment", text=
+        "indicate successful run of triggered service")
+      boolean success;
+
+      @verbatim (language="comment", text=
+        "informational, e.g. for error messages")
+      string message;
+    };
+  };
+};
diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md
index 03a1aa92938bf5..ad323ba17392fd 100644
--- a/libraries/AP_DDS/README.md
+++ b/libraries/AP_DDS/README.md
@@ -209,6 +209,8 @@ nanosec: 729410000
 $ ros2 service list
 /ap/arm_motors
 /ap/mode_switch
+/ap/prearm_check
+/ap/experimental/takeoff
 ---
 ```
 
@@ -234,6 +236,8 @@ List the available services:
 $ ros2 service list -t
 /ap/arm_motors [ardupilot_msgs/srv/ArmMotors]
 /ap/mode_switch [ardupilot_msgs/srv/ModeSwitch]
+/ap/prearm_check [std_srvs/srv/Trigger]
+/ap/experimental/takeoff [ardupilot_msgs/srv/Takeoff]
 ```
 
 Call the arm motors service:
@@ -256,6 +260,30 @@ response:
 ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4)
 ```
 
+Call the prearm check service:
+
+```bash
+$ ros2 service call /ap/prearm_check std_srvs/srv/Trigger
+requester: making request: std_srvs.srv.Trigger_Request()
+
+response:
+std_srvs.srv.Trigger_Response(success=False, message='Vehicle is Not Armable')
+
+or
+
+std_srvs.srv.Trigger_Response(success=True, message='Vehicle is Armable')
+```
+
+Call the takeoff service:
+
+```bash
+$ ros2 service call /ap/experimental/takeoff ardupilot_msgs/srv/Takeoff "{alt: 10.5}"
+requester: making request: ardupilot_msgs.srv.Takeoff_Request(alt=10.5)
+
+response:
+ardupilot_msgs.srv.Takeoff_Response(status=True)
+```
+
 ## Commanding using ROS 2 Topics
 
 The following topic can be used to control the vehicle.
@@ -280,7 +308,7 @@ ros2 topic pub /ap/cmd_gps_pose ardupilot_msgs/msg/GlobalPosition "{latitude: 34
 publisher: beginning loop
 publishing #1: ardupilot_msgs.msg.GlobalPosition(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), coordinate_frame=0, type_mask=0, latitude=34.0, longitude=118.0, altitude=1000.0, velocity=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)), acceleration_or_force=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)), yaw=0.0)
 ```
-
+ 
 ## Contributing to `AP_DDS` library
 
 ### Adding DDS messages to Ardupilot
diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp
index 7382f57033c9de..fd38e303c42535 100644
--- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp
+++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp
@@ -456,7 +456,7 @@ bool CanardInterface::add_11bit_driver(CANSensor *sensor)
 }
 
 // handler for outgoing frames for auxillary drivers
-bool CanardInterface::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us)
+bool CanardInterface::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us)
 {
     const uint64_t tx_deadline_us = AP_HAL::micros64() + timeout_us;
     bool ret = false;
diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.h b/libraries/AP_DroneCAN/AP_Canard_iface.h
index 43d786639fdf05..671722028dffc7 100644
--- a/libraries/AP_DroneCAN/AP_Canard_iface.h
+++ b/libraries/AP_DroneCAN/AP_Canard_iface.h
@@ -54,7 +54,7 @@ class CanardInterface : public Canard::Interface {
     bool add_11bit_driver(CANSensor *sensor);
 
     // handler for outgoing frames for auxillary drivers
-    bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us);
+    bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us);
     
 #if AP_TEST_DRONECAN_DRIVERS
     static CanardInterface& get_test_iface() { return test_iface; }
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
index 21243e5d08c40f..a8c9a531c33e43 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp
+++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
@@ -368,45 +368,50 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters)
     }
 
     // Roundup all subscribers from supported drivers
+    bool subscribed = true;
 #if AP_GPS_DRONECAN_ENABLED
-    AP_GPS_DroneCAN::subscribe_msgs(this);
+    subscribed = subscribed && AP_GPS_DroneCAN::subscribe_msgs(this);
 #endif
 #if AP_COMPASS_DRONECAN_ENABLED
-    AP_Compass_DroneCAN::subscribe_msgs(this);
+    subscribed = subscribed && AP_Compass_DroneCAN::subscribe_msgs(this);
 #endif
 #if AP_BARO_DRONECAN_ENABLED
-    AP_Baro_DroneCAN::subscribe_msgs(this);
+    subscribed = subscribed && AP_Baro_DroneCAN::subscribe_msgs(this);
 #endif
-    AP_BattMonitor_DroneCAN::subscribe_msgs(this);
+    subscribed = subscribed && AP_BattMonitor_DroneCAN::subscribe_msgs(this);
 #if AP_AIRSPEED_DRONECAN_ENABLED
-    AP_Airspeed_DroneCAN::subscribe_msgs(this);
+    subscribed = subscribed && AP_Airspeed_DroneCAN::subscribe_msgs(this);
 #endif
 #if AP_OPTICALFLOW_HEREFLOW_ENABLED
-    AP_OpticalFlow_HereFlow::subscribe_msgs(this);
+    subscribed = subscribed && AP_OpticalFlow_HereFlow::subscribe_msgs(this);
 #endif
 #if AP_RANGEFINDER_DRONECAN_ENABLED
-    AP_RangeFinder_DroneCAN::subscribe_msgs(this);
+    subscribed = subscribed && AP_RangeFinder_DroneCAN::subscribe_msgs(this);
 #endif
 #if AP_RCPROTOCOL_DRONECAN_ENABLED
-    AP_RCProtocol_DroneCAN::subscribe_msgs(this);
+    subscribed = subscribed && AP_RCProtocol_DroneCAN::subscribe_msgs(this);
 #endif
 #if AP_EFI_DRONECAN_ENABLED
-    AP_EFI_DroneCAN::subscribe_msgs(this);
+    subscribed = subscribed && AP_EFI_DroneCAN::subscribe_msgs(this);
 #endif
 
 #if AP_PROXIMITY_DRONECAN_ENABLED
-    AP_Proximity_DroneCAN::subscribe_msgs(this);
+    subscribed = subscribed && AP_Proximity_DroneCAN::subscribe_msgs(this);
 #endif
 #if HAL_MOUNT_XACTI_ENABLED
-    AP_Mount_Xacti::subscribe_msgs(this);
+    subscribed = subscribed && AP_Mount_Xacti::subscribe_msgs(this);
 #endif
 #if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED
-    AP_TemperatureSensor_DroneCAN::subscribe_msgs(this);
+    subscribed = subscribed && AP_TemperatureSensor_DroneCAN::subscribe_msgs(this);
 #endif
 #if AP_RPM_DRONECAN_ENABLED
-    AP_RPM_DroneCAN::subscribe_msgs(this);
+    subscribed = subscribed && AP_RPM_DroneCAN::subscribe_msgs(this);
 #endif
 
+    if (!subscribed) {
+        AP_BoardConfig::allocation_error("DroneCAN callback");
+    }
+
     act_out_array.set_timeout_ms(5);
     act_out_array.set_priority(CANARD_TRANSFER_PRIORITY_HIGH);
 
@@ -1956,7 +1961,7 @@ bool AP_DroneCAN::add_11bit_driver(CANSensor *sensor)
 }
 
 // handler for outgoing frames for auxillary drivers
-bool AP_DroneCAN::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us)
+bool AP_DroneCAN::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us)
 {
     if (out_frame.isExtended()) {
         // don't allow extended frames to be sent by auxillary driver
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h
index 2d6e5ae2b7f78a..9a7ae54c6639c3 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN.h
+++ b/libraries/AP_DroneCAN/AP_DroneCAN.h
@@ -95,7 +95,7 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     bool add_11bit_driver(CANSensor *sensor) override;
 
     // handler for outgoing frames for auxillary drivers
-    bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) override;
+    bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us) override;
     
     uint8_t get_driver_index() const { return _driver_index; }
 
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
index e08d3486c0cdc9..d68f6a4c67dc62 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
+++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
@@ -150,10 +150,6 @@ class AP_DroneCAN_DNA_Server
     //Initialises publisher and Server Record for specified uavcan driver
     bool init(uint8_t own_unique_id[], uint8_t own_unique_id_len, uint8_t node_id);
 
-    /* Subscribe to the messages to be handled for maintaining and allocating
-    Node ID list */
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
-
     //report the server state, along with failure message if any
     bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const;
 
diff --git a/libraries/AP_EFI/AP_EFI_DroneCAN.cpp b/libraries/AP_EFI/AP_EFI_DroneCAN.cpp
index 90f33cac5da8e7..736196f63be86e 100644
--- a/libraries/AP_EFI/AP_EFI_DroneCAN.cpp
+++ b/libraries/AP_EFI/AP_EFI_DroneCAN.cpp
@@ -21,15 +21,11 @@ AP_EFI_DroneCAN::AP_EFI_DroneCAN(AP_EFI &_frontend) :
 }
 
 // links the DroneCAN message to this backend
-void AP_EFI_DroneCAN::subscribe_msgs(AP_DroneCAN *ap_dronecan)
+bool AP_EFI_DroneCAN::subscribe_msgs(AP_DroneCAN *ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &trampoline_status, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("status_sub");
-    }
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &trampoline_status, driver_index) != nullptr);
 }
 
 // Called from frontend to update with the readings received by handler
diff --git a/libraries/AP_EFI/AP_EFI_DroneCAN.h b/libraries/AP_EFI/AP_EFI_DroneCAN.h
index 0512d8ea1f7ca3..2d067554acbf59 100644
--- a/libraries/AP_EFI/AP_EFI_DroneCAN.h
+++ b/libraries/AP_EFI/AP_EFI_DroneCAN.h
@@ -13,7 +13,7 @@ class AP_EFI_DroneCAN : public AP_EFI_Backend {
 
     void update() override;
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
     static void trampoline_status(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ice_reciprocating_Status &msg);
 
 private:
diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.cpp b/libraries/AP_ExternalControl/AP_ExternalControl.cpp
index e0e92f0a299113..92a97f18b75727 100644
--- a/libraries/AP_ExternalControl/AP_ExternalControl.cpp
+++ b/libraries/AP_ExternalControl/AP_ExternalControl.cpp
@@ -5,6 +5,16 @@
 // singleton instance
 AP_ExternalControl *AP_ExternalControl::singleton;
 
+bool AP_ExternalControl::arm(AP_Arming::Method method, bool do_arming_checks)
+{
+    return AP::arming().arm(method, do_arming_checks);
+}
+
+bool AP_ExternalControl::disarm(AP_Arming::Method method, bool do_disarm_checks)
+{
+    return AP::arming().disarm(method, do_disarm_checks);
+}
+
 AP_ExternalControl::AP_ExternalControl()
 {
     singleton = this;
diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.h b/libraries/AP_ExternalControl/AP_ExternalControl.h
index 34228e2b7ab4ff..57670008f48968 100644
--- a/libraries/AP_ExternalControl/AP_ExternalControl.h
+++ b/libraries/AP_ExternalControl/AP_ExternalControl.h
@@ -8,6 +8,7 @@
 
 #if AP_EXTERNAL_CONTROL_ENABLED
 
+#include <AP_Arming/AP_Arming.h>
 #include <AP_Common/Location.h>
 #include <AP_Math/AP_Math.h>
 
@@ -32,6 +33,16 @@ class AP_ExternalControl
         return false;
     }
 
+    /*
+        Arm the vehicle
+    */
+    virtual bool arm(AP_Arming::Method method, bool do_arming_checks) WARN_IF_UNUSED;
+
+    /*
+        Disarm the vehicle
+    */
+    virtual bool disarm(AP_Arming::Method method, bool do_disarm_checks) WARN_IF_UNUSED;
+
     static AP_ExternalControl *get_singleton(void) WARN_IF_UNUSED {
         return singleton;
     }
diff --git a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp
index 28c1ea1b0eb2a2..6754170703a763 100644
--- a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp
+++ b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp
@@ -30,7 +30,11 @@
 #include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
+
+// QURT HAL already has a declaration of errno in errno.h
+#if CONFIG_HAL_BOARD != HAL_BOARD_QURT
 extern int errno;
+#endif
 
 #define IDLE_TIMEOUT_MS 30000
 
diff --git a/libraries/AP_Filesystem/AP_Filesystem_Param.cpp b/libraries/AP_Filesystem/AP_Filesystem_Param.cpp
index 31a53acc0d3014..d79f2cd7b3a09a 100644
--- a/libraries/AP_Filesystem/AP_Filesystem_Param.cpp
+++ b/libraries/AP_Filesystem/AP_Filesystem_Param.cpp
@@ -29,7 +29,11 @@
 #define PACKED_NAME "param.pck"
 
 extern const AP_HAL::HAL& hal;
+
+// QURT HAL already has a declaration of errno in errno.h
+#if CONFIG_HAL_BOARD != HAL_BOARD_QURT
 extern int errno;
+#endif
 
 int AP_Filesystem_Param::open(const char *fname, int flags, bool allow_absolute_path)
 {
diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
index 26212761307b40..c6e69d626c3e29 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
+++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
@@ -77,7 +77,7 @@ for FrSky SPort Passthrough
 #define WIND_ANGLE_LIMIT            0x7F
 #define WIND_SPEED_OFFSET           7
 #define WIND_APPARENT_ANGLE_OFFSET  15
-#define WIND_APPARENT_SPEED_OFFSET  23
+#define WIND_APPARENT_SPEED_OFFSET  22
 // for waypoint data
 #define WP_NUMBER_LIMIT             2047
 #define WP_DISTANCE_LIMIT           1023000
@@ -781,7 +781,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_wind(void)
         // true wind speed in dm/s
         value |= prep_number(roundf(windvane->get_true_wind_speed() * 10), 2, 1) << WIND_SPEED_OFFSET;
         // apparent wind angle in 3 degree increments -180,180 (signed)
-        value |= prep_number(roundf(degrees(windvane->get_apparent_wind_direction_rad()) * (1.0f/3.0f)), 2, 0);
+        value |= prep_number(roundf(degrees(windvane->get_apparent_wind_direction_rad()) * (1.0f/3.0f)), 2, 0) << WIND_APPARENT_ANGLE_OFFSET;
         // apparent wind speed in dm/s
         value |= prep_number(roundf(windvane->get_apparent_wind_speed() * 10), 2, 1) << WIND_APPARENT_SPEED_OFFSET;
     }
diff --git a/libraries/AP_GPS/AP_GPS_Blended.h b/libraries/AP_GPS/AP_GPS_Blended.h
index b68eb0f9660ed7..77fe25ffb88844 100644
--- a/libraries/AP_GPS/AP_GPS_Blended.h
+++ b/libraries/AP_GPS/AP_GPS_Blended.h
@@ -64,7 +64,6 @@ class AP_GPS_Blended : public AP_GPS_Backend
     Vector3f _blended_antenna_offset; // blended antenna offset
     float _blended_lag_sec; // blended receiver lag in seconds
     float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
-    bool _output_is_blended; // true when a blended GPS solution being output
     uint8_t _blend_health_counter;  // 0 = perfectly health, 100 = very unhealthy
 
     AP_GPS::GPS_timing &timing;
diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp
index 8bd8cb7e373d9d..aeec0442289af3 100644
--- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp
+++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp
@@ -89,36 +89,19 @@ AP_GPS_DroneCAN::~AP_GPS_DroneCAN()
 #endif
 }
 
-void AP_GPS_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_GPS_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_fix2_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("status_sub");
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_aux_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("status_sub");
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_heading_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("status_sub");
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_status_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("status_sub");
-    }
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_fix2_msg_trampoline, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_aux_msg_trampoline, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_heading_msg_trampoline, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_status_msg_trampoline, driver_index) != nullptr)
 #if GPS_MOVING_BASELINE
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_moving_baseline_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("moving_baseline_sub");
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_relposheading_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("relposheading_sub");
-    }
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_moving_baseline_msg_trampoline, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_relposheading_msg_trampoline, driver_index) != nullptr)
 #endif
+    ;
 }
 
 AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.h b/libraries/AP_GPS/AP_GPS_DroneCAN.h
index 1de0b6b8f40bc0..a0b62c8e5b3d71 100644
--- a/libraries/AP_GPS/AP_GPS_DroneCAN.h
+++ b/libraries/AP_GPS/AP_GPS_DroneCAN.h
@@ -44,7 +44,7 @@ class AP_GPS_DroneCAN : public AP_GPS_Backend {
 
     const char *name() const override { return _name; }
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
     static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state);
 
     static void handle_fix2_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Fix2& msg);
diff --git a/libraries/AP_GSOF/AP_GSOF.h b/libraries/AP_GSOF/AP_GSOF.h
index 5fb78fd4d4234f..1aeda025643879 100644
--- a/libraries/AP_GSOF/AP_GSOF.h
+++ b/libraries/AP_GSOF/AP_GSOF.h
@@ -127,8 +127,5 @@ class AP_GSOF
 
     static const uint8_t STX = 0x02;
     static const uint8_t ETX = 0x03;
-
-    uint8_t packetcount;
-    uint32_t gsofmsg_time;
 };
 #endif // AP_GSOF_ENABLED
diff --git a/libraries/AP_HAL/Util.cpp b/libraries/AP_HAL/Util.cpp
index 374fc77c48c675..7b69c58d008a7e 100644
--- a/libraries/AP_HAL/Util.cpp
+++ b/libraries/AP_HAL/Util.cpp
@@ -9,6 +9,8 @@
 #else
 #include <time.h>
 #endif
+#include <AP_InternalError/AP_InternalError.h>
+#include <AP_Math/AP_Math.h>
 
 /* Helper class implements AP_HAL::Print so we can use utility/vprintf */
 class BufferPrinter : public AP_HAL::BetterStream {
diff --git a/libraries/AP_HAL/Util.h b/libraries/AP_HAL/Util.h
index 4f39702bd5191f..a4eca9ce2633a3 100644
--- a/libraries/AP_HAL/Util.h
+++ b/libraries/AP_HAL/Util.h
@@ -149,16 +149,15 @@ class AP_HAL::Util {
     virtual void free_type(void *ptr, size_t size, Memory_Type mem_type) { return free(ptr); }
 
 #if ENABLE_HEAP
-    // heap functions, note that a heap once alloc'd cannot be dealloc'd
-    virtual void *allocate_heap_memory(size_t size) = 0;
-    virtual void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) = 0;
+    /*
+      heap functions used by non-scripting
+     */
 #if USE_LIBC_REALLOC
-    virtual void *std_realloc(void *ptr, size_t new_size) { return realloc(ptr, new_size); }
+    virtual void *std_realloc(void *ptr, uint32_t new_size) { return realloc(ptr, new_size); }
 #else
-    virtual void *std_realloc(void *ptr, size_t new_size) = 0;
+    virtual void *std_realloc(void *ptr, uint32_t new_size) = 0;
 #endif // USE_LIBC_REALLOC
-#endif // ENABLE_HEAP
-
+#endif
 
     /**
        how much free memory do we have in bytes. If unknown return 4096
diff --git a/libraries/AP_HAL/board/esp32.h b/libraries/AP_HAL/board/esp32.h
index 1969a0066f67b1..e927165b8cd25b 100644
--- a/libraries/AP_HAL/board/esp32.h
+++ b/libraries/AP_HAL/board/esp32.h
@@ -52,6 +52,10 @@
 #define __LITTLE_ENDIAN  1234
 #define __BYTE_ORDER     __LITTLE_ENDIAN
 
+// ArduPilot uses __RAMFUNC__ to place functions in fast instruction RAM
+#define __RAMFUNC__ IRAM_ATTR
+
+
 // whenver u get ... error: "xxxxxxx" is not defined, evaluates to 0 [-Werror=undef]  just define it below as 0
 #define CONFIG_SPIRAM_ALLOW_BSS_SEG_EXTERNAL_MEMORY 0
 #define XCHAL_ERRATUM_453 0
diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp
index 13a06048b129c8..b388e5cf77eff8 100644
--- a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp
+++ b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp
@@ -894,7 +894,7 @@ void AnalogIn::update_power_flags(void)
 #endif
 
     if (_power_flags != 0 &&
-        _power_flags != flags &&
+        (_power_flags&~MAV_POWER_STATUS_CHANGED) != (flags&~MAV_POWER_STATUS_CHANGED) &&
         hal.util->get_soft_armed()) {
         // the power status has changed while armed
         flags |= MAV_POWER_STATUS_CHANGED;
diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp
index 5b6e7057351af1..1012216b023c86 100644
--- a/libraries/AP_HAL_ChibiOS/Util.cpp
+++ b/libraries/AP_HAL_ChibiOS/Util.cpp
@@ -97,21 +97,11 @@ void Util::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type)
 
 
 #if ENABLE_HEAP
-
-void *Util::allocate_heap_memory(size_t size)
-{
-    memory_heap_t *heap = (memory_heap_t *)malloc(size + sizeof(memory_heap_t));
-    if (heap == nullptr) {
-        return nullptr;
-    }
-    chHeapObjectInit(heap, heap + 1U, size);
-    return heap;
-}
-
 /*
-  realloc implementation thanks to wolfssl, used by AP_Scripting
+  realloc implementation thanks to wolfssl, used by ExpandingString
+  and ExpandingArray
  */
-void *Util::std_realloc(void *addr, size_t size)
+void *Util::std_realloc(void *addr, uint32_t size)
 {
     if (size == 0) {
        free(addr);
@@ -128,33 +118,6 @@ void *Util::std_realloc(void *addr, size_t size)
     return new_mem;
 }
 
-void *Util::heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size)
-{
-    if (heap == nullptr) {
-        return nullptr;
-    }
-    if (new_size == 0) {
-        if (ptr != nullptr) {
-            chHeapFree(ptr);
-        }
-        return nullptr;
-    }
-    if (ptr == nullptr) {
-        return chHeapAlloc((memory_heap_t *)heap, new_size);
-    }
-    void *new_mem = chHeapAlloc((memory_heap_t *)heap, new_size);
-    if (new_mem != nullptr) {
-        const size_t old_size2 = chHeapGetSize(ptr);
-#if defined(HAL_DEBUG_BUILD) && !defined(IOMCU_FW)
-        if (new_size != 0 && old_size2 != old_size) {
-            INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
-        }
-#endif
-        memcpy(new_mem, ptr, old_size2 > new_size ? new_size : old_size2);
-        chHeapFree(ptr);
-    }
-    return new_mem;
-}
 #endif // ENABLE_HEAP
 
 #endif // CH_CFG_USE_HEAP
diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h
index f11dbf7239aadb..5ff46016e77e9b 100644
--- a/libraries/AP_HAL_ChibiOS/Util.h
+++ b/libraries/AP_HAL_ChibiOS/Util.h
@@ -49,10 +49,7 @@ class ChibiOS::Util : public AP_HAL::Util {
     void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override;
 
 #if ENABLE_HEAP
-    // heap functions, note that a heap once alloc'd cannot be dealloc'd
-    virtual void *allocate_heap_memory(size_t size) override;
-    virtual void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) override;
-    virtual void *std_realloc(void *ptr, size_t new_size) override;
+    void *std_realloc(void *ptr, uint32_t new_size) override;
 #endif // ENABLE_HEAP
 
     /*
@@ -142,10 +139,6 @@ class ChibiOS::Util : public AP_HAL::Util {
     FlashBootloader flash_bootloader() override;
 #endif
 
-#if ENABLE_HEAP
-    static memory_heap_t scripting_heap;
-#endif // ENABLE_HEAP
-
     // stm32F4 and F7 have 20 total RTC backup registers. We use the first one for boot type
     // flags, so 19 available for persistent data
     static_assert(sizeof(persistent_data) <= 19*4, "watchdog persistent data too large");
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/AET-H743-Basic_core_board.png b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/AET-H743-Basic_core_board.png
new file mode 100644
index 00000000000000..af10c4ea12a12e
Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/AET-H743-Basic_core_board.png differ
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/AET-H743-Basic_overview.jpg b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/AET-H743-Basic_overview.jpg
new file mode 100644
index 00000000000000..abec38a65d5940
Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/AET-H743-Basic_overview.jpg differ
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/AET-H743-Basic_power_board.png b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/AET-H743-Basic_power_board.png
new file mode 100644
index 00000000000000..dd1906a5bece94
Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/AET-H743-Basic_power_board.png differ
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/README.md b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/README.md
new file mode 100644
index 00000000000000..8108d09740d3ab
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/README.md
@@ -0,0 +1,142 @@
+# AET-H743-Basic Flight Controller
+
+The AET-H743-Basic is a flight controller designed and produced by AeroEggTech
+
+## Features
+
+ - STM32H743 microcontroller
+ - Dual ICM42688P IMUs
+ - 13 PWM / Dshot outputs
+ - 8 UARTs, one with CTS/RTS flow control pins
+ - 1 CAN
+ - Dedicated USB board
+ - DPS310 or SPL06 barometer
+ - 5V/6V/7V 10A Servo rail BEC
+ - 9V 2A BEC for VTX, GPIO controlled
+ - 5V 4A BEC
+ - MicroSD Card Slot
+ - 2-way camera input
+ - AT7456E OSD
+ - 2 I2Cs
+
+## Physical
+
+![AET-H743-Basic overview](AET-H743-Basic_overview.jpg)
+
+![AET-H743-Basic core board](AET-H743-Basic_core_board.png)
+
+![AET-H743-Basic power board](AET-H743-Basic_power_board.png)
+
+
+## Mechanical
+
+ - Dimensions: 36 x 47 x 17 mm
+ - Weight: 45g
+
+
+## Power supply
+
+The AET-H743-Basic supports 2-6s Li battery input. It has 3 ways of BEC, which result in 6 ways of power supplys. Please see the table below.
+
+| Power symbol | Power source | Max power (current) |
+|--------------|--------------|---------------------|
+| 5V | from 5V BEC | 20W (4A) |
+| 9V | from 9V BEC | 18W (2A) |
+| 9Vsw | from 9V BEC, controlled by MCU with an NPN MOS | 10W (1A) |
+| 4V5 | from USB or 5V BEC, with a diode | 5W (1A) |
+| VX | from Servo rail VX BEC, default 5V, can be changed to 6V or 7V | 50W (10A) |
+| BAT | directly from battery | (5A) |
+
+
+## Loading Firmware
+
+Initial firmware load can be done with DFU by plugging in USB with the bootloader button pressed. Then you should load the "with_bl.hex" firmware, using your favorite DFU loading tool, such as Mission Planner.
+
+Once the initial firmware is loaded you can update the firmware using any ArduPilot ground station software. Updates should be done with the "\*.apj" firmware files.
+
+## UART Mapping
+
+All UARTs are DMA capable.
+
+ - SERIAL0 -> USB
+ - SERIAL1 -> UART1 (MAVLink2)
+ - SERIAL2 -> UART2 (GPS)
+ - SERIAL3 -> UART3 (MAVLink2)
+ - SERIAL4 -> UART4 (GPS2, RX4 is also available as ESC telem if protocol is changed for this UART)
+ - SERIAL5 -> USB (SLCAN)
+ - SERIAL6 -> UART6 (RCIN)
+ - SERIAL7 -> UART7 (MAVLink2, Integrated Bluetooth module)
+ - SERIAL8 -> UART8 (User)
+
+
+## RC Input
+
+The default RC input is configured on the UART6 and supports all RC protocols except PPM. The SBUS pin is inverted and connected to RX6. RC can be attached to any UART port as long as the serial port protocol is set to `SERIALn_PROTOCOL=23` and SERIAL6_Protocol is changed to something other than '23'.
+
+
+## OSD Support
+
+The AET-H743-Basic supports onboard analog SD OSD using a AT7456 chip. The analog VTX should connect to the VTX pin.
+
+
+## PWM Output
+
+The AET-H743-Basic supports up to 13 PWM outputs.
+
+All the channels support DShot.
+
+Outputs are grouped and every output within a group must use the same output protocol:
+
+1, 2 are Group 1;
+
+3, 4, 5, 6 are Group 2;
+
+7, 8, 9, 10 are Group 3;
+
+11, 12 are Group 4;
+
+13(LED) is Group 5;
+
+Output 13 can be used as LED neopixel output;
+
+## Battery Monitoring
+
+The board has two internal voltage sensors and one integrated current sensor, and a second external current sensor input.
+
+The voltage sensors can handle up to 6S LiPo batteries.
+
+The first voltage/current sensor is enabled by default and the pin inputs for the second, unenabled sensor are also set by default:
+* BATT_MONITOR 4
+* BATT_VOLT_PIN 10
+* BATT_CURR_PIN 11
+* BATT_VOLT_MULT 11
+* BATT_CURR_SCALE 40
+* BATT2_VOLT_PIN 18
+* BATT2_CURR_PIN 7
+* BATT2_VOLT_MULT 11
+
+## Compass
+
+The AET-H743-Basic has no built-in compass, so if needed, you should use an external compass.
+
+## Analog cameras
+
+The AET-H743-Basic supports up to 2 cameras, connected to pin CAM1 and CAM2. You can select the video signal to VTX from camera by an RC channel. Set the parameters below:
+
+- RELAY2_FUNCTION = 1
+- RELAY_PIN2 = 82
+- RC8_OPTION = 34
+
+## 9V switch
+
+The 9Vsw power supply can be controlled by an RC channel. Set the parameters below:
+
+- RELAY1_FUNCTION = 1
+- RELAY_PIN = 81
+- RC7_OPTION = 28
+
+## Bluetooth
+
+The AET-H743-Basic support both legacy bluetooth SPP and BLE serial. The bluetooth uses UART7 as serial port. Search for `AET-H743-SPP` or `AET-H743-BLE` to connect.
+
+Note: you should connect a battery to the board so the bluetooth module can work.
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/defaults.parm
new file mode 100644
index 00000000000000..731906830a3a2b
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/defaults.parm
@@ -0,0 +1,3 @@
+# setup for LEDs on chan13
+SERVO13_FUNCTION 120
+NTF_LED_TYPES 257
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/hwdef-bl.dat
new file mode 100644
index 00000000000000..5eee362f08c5b9
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/hwdef-bl.dat
@@ -0,0 +1,49 @@
+# hw definition file for processing by chibios_pins.py
+# for AET H743-Basic bootloader
+
+# MCU class and specific type
+MCU STM32H7xx STM32H743xx
+
+# board ID for firmware load
+APJ_BOARD_ID AP_HW_AET-H743-Basic
+
+# crystal frequency, setup to use external oscillator
+OSCILLATOR_HZ 8000000
+
+FLASH_SIZE_KB 2048
+
+# bootloader starts at zero offset
+FLASH_RESERVE_START_KB 0
+
+# the location where the bootloader will put the firmware
+# the H743 has 128k sectors
+FLASH_BOOTLOADER_LOAD_KB 128
+
+
+# order of UARTs (and USB). Allow bootloading on USB and telem1
+SERIAL_ORDER OTG1 UART7
+
+# UART7 (telem1)
+PE7 UART7_RX UART7 NODMA
+PE8 UART7_TX UART7 NODMA
+
+# PA10 IO-debug-console
+PA11 OTG_FS_DM OTG1
+PA12 OTG_FS_DP OTG1
+
+PA13 JTMS-SWDIO SWD
+PA14 JTCK-SWCLK SWD
+
+# make sure Vsw is on during bootloader
+PD10  PINIO1 OUTPUT LOW
+
+PE3 LED_BOOTLOADER OUTPUT LOW
+define HAL_LED_ON 0
+
+# Add CS pins to ensure they are high in bootloader
+PC15 IMU1_CS CS
+PB12 MAX7456_CS CS
+PE11 IMU2_CS CS
+PD4 EXT_CS1 CS
+PE2 EXT_CS2 CS
+PC13 IMU3_CS CS
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/hwdef.dat
new file mode 100644
index 00000000000000..e16c0b29f5e932
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/hwdef.dat
@@ -0,0 +1,205 @@
+# hw definition file for processing by chibios_pins.py
+# for AET H743-Basic
+
+# MCU class and specific type
+MCU STM32H7xx STM32H743xx
+
+# board ID for firmware load
+APJ_BOARD_ID AP_HW_AET-H743-Basic
+
+# crystal frequency, setup to use external oscillator
+OSCILLATOR_HZ 8000000
+
+FLASH_SIZE_KB 2048
+env OPTIMIZE -Os
+
+USB_STRING_MANUFACTURER "AET"
+
+# bootloader takes first sector
+FLASH_RESERVE_START_KB 128
+
+# ChibiOS system timer
+STM32_ST_USE_TIMER 12
+define CH_CFG_ST_RESOLUTION 16
+
+# USB
+PA11 OTG_FS_DM OTG1
+PA12 OTG_FS_DP OTG1
+
+# SPI1 for IMU1 (ICM-42688)
+PA5 SPI1_SCK SPI1
+PA6 SPI1_MISO SPI1
+PD7 SPI1_MOSI SPI1
+PC15 IMU1_CS CS
+
+# SPI2 for MAX7456 OSD
+PB12 MAX7456_CS CS
+PB13 SPI2_SCK SPI2
+PB14 SPI2_MISO SPI2
+PB15 SPI2_MOSI SPI2
+
+# SPI4 for IMU3 (ICM-42688)
+PC13 IMU3_CS CS
+PE12 SPI4_SCK SPI4
+PE13 SPI4_MISO SPI4
+PE14 SPI4_MOSI SPI4
+
+# two I2C bus
+I2C_ORDER I2C2 I2C1
+
+# I2C1
+PB6 I2C1_SCL I2C1
+PB7 I2C1_SDA I2C1
+
+# I2C2
+PB10 I2C2_SCL I2C2
+PB11 I2C2_SDA I2C2
+
+# ADC
+PC0 BATT_VOLTAGE_SENS ADC1 SCALE(1)
+PC1 BATT_CURRENT_SENS ADC1 SCALE(1)
+PA4 BATT2_VOLTAGE_SENS ADC1 SCALE(1)
+PA7 BATT2_CURRENT_SENS ADC1 SCALE(1)
+
+define HAL_BATT_MONITOR_DEFAULT 4
+define HAL_BATT_VOLT_PIN 10
+define HAL_BATT_CURR_PIN 11
+define HAL_BATT2_VOLT_PIN 18
+define HAL_BATT2_CURR_PIN 7
+define HAL_BATT_VOLT_SCALE 11.0
+define HAL_BATT_CURR_SCALE 40.0
+define HAL_BATT2_VOLT_SCALE 11.0
+
+PC4 PRESSURE_SENS ADC1 SCALE(2)
+define HAL_DEFAULT_AIRSPEED_PIN 4
+
+PC5 RSSI_ADC ADC1
+define BOARD_RSSI_ANA_PIN 8
+
+# LED
+# green LED1 marked as B/E
+# blue LED0 marked as ACT
+PE3 LED0 OUTPUT LOW GPIO(90) # blue
+PE4 LED1 OUTPUT LOW GPIO(91) # green
+define HAL_GPIO_A_LED_PIN 91
+define HAL_GPIO_B_LED_PIN 90
+
+define HAL_GPIO_LED_ON 1
+define AP_NOTIFY_GPIO_LED_2_ENABLED 1
+
+# order of UARTs (and USB)
+SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 OTG2 USART6 UART7 UART8
+
+# USART1
+PA10 USART1_RX USART1
+PA9  USART1_TX USART1
+
+# USART2
+PD5 USART2_TX USART2
+PD6 USART2_RX USART2
+
+# USART3
+PD9 USART3_RX USART3
+PD8 USART3_TX USART3
+
+# UART4
+PB9 UART4_TX UART4
+PB8 UART4_RX UART4
+
+# USART6
+PC6 USART6_TX USART6
+PC7 USART6_RX USART6
+
+# UART7
+PE7 UART7_RX UART7
+PE8 UART7_TX UART7
+PE10 UART7_CTS UART7
+PE9  UART7_RTS UART7
+
+# UART8
+PE0 UART8_RX UART8
+PE1 UART8_TX UART8
+
+# Serial functions
+define DEFAULT_SERIAL2_PROTOCOL SerialProtocol_GPS
+define DEFAULT_SERIAL3_BAUD 115
+define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_MAVLink2
+define DEFAULT_SERIAL4_PROTOCOL SerialProtocol_GPS
+define DEFAULT_SERIAL6_PROTOCOL SerialProtocol_RCIN
+define DEFAULT_SERIAL7_BAUD 115
+define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_MAVLink2
+
+# CAN bus
+PD0 CAN1_RX CAN1
+PD1 CAN1_TX CAN1
+PD3 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(70)
+
+# Motors
+PB0  TIM8_CH2N TIM8  PWM(1)  GPIO(50)
+PB1  TIM8_CH3N TIM8  PWM(2)  GPIO(51)
+PA0  TIM5_CH1  TIM5  PWM(3)  GPIO(52)
+PA1  TIM5_CH2  TIM5  PWM(4)  GPIO(53)
+PA2  TIM5_CH3  TIM5  PWM(5)  GPIO(54)
+PA3  TIM5_CH4  TIM5  PWM(6)  GPIO(55)
+PD12 TIM4_CH1 TIM4 PWM(7) GPIO(56)
+PD13 TIM4_CH2 TIM4 PWM(8) GPIO(57)
+PD14 TIM4_CH3 TIM4 PWM(9) GPIO(58)
+PD15 TIM4_CH4 TIM4 PWM(10) GPIO(59)
+PE5  TIM15_CH1 TIM15 PWM(11) GPIO(60)
+PE6  TIM15_CH2 TIM15 PWM(12) GPIO(61)
+PA8 TIM1_CH1 TIM1 PWM(13) GPIO(62) # for WS2812 LED
+
+# Beeper
+PA15 TIM2_CH1 TIM2 GPIO(32) ALARM
+
+# microSD support
+PC8 SDMMC1_D0 SDMMC1
+PC9 SDMMC1_D1 SDMMC1
+PC10 SDMMC1_D2 SDMMC1
+PC11 SDMMC1_D3 SDMMC1
+PC12 SDMMC1_CK SDMMC1
+PD2 SDMMC1_CMD SDMMC1
+
+# GPIOs
+PD10  PINIO1 OUTPUT GPIO(81) LOW
+PD11  PINIO2 OUTPUT GPIO(82) LOW
+
+DMA_PRIORITY S*
+
+define HAL_STORAGE_SIZE 32768
+
+# use last 2 pages for flash storage
+# H743 has 16 pages of 128k each
+STORAGE_FLASH_PAGE 14
+
+# spi devices
+SPIDEV icm42688_0  SPI1 DEVID1 IMU1_CS      MODE3  2*MHZ 16*MHZ   # Clock is 100Mhz so highest clock <= 24Mhz is 100Mhz/8
+SPIDEV icm42688_1  SPI4 DEVID1 IMU3_CS      MODE3  2*MHZ 16*MHZ
+SPIDEV osd         SPI2 DEVID4 MAX7456_CS   MODE0 10*MHZ 10*MHZ
+
+
+DMA_NOSHARE SPI1* SPI4*
+
+# no built-in compass, but probe the i2c bus for all possible
+# external compass types
+define ALLOW_ARM_NO_COMPASS
+define HAL_PROBE_EXTERNAL_I2C_COMPASSES
+define HAL_I2C_INTERNAL_MASK 0
+define HAL_COMPASS_AUTO_ROT_DEFAULT 2
+
+# two IMUs
+# ICM42688P, ICM42688P
+IMU Invensensev3 SPI:icm42688_0 ROTATION_YAW_180
+IMU Invensensev3 SPI:icm42688_1 ROTATION_YAW_270
+define HAL_DEFAULT_INS_FAST_SAMPLE 1
+
+# BAROs
+BARO SPL06  I2C:0:0x76
+BARO DPS310 I2C:0:0x76
+
+define HAL_OS_FATFS_IO 1
+
+# setup for OSD
+define OSD_ENABLED 1
+define HAL_OSD_TYPE_DEFAULT 1
+ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/H743Pinout.png b/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/H743Pinout.png
deleted file mode 100644
index f65bf4302c65c8..00000000000000
Binary files a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/H743Pinout.png and /dev/null differ
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/H743StampFront&Back.png b/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/H743StampFront&Back.png
deleted file mode 100644
index 207616eb109ce3..00000000000000
Binary files a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/H743StampFront&Back.png and /dev/null differ
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/H743_SD.png b/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/H743_SD.png
new file mode 100644
index 00000000000000..da099460006640
Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/H743_SD.png differ
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/README.md b/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/README.md
index 50160b10e8709f..eb48dcbc40629e 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/README.md
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/README.md
@@ -2,19 +2,19 @@
 
 # CBUnmanned H743 Stamp
 
-The [CBUnmanned H743 Stamp](https://cbunmanned.com/store) is a flight controller loosely based on the FMUv6 standards & is designed for low volume OEMs as a drop in way to add ArduPilot to their custom hardware builds. It is a part of CBUnmanned's wider ["Stamp" Eco-System](https://wiki.cbunmanned.com/wiki/cbunmanned-stamp-eco-system), which brings together all the typical avionics hardware into a neat custom carrier PCB. Mounting footprints and symbols are available along with examples of basic usage on the  [Wiki](https://wiki.cbunmanned.com/wiki/cbunmanned-stamp-eco-system/h743-flight-controller).
+The [CBUnmanned H743 Stamp](https://cbunmanned.com/store) is a flight controller loosely based on the FMUv6 standards & is designed for low volume OEMs as a drop in way to add ArduPilot to their custom hardware builds. It is a part of CBUnmanned's wider ["Stamp" Eco-System](https://cbunmanned.com), which brings together all the typical avionics hardware into a neat custom carrier PCB. Mounting footprints and symbols are available along with examples of basic usage on the  [Wiki](https://wiki.cbunmanned.com/).
 
-![H743StampFront&Back](H743StampFront&Back.png "H743FB")
+![H743StampFront&Back](H743_SD.png "H743FB")
 
 ## Features
 - Class leading H7 SOC.
 - Triple IMU sensors for extra redundancy.
 - Based on the FMU-V6 standards.
 - Micro SD Card for Logging/LUA Scripting.
-- Direct solder mounting or optional 1.27mm header.
+- 1.27mm header
 - x1 Ethernet and x2 CAN for easy integration with the next generation of UAV accessories.
 - All complicated/supporting circuitry is on-board, just power with 5v.
-- Just 22mm x 24.25mm & 1.9g.
+- Just 22mm x 24.25mm & 3g.
 
 ## Specifications
 - Processor
@@ -26,7 +26,7 @@ The [CBUnmanned H743 Stamp](https://cbunmanned.com/store) is a flight controller
 - Sensors
   - x2 Ivensense ICM-42688 IMU
   - x1 Ivensense ICM-42670 IMU
-  - x1 Infineon DPS310 Barometer
+  - x1 BMP280 Barometer
   - x1 Bosch BMM150 Magnetometer
  
 - Power
@@ -50,7 +50,7 @@ The [CBUnmanned H743 Stamp](https://cbunmanned.com/store) is a flight controller
 
 ![H743 Stamp Pinout](H743Pinout.png "H743")
 
-### UART Mapping (Yellow Fade)
+### UART Mapping 
 
  Ardupilot -> STM32
  - SERIAL0 -> USB
@@ -75,10 +75,10 @@ USART 6 Tx is available for use with bi directional protocols.
 
 An optional IOMCU can be connected to this serial port, a compatible custom build of the firmware required.
 
-### CAN Ports (Light Green Fade)
+### CAN Ports 
 2 CAN buses are available, each with a built in 120 ohm termination resistor.
 
-### I2C (Maroon Fade)
+### I2C
 I2C 1 - Internal for BMM150 Compass.
 
 I2C 2 - Internal for DPS310 Barometer.
@@ -87,10 +87,10 @@ I2C 3 - External With internal 2.2k Pull Up.
 
 I2C 4 - External With internal 2.2k Pull Up.
 
-### SPI (Cyan Fade)
+### SPI
 SPI 4 is available for use with external sensors alongside a Chip Select and Data Ready pin, compatible custom build of the firmware required.
 
-### PWM Output (Blue Fade)
+### PWM Output
 The Stamp supports up to 10 PWM outputs with D-Shot. 
 
 The PWM outputs are in 3 groups:
@@ -105,11 +105,11 @@ BiDirectional DShot available on the first 8 outputs.
 
 A buzzer alarm signal is available on Timer 14.
 
-### Analog Inputs (Purple Fade)
+### Analog Inputs 
 
 The board has two ADC input channels for Voltage (0-3.3v) and Current (0-3.3v) measurement. Settings are dependent on the external hardware used. 
 
-### Ethernet (Green Fade)
+### Ethernet
 Ethernet is available on 4 output pads and has internal magnetics supporting direct connection to external equipment, no need for a large RJ45 connector.
 
 ### Compass
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat
index 474e761991cb7b..639e0c6256b4d7 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CBU-H7-Stamp/hwdef.dat
@@ -149,7 +149,7 @@ PB13 CAN2_TX CAN2
 PB9 I2C1_SDA I2C1
 PB8 I2C1_SCL I2C1
 
-# I2C2 - DPS310
+# I2C2 - BMP280
 PF1 I2C2_SCL I2C2
 PF0 I2C2_SDA I2C2
 
@@ -193,7 +193,7 @@ define AP_NOTIFY_GPIO_LED_1_PIN 90
 define HAL_GPIO_LED_ON 1
 
 # barometers
-BARO DPS310 I2C:1:0x77
+BARO BMP280 I2C:1:0x76	
 
 # compass
 COMPASS BMM150 I2C:0:0x10 false ROTATION_YAW_180
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeBlack+/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeBlack+/hwdef.dat
index 39c7cfa10d1dcd..851814d8087979 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CubeBlack+/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeBlack+/hwdef.dat
@@ -19,4 +19,6 @@ IMU Invensensev2 SPI:icm20948_ext ROTATION_PITCH_180
 IMU Invensense SPI:mpu9250 ROTATION_YAW_270
 
 undef ROMFS
-ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
+ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cube_highpolh.bin
+
+define AP_IOMCU_PROFILED_SUPPORT_ENABLED 1
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeBlack/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeBlack/hwdef.dat
index cccd5a28e00f3a..04aa6bf20f8fcf 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CubeBlack/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeBlack/hwdef.dat
@@ -102,8 +102,9 @@ define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 5
 
 # default the 2nd interface to SLCAN
 define HAL_OTG2_PROTOCOL SerialProtocol_SLCAN
-ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_dshot_highpolh.bin
-ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
+ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin
+ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cube_highpolh.bin
 
+define AP_IOMCU_PROFILED_SUPPORT_ENABLED 1
 # enable support for dshot on iomcu
 define HAL_WITH_IO_MCU_DSHOT 1
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeNode-ETH/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeNode-ETH/hwdef.dat
index 6da6a16c7a131e..4aef18a5c3e421 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CubeNode-ETH/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeNode-ETH/hwdef.dat
@@ -3,19 +3,25 @@ include ../CubeNode/hwdef.dat
 # we need RTS/CTS for the PPP link
 undef PE0
 undef PE1
-undef PC11
+undef PC12
 undef HAL_PERIPH_ENABLE_IMU
+undef HAL_GCS_ENABLED
 
 # need to use UART8 to get RTS/CTS
 PE1 UART8_TX UART8
 PE0 UART8_RX UART8
 PA10 UART8_RTS UART8
-PC11 UART8_CTS_GPIO UART8
+PC12 UART8_CTS_GPIO UART8
 
 SERIAL_ORDER OTG1 UART8
 
 PA4 VDD_5V_SENS ADC1 SCALE(2)
 
+# LEDs
+undef PC11
+PC11 LED0 OUTPUT LOW GPIO(0)
+PB14 LED1 OUTPUT LOW GPIO(1)
+
 undef HAL_USE_ADC
 define HAL_USE_ADC TRUE
 define HAL_WITH_MCU_MONITORING 1
@@ -28,3 +34,5 @@ include ../include/network_PPPGW.inc
 define HAL_MONITOR_THREAD_ENABLED 1
 
 define AP_NETWORKING_TESTS_ENABLED 1
+
+define HAL_PERIPH_SHOW_SERIAL_MANAGER_PARAMS 1
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc
index f7d85e808225cd..b34eb0a9faf4b0 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc
@@ -296,12 +296,14 @@ define HAL_GPIO_PWM_VOLT_3v3 1
 # we can automatically update the IOMCU firmware on boot. The format
 # is "ROMFS ROMFS-filename source-filename". Paths are relative to the
 # ardupilot root.
-ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
+ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cube_highpolh.bin
 
 # enable support for dshot on iomcu
-ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_dshot_highpolh.bin
+ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin
 define HAL_WITH_IO_MCU_DSHOT 1
 
+define AP_IOMCU_PROFILED_SUPPORT_ENABLED 1
+
 DMA_NOSHARE SPI1* SPI4* USART6*
 
 # for users who have replaced their CubeSolo with a CubeOrange:
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat
index a7a89aaae758a0..f0ea70db249208 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat
@@ -105,15 +105,15 @@ PE5    SPI4_MISO    SPI4
 PE6    SPI4_MOSI    SPI4
 
 # Sensors
-SPIDEV icm42688_0       SPI1 DEVID1  ICM42688_0_CS      MODE3  2*MHZ  8*MHZ
+SPIDEV icm42688_0       SPI1 DEVID1  ICM42688_0_CS      MODE3  2*MHZ  24*MHZ
 SPIDEV ms5611_0         SPI1 DEVID2  BARO_0_CS          MODE3 20*MHZ 20*MHZ
 
-SPIDEV icm42688_1       SPI2 DEVID1  ICM42688_1_CS      MODE3  2*MHZ  8*MHZ
+SPIDEV icm42688_1       SPI2 DEVID1  ICM42688_1_CS      MODE3  2*MHZ  24*MHZ
 SPIDEV rm3100           SPI2 DEVID2  RM3100_CS          MODE3  1*MHZ  1*MHZ
 
 SPIDEV icm20649         SPI4 DEVID1  ICM_2_CS           MODE3  4*MHZ  8*MHZ
 # alternative to icm20649
-SPIDEV icm45686         SPI4 DEVID1  ICM_2_CS           MODE3  2*MHZ  8*MHZ
+SPIDEV icm45686         SPI4 DEVID1  ICM_2_CS           MODE3  2*MHZ  24*MHZ
 SPIDEV ms5611_1         SPI4 DEVID2  BARO_1_CS          MODE3 20*MHZ 20*MHZ
 
 IMU Invensensev3 SPI:icm42688_1 ROTATION_YAW_90
@@ -121,6 +121,9 @@ IMU Invensensev3 SPI:icm42688_0 ROTATION_PITCH_180_YAW_270
 IMU Invensensev2 SPI:icm20649 ROTATION_PITCH_180
 IMU Invensensev3 SPI:icm45686 ROTATION_YAW_180
 
+define HAL_INS_HIGHRES_SAMPLE 5
+define ICM45686_CLKIN 1
+
 BARO MS56XX SPI:ms5611_0
 BARO MS56XX SPI:ms5611_1
 
@@ -153,7 +156,7 @@ define HAL_CHIBIOS_ARCH_FMUV3 1
 define BOARD_TYPE_DEFAULT 3
 
 # MCO output
-PA8    RCC_MCO_1    OUTPUT LOW
+PA8    RCC_MCO_1    INPUT
 
 # I2C
 PF0    I2C2_SDA    I2C2
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat
index 55ac0d221db2c1..2c32d6813560e8 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat
@@ -445,9 +445,10 @@ STORAGE_FLASH_PAGE 1
 # we can automatically update the IOMCU firmware on boot. The format
 # is "ROMFS ROMFS-filename source-filename". Paths are relative to the
 # ardupilot root
-ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
+ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cube_highpolh.bin
 
 # enable support for dshot on iomcu
-ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_dshot_highpolh.bin
+ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin
 define HAL_WITH_IO_MCU_DSHOT 1
 
+define AP_IOMCU_PROFILED_SUPPORT_ENABLED 1
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/F4BY_F427/README.md b/libraries/AP_HAL_ChibiOS/hwdef/F4BY_F427/README.md
new file mode 100644
index 00000000000000..f096ef850c56a5
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/F4BY_F427/README.md
@@ -0,0 +1,22 @@
+# F4BY Flight Controller MCU upgrade
+
+The F4BY flight controller shop: https://f4by.com/en/?order/our_product
+
+The instructions are available here: https://f4by.com/en/?doc/fc_f4by_v2.1.5
+
+## Howto
+for self upgrage old fc: 
+replace old MCU STM32F407VGT (1MB Flash) with STM32F427VET rev3 or above (2MB Flash)
+
+
+## Features
+
+ - Full Ardupilot features support (exclude LUA Script)
+
+
+## Loading Firmware
+
+Initial firmware load can be done with DFU by plugging in USB with the
+boot solder pads connected. Then you should load the "with_bl.hex"
+firmware, using your favourite DFU loading tool.
+
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/F4BY_F427/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/F4BY_F427/hwdef-bl.dat
new file mode 100644
index 00000000000000..60889f42de5ab6
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/F4BY_F427/hwdef-bl.dat
@@ -0,0 +1,42 @@
+# hw definition file for processing by chibios_hwdef.py
+# for f4by bootloader
+
+MCU STM32F4xx STM32F427xx
+
+APJ_BOARD_ID AP_HW_F4BY_F427
+
+OSCILLATOR_HZ 8000000
+
+STM32_ST_USE_TIMER 5
+
+# flash size
+FLASH_SIZE_KB 2048
+
+# order of UARTs (and USB)
+SERIAL_ORDER OTG1 USART2
+
+PE3 LED_BOOTLOADER OUTPUT
+PE2 LED_ACTIVITY OUTPUT
+define HAL_LED_ON 1
+
+PA9 VBUS INPUT
+
+PA11 OTG_FS_DM OTG1
+PA12 OTG_FS_DP OTG1
+
+PA13 JTMS-SWDIO SWD
+PA14 JTCK-SWCLK SWD
+
+PD5 USART2_TX USART2
+PD6 USART2_RX USART2
+
+FLASH_USE_MAX_KB 16
+FLASH_RESERVE_START_KB 0
+
+# location of application code
+FLASH_BOOTLOADER_LOAD_KB 16
+
+# Add CS pins to ensure they are high in bootloader
+PA4 MPU_CS CS
+PB12 FRAM_CS CS SPEED_VERYLOW
+PE15 FLASH_CS CS
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/F4BY_F427/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/F4BY_F427/hwdef.dat
new file mode 100644
index 00000000000000..c7a5ee84733990
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/F4BY_F427/hwdef.dat
@@ -0,0 +1,177 @@
+# hw definition file for processing by chibios_hwdef.py
+# for F4BY v2.1.5 board description http://swift-flyer.com/?page_id=83
+
+# MCU class and specific type
+MCU STM32F4xx STM32F427xx
+
+# board ID for firmware load
+APJ_BOARD_ID AP_HW_F4BY_F427
+
+# USB setup
+USB_VENDOR 0x27AC # Swift-Flyer
+USB_PRODUCT 0x0201 # fmu usb driver
+USB_STRING_MANUFACTURER "Swift-Flyer"
+PA11 OTG_FS_DM OTG1
+PA12 OTG_FS_DP OTG1
+# this is the pin that senses USB being connected. It is an input pin
+# setup as OPENDRAIN
+PA9 VBUS INPUT OPENDRAIN
+
+
+FLASH_SIZE_KB 2048
+FLASH_RESERVE_START_KB 16
+define HAL_STORAGE_SIZE 16384
+env OPTIMIZE -O2 
+
+# crystal frequency
+OSCILLATOR_HZ 8000000
+
+# ChibiOS system timer
+STM32_ST_USE_TIMER 5
+
+
+
+PA13 JTMS-SWDIO SWD
+PA14 JTCK-SWCLK SWD
+
+# the normal usage of this ordering is:
+# 1) SERIAL0: console (primary mavlink, usually USB)
+# 2) SERIAL3: primary GPS
+# 3) SERIAL1: telem1
+# 4) SERIAL2: telem2
+# 5) SERIAL4: GPS2
+# 6) SERIAL5: extra UART (usually RTOS debug console)
+
+# use UART for stdout, so no STDOUT_SERIAL
+#STDOUT_SERIAL SD5
+#STDOUT_BAUDRATE 57600
+
+
+
+SERIAL_ORDER OTG1 USART2 USART1 USART3 UART4 UART5
+
+# UART1 as board 2.1.5 for serial 3 gps
+PB6 USART1_TX USART1
+PB7 USART1_RX USART1
+
+# USART2 serial2 telem2
+PD5 USART2_TX USART2 NODMA
+PD6 USART2_RX USART2
+
+# USART3 serial3 telem1
+PD8 USART3_TX USART3 NODMA
+PD9 USART3_RX USART3
+
+PC10 UART4_TX UART4 NODMA
+PC11 UART4_RX UART4
+
+# SHARE dma with I2C2_TX
+PC12 UART5_TX UART5 NODMA
+PD2  UART5_RX UART5 NODMA
+
+
+#SPI1 for MPU
+PA5 SPI1_SCK SPI1
+PA6 SPI1_MISO SPI1
+PA7 SPI1_MOSI SPI1
+PA4 MPU_CS CS
+
+# spi bus for dataflash AND SD
+PB13 SPI2_SCK SPI2
+PB14 SPI2_MISO SPI2
+PB15 SPI2_MOSI SPI2
+
+
+PB12 FRAM_CS CS SPEED_VERYLOW
+PE15 FLASH_CS CS
+
+SPIDEV mpu6000 SPI1 DEVID1 MPU_CS   MODE3 1*MHZ   8*MHZ
+SPIDEV ramtron SPI2 DEVID2 FRAM_CS  MODE3 8*MHZ   8*MHZ
+SPIDEV sdcard  SPI2 DEVID3 FLASH_CS MODE0 400*KHZ 25*MHZ
+
+# one IMU
+IMU Invensense SPI:mpu6000 ROTATION_NONE
+
+# one baro, check both addresses
+BARO MS56XX I2C:0:0x76
+BARO MS56XX I2C:0:0x77
+
+# enable RAMTROM parameter storage
+define HAL_WITH_RAMTRON 1
+# enable FAT filesystem support
+define HAL_OS_FATFS_IO 1
+
+# this defines the default maximum clock on I2C devices.
+define HAL_I2C_MAX_CLOCK 100000
+I2C_ORDER I2C2 I2C1
+PB8 I2C1_SCL I2C1
+PB9 I2C1_SDA I2C1
+PB10 I2C2_SCL I2C2
+PB11 I2C2_SDA I2C2
+
+# look for I2C compass
+COMPASS HMC5843 I2C:0:0x1E false ROTATION_YAW_270
+define HAL_PROBE_EXTERNAL_I2C_COMPASSES
+
+# PWM out pins
+PA0 TIM2_CH1 TIM2 PWM(1) GPIO(50)
+PA1 TIM2_CH2 TIM2 PWM(2) GPIO(51)
+PA2 TIM2_CH3 TIM2 PWM(3) GPIO(52)
+PA3 TIM2_CH4 TIM2 PWM(4) GPIO(53)
+PE9 TIM1_CH1 TIM1 PWM(5) GPIO(54)
+PE11 TIM1_CH2 TIM1 PWM(6) GPIO(55)
+PE13 TIM1_CH3 TIM1 PWM(7) GPIO(56)
+PE14 TIM1_CH4 TIM1 PWM(8) GPIO(57)
+
+PD13 TIM4_CH2 TIM4 PWM(9) GPIO(58)
+PD12 TIM4_CH1 TIM4 PWM(10) GPIO(59)
+PD15 TIM4_CH4 TIM4 PWM(11) GPIO(60)
+PD14 TIM4_CH3 TIM4 PWM(12) GPIO(61)
+
+# also USART6_RX for unidirectional RC,including PPM
+PC7 TIM8_CH2 TIM8 RCININT PULLDOWN LOW DMA_CH0
+# PC6 TIM8_CH1 TIM8 RCININT PULLDOWN LOW DMA_CH0
+
+
+# New style Pixracer LED configuration for master repo
+define AP_NOTIFY_GPIO_LED_RGB_ENABLED 1
+
+PE3 LED_RED   OUTPUT GPIO(10)
+PE2 LED_GREEN OUTPUT GPIO(11)
+PE1 LED_BLUE  OUTPUT GPIO(12)
+PE0 LED_YELOW OUTPUT GPIO(13)
+
+define AP_NOTIFY_GPIO_LED_RGB_RED_PIN 10
+define AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN 11
+define AP_NOTIFY_GPIO_LED_RGB_BLUE_PIN 12
+
+
+
+PC0 PRESSURE_SENS ADC1 SCALE(1)
+PC1 RSSI_IN ADC1
+PC2 BATT_CURRENT_SENS ADC1 SCALE(2)
+PC3 BATT_VOLTAGE_SENS ADC1 SCALE(2)
+PC4 VDD_5V_SENS ADC1 SCALE(2)
+PC5 FMU_SERVORAIL_VCC_SENS ADC1 SCALE(2)
+
+
+PE5 TIM9_CH1 TIM9 ALARM
+PC14 EXTERN_GPIO1 OUTPUT GPIO(1)
+PC13 EXTERN_GPIO2 OUTPUT GPIO(2)
+PE4  EXTERN_GPIO3 OUTPUT GPIO(3)
+PE6  EXTERN_GPIO4 OUTPUT GPIO(4)
+PC9  EXTERN_GPIO5 OUTPUT GPIO(5)
+
+#  IRQ for MPU6000
+PB0 EXTI_MPU6000 INPUT PULLUP
+PB1 DRDY_HMC5883 INPUT PULLUP
+
+# this constants with HAL_
+define HAL_BATT_MONITOR_DEFAULT 4
+define HAL_BATT_VOLT_PIN 13
+define HAL_BATT_CURR_PIN 12
+define HAL_BATT_VOLT_SCALE 16.04981
+define HAL_BATT_CURR_SCALE 100
+
+# this constant with AP_
+define AP_BATT_CURR_AMP_OFFSET_DEFAULT 2.316
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF4/hwdef.dat
index eea0b4785433c6..37fcf36778e079 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF4/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF4/hwdef.dat
@@ -139,8 +139,10 @@ IMU Invensense SPI:mpu6000 ROTATION_YAW_180
 
 # one baro
 BARO BMP280 I2C:0:0x76
+BARO SPL06 I2C:0:0x76
 define AP_BARO_BACKEND_DEFAULT_ENABLED 0
 define AP_BARO_BMP280_ENABLED 1
+define AP_BARO_SPL06_ENABLED 1
 
 # enable logging to dataflash
 define HAL_LOGGING_DATAFLASH_ENABLED 1
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-Wing/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-Wing/hwdef.dat
index cd98727eb9e926..53ae134e5bc5df 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-Wing/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7-Wing/hwdef.dat
@@ -204,6 +204,7 @@ IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_270
 
 # BMP280 integrated on I2C4 bus
 BARO BMP280 I2C:0:0x76
+BARO SPL06 I2C:0:0x76
 
 define HAL_OS_FATFS_IO 1
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat
index 11dfb4ae418f85..5ce9b33c622d03 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat
@@ -150,6 +150,7 @@ IMU Invensensev3 SPI:icm42688 ROTATION_ROLL_180_YAW_270
 
 # one BARO
 BARO BMP280 I2C:0:0x76
+BARO SPL06 I2C:0:0x76
 
 define HAL_OS_FATFS_IO 1
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini/hwdef.dat
index 434d61bdb3ea21..258a9ea05428f2 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7Mini/hwdef.dat
@@ -153,6 +153,7 @@ IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_90
 
 # one BARO
 BARO BMP280 I2C:0:0x76
+BARO SPL06 I2C:0:0x76
 
 # setup for OSD
 define OSD_ENABLED 1
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/LongBowF405WING/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/LongBowF405WING/hwdef.dat
index e0797d7a34bc24..01db0f16784dac 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/LongBowF405WING/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/LongBowF405WING/hwdef.dat
@@ -182,13 +182,11 @@ include ../include/minimize_fpv_osd.inc
 #undef AP_LANDINGGEAR_ENABLED
 #undef HAL_MOUNT_ENABLED
 #undef HAL_MOUNT_SERVO_ENABLED
-#undef QAUTOTUNE_ENABLED
 
 #define AP_CAMERA_MOUNT_ENABLED 1
 #define AP_LANDINGGEAR_ENABLED 1
 #define HAL_MOUNT_ENABLED 1
 #define HAL_MOUNT_SERVO_ENABLED 1
-#define QAUTOTUNE_ENABLED 1
 
 #define DEFAULT_NTF_LED_TYPES 257
 
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MFE_POS3_CAN/MFE_POS3_CAN-1.png b/libraries/AP_HAL_ChibiOS/hwdef/MFE_POS3_CAN/MFE_POS3_CAN-1.png
new file mode 100644
index 00000000000000..d12d1fc728e07c
Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/MFE_POS3_CAN/MFE_POS3_CAN-1.png differ
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MFE_POS3_CAN/README.md b/libraries/AP_HAL_ChibiOS/hwdef/MFE_POS3_CAN/README.md
new file mode 100644
index 00000000000000..c3635c32e47d30
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MFE_POS3_CAN/README.md
@@ -0,0 +1,66 @@
+
+## MFE_POS3_CAN
+
+The MFE_POS3_CAN is sold by a range of resellers listed on the makeflyeasy(http://www.makeflyeasy.com)
+
+## Features
+
+• STM32F427VIT6 microcontroller
+
+• RM3100 compass
+
+• NEO-M9N 
+
+• one CAN port
+
+• Blue led
+
+
+## Picture
+
+![MFE_POS3_CAN](MFE_POS3_CAN-1.png "MFE_POS3_CAN-1")
+
+## Pinout
+ 
+ Connector pin assignments
+=========================
+
+CAN1 ports
+---------------
+
+   <table border="1" class="docutils">
+   <tbody>
+   <tr>
+   <th>PIN</th>
+   <th>SIGNAL</th>
+   <th>VOLT</th>
+   </tr>
+   <tr>
+   <td>1</td>
+   <td>VCC</td>
+   <td>+5V</td>
+   </tr>
+   <tr>
+   <td>2</td>
+   <td>CAN_H</td>
+   <td>+12V</td>
+   </tr>
+   <tr>
+   <td>3</td>
+   <td>CAN_L</td>
+   <td>+12V</td>
+   </tr>
+   <tr>
+   <td>4</td>
+   <td>GND</td>
+   <td>GND</td>
+   </tr>
+   </tbody>
+   </table>
+
+
+Where to Buy
+============
+
+`makeflyeasy <http://www.makeflyeasy.com>`
+
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MFE_POS3_CAN/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MFE_POS3_CAN/hwdef-bl.dat
new file mode 100644
index 00000000000000..6d5cb6d930cd6f
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MFE_POS3_CAN/hwdef-bl.dat
@@ -0,0 +1,76 @@
+# hw definition file for processing by chibios_pins.py
+
+# MCU class and specific type
+MCU STM32F4xx STM32F427xx
+
+FLASH_RESERVE_START_KB 0
+FLASH_BOOTLOADER_LOAD_KB 32
+
+# reserve some space for params
+APP_START_OFFSET_KB 4
+
+# 128k flash part
+FLASH_SIZE_KB 2048
+
+# board ID for firmware load
+APJ_BOARD_ID AP_HW_MFE_POS3_CAN
+
+# setup build for a peripheral firmware
+env AP_PERIPH 1
+
+# debug on USART1
+STDOUT_SERIAL SD1
+STDOUT_BAUDRATE 57600
+
+# crystal frequency
+OSCILLATOR_HZ 8000000
+
+define CH_CFG_ST_FREQUENCY 1000000
+
+# order of UARTs
+SERIAL_ORDER 
+
+# blue LED
+PC6 LED_BOOTLOADER OUTPUT HIGH
+define HAL_LED_ON 1
+
+#PA0 LED_RED OUTPUT LOW
+#PA2 LED_GREEN OUTPUT LOW
+#PA3 LED_SAFETY OUTPUT LOW
+#PA4 VDD_3V3_SENSORS_EN OUTPUT LOW
+
+# USART1
+PB6 USART1_TX USART1
+PB7 USART1_RX USART1
+
+# SWD debugging
+PA13 JTMS-SWDIO SWD
+PA14 JTCK-SWCLK SWD
+
+define HAL_USE_SERIAL TRUE
+
+define STM32_SERIAL_USE_USART1 TRUE
+define STM32_SERIAL_USE_USART2 TRUE
+define STM32_SERIAL_USE_USART3 FALSE
+
+define HAL_NO_GPIO_IRQ
+define HAL_USE_EMPTY_IO TRUE
+
+# avoid timer and RCIN threads to save memory
+define HAL_NO_TIMER_THREAD
+
+define DMA_RESERVE_SIZE 0
+
+# enable CAN support
+PB8 CAN1_RX CAN1
+PB9 CAN1_TX CAN1
+PB5 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW
+
+# make bl baudrate match debug baudrate for easier debugging
+define BOOTLOADER_BAUDRATE 57600
+
+# use a smaller bootloader timeout
+define HAL_BOOTLOADER_TIMEOUT 2500
+
+# Add CS pins to ensure they are high in bootloader
+PA4 MAG_CS CS
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MFE_POS3_CAN/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MFE_POS3_CAN/hwdef.dat
new file mode 100644
index 00000000000000..e06a0a111c724a
--- /dev/null
+++ b/libraries/AP_HAL_ChibiOS/hwdef/MFE_POS3_CAN/hwdef.dat
@@ -0,0 +1,108 @@
+# hw definition file for processing by chibios_pins.py
+# MCU class and specific type
+
+#AUTOBUILD_TARGETS None
+
+# MCU class and specific type
+MCU STM32F4xx STM32F427xx
+
+FLASH_RESERVE_START_KB 36
+
+STORAGE_FLASH_PAGE 16
+define HAL_STORAGE_SIZE 8192
+
+# board ID for firmware load
+APJ_BOARD_ID AP_HW_MFE_POS3_CAN
+
+# setup build for a peripheral firmware
+env AP_PERIPH 1
+
+# crystal frequency
+OSCILLATOR_HZ 8000000
+
+define CH_CFG_ST_FREQUENCY 1000000
+
+FLASH_SIZE_KB 2048
+
+
+# order of UARTs
+SERIAL_ORDER USART1 USART2
+
+# LEDs
+PC6 LED OUTPUT LOW
+define HAL_GPIO_LED_ON  1
+
+# USART2, GPS
+PD5 USART2_TX USART2
+PD6 USART2_RX USART2
+
+# USART1, debug, disabled to save flash
+PB6 USART1_TX USART1
+PB7 USART1_RX USART1
+
+# SWD debugging
+PA13 JTMS-SWDIO SWD
+PA14 JTCK-SWCLK SWD
+
+# I2C2 bus
+PB11 I2C2_SDA I2C2
+PB10 I2C2_SCL I2C2
+
+
+# I2C buses
+I2C_ORDER I2C2
+
+# one SPI bus
+PA5 SPI1_SCK SPI1
+PA6 SPI1_MISO SPI1
+PA7 SPI1_MOSI SPI1
+
+# SPI CS
+PA4  MAG_CS CS
+
+# GPS PPS
+PA10 GPS_PPS_IN INPUT
+
+# SPI devices
+SPIDEV rm3100 SPI1 DEVID1 MAG_CS  MODE3   1*MHZ    1*MHZ
+
+# compass
+COMPASS RM3100 SPI:rm3100 false ROTATION_NONE
+COMPASS IST8310 I2C:0:0x0C false ROTATION_NONE
+
+define HAL_USE_ADC FALSE
+define STM32_ADC_USE_ADC1 FALSE
+define HAL_DISABLE_ADC_DRIVER TRUE
+
+define HAL_NO_GPIO_IRQ
+define HAL_USE_RTC FALSE
+define DMA_RESERVE_SIZE 0
+
+# enable CAN support
+PB8 CAN1_RX CAN1
+PB9 CAN1_TX CAN1
+PB5 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW
+
+define HAL_DEVICE_THREAD_STACK 768
+
+# we setup a small defaults.parm
+define AP_PARAM_MAX_EMBEDDED_PARAM 256
+
+# disable dual GPS and GPS blending to save flash space
+define GPS_MAX_RECEIVERS 1
+define GPS_MAX_INSTANCES 1
+define HAL_COMPASS_MAX_SENSORS 1
+
+# add support for moving baseline yaw
+define GPS_MOVING_BASELINE 1
+
+# GPS+MAG+BARO+LEDs
+define HAL_PERIPH_ENABLE_GPS
+define HAL_PERIPH_ENABLE_MAG
+
+# GPS on 1st port
+define HAL_PERIPH_GPS_PORT_DEFAULT 1
+
+# keep ROMFS uncompressed as we don't have enough RAM
+# to uncompress the bootloader at runtime
+env ROMFS_UNCOMPRESSED True
\ No newline at end of file
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat
index 6093b76e0aac0e..5bc0e9cbf912d9 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat
@@ -174,12 +174,10 @@ include ../include/minimize_fpv_osd.inc
 undef AP_CAMERA_MOUNT_ENABLED
 undef HAL_MOUNT_ENABLED
 undef HAL_MOUNT_SERVO_ENABLED
-undef QAUTOTUNE_ENABLED
 
 define AP_CAMERA_MOUNT_ENABLED 1
 define HAL_MOUNT_ENABLED 1
 define AP_MOUNT_BACKEND_DEFAULT_ENABLED 0
 define HAL_MOUNT_SERVO_ENABLED 1
-define QAUTOTUNE_ENABLED 1
 
 define DEFAULT_NTF_LED_TYPES 257
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc
index 005fb89895002b..3c6e149ee06ec9 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc
+++ b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc
@@ -102,7 +102,6 @@ define AP_LANDINGGEAR_ENABLED APM_BUILD_COPTER_OR_HELI
 # Plane-specific defines; these defines are only used in the Plane
 #  directory, but are seen across the entire codebase:
 define AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED 0
-define QAUTOTUNE_ENABLED 0
 
 # Copter-specific defines; these defines are only used in the Copter
 #  directory, but are seen across the entire codebase:
diff --git a/libraries/AP_HAL_ESP32/RCInput.cpp b/libraries/AP_HAL_ESP32/RCInput.cpp
index 2588f103f7e7ed..8e8a3dea6bdc1e 100644
--- a/libraries/AP_HAL_ESP32/RCInput.cpp
+++ b/libraries/AP_HAL_ESP32/RCInput.cpp
@@ -12,14 +12,24 @@
  * You should have received a copy of the GNU General Public License along
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
-#include <AP_Math/AP_Math.h>
+
 #include <GCS_MAVLink/GCS.h>
 
 #include "AP_HAL_ESP32.h"
 #include "RCInput.h"
 
+#include <AP_RCProtocol/AP_RCProtocol_config.h>
+
+#include <AP_Math/AP_Math.h>
+
+#ifndef HAL_NO_UARTDRIVER
+#include <GCS_MAVLink/GCS.h>
+#endif
+
 using namespace ESP32;
 
+extern const AP_HAL::HAL& hal;
+
 void RCInput::init()
 {
     if (_init) {
@@ -31,10 +41,26 @@ void RCInput::init()
 
 #ifdef HAL_ESP32_RCIN
     sig_reader.init();
+    pulse_input_enabled = true;
 #endif
+
     _init = true;
 }
 
+/*
+  enable or disable pulse input for RC input. This is used to reduce
+  load when we are decoding R/C via a UART
+*/
+void RCInput::pulse_input_enable(bool enable)
+{
+    pulse_input_enabled = enable;
+#if HAL_ESP32_RCIN
+    if (!enable) {
+        sig_reader.disable();
+    }
+#endif
+}
+
 bool RCInput::new_input()
 {
     if (!_init) {
@@ -89,23 +115,29 @@ uint8_t RCInput::read(uint16_t* periods, uint8_t len)
 
 void RCInput::_timer_tick(void)
 {
-#if AP_RCPROTOCOL_ENABLED
     if (!_init) {
         return;
     }
+#ifndef HAL_NO_UARTDRIVER
+    const char *rc_protocol = nullptr;
+    RCSource source = last_source;
+#endif
+
+#if AP_RCPROTOCOL_ENABLED
     AP_RCProtocol &rcprot = AP::RC();
 
 #ifdef HAL_ESP32_RCIN
-    uint32_t width_s0, width_s1;
-    while (sig_reader.read(width_s0, width_s1)) {
-        rcprot.process_pulse(width_s0, width_s1);
-
+    if (pulse_input_enabled) {
+        uint32_t width_s0, width_s1;
+        while (sig_reader.read(width_s0, width_s1)) {
+            rcprot.process_pulse(width_s0, width_s1);
+        }
     }
-
-#ifndef HAL_NO_UARTDRIVER
-    const char *rc_protocol = nullptr;
 #endif
 
+#endif  // AP_RCPROTOCOL_ENABLED
+
+#if AP_RCPROTOCOL_ENABLED
     if (rcprot.new_input()) {
         WITH_SEMAPHORE(rcin_mutex);
         _rcin_timestamp_last_signal = AP_HAL::micros();
@@ -113,18 +145,36 @@ void RCInput::_timer_tick(void)
         _num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS);
         rcprot.read(_rc_values, _num_channels);
         _rssi = rcprot.get_RSSI();
+        _rx_link_quality = rcprot.get_rx_link_quality();
 #ifndef HAL_NO_UARTDRIVER
         rc_protocol = rcprot.protocol_name();
+        source = rcprot.using_uart() ? RCSource::RCPROT_BYTES : RCSource::RCPROT_PULSES;
+        // printf("RCInput: decoding %s", last_protocol);
 #endif
     }
+#endif // AP_RCPROTOCOL_ENABLED
 
 #ifndef HAL_NO_UARTDRIVER
-    if (rc_protocol && rc_protocol != last_protocol) {
+    if (rc_protocol && (rc_protocol != last_protocol || source != last_source)) {
         last_protocol = rc_protocol;
-        GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "RCInput: decoding %s", last_protocol);
+        last_source = source;
+        GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "RCInput: decoding %s(%u)", last_protocol, unsigned(source));
     }
 #endif
 
+    // note, we rely on the vehicle code checking new_input()
+    // and a timeout for the last valid input to handle failsafe
+}
+
+/*
+  start a bind operation, if supported
+ */
+bool RCInput::rc_bind(int dsmMode)
+{
+#if AP_RCPROTOCOL_ENABLED
+    // ask AP_RCProtocol to start a bind
+    AP::RC().start_bind();
 #endif
-#endif // AP_RCPROTOCOL_ENABLED
+
+    return true;
 }
diff --git a/libraries/AP_HAL_ESP32/RCInput.h b/libraries/AP_HAL_ESP32/RCInput.h
index 370753d07f776f..7e8e885b1b90ac 100644
--- a/libraries/AP_HAL_ESP32/RCInput.h
+++ b/libraries/AP_HAL_ESP32/RCInput.h
@@ -17,9 +17,10 @@
 
 #include "AP_HAL_ESP32.h"
 #include "Semaphores.h"
-#include "RmtSigReader.h"
+
 #include <AP_RCProtocol/AP_RCProtocol.h>
 
+#include "RmtSigReader.h"
 
 #ifndef RC_INPUT_MAX_CHANNELS
 #define RC_INPUT_MAX_CHANNELS 18
@@ -33,21 +34,42 @@ class ESP32::RCInput : public AP_HAL::RCInput
     uint8_t num_channels() override;
     uint16_t read(uint8_t ch) override;
     uint8_t read(uint16_t* periods, uint8_t len) override;
-    void _timer_tick(void);
 
-    const char *protocol() const override
-    {
-        return last_protocol;
+    /* enable or disable pulse input for RC input. This is used to
+       reduce load when we are decoding R/C via a UART */
+    void pulse_input_enable(bool enable) override;
+
+    int16_t get_rssi(void) override {
+        return _rssi;
+    }
+    int16_t get_rx_link_quality(void) override {
+        return _rx_link_quality;
     }
+    const char *protocol() const override { return last_protocol; }
+
+    void _timer_tick(void);
+    bool rc_bind(int dsmMode) override;
+
 private:
     uint16_t _rc_values[RC_INPUT_MAX_CHANNELS] = {0};
+
     uint64_t _last_read;
     uint8_t _num_channels;
     Semaphore rcin_mutex;
     int16_t _rssi = -1;
+    int16_t _rx_link_quality = -1;
     uint32_t _rcin_timestamp_last_signal;
     bool _init;
     const char *last_protocol;
 
+    enum class RCSource {
+        NONE = 0,
+        IOMCU = 1,
+        RCPROT_PULSES = 2,
+        RCPROT_BYTES = 3,
+    } last_source;
+
+    bool pulse_input_enabled;
+
     ESP32::RmtSigReader sig_reader;
 };
diff --git a/libraries/AP_HAL_ESP32/Scheduler.cpp b/libraries/AP_HAL_ESP32/Scheduler.cpp
index 8b683b6e34dc30..39413f9331b2b3 100644
--- a/libraries/AP_HAL_ESP32/Scheduler.cpp
+++ b/libraries/AP_HAL_ESP32/Scheduler.cpp
@@ -565,7 +565,9 @@ void IRAM_ATTR Scheduler::_main_thread(void *arg)
         sched->delay_microseconds(250);
 
         // run stats periodically
+#ifdef SCHEDDEBUG
         sched->print_stats();
+#endif
         sched->print_main_loop_rate();
 
         if (ESP_OK != esp_task_wdt_reset()) {
diff --git a/libraries/AP_HAL_ESP32/Scheduler.h b/libraries/AP_HAL_ESP32/Scheduler.h
index db12ba73d89bd7..4946735f6c7da4 100644
--- a/libraries/AP_HAL_ESP32/Scheduler.h
+++ b/libraries/AP_HAL_ESP32/Scheduler.h
@@ -85,16 +85,16 @@ class ESP32::Scheduler : public AP_HAL::Scheduler
     static const int IO_PRIO      = 5;
     static const int STORAGE_PRIO = 4;
 
-    static const int TIMER_SS 	  = 4096;
-    static const int MAIN_SS      = 8192;
-    static const int RCIN_SS      = 4096;
-    static const int RCOUT_SS     = 4096;
-    static const int WIFI_SS1     = 6192;
-    static const int WIFI_SS2     = 6192;
-    static const int UART_SS      = 2048;
-    static const int DEVICE_SS    = 4096;
-    static const int IO_SS        = 4096;
-    static const int STORAGE_SS   = 8192;
+    static const int TIMER_SS     = 1024*3;
+    static const int MAIN_SS      = 1024*5;
+    static const int RCIN_SS      = 1024*3;
+    static const int RCOUT_SS     = 1024*1.5;
+    static const int WIFI_SS1     = 1024*2.25;
+    static const int WIFI_SS2     = 1024*2.25;
+    static const int UART_SS      = 1024*2.25;
+    static const int DEVICE_SS    = 1024*4;     // DEVICEBUS/s
+    static const int IO_SS        = 1024*3.5;   // APM_IO
+    static const int STORAGE_SS   = 1024*2;     // APM_STORAGE
 
 private:
     AP_HAL::HAL::Callbacks *callbacks;
diff --git a/libraries/AP_HAL_ESP32/Util.cpp b/libraries/AP_HAL_ESP32/Util.cpp
index 8585e23fc81516..6c4f66d18d2d78 100644
--- a/libraries/AP_HAL_ESP32/Util.cpp
+++ b/libraries/AP_HAL_ESP32/Util.cpp
@@ -92,36 +92,10 @@ void Util::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type)
 
 
 #if ENABLE_HEAP
-
-void *Util::allocate_heap_memory(size_t size)
-{
-    void *buf = calloc(1, size);
-    if (buf == nullptr) {
-        return nullptr;
-    }
-
-    multi_heap_handle_t *heap = (multi_heap_handle_t *)calloc(1, sizeof(multi_heap_handle_t));
-    if (heap != nullptr) {
-        auto hp = multi_heap_register(buf, size);
-        memcpy(heap, &hp, sizeof(multi_heap_handle_t));
-    }
-
-    return heap;
-}
-
-void *Util::heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size)
-{
-    if (heap == nullptr) {
-        return nullptr;
-    }
-
-    return multi_heap_realloc(*(multi_heap_handle_t *)heap, ptr, new_size);
-}
-
 /*
-  realloc implementation thanks to wolfssl, used by AP_Scripting
+  realloc implementation thanks to wolfssl, used by ExpandingString
  */
-void *Util::std_realloc(void *addr, size_t size)
+void *Util::std_realloc(void *addr, uint32_t size)
 {
     if (size == 0) {
         free(addr);
diff --git a/libraries/AP_HAL_ESP32/Util.h b/libraries/AP_HAL_ESP32/Util.h
index 8e9ba9dc942218..1e5a78bf097961 100644
--- a/libraries/AP_HAL_ESP32/Util.h
+++ b/libraries/AP_HAL_ESP32/Util.h
@@ -36,9 +36,7 @@ class ESP32::Util : public AP_HAL::Util
 
 #if ENABLE_HEAP
     // heap functions, note that a heap once alloc'd cannot be dealloc'd
-    virtual void *allocate_heap_memory(size_t size) override;
-    virtual void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) override;
-    virtual void *std_realloc(void *ptr, size_t new_size) override;
+    virtual void *std_realloc(void *ptr, uint32_t new_size) override;
 #endif // ENABLE_HEAP
 
     /*
@@ -85,10 +83,6 @@ class ESP32::Util : public AP_HAL::Util
     FlashBootloader flash_bootloader() override;
 #endif
 
-#if ENABLE_HEAP
-    // static memory_heap_t scripting_heap;
-#endif // ENABLE_HEAP
-
     // stm32F4 and F7 have 20 total RTC backup registers. We use the first one for boot type
     // flags, so 19 available for persistent data
     static_assert(sizeof(persistent_data) <= 19*4, "watchdog persistent data too large");
diff --git a/libraries/AP_HAL_ESP32/boards/esp32empty.h b/libraries/AP_HAL_ESP32/boards/esp32empty.h
index d95e30f9b2af95..bfea30b74d8cce 100644
--- a/libraries/AP_HAL_ESP32/boards/esp32empty.h
+++ b/libraries/AP_HAL_ESP32/boards/esp32empty.h
@@ -78,8 +78,10 @@
 //SPI Devices
 #define HAL_ESP32_SPI_DEVICES {}
 
-//RCIN
-#define HAL_ESP32_RCIN GPIO_NUM_36
+//RMT pin number
+#define HAL_ESP32_RMT_RX_PIN_NUMBER 4
+//RCIN pin number - NOTE: disabled due to issue with legacy rmt library.
+// #define HAL_ESP32_RCIN GPIO_NUM_36
 
 //RCOUT
 #define HAL_ESP32_RCOUT {GPIO_NUM_25, GPIO_NUM_27, GPIO_NUM_33, GPIO_NUM_32, GPIO_NUM_22, GPIO_NUM_21}
@@ -124,9 +126,6 @@
 //LED
 #define DEFAULT_NTF_LED_TYPES Notify_LED_None
 
-//RMT pin number
-#define HAL_ESP32_RMT_RX_PIN_NUMBER 4
-
 //SD CARD
 // Do u want to use mmc or spi mode for the sd card, this is board specific,
 // as mmc uses specific pins but is quicker,
diff --git a/libraries/AP_HAL_Linux/Util.cpp b/libraries/AP_HAL_Linux/Util.cpp
index d25bc87a3d809f..0a03fe4c81ac40 100644
--- a/libraries/AP_HAL_Linux/Util.cpp
+++ b/libraries/AP_HAL_Linux/Util.cpp
@@ -241,65 +241,6 @@ int Util::get_hw_arm32()
     return -ENOENT;
 }
 
-#if ENABLE_HEAP
-void *Util::allocate_heap_memory(size_t size)
-{
-    struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap));
-    if (new_heap != nullptr) {
-        new_heap->max_heap_size = size;
-        new_heap->current_heap_usage = 0;
-    }
-    return (void *)new_heap;
-}
-
-void *Util::heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size)
-{
-    if (h == nullptr) {
-        return nullptr;
-    }
-
-    struct heap *heapp = (struct heap*)h;
-
-    // extract appropriate headers. We use the old_size from the
-    // header not from the caller. We use SITL to catch cases they
-    // don't match (which would be a lua bug)
-    old_size = 0;
-    heap_allocation_header *old_header = nullptr;
-    if (ptr != nullptr) {
-        old_header = ((heap_allocation_header *)ptr) - 1;
-        old_size = old_header->allocation_size;
-    }
-
-    if ((heapp->current_heap_usage + new_size - old_size) > heapp->max_heap_size) {
-        // fail the allocation as we don't have the memory. Note that we don't simulate fragmentation
-        return nullptr;
-    }
-
-    heapp->current_heap_usage -= old_size;
-    if (new_size == 0) {
-       free(old_header);
-       return nullptr;
-    }
-
-    heap_allocation_header *new_header = (heap_allocation_header *)malloc(new_size + sizeof(heap_allocation_header));
-    if (new_header == nullptr) {
-        // total failure to allocate, this is very surprising in SITL
-        return nullptr;
-    }
-    heapp->current_heap_usage += new_size;
-    new_header->allocation_size = new_size;
-    void *new_mem = new_header + 1;
-
-    if (ptr == nullptr) {
-        return new_mem;
-    }
-    memcpy(new_mem, ptr, old_size > new_size ? new_size : old_size);
-    free(old_header);
-    return new_mem;
-}
-
-#endif // ENABLE_HEAP
-
 /**
  * This method will read random values with set size.
  */
diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h
index 11a29dacb31c57..5374b1d95df128 100644
--- a/libraries/AP_HAL_Linux/Util.h
+++ b/libraries/AP_HAL_Linux/Util.h
@@ -71,12 +71,6 @@ class Util : public AP_HAL::Util {
     bool get_system_id(char buf[50]) override;
     bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override;
 
-#if ENABLE_HEAP
-    // heap functions, note that a heap once alloc'd cannot be dealloc'd
-    virtual void *allocate_heap_memory(size_t size) override;
-    virtual void *heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size) override;
-#endif // ENABLE_HEAP
-    
     /*
      * Write a string as specified by @fmt to the file in @path. Note this
      * should not be used on hot path since it will open, write and close the
@@ -115,18 +109,6 @@ class Util : public AP_HAL::Util {
     const char *custom_storage_directory = nullptr;
     const char *custom_defaults = HAL_PARAM_DEFAULTS_PATH;
     static const char *_hw_names[UTIL_NUM_HARDWARES];
-
-#if ENABLE_HEAP
-    struct heap_allocation_header {
-        size_t allocation_size; // size of allocated block, not including this header
-    };
-
-    struct heap {
-      size_t max_heap_size;
-      size_t current_heap_usage;
-    };
-#endif // ENABLE_HEAP
-
 };
 
 }
diff --git a/libraries/AP_HAL_QURT/UARTDriver.cpp b/libraries/AP_HAL_QURT/UARTDriver.cpp
index f1a056df10812f..db91beb6025d95 100644
--- a/libraries/AP_HAL_QURT/UARTDriver.cpp
+++ b/libraries/AP_HAL_QURT/UARTDriver.cpp
@@ -150,9 +150,19 @@ UARTDriver_MAVLinkUDP::UARTDriver_MAVLinkUDP(uint8_t instance) : inst(instance)
     register_mavlink_data_callback(instance, _mavlink_data_cb, (void *) this);
 }
 
+void UARTDriver_MAVLinkUDP::check_rx_seq(uint32_t seq)
+{
+	if (seq != rx_seq)
+	{
+		HAP_PRINTF("Sequence mismatch for instance %u. Expected %u, got %u", inst, rx_seq, seq);
+	}
+	rx_seq++;
+}
+
 void UARTDriver_MAVLinkUDP::_mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p)
 {
     auto *driver = (UARTDriver_MAVLinkUDP *)p;
+    driver->check_rx_seq(msg->seq);
     driver->_readbuf.write(msg->data, msg->data_length);
 }
 
@@ -183,7 +193,7 @@ bool UARTDriver_MAVLinkUDP::_write_pending_bytes(void)
     }
     msg.msg_id = QURT_MSG_ID_MAVLINK_MSG;
     msg.inst = inst;
-    msg.seq = seq++;
+    msg.seq = tx_seq++;
     msg.data_length = _writebuf.read(msg.data, n);
 
     return qurt_rpc_send(msg);
diff --git a/libraries/AP_HAL_QURT/UARTDriver.h b/libraries/AP_HAL_QURT/UARTDriver.h
index d27ef2d6b79596..575e27707ba63a 100644
--- a/libraries/AP_HAL_QURT/UARTDriver.h
+++ b/libraries/AP_HAL_QURT/UARTDriver.h
@@ -83,6 +83,8 @@ class QURT::UARTDriver_MAVLinkUDP : public QURT::UARTDriver
 
     bool _write_pending_bytes(void) override;
 
+    void check_rx_seq(uint32_t seq);
+
     uint32_t bw_in_bytes_per_second() const override
     {
         return 250000 * 3;
@@ -95,7 +97,8 @@ class QURT::UARTDriver_MAVLinkUDP : public QURT::UARTDriver
 private:
     static void _mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p);
     uint8_t inst;
-    uint32_t seq;
+    uint32_t tx_seq;
+    uint32_t rx_seq;
 };
 
 /*
diff --git a/libraries/AP_HAL_QURT/Util.cpp b/libraries/AP_HAL_QURT/Util.cpp
index a0891c3cc34da2..23444db8c9d25d 100644
--- a/libraries/AP_HAL_QURT/Util.cpp
+++ b/libraries/AP_HAL_QURT/Util.cpp
@@ -32,62 +32,3 @@ Util::safety_state Util::safety_switch_state(void)
     }
     return SAFETY_ARMED;
 }
-
-#if ENABLE_HEAP
-void *Util::allocate_heap_memory(size_t size)
-{
-    struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap));
-    if (new_heap != nullptr) {
-        new_heap->max_heap_size = size;
-        new_heap->current_heap_usage = 0;
-    }
-    return (void *)new_heap;
-}
-
-void *Util::heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size)
-{
-    if (h == nullptr) {
-        return nullptr;
-    }
-
-    struct heap *heapp = (struct heap*)h;
-
-    // extract appropriate headers. We use the old_size from the
-    // header not from the caller. We use SITL to catch cases they
-    // don't match (which would be a lua bug)
-    old_size = 0;
-    heap_allocation_header *old_header = nullptr;
-    if (ptr != nullptr) {
-        old_header = ((heap_allocation_header *)ptr) - 1;
-        old_size = old_header->allocation_size;
-    }
-
-    if ((heapp->current_heap_usage + new_size - old_size) > heapp->max_heap_size) {
-        // fail the allocation as we don't have the memory. Note that we don't simulate fragmentation
-        return nullptr;
-    }
-
-    heapp->current_heap_usage -= old_size;
-    if (new_size == 0) {
-        free(old_header);
-        return nullptr;
-    }
-
-    heap_allocation_header *new_header = (heap_allocation_header *)malloc(new_size + sizeof(heap_allocation_header));
-    if (new_header == nullptr) {
-        // can't get the new memory, old memory is kept
-        return nullptr;
-    }
-    heapp->current_heap_usage += new_size;
-    new_header->allocation_size = new_size;
-    void *new_mem = new_header + 1;
-
-    if (ptr == nullptr) {
-        return new_mem;
-    }
-    memcpy(new_mem, ptr, old_size > new_size ? new_size : old_size);
-    free(old_header);
-    return new_mem;
-}
-
-#endif // ENABLE_HEAP
diff --git a/libraries/AP_HAL_QURT/Util.h b/libraries/AP_HAL_QURT/Util.h
index f1eea1493c7ab0..4fb0618beb40df 100644
--- a/libraries/AP_HAL_QURT/Util.h
+++ b/libraries/AP_HAL_QURT/Util.h
@@ -25,19 +25,4 @@ class QURT::Util : public AP_HAL::Util
       return state of safety switch, if applicable
      */
     enum safety_state safety_switch_state(void) override;
-    
-#if ENABLE_HEAP
-    // heap functions, note that a heap once alloc'd cannot be dealloc'd
-    virtual void *allocate_heap_memory(size_t size) override;
-    virtual void *heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size) override;
-
-    struct heap_allocation_header {
-        uint64_t allocation_size; // size of allocated block, not including this header
-    };
-
-    struct heap {
-        size_t max_heap_size;
-        size_t current_heap_usage;
-    };
-#endif // ENABLE_HEAP
 };
diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp
index 6f331f6a1ffa6d..969f697620fb36 100644
--- a/libraries/AP_HAL_QURT/replace.cpp
+++ b/libraries/AP_HAL_QURT/replace.cpp
@@ -151,7 +151,6 @@ typedef void (*mavlink_data_callback_t)(const struct qurt_rpc_msg *msg, void* p)
 
 static mavlink_data_callback_t mav_cb[MAX_MAVLINK_INSTANCES];
 static void *mav_cb_ptr[MAX_MAVLINK_INSTANCES];
-static uint32_t expected_seq;
 
 void register_mavlink_data_callback(uint8_t instance, mavlink_data_callback_t func, void *p)
 {
@@ -172,10 +171,6 @@ int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes)
     if (msg->data_length + QURT_RPC_MSG_HEADER_LEN != data_len_in_bytes) {
         return 0;
     }
-    if (msg->seq != expected_seq) {
-        HAP_PRINTF("Bad sequence %u %u", msg->seq, expected_seq);
-    }
-    expected_seq = msg->seq + 1;
 
     switch (msg->msg_id) {
     case QURT_MSG_ID_MAVLINK_MSG: {
diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp
index 54a530bc447b17..47a625d4d77670 100644
--- a/libraries/AP_HAL_SITL/SITL_State_common.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp
@@ -304,13 +304,9 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const
         return ais;
 #endif
     } else if (strncmp(name, "gps", 3) == 0) {
-        const char *p = strchr(name, ':');
-        if (p == nullptr) {
-            AP_HAL::panic("Need a GPS number (e.g. sim:gps:1)");
-        }
-        uint8_t x = atoi(p+1);
+        uint8_t x = atoi(arg);
         if (x <= 0 || x > ARRAY_SIZE(gps)) {
-            AP_HAL::panic("Bad GPS number %u", x);
+            AP_HAL::panic("Bad GPS number %u (%s)", x, arg);
         }
         gps[x-1] = NEW_NOTHROW SITL::GPS(x-1);
         return gps[x-1];
diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h
index 95ef129c1cce7a..472f7dfefa6601 100644
--- a/libraries/AP_HAL_SITL/SITL_State_common.h
+++ b/libraries/AP_HAL_SITL/SITL_State_common.h
@@ -231,7 +231,7 @@ class HALSITL::SITL_State_Common {
     const char *defaults_path = HAL_PARAM_DEFAULTS_PATH;
 
     // simulated GPS devices
-    SITL::GPS *gps[2];  // constrained by # of parameter sets
+    SITL::GPS *gps[AP_SIM_MAX_GPS_SENSORS];  // constrained by # of parameter sets
 
     // Simulated ELRS radio
     SITL::ELRS *elrs;
diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp
index e809f4a43bb58e..358fb614824129 100644
--- a/libraries/AP_HAL_SITL/UARTDriver.cpp
+++ b/libraries/AP_HAL_SITL/UARTDriver.cpp
@@ -77,11 +77,11 @@ void UARTDriver::_begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
     if (strcmp(path, "GPS1") == 0) {
         /* gps */
         _connected = true;
-        _sim_serial_device = _sitlState->create_serial_sim("gps:1", "", _portNumber);
+        _sim_serial_device = _sitlState->create_serial_sim("gps", "1", _portNumber);
     } else if (strcmp(path, "GPS2") == 0) {
         /* 2nd gps */
         _connected = true;
-        _sim_serial_device = _sitlState->create_serial_sim("gps:2", "", _portNumber);
+        _sim_serial_device = _sitlState->create_serial_sim("gps", "2", _portNumber);
     } else {
         /* parse type:args:flags string for path. 
            For example:
diff --git a/libraries/AP_HAL_SITL/Util.cpp b/libraries/AP_HAL_SITL/Util.cpp
index 1805e21720c594..753e97b8ce4ab4 100644
--- a/libraries/AP_HAL_SITL/Util.cpp
+++ b/libraries/AP_HAL_SITL/Util.cpp
@@ -85,68 +85,6 @@ bool HALSITL::Util::get_system_id(char buf[50])
     return get_system_id_unformatted((uint8_t *)buf, len);
 }
 
-#if ENABLE_HEAP
-void *HALSITL::Util::allocate_heap_memory(size_t size)
-{
-    struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap));
-    if (new_heap != nullptr) {
-        new_heap->scripting_max_heap_size = size;
-        new_heap->current_heap_usage = 0;
-    }
-    return (void *)new_heap;
-}
-
-void *HALSITL::Util::heap_realloc(void *heap_ptr, void *ptr, size_t old_size, size_t new_size)
-{
-    if (heap_ptr == nullptr) {
-        return nullptr;
-    }
-
-    struct heap *heapp = (struct heap*)heap_ptr;
-
-    // extract appropriate headers
-    size_t old_size_header = 0;
-    heap_allocation_header *old_header = nullptr;
-    if (ptr != nullptr) {
-        old_header = ((heap_allocation_header *)ptr) - 1;
-        old_size_header = old_header->allocation_size;
-#if !defined(HAL_BUILD_AP_PERIPH)
-        if (old_size_header != old_size && new_size != 0) {
-            INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
-        }
-#endif
-    }
-
-    if ((heapp->current_heap_usage + new_size - old_size) > heapp->scripting_max_heap_size) {
-        // fail the allocation as we don't have the memory. Note that we don't simulate fragmentation
-        return nullptr;
-    }
-
-    heapp->current_heap_usage -= old_size_header;
-    if (new_size == 0) {
-       free(old_header);
-       return nullptr;
-    }
-
-    heap_allocation_header *new_header = (heap_allocation_header *)malloc(new_size + sizeof(heap_allocation_header));
-    if (new_header == nullptr) {
-        // total failure to allocate, this is very surprising in SITL
-        return nullptr;
-    }
-    heapp->current_heap_usage += new_size;
-    new_header->allocation_size = new_size;
-    void *new_mem = new_header + 1;
-
-    if (ptr == nullptr) {
-        return new_mem;
-    }
-    memcpy(new_mem, ptr, old_size > new_size ? new_size : old_size);
-    free(old_header);
-    return new_mem;
-}
-
-#endif // ENABLE_HEAP
-
 #if !defined(HAL_BUILD_AP_PERIPH)
 enum AP_HAL::Util::safety_state HALSITL::Util::safety_switch_state(void)
 {
diff --git a/libraries/AP_HAL_SITL/Util.h b/libraries/AP_HAL_SITL/Util.h
index ac73c6a365a236..66cfd2a97737ea 100644
--- a/libraries/AP_HAL_SITL/Util.h
+++ b/libraries/AP_HAL_SITL/Util.h
@@ -21,8 +21,8 @@ class HALSITL::Util : public AP_HAL::Util {
        how much free memory do we have in bytes. 
      */
     uint32_t available_memory(void) override {
-        // SITL is assumed to always have plenty of memory. Return 128k for now
-        return 0x20000;
+        // SITL is assumed to always have plenty of memory. Return 512k for now
+        return 512*1024;
     }
 
     // get path to custom defaults file for AP_Param
@@ -43,12 +43,6 @@ class HALSITL::Util : public AP_HAL::Util {
     bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override;
     void dump_stack_trace();
 
-#if ENABLE_HEAP
-    // heap functions, note that a heap once alloc'd cannot be dealloc'd
-    void *allocate_heap_memory(size_t size) override;
-    void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) override;
-#endif // ENABLE_HEAP
-
 #ifdef WITH_SITL_TONEALARM
     bool toneAlarm_init(uint8_t types) override { return _toneAlarm.init(); }
     void toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override {
@@ -90,17 +84,6 @@ class HALSITL::Util : public AP_HAL::Util {
     static ToneAlarm_SF _toneAlarm;
 #endif
 
-#if ENABLE_HEAP
-    struct heap_allocation_header {
-        size_t allocation_size; // size of allocated block, not including this header
-    };
-
-    struct heap {
-      size_t scripting_max_heap_size;
-      size_t current_heap_usage;
-    };
-#endif // ENABLE_HEAP
-
     int saved_argc;
     char *const *saved_argv;
 
diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index e1c6f5d8634442..93f0b7bc1dcb2d 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -43,6 +43,7 @@ enum ioevents {
     IOEVENT_SET_DSHOT_PERIOD,
     IOEVENT_SET_CHANNEL_MASK,
     IOEVENT_DSHOT,
+    IOEVENT_PROFILED,
 };
 
 // max number of consecutve protocol failures we accept before raising
@@ -89,7 +90,9 @@ void AP_IOMCU::init(void)
         crc_is_ok = true;
     }
 #endif
-
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    use_safety_as_led = boardconfig->use_safety_as_led();
+#endif
     if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_IOMCU::thread_main, void), "IOMCU",
                                       1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
         AP_HAL::panic("Unable to allocate IOMCU thread");
@@ -300,6 +303,16 @@ void AP_IOMCU::thread_main(void)
         }
         mask &= ~EVENT_MASK(IOEVENT_DSHOT);
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+        if (mask & EVENT_MASK(IOEVENT_PROFILED)) {
+            if (!write_registers(PAGE_PROFILED, 0, sizeof(profiled)/sizeof(uint16_t), (const uint16_t*)&profiled)) {
+                event_failed(mask);
+                continue;
+            }
+        }
+        mask &= ~EVENT_MASK(IOEVENT_PROFILED);
+#endif
+
         // check for regular timed events
         uint32_t now = AP_HAL::millis();
         if (now - last_rc_read_ms > 20) {
@@ -1439,6 +1452,23 @@ void AP_IOMCU::toggle_GPIO(uint8_t pin)
     trigger_event(IOEVENT_GPIO);
 }
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+// set profiled R G B values
+void AP_IOMCU::set_profiled(uint8_t r, uint8_t g, uint8_t b)
+{
+    if (!use_safety_as_led) {
+        return;
+    }
+    if (r == profiled.red && g == profiled.green && b == profiled.blue) {
+        return;
+    }
+    profiled.magic = PROFILED_ENABLE_MAGIC;
+    profiled.red = r;
+    profiled.green = g;
+    profiled.blue = b;
+    trigger_event(IOEVENT_PROFILED);
+}
+#endif
 
 namespace AP {
     AP_IOMCU *iomcu(void) {
diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
index da6d55a03dce7c..3b4f315a1406b4 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.h
+++ b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -176,6 +176,11 @@ class AP_IOMCU
     // toggle a output pin
     void toggle_GPIO(uint8_t pin);
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    // set profiled values
+    void set_profiled(uint8_t r, uint8_t g, uint8_t b);
+#endif
+
     // channel group masks
     const uint8_t ch_masks[3] = { 0x03,0x0C,0xF0 };
 
@@ -297,6 +302,11 @@ class AP_IOMCU
     // output mode values
     struct page_mode_out mode_out;
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    // profiled control
+    struct page_profiled profiled {0, 255, 255, 255}; // by default, white
+#endif
+
     // IMU heater duty cycle
     uint8_t heater_duty_cycle;
 
@@ -310,6 +320,9 @@ class AP_IOMCU
     bool detected_io_reset;
     bool initialised;
     bool is_chibios_backend;
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    bool use_safety_as_led;
+#endif
 
     uint32_t protocol_fail_count;
     uint32_t protocol_count;
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index adac0faefe9236..e3277b764e2957 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -489,6 +489,10 @@ void AP_IOMCU_FW::update()
         last_status_ms = now;
         page_status_update();
     }
+
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    profiled_update();
+#endif
 #ifdef HAL_WITH_BIDIR_DSHOT
     // EDT updates are semt at ~1Hz per ESC, but we want to make sure
     // that we don't delay updates unduly so sample at 5Hz
@@ -1029,6 +1033,20 @@ bool AP_IOMCU_FW::handle_code_write()
         break;
     }
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    case PAGE_PROFILED:
+        if (rx_io_packet.count != 2 || (rx_io_packet.regs[0] & 0xFF) != PROFILED_ENABLE_MAGIC) {
+            return false;
+        }
+        profiled_brg[0] = rx_io_packet.regs[0] >> 8;
+        profiled_brg[1] = rx_io_packet.regs[1] & 0xFF;
+        profiled_brg[2] = rx_io_packet.regs[1] >> 8;
+        // push new led data
+        profiled_num_led_pushed = 0;
+        profiled_control_enabled = true;
+        break;
+#endif
+
     default:
         break;
     }
@@ -1063,6 +1081,48 @@ void AP_IOMCU_FW::calculate_fw_crc(void)
     reg_setup.crc[1] = sum >> 16;
 }
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+// bitbang profiled bitstream, 8-10 chunks at a time
+// Max time taken per call is ~7us
+void AP_IOMCU_FW::profiled_update(void)
+{
+    if (profiled_num_led_pushed > PROFILED_LED_LEN) {
+        profiled_byte_index = 0;
+        profiled_leading_zeros = PROFILED_LEADING_ZEROS;
+        return;
+    }
+
+    // push 10 zero leading bits at a time
+    if (profiled_leading_zeros != 0) {
+        for (uint8_t i = 0; i < 10; i++) {
+            palClearLine(HAL_GPIO_PIN_SAFETY_INPUT);
+            palSetLine(HAL_GPIO_PIN_SAFETY_INPUT);
+            profiled_leading_zeros--;
+        }
+        return;
+    }
+
+    if ((profiled_byte_index == 0) ||
+        (profiled_byte_index == PROFILED_OUTPUT_BYTE_LEN)) {
+        // start bit
+        palClearLine(HAL_GPIO_PIN_SAFETY_INPUT);
+        palSetLine(HAL_GPIO_PIN_SAFETY_LED);
+        palSetLine(HAL_GPIO_PIN_SAFETY_INPUT);
+        profiled_byte_index = 0;
+        profiled_num_led_pushed++;
+    }
+
+    uint8_t byte_val = profiled_brg[profiled_byte_index];
+    for (uint8_t i = 0; i < 8; i++) {
+        palClearLine(HAL_GPIO_PIN_SAFETY_INPUT);
+        palWriteLine(HAL_GPIO_PIN_SAFETY_LED, byte_val & 1);
+        byte_val >>= 1;
+        palSetLine(HAL_GPIO_PIN_SAFETY_INPUT);
+    }
+
+    profiled_byte_index++;
+}
+#endif
 
 /*
   update safety state
@@ -1076,6 +1136,15 @@ void AP_IOMCU_FW::safety_update(void)
     }
     safety_update_ms = now;
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    if (profiled_control_enabled) {
+        // set line mode to output for safety input pin
+        palSetLineMode(HAL_GPIO_PIN_SAFETY_INPUT, PAL_MODE_OUTPUT_PUSHPULL);
+        palSetLineMode(HAL_GPIO_PIN_SAFETY_LED, PAL_MODE_OUTPUT_PUSHPULL);
+        return;
+    }
+#endif
+
     bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_INPUT);
     if (safety_pressed) {
         if (reg_status.flag_safety_off && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_ON)) {
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h
index 51dbe418bfd03d..df67719f1ab588 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.h
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h
@@ -17,6 +17,10 @@
 #define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
 #define SERVO_COUNT 8
 
+#define PROFILED_LED_LEN         2
+#define PROFILED_OUTPUT_BYTE_LEN 3
+#define PROFILED_LEADING_ZEROS   50
+
 class AP_IOMCU_FW {
 public:
     void process_io_packet();
@@ -36,6 +40,9 @@ class AP_IOMCU_FW {
     bool handle_code_write();
     bool handle_code_read();
     void schedule_reboot(uint32_t time_ms);
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    void profiled_update();
+#endif
     void safety_update();
     void rcout_config_update();
     void rcin_serial_init();
@@ -81,6 +88,14 @@ class AP_IOMCU_FW {
 
     uint16_t last_channel_mask;
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    uint8_t profiled_byte_index;
+    uint8_t profiled_leading_zeros;
+    uint8_t profiled_num_led_pushed;
+    uint8_t profiled_brg[3];
+    bool profiled_control_enabled;
+#endif
+
     // CONFIG values
     struct page_config config;
 
@@ -117,6 +132,11 @@ class AP_IOMCU_FW {
     // output mode values
     struct page_mode_out mode_out;
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    // profiled control values
+    struct page_profiled profiled;
+#endif
+
     uint16_t last_output_mode_mask;
     uint16_t last_output_bdmask;
     uint16_t last_output_esc_type;
diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
index f13580ec47444b..cc43005f042a1b 100644
--- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h
+++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h
@@ -7,6 +7,10 @@
   common protocol definitions between AP_IOMCU and iofirmware
  */
 
+#ifndef AP_IOMCU_PROFILED_SUPPORT_ENABLED
+#define AP_IOMCU_PROFILED_SUPPORT_ENABLED 0
+#endif
+
 // 22 is enough for the rc_input page in one transfer
 #define PKT_MAX_REGS 22
 // The number of channels that can be propagated - due to SBUS_OUT is higher than the physical channels
@@ -68,6 +72,9 @@ enum iopage {
     PAGE_RAW_DSHOT_TELEM_5_8 = 205,
     PAGE_RAW_DSHOT_TELEM_9_12 = 206,
     PAGE_RAW_DSHOT_TELEM_13_16 = 207,
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    PAGE_PROFILED = 208,
+#endif
 };
 
 // setup page registers
@@ -114,6 +121,8 @@ enum iopage {
 #define PAGE_REG_SETUP_FORCE_SAFETY_ON  14
 #define FORCE_SAFETY_MAGIC 22027
 
+#define PROFILED_ENABLE_MAGIC 123
+
 struct page_config {
     uint16_t protocol_version;
     uint16_t protocol_version2;
@@ -230,3 +239,12 @@ struct page_dshot_telem {
     uint8_t   edt2_stress[4];
 #endif
 };
+
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+struct __attribute__((packed, aligned(2))) page_profiled {
+    uint8_t magic;
+    uint8_t blue;
+    uint8_t red;
+    uint8_t green;
+};
+#endif
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
index ddfc05eac251f8..adccda60235199 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_ADIS1647x.cpp
@@ -171,22 +171,22 @@ bool AP_InertialSensor_ADIS1647x::check_product_id(uint16_t &prod_id)
         opmode = OpMode::Delta32;
         expected_sample_rate_hz = 1200;
         accel_scale = 392.0 / 2097152000.0;
-        dvel_scale = 400.0 / 0x7FFFFFFF;
+        dvel_scale = 400.0 / (float)0x7FFFFFFF;
         _clip_limit = (40.0f - 0.5f) * GRAVITY_MSS;
         // RANG_MDL register used for gyro range
         uint16_t rang_mdl = read_reg16(REG_RANG_MDL);
         switch ((rang_mdl >> 2) & 3) {
         case 0:
             gyro_scale = radians(125) / 0x4E200000;
-            dangle_scale = radians(360.0 / 0x7FFFFFFF);
+            dangle_scale = radians(360.0 / (float)0x7FFFFFFF);
             break;
         case 1:
             gyro_scale = radians(500) / 0x4E200000;
-            dangle_scale = radians(720.0 / 0x7FFFFFFF);
+            dangle_scale = radians(720.0 / (float)0x7FFFFFFF);
             break;
         case 3:
             gyro_scale = radians(2000) / 0x4E200000;
-            dangle_scale = radians(2160.0 / 0x7FFFFFFF);
+            dangle_scale = radians(2160.0 / (float)0x7FFFFFFF);
             break;
         default:
             return false;
diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h
index 432ce26376bb56..d2c014eb3341c5 100644
--- a/libraries/AP_Logger/LogStructure.h
+++ b/libraries/AP_Logger/LogStructure.h
@@ -556,6 +556,7 @@ struct PACKED log_Performance {
     uint32_t i2c_count;
     uint32_t i2c_isr_count;
     uint32_t extra_loop_us;
+    uint64_t rtc;
 };
 
 struct PACKED log_SRTL {
@@ -799,7 +800,7 @@ struct PACKED log_VER {
 // @Field: txp: transmitted packet count
 // @Field: rxp: received packet count
 // @Field: rxdp: perceived number of packets we never received
-// @Field: flags: compact representation of some stage of the channel
+// @Field: flags: compact representation of some state of the channel
 // @FieldBitmaskEnum: flags: GCS_MAVLINK::Flags
 // @Field: ss: stream slowdown is the number of ms being added to each message to fit within bandwidth
 // @Field: tf: times buffer was full when a message was going to be sent
@@ -895,14 +896,15 @@ struct PACKED log_VER {
 // @Field: MaxT: Maximum loop time
 // @Field: Mem: Free memory available
 // @Field: Load: System processor load
-// @Field: IntE: Internal error mask; which internal errors have been detected
-// @FieldBitmaskEnum: IntE: AP_InternalError::error_t
+// @Field: InE: Internal error mask; which internal errors have been detected
+// @FieldBitmaskEnum: InE: AP_InternalError::error_t
 // @Field: ErrL: Internal error line number; last line number on which a internal error was detected
-// @Field: ErrC: Internal error count; how many internal errors have been detected
+// @Field: ErC: Internal error count; how many internal errors have been detected
 // @Field: SPIC: Number of SPI transactions processed
 // @Field: I2CC: Number of i2c transactions processed
 // @Field: I2CI: Number of i2c interrupts serviced
 // @Field: Ex: number of microseconds being added to each loop to address scheduler overruns
+// @Field: R: RTC time, time since Unix epoch
 
 // @LoggerMessage: POWR
 // @Description: System power information
@@ -1211,7 +1213,7 @@ LOG_STRUCTURE_FROM_MOUNT \
     LOG_STRUCTURE_FROM_BEACON                                       \
     LOG_STRUCTURE_FROM_PROXIMITY                                    \
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance),                     \
-      "PM",  "QHHHIIHHIIIIII", "TimeUS,LR,NLon,NL,MaxT,Mem,Load,ErrL,IntE,ErrC,SPIC,I2CC,I2CI,Ex", "sz---b%------s", "F----0A------F" }, \
+      "PM",  "QHHHIIHHIIIIIIQ", "TimeUS,LR,NLon,NL,MaxT,Mem,Load,ErrL,InE,ErC,SPIC,I2CC,I2CI,Ex,R", "sz---b%------ss", "F----0A------FF" }, \
     { LOG_SRTL_MSG, sizeof(log_SRTL), \
       "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }, \
 LOG_STRUCTURE_FROM_AVOIDANCE \
diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp
index 22b4ce29501316..d27beca79e9ca2 100644
--- a/libraries/AP_Mission/AP_Mission.cpp
+++ b/libraries/AP_Mission/AP_Mission.cpp
@@ -2446,16 +2446,22 @@ bool AP_Mission::jump_to_closest_mission_leg(const Location &current_loc)
     uint16_t landing_start_index = 0;
     float min_distance = -1;
 
+    // This defines the maximum number of waypoints that will be searched, this limits the worst case runtime
+    uint16_t search_remaining = 1000;
+
     // Go through mission and check each DO_RETURN_PATH_START
     for (uint16_t i = 1; i < num_commands(); i++) {
-        Mission_Command tmp;
-        if (read_cmd_from_storage(i, tmp) && (tmp.id == MAV_CMD_DO_RETURN_PATH_START)) {
+        if (get_command_id(i) == uint16_t(MAV_CMD_DO_RETURN_PATH_START)) {
             uint16_t tmp_index;
             float tmp_distance;
-            if (distance_to_mission_leg(i, tmp_distance, tmp_index, current_loc) && (min_distance < 0 || tmp_distance <= min_distance)){
+            if (distance_to_mission_leg(i, search_remaining, tmp_distance, tmp_index, current_loc) && (min_distance < 0 || tmp_distance <= min_distance)){
                 min_distance = tmp_distance;
                 landing_start_index = tmp_index;
             }
+            if (search_remaining == 0) {
+                // Run out of time to search, stop and return the best so far
+                break;
+            }
         }
     }
 
@@ -2626,7 +2632,7 @@ bool AP_Mission::distance_to_landing(uint16_t index, float &tot_distance, Locati
 
 // Approximate the distance travelled to return to the mission path. DO_JUMP commands are observed in look forward.
 // Stop searching once reaching a landing or do-land-start
-bool AP_Mission::distance_to_mission_leg(uint16_t start_index, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc)
+bool AP_Mission::distance_to_mission_leg(uint16_t start_index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc)
 {
     Location prev_loc;
     Mission_Command temp_cmd;
@@ -2642,7 +2648,7 @@ bool AP_Mission::distance_to_mission_leg(uint16_t start_index, float &rejoin_dis
 
     // run through remainder of mission to approximate a distance to landing
     uint16_t index = start_index;
-    for (uint8_t i=0; i<UINT8_MAX; i++) {
+    for (; search_remaining > 0; search_remaining--) {
         // search until the end of the mission command list
         for (uint16_t cmd_index = index; cmd_index <= (unsigned)_cmd_total; cmd_index++) {
             if (get_next_cmd(cmd_index, temp_cmd, true, false)) {
diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h
index 8926749d43596f..f3a1c8a043fa92 100644
--- a/libraries/AP_Mission/AP_Mission.h
+++ b/libraries/AP_Mission/AP_Mission.h
@@ -869,7 +869,7 @@ class AP_Mission
 
     // Approximate the distance traveled to return to the mission path. DO_JUMP commands are observed in look forward.
     // Stop searching once reaching a landing or do-land-start
-    bool distance_to_mission_leg(uint16_t index, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc);
+    bool distance_to_mission_leg(uint16_t index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc);
 
     // calculate the location of a resume cmd wp
     bool calc_rewind_pos(Mission_Command& rewind_cmd);
diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp
index 86296d814be01f..410de2660ce690 100644
--- a/libraries/AP_Mount/AP_Mount_Xacti.cpp
+++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp
@@ -24,7 +24,6 @@ extern const AP_HAL::HAL& hal;
 #define AP_MOUNT_XACTI_DEBUG 0
 #define debug(fmt, args ...) do { if (AP_MOUNT_XACTI_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Xacti: " fmt, ## args); } } while (0)
 
-bool AP_Mount_Xacti::_subscribed = false;
 AP_Mount_Xacti::DetectedModules AP_Mount_Xacti::_detected_modules[];
 HAL_Semaphore AP_Mount_Xacti::_sem_registry;
 const char* AP_Mount_Xacti::send_text_prefix = "Xacti:";
@@ -417,25 +416,13 @@ void AP_Mount_Xacti::send_target_angles(float pitch_rad, float yaw_rad, bool yaw
 }
 
 // subscribe to Xacti DroneCAN messages
-void AP_Mount_Xacti::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_Mount_Xacti::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    // return immediately if DroneCAN is unavailable
-    if (ap_dronecan == nullptr) {
-        GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s DroneCAN subscribe failed", send_text_prefix);
-        return;
-    }
-
-    _subscribed = true;
+    const auto driver_index = ap_dronecan->get_driver_index();
 
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_gimbal_attitude_status, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("gimbal_attitude_status_sub");
-        _subscribed = false;
-    }
-
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_gnss_status_req, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("gnss_status_req_sub");
-        _subscribed = false;
-    }
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_gimbal_attitude_status, driver_index) != nullptr)
+        && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_gnss_status_req, driver_index) != nullptr)
+    ;
 }
 
 // register backend in detected modules array used to map DroneCAN port and node id to backend
diff --git a/libraries/AP_Mount/AP_Mount_Xacti.h b/libraries/AP_Mount/AP_Mount_Xacti.h
index 0057f4ae371ad8..14643a7acc49ec 100644
--- a/libraries/AP_Mount/AP_Mount_Xacti.h
+++ b/libraries/AP_Mount/AP_Mount_Xacti.h
@@ -75,7 +75,7 @@ class AP_Mount_Xacti : public AP_Mount_Backend
     void send_camera_settings(mavlink_channel_t chan) const override;
 
     // subscribe to Xacti DroneCAN messages
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
 
     // xacti specific message handlers
     static void handle_gimbal_attitude_status(AP_DroneCAN* ap_dronecan, const CanardRxTransfer& transfer, const com_xacti_GimbalAttitudeStatus &msg);
@@ -267,7 +267,6 @@ class AP_Mount_Xacti : public AP_Mount_Backend
     bool _camera_error;                             // true if status reports camera error
 
     // DroneCAN related variables
-    static bool _subscribed;                        // true once subscribed to receive DroneCAN messages
     static struct DetectedModules {
         AP_Mount_Xacti *driver;                     // pointer to Xacti backends
         AP_DroneCAN* ap_dronecan;                   // DroneCAN interface used by this backend
diff --git a/libraries/AP_MultiHeap/AP_MultiHeap.cpp b/libraries/AP_MultiHeap/AP_MultiHeap.cpp
new file mode 100644
index 00000000000000..ff81139cc839bc
--- /dev/null
+++ b/libraries/AP_MultiHeap/AP_MultiHeap.cpp
@@ -0,0 +1,206 @@
+/*
+  multiple heap interface, allowing for an allocator that uses
+  multiple underlying heaps to cope with multiple memory regions on
+  STM32 boards
+ */
+
+#include "AP_MultiHeap.h"
+
+#if ENABLE_HEAP
+#include <AP_Math/AP_Math.h>
+#include <stdio.h>
+
+/*
+  allow up to 10 heaps
+ */
+#ifndef MAX_HEAPS
+#define MAX_HEAPS 10
+#endif
+
+extern const AP_HAL::HAL &hal;
+
+/*
+  create heaps with a total memory size, splitting over at most
+  max_heaps
+ */
+bool MultiHeap::create(uint32_t total_size, uint8_t max_heaps, bool _allow_expansion, uint32_t _reserve_size)
+{
+    max_heaps = MIN(MAX_HEAPS, max_heaps);
+    if (heaps != nullptr) {
+        // don't allow double allocation
+        return false;
+    }
+    heaps = NEW_NOTHROW Heap[max_heaps];
+    if (heaps == nullptr) {
+        return false;
+    }
+    num_heaps = max_heaps;
+    for (uint8_t i=0; i<max_heaps; i++) {
+        uint32_t alloc_size = total_size;
+        while (alloc_size > 0) {
+            heaps[i].hp = heap_create(alloc_size);
+            if (heaps[i].hp != nullptr) {
+                total_size -= alloc_size;
+                sum_size += alloc_size;
+                break;
+            }
+            alloc_size *= 0.9;
+        }
+        if (total_size == 0) {
+            break;
+        }
+    }
+    if (total_size != 0) {
+        destroy();
+        return false;
+    }
+
+    allow_expansion = _allow_expansion;
+    reserve_size = _reserve_size;
+
+    return true;
+}
+
+// destroy heap
+void MultiHeap::destroy(void)
+{
+    if (!available()) {
+        return;
+    }
+    for (uint8_t i=0; i<num_heaps; i++) {
+        if (heaps[i].hp != nullptr) {
+            heap_destroy(heaps[i].hp);
+            heaps[i].hp = nullptr;
+        }
+    }
+    delete[] heaps;
+    heaps = nullptr;
+    num_heaps = 0;
+    sum_size = 0;
+    expanded_to = 0;
+}
+
+// return true if heap is available for operations
+bool MultiHeap::available(void) const
+{
+    return heaps != nullptr && heaps[0].hp != nullptr;
+}
+
+/*
+  allocate memory from a heap
+ */
+void *MultiHeap::allocate(uint32_t size)
+{
+    if (!available() || size == 0) {
+        return nullptr;
+    }
+    for (uint8_t i=0; i<num_heaps; i++) {
+        if (heaps[i].hp == nullptr) {
+            break;
+        }
+        void *newptr = heap_allocate(heaps[i].hp, size);
+        if (newptr != nullptr) {
+            last_failed = false;
+            return newptr;
+        }
+    }
+    if (!allow_expansion || !last_failed) {
+        /*
+          we only allow expansion when the last allocation
+          failed. This gives the lua engine a chance to use garbage
+          collection to recover memory
+         */
+        last_failed = true;
+        return nullptr;
+    }
+
+    if (!hal.util->get_soft_armed()) {
+        // only expand the available heaps when armed. When disarmed
+        // user should fix their SCR_HEAP_SIZE parameter
+        last_failed = true;
+        return nullptr;
+    }
+
+    /*
+      vehicle is armed and MultiHeap (for scripting) is out of
+      memory. We will see if we can add a new heap from available
+      memory if we have at least reserve_size bytes free
+     */
+    const uint32_t available = hal.util->available_memory();
+    const uint32_t heap_overhead = 128; // conservative value, varies with HAL
+    const uint32_t min_size = size + heap_overhead;
+    if (available < reserve_size+min_size) {
+        last_failed = true;
+        return nullptr;
+    }
+
+    // round up to a minimum of 30k to allocate, and allow for heap overhead
+    const uint32_t round_to = 30*1024U;
+    const uint32_t alloc_size = MIN(available - reserve_size, MAX(size+heap_overhead, round_to));
+    if (alloc_size < min_size) {
+        last_failed = true;
+        return nullptr;
+    }
+    for (uint8_t i=0; i<num_heaps; i++) {
+        if (heaps[i].hp == nullptr) {
+            heaps[i].hp = heap_create(alloc_size);
+            if (heaps[i].hp == nullptr) {
+                last_failed = true;
+                return nullptr;
+            }
+            sum_size += alloc_size;
+            expanded_to = sum_size;
+            void *p = heap_allocate(heaps[i].hp, size);
+            last_failed = p == nullptr;
+            return p;
+        }
+    }
+    last_failed = true;
+    return nullptr;
+}
+
+/*
+  free memory from a heap
+ */
+void MultiHeap::deallocate(void *ptr)
+{
+    if (!available() || ptr == nullptr) {
+        return;
+    }
+    heap_free(ptr);
+}
+
+/*
+  change size of an allocation, operates like realloc(), but requires
+  the old_size when ptr is not NULL
+ */
+void *MultiHeap::change_size(void *ptr, uint32_t old_size, uint32_t new_size)
+{
+    if (new_size == 0) {
+        deallocate(ptr);
+        return nullptr;
+    }
+    /*
+      we don't want to require the underlying allocation system to
+      support realloc() and we also want to be able to handle the case
+      of having to move the allocation to a new heap, so we do a
+      simple alloc/copy/deallocate for reallocation
+     */
+    void *newp = allocate(new_size);
+    if (ptr == nullptr) {
+        return newp;
+    }
+    if (newp == nullptr) {
+        if (old_size >= new_size) {
+            // Lua assumes that the allocator never fails when osize >= nsize
+            // the best we can do is return the old pointer
+            return ptr;
+        }
+        return nullptr;
+    }
+    memcpy(newp, ptr, MIN(old_size, new_size));
+    deallocate(ptr);
+    return newp;
+}
+
+#endif // ENABLE_HEAP
diff --git a/libraries/AP_MultiHeap/AP_MultiHeap.h b/libraries/AP_MultiHeap/AP_MultiHeap.h
new file mode 100644
index 00000000000000..3b67f11739e93d
--- /dev/null
+++ b/libraries/AP_MultiHeap/AP_MultiHeap.h
@@ -0,0 +1,83 @@
+/*
+  multiple heap interface, allowing for an allocator that uses
+  multiple underlying heaps to cope with multiple memory regions on
+  STM32 boards
+ */
+
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/Util.h>
+
+#if ENABLE_HEAP
+
+#include <stdint.h>
+#include <stdbool.h>
+
+class MultiHeap {
+public:
+    /*
+      allocate/deallocate heaps
+     */
+    bool create(uint32_t total_size, uint8_t max_heaps, bool allow_expansion, uint32_t reserve_size);
+    void destroy(void);
+
+    // return true if the heap is available for operations
+    bool available(void) const;
+
+    // allocate memory within heaps
+    void *allocate(uint32_t size);
+    void deallocate(void *ptr);
+
+    // change allocated size of a pointer - this operates in a similar
+    // fashion to realloc, but requires an (accurate!) old_size value
+    // when ptr is not NULL. This is guaranteed by the lua scripting
+    // allocation API
+    void *change_size(void *ptr, uint32_t old_size, uint32_t new_size);
+
+    /*
+      get the size that we have expanded to. Used by error reporting in scripting
+     */
+    uint32_t get_expansion_size(void) const {
+        return expanded_to;
+    }
+
+private:
+    struct Heap {
+        void *hp;
+    };
+    struct Heap *heaps;
+
+    uint8_t num_heaps;
+    bool allow_expansion;
+    uint32_t reserve_size;
+    uint32_t sum_size;
+    uint32_t expanded_to;
+
+    // we only do heap expansion if the last allocation failed this
+    // encourages the lua scripting engine to garbage collect to
+    // re-use memory when possible
+    bool last_failed;
+
+
+    /*
+      low level allocation functions
+     */
+    /*
+      heap functions used by lua scripting
+    */
+    // allocate a new heap
+    void *heap_create(uint32_t size);
+
+    // destroy a heap
+    void heap_destroy(void *ptr);
+
+    // allocate some memory on a specific heap
+    void *heap_allocate(void *heap, uint32_t size);
+
+    // free some memory that was allocated by heap_allocate. The implementation must
+    // be able to determine which heap the allocation was from using the pointer
+    void heap_free(void *ptr);
+};
+
+#endif // ENABLE_HEAP
diff --git a/libraries/AP_MultiHeap/MultiHeap_chibios.cpp b/libraries/AP_MultiHeap/MultiHeap_chibios.cpp
new file mode 100644
index 00000000000000..d3225cd3b7b388
--- /dev/null
+++ b/libraries/AP_MultiHeap/MultiHeap_chibios.cpp
@@ -0,0 +1,47 @@
+/*
+  allocation backend functions using native ChibiOS chHeap API
+ */
+
+#include "AP_MultiHeap.h"
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#if ENABLE_HEAP && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+
+#include <ch.h>
+#include <hal.h>
+
+/*
+  heap functions used by lua scripting
+ */
+void *MultiHeap::heap_create(uint32_t size)
+{
+    memory_heap_t *heap = (memory_heap_t *)malloc(size + sizeof(memory_heap_t));
+    if (heap == nullptr) {
+        return nullptr;
+    }
+    chHeapObjectInit(heap, heap + 1U, size);
+    return heap;
+}
+
+void MultiHeap::heap_destroy(void *ptr)
+{
+    free(ptr);
+}
+
+void *MultiHeap::heap_allocate(void *heap, uint32_t size)
+{
+    if (heap == nullptr) {
+        return nullptr;
+    }
+    if (size == 0) {
+        return nullptr;
+    }
+    return chHeapAlloc((memory_heap_t *)heap, size);
+}
+
+void MultiHeap::heap_free(void *ptr)
+{
+    return chHeapFree(ptr);
+}
+
+#endif // ENABLE_HEAP && CONFIG_HAL_BOARD
diff --git a/libraries/AP_MultiHeap/MultiHeap_malloc.cpp b/libraries/AP_MultiHeap/MultiHeap_malloc.cpp
new file mode 100644
index 00000000000000..857d01e76bfc82
--- /dev/null
+++ b/libraries/AP_MultiHeap/MultiHeap_malloc.cpp
@@ -0,0 +1,119 @@
+#include "AP_MultiHeap.h"
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#if ENABLE_HEAP && CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
+
+/*
+  on systems other than chibios we map the allocation to the system
+  malloc/free functions
+ */
+
+#include <AP_InternalError/AP_InternalError.h>
+#include <stdlib.h>
+
+/*
+  low level allocator interface using system malloc
+ */
+/*
+  default functions for heap management for lua scripting for systems
+  that don't have the ability to create separable heaps
+ */
+
+struct heap_allocation_header {
+    struct heap *hp;
+    uint32_t allocation_size; // size of allocated block, not including this header
+};
+
+#define HEAP_MAGIC 0x5681ef9f
+
+struct heap {
+    uint32_t magic;
+    uint32_t max_heap_size;
+    uint32_t current_heap_usage;
+};
+
+void *MultiHeap::heap_create(uint32_t size)
+{
+    struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap));
+    if (new_heap != nullptr) {
+        new_heap->max_heap_size = size;
+    }
+    new_heap->magic = HEAP_MAGIC;
+    return (void *)new_heap;
+}
+
+void MultiHeap::heap_destroy(void *heap_ptr)
+{
+    struct heap *heapp = (struct heap*)heap_ptr;
+    if (heapp->magic != HEAP_MAGIC) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        return;
+    }
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    if (heapp->current_heap_usage != 0) {
+        // lua should guarantee that there is no memory still
+        // allocated when we destroy the heap. Throw an error in SITL
+        // if this proves not to be true
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+    }
+#endif
+
+    // free the heap structure
+    free(heapp);
+}
+
+/*
+  allocate memory on a specific heap
+ */
+void *MultiHeap::heap_allocate(void *heap_ptr, uint32_t size)
+{
+    if (heap_ptr == nullptr || size == 0) {
+        return nullptr;
+    }
+    struct heap *heapp = (struct heap*)heap_ptr;
+    if (heapp->magic != HEAP_MAGIC) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        return nullptr;
+    }
+
+    if (heapp->current_heap_usage + size > heapp->max_heap_size) {
+        // fail the allocation as we don't have the memory. Note that
+        // we don't simulate the fragmentation that we would get with
+        // HAL_ChibiOS
+        return nullptr;
+    }
+
+    auto *header = (heap_allocation_header *)malloc(size + sizeof(heap_allocation_header));
+    if (header == nullptr) {
+        // can't allocate the new memory
+        return nullptr;
+    }
+    header->hp = heapp;
+    header->allocation_size = size;
+
+    heapp->current_heap_usage += size;
+
+    return header + 1;
+}
+
+/*
+  free memory from a previous heap_allocate call
+ */
+void MultiHeap::heap_free(void *ptr)
+{
+    // extract header
+    auto *header = ((struct heap_allocation_header *)ptr)-1;
+    auto *heapp = header->hp;
+    if (heapp->magic != HEAP_MAGIC) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        return;
+    }
+    const uint32_t size = header->allocation_size;
+    heapp->current_heap_usage -= size;
+
+    // free the memory
+    free(header);
+}
+
+#endif // ENABLE_HEAP && CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
diff --git a/libraries/AP_MultiHeap/tests/test_multiheap.cpp b/libraries/AP_MultiHeap/tests/test_multiheap.cpp
new file mode 100644
index 00000000000000..168ed933cf0b3b
--- /dev/null
+++ b/libraries/AP_MultiHeap/tests/test_multiheap.cpp
@@ -0,0 +1,57 @@
+#include <AP_gtest.h>
+#include <AP_MultiHeap/AP_MultiHeap.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+TEST(MultiHeap, Tests)
+{
+    static MultiHeap h;
+
+    EXPECT_TRUE(h.create(150000, 10, true, 10000));
+    EXPECT_TRUE(h.available());
+
+    const uint32_t max_allocs = 1000;
+    struct alloc {
+        void *ptr;
+        uint32_t size;
+    };
+    auto *allocs = new alloc[max_allocs];
+    allocs[0].size = 640;
+    allocs[0].ptr = h.allocate(allocs[0].size);
+    for (uint32_t i=1; i<2; i++) {
+        auto &a = allocs[i];
+        a.size = 30;
+        a.ptr = h.allocate(a.size);
+    }
+    allocs[0].ptr = h.change_size(allocs[0].ptr, allocs[0].size, 50);
+    allocs[0].size = 50;
+
+    for (uint32_t i=0; i<5000; i++) {
+        uint16_t idx = get_random16() % max_allocs;
+        auto &a = allocs[idx];
+        if (a.ptr == nullptr) {
+            const uint16_t size = get_random16() % 150;
+            a.ptr = h.allocate(size);
+            //printf("a.ptr=%p %u -> %u\n", a.ptr, a.size, size);
+            EXPECT_TRUE(size==0?a.ptr == nullptr : a.ptr != nullptr);
+            a.size = size;
+        } else {
+            const uint16_t size = get_random16() % 150;
+            a.ptr = h.change_size(a.ptr, a.size, size);
+            //printf("a.ptr=%p %u -> %u\n", a.ptr, a.size, size);
+            EXPECT_TRUE(size==0?a.ptr == nullptr : a.ptr != nullptr);
+            a.size = size;
+        }
+    }
+    for (uint32_t i=0; i<max_allocs; i++) {
+        auto &a = allocs[i];
+        h.deallocate(a.ptr);
+        a.size = 0;
+    }
+    h.destroy();
+    delete[] allocs;
+}
+
+AP_GTEST_MAIN()
diff --git a/libraries/AP_MultiHeap/tests/wscript b/libraries/AP_MultiHeap/tests/wscript
new file mode 100644
index 00000000000000..74d6a6a42952a1
--- /dev/null
+++ b/libraries/AP_MultiHeap/tests/wscript
@@ -0,0 +1,8 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+    bld.ap_find_tests(
+        use='ap',
+        DOUBLE_PRECISION_SOURCES = []
+    )
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
index 8f73f8ae02c71d..98aaf8cd27fbf7 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
@@ -13,6 +13,26 @@
 // Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
 void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
 {
+    // if reset source is not specified then use user defined velocity source
+    if (velResetSource == resetDataSource::DEFAULT) {
+        switch (frontend->sources.getVelXYSource()) {
+        case AP_NavEKF_Source::SourceXY::GPS:
+            velResetSource = resetDataSource::GPS;
+            break;
+        case AP_NavEKF_Source::SourceXY::BEACON:
+            velResetSource = resetDataSource::RNGBCN;
+            break;
+        case AP_NavEKF_Source::SourceXY::EXTNAV:
+            velResetSource = resetDataSource::EXTNAV;
+            break;
+        case AP_NavEKF_Source::SourceXY::NONE:
+        case AP_NavEKF_Source::SourceXY::OPTFLOW:
+        case AP_NavEKF_Source::SourceXY::WHEEL_ENCODER:
+            // unhandled sources so stick with the default
+            break;
+        }
+    }
+
     // Store the velocity before the reset so that we can record the reset delta
     velResetNE.x = stateStruct.velocity.x;
     velResetNE.y = stateStruct.velocity.y;
@@ -73,6 +93,26 @@ void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
 // resets position states to last GPS measurement or to zero if in constant position mode
 void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
 {
+    // if reset source is not specified thenn use the user defined position source
+    if (posResetSource == resetDataSource::DEFAULT) {
+        switch (frontend->sources.getPosXYSource()) {
+        case AP_NavEKF_Source::SourceXY::GPS:
+            posResetSource = resetDataSource::GPS;
+            break;
+        case AP_NavEKF_Source::SourceXY::BEACON:
+            posResetSource = resetDataSource::RNGBCN;
+            break;
+        case AP_NavEKF_Source::SourceXY::EXTNAV:
+            posResetSource = resetDataSource::EXTNAV;
+            break;
+        case AP_NavEKF_Source::SourceXY::NONE:
+        case AP_NavEKF_Source::SourceXY::OPTFLOW:
+        case AP_NavEKF_Source::SourceXY::WHEEL_ENCODER:
+            // invalid sources so stick with the default
+            break;
+        }
+    }
+
     // Store the position before the reset so that we can record the reset delta
     posResetNE.x = stateStruct.position.x;
     posResetNE.y = stateStruct.position.y;
diff --git a/libraries/AP_Networking/AP_Networking_tests.cpp b/libraries/AP_Networking/AP_Networking_tests.cpp
index 48af6322828f0b..5494c1012648e0 100644
--- a/libraries/AP_Networking/AP_Networking_tests.cpp
+++ b/libraries/AP_Networking/AP_Networking_tests.cpp
@@ -41,7 +41,7 @@ void AP_Networking::start_tests(void)
     if (param.tests & TEST_CONNECTOR_LOOPBACK) {
         hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::test_connector_loopback, void),
                                      "connector_loopback",
-                                     8192, AP_HAL::Scheduler::PRIORITY_UART, -1);
+                                     8192, AP_HAL::Scheduler::PRIORITY_IO, -1);
     }
 }
 
diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
index 0559fd96696b8a..b103f13168d9b1 100644
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/libraries/AP_Notify/AP_Notify.cpp
@@ -42,6 +42,7 @@
 #include "ProfiLED.h"
 #include "ScriptingLED.h"
 #include "DShotLED.h"
+#include "ProfiLED_IOMCU.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -115,7 +116,11 @@ AP_Notify *AP_Notify::_singleton;
 #endif  // defined (DEFAULT_NTF_LED_TYPES)
 
 #ifndef DEFAULT_NTF_LED_TYPES
-    #define DEFAULT_NTF_LED_TYPES (Notify_LED_Board | I2C_LEDS)
+    #if HAL_WITH_IO_MCU && AP_IOMCU_PROFILED_SUPPORT_ENABLED
+      #define DEFAULT_NTF_LED_TYPES (Notify_LED_Board | Notify_LED_ProfiLED_IOMCU | I2C_LEDS)
+    #else
+        #define DEFAULT_NTF_LED_TYPES (Notify_LED_Board | I2C_LEDS)
+    #endif
 #endif // DEFAULT_NTF_LED_TYPES
 
 #ifndef BUZZER_ENABLE_DEFAULT
@@ -203,7 +208,7 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = {
     // @Param: LED_TYPES
     // @DisplayName: LED Driver Types
     // @Description: Controls what types of LEDs will be enabled
-    // @Bitmask: 0:Built-in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:DroneCAN, 6:NCP5623 External, 7:NCP5623 Internal, 8:NeoPixel, 9:ProfiLED, 10:Scripting, 11:DShot, 12:ProfiLED_SPI, 13:LP5562 External, 14: LP5562 Internal, 15:IS31FL3195 External, 16: IS31FL3195 Internal, 17: DiscreteRGB, 18: NeoPixelRGB
+    // @Bitmask: 0:Built-in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:DroneCAN, 6:NCP5623 External, 7:NCP5623 Internal, 8:NeoPixel, 9:ProfiLED, 10:Scripting, 11:DShot, 12:ProfiLED_SPI, 13:LP5562 External, 14: LP5562 Internal, 15:IS31FL3195 External, 16: IS31FL3195 Internal, 17: DiscreteRGB, 18: NeoPixelRGB, 19:ProfiLED_IOMCU
     // @User: Advanced
     AP_GROUPINFO("LED_TYPES", 6, AP_Notify, _led_type, DEFAULT_NTF_LED_TYPES),
 
@@ -368,6 +373,11 @@ void AP_Notify::add_backends(void)
                 ADD_BACKEND(NEW_NOTHROW DShotLED());
                 break;
 #endif
+#if HAL_WITH_IO_MCU && AP_IOMCU_PROFILED_SUPPORT_ENABLED
+            case Notify_LED_ProfiLED_IOMCU:
+                ADD_BACKEND(NEW_NOTHROW ProfiLED_IOMCU());
+                break;
+#endif
 #if AP_NOTIFY_LP5562_ENABLED
             case Notify_LED_LP5562_I2C_External:
                 FOREACH_I2C_EXTERNAL(b) {
diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h
index 5467dfc0df3de5..3f3cf9507ed213 100644
--- a/libraries/AP_Notify/AP_Notify.h
+++ b/libraries/AP_Notify/AP_Notify.h
@@ -17,6 +17,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
 #include "AP_Notify_config.h"
+#include <AP_IOMCU/AP_IOMCU.h>
 
 #include "NotifyDevice.h"
 
@@ -97,6 +98,9 @@ class AP_Notify
 #endif
 #if AP_NOTIFY_NEOPIXEL_ENABLED
         Notify_LED_NeoPixelRGB              = (1 << 18), // NeoPixel AdaFruit 4544 Worldsemi WS2811
+#endif
+#if HAL_WITH_IO_MCU && AP_IOMCU_PROFILED_SUPPORT_ENABLED
+        Notify_LED_ProfiLED_IOMCU           = (1 << 19), // ProfiLED IOMCU
 #endif
         Notify_LED_MAX
     };
diff --git a/libraries/AP_Notify/ProfiLED_IOMCU.h b/libraries/AP_Notify/ProfiLED_IOMCU.h
new file mode 100644
index 00000000000000..ae4a9e0124fd1c
--- /dev/null
+++ b/libraries/AP_Notify/ProfiLED_IOMCU.h
@@ -0,0 +1,40 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include "RGBLed.h"
+#include <AP_Common/AP_Common.h>
+#include <AP_IOMCU/AP_IOMCU.h>
+
+#if HAL_WITH_IO_MCU && AP_IOMCU_PROFILED_SUPPORT_ENABLED
+
+class ProfiLED_IOMCU : public RGBLed {
+public:
+    ProfiLED_IOMCU() : RGBLed(0, 0xFF, 0x7F, 0x33) {}
+
+    bool init(void) override { return true; }
+
+protected:
+    bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override {
+        const auto iomcu = AP::iomcu();
+        if (iomcu == nullptr) {
+            return false;
+        }
+        iomcu->set_profiled(r, g, b);
+        return true;
+    }
+};
+
+#endif
\ No newline at end of file
diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp
index 123b3408c3abd3..9ba5a98dadbda1 100644
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp
@@ -26,15 +26,11 @@ AP_OpticalFlow_HereFlow::AP_OpticalFlow_HereFlow(AP_OpticalFlow &flow) :
 }
 
 //links the HereFlow messages to the backend
-void AP_OpticalFlow_HereFlow::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_OpticalFlow_HereFlow::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("measurement_sub");
-    }
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, driver_index) != nullptr);
 }
 
 //updates driver states based on received HereFlow messages
diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h
index afc75841631a07..7b457bf6c96161 100644
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h
@@ -15,7 +15,7 @@ class AP_OpticalFlow_HereFlow : public OpticalFlow_backend {
 
     void update() override;
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
 
     static void handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const com_hex_equipment_flow_Measurement &msg);
 
diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp
index 2249d98b0d5648..9962780398a661 100644
--- a/libraries/AP_Param/AP_Param.cpp
+++ b/libraries/AP_Param/AP_Param.cpp
@@ -268,7 +268,12 @@ void AP_Param::check_group_info(const struct AP_Param::GroupInfo *  group_info,
         if (size == 0) {
             FATAL("invalid type in %s", group_info[i].name);
         }
-        if (prefix_length + strlen(group_info[i].name) > 16) {
+        uint8_t param_name_length = prefix_length + strlen(group_info[i].name);
+        if (type == AP_PARAM_VECTOR3F) {
+            // need room for _X/_Y/_Z
+            param_name_length += 2;
+        }
+        if (param_name_length > 16) {
             FATAL("suffix is too long in %s", group_info[i].name);
         }
         (*total_size) += size + sizeof(struct Param_header);
diff --git a/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp b/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp
index caa2b03f078ae9..430890fda1e94b 100644
--- a/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp
+++ b/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp
@@ -32,15 +32,11 @@ ObjectBuffer_TS<AP_Proximity_DroneCAN::ObstacleItem> AP_Proximity_DroneCAN::item
 
 
 //links the Proximity DroneCAN message to this backend
-void AP_Proximity_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_Proximity_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("measurement_sub");
-    }
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, driver_index) != nullptr);
 }
 
 //Method to find the backend relating to the node id
diff --git a/libraries/AP_Proximity/AP_Proximity_DroneCAN.h b/libraries/AP_Proximity/AP_Proximity_DroneCAN.h
index 4ccda3610d89e3..db9f06094e0b7d 100644
--- a/libraries/AP_Proximity/AP_Proximity_DroneCAN.h
+++ b/libraries/AP_Proximity/AP_Proximity_DroneCAN.h
@@ -23,7 +23,7 @@ class AP_Proximity_DroneCAN : public AP_Proximity_Backend
 
     static AP_Proximity_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t address, bool create_new);
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
 
     static void handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_proximity_sensor_Proximity &msg);
 
diff --git a/libraries/AP_Quicktune/AP_Quicktune.cpp b/libraries/AP_Quicktune/AP_Quicktune.cpp
new file mode 100644
index 00000000000000..c8c5e5a5e573c4
--- /dev/null
+++ b/libraries/AP_Quicktune/AP_Quicktune.cpp
@@ -0,0 +1,689 @@
+/*
+  C++ implementation of quicktune based on original lua script
+ */
+
+// quicktune is not performance sensitive, save flash
+#pragma GCC optimize("Os")
+
+#include "AP_Quicktune.h"
+
+#if AP_QUICKTUNE_ENABLED
+
+#define UPDATE_RATE_HZ 40
+#define STAGE_DELAY 4000
+#define PILOT_INPUT_DELAY 4000
+#define YAW_FLTE_MAX 2.0
+#define FLTD_MUL 0.5
+#define FLTT_MUL 0.5
+#define DEFAULT_SMAX 50.0
+#define OPTIONS_TWO_POSITION (1<<0)
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_RCMapper/AP_RCMapper.h>
+#include <AP_Logger/AP_Logger.h>
+#include <AP_Arming/AP_Arming.h>
+#include <GCS_MAVLink/GCS.h>
+
+const AP_Param::GroupInfo AP_Quicktune::var_info[] = {
+    // @Param: ENABLE
+    // @DisplayName: Quicktune enable
+    // @Description: Enable quicktune system
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    AP_GROUPINFO_FLAGS("ENABLE", 1, AP_Quicktune, enable, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: AXES
+    // @DisplayName: Quicktune axes
+    // @Description: Axes to tune
+    // @Bitmask: 0:Roll,1:Pitch,2:Yaw
+    // @User: Standard
+    AP_GROUPINFO("AXES", 2, AP_Quicktune, axes_enabled, 7),
+
+    // @Param: DOUBLE_TIME
+    // @DisplayName: Quicktune doubling time
+    // @Description: Time to double a tuning parameter. Raise this for a slower tune.
+    // @Range: 5 20
+    // @Units: s
+    // @User: Standard
+    AP_GROUPINFO("DOUBLE_TIME", 3, AP_Quicktune, double_time, 10),
+
+    // @Param: GAIN_MARGIN
+    // @DisplayName: Quicktune gain margin
+    // @Description: Reduction in gain after oscillation detected. Raise this number to get a more conservative tune
+    // @Range: 20 80
+    // @Units: %
+    // @User: Standard
+    AP_GROUPINFO("GAIN_MARGIN", 4, AP_Quicktune, gain_margin, 60),
+
+    // @Param: OSC_SMAX
+    // @DisplayName: Quicktune oscillation rate threshold
+    // @Description: Threshold for oscillation detection. A lower value will lead to a more conservative tune.
+    // @Range: 1 10
+    // @User: Standard
+    AP_GROUPINFO("OSC_SMAX", 5, AP_Quicktune, osc_smax, 4),
+
+    // @Param: YAW_P_MAX
+    // @DisplayName: Quicktune Yaw P max
+    // @Description: Maximum value for yaw P gain
+    // @Range: 0.1 3
+    // @User: Standard
+    AP_GROUPINFO("YAW_P_MAX", 6, AP_Quicktune, yaw_p_max, 0.5),
+
+    // @Param: YAW_D_MAX
+    // @DisplayName: Quicktune Yaw D max
+    // @Description: Maximum value for yaw D gain
+    // @Range: 0.001 1
+    // @User: Standard
+    AP_GROUPINFO("YAW_D_MAX", 7, AP_Quicktune, yaw_d_max, 0.01),
+
+    // @Param: RP_PI_RATIO
+    // @DisplayName: Quicktune roll/pitch PI ratio
+    // @Description: Ratio between P and I gains for roll and pitch. Raise this to get a lower I gain
+    // @Range: 1.0 2.0
+    // @User: Standard
+    AP_GROUPINFO("RP_PI_RATIO", 8, AP_Quicktune, rp_pi_ratio, 1.0),
+
+    // @Param: Y_PI_RATIO
+    // @DisplayName: Quicktune Yaw PI ratio
+    // @Description: Ratio between P and I gains for yaw. Raise this to get a lower I gain
+    // @Range: 1.0 20
+    // @User: Standard
+    AP_GROUPINFO("Y_PI_RATIO", 9, AP_Quicktune, y_pi_ratio, 10),
+
+    // @Param: AUTO_FILTER
+    // @DisplayName: Quicktune auto filter enable
+    // @Description: When enabled the PID filter settings are automatically set based on INS_GYRO_FILTER
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    AP_GROUPINFO("AUTO_FILTER", 10, AP_Quicktune, auto_filter, 1),
+
+    // @Param: AUTO_SAVE
+    // @DisplayName: Quicktune auto save
+    // @Description: Number of seconds after completion of tune to auto-save. This is useful when using a 2 position switch for quicktune. Zero (the default value) disables automatic saving, and you will need to have a 3 position switch to save or use GCS auxilliary functions.
+    // @Units: s
+    // @User: Standard
+    AP_GROUPINFO("AUTO_SAVE", 11, AP_Quicktune, auto_save, 0),
+
+    // @Param: REDUCE_MAX
+    // @DisplayName: Quicktune maximum gain reduction
+    // @Description: This controls how much quicktune is allowed to lower gains from the original gains. If the vehicle already has a reasonable tune and is not oscillating then you can set this to zero to prevent gain reductions. The default of 20% is reasonable for most vehicles. Using a maximum gain reduction lowers the chance of an angle P oscillation happening if quicktune gets a false positive oscillation at a low gain, which can result in very low rate gains and a dangerous angle P oscillation.
+    // @Units: %
+    // @Range: 0 100
+    // @User: Standard
+    AP_GROUPINFO("REDUCE_MAX", 12, AP_Quicktune, reduce_max, 20),
+
+    // @Param: OPTIONS
+    // @DisplayName: Quicktune options
+    // @Description: Additional options. When the Two Position Switch option is enabled then a high switch position will start the tune, low will disable the tune. you should also set a QUIK_AUTO_SAVE time so that you will be able to save the tune.
+    // @Bitmask: 0:UseTwoPositionSwitch
+    // @User: Standard
+    AP_GROUPINFO("OPTIONS", 13, AP_Quicktune, options, 0),
+
+    // @Param: ANGLE_MAX
+    // @DisplayName: maximum angle error for tune abort
+    // @Description: If while tuning the angle error goes over this limit then the tune will aborts to prevent a bad oscillation in the case of the tuning algorithm failing. If you get an error "Quicktune: attitude error ABORTING" and you think it is a false positive then you can either raise this parameter or you can try increasing the QWIK_DOUBLE_TIME to do the tune more slowly.
+    // @Units: deg
+    // @User: Standard
+    AP_GROUPINFO("ANGLE_MAX", 14, AP_Quicktune, angle_max, 10),
+    
+    AP_GROUPEND
+};
+
+// Call at loop rate
+void AP_Quicktune::update(bool mode_supports_quicktune)
+{
+    if (enable < 1) {
+        if (need_restore) {
+            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "QuickTune disabled");
+            abort_tune();
+        }
+        return;
+    }
+    const uint32_t now = AP_HAL::millis();
+
+    if (!mode_supports_quicktune) {
+        /*
+          user has switched to a non-quicktune mode. If we have
+          pending parameter changes then revert
+         */
+        if (need_restore) {
+            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "QuickTune aborted");
+        }
+        abort_tune();
+        return;
+    }
+
+    if (need_restore) {
+        const float att_error = AC_AttitudeControl::get_singleton()->get_att_error_angle_deg();
+        if (angle_max > 0 && att_error > angle_max) {
+            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Quicktune: attitude error %.1fdeg - ABORTING", att_error);
+            abort_tune();
+            return;
+        }
+    }
+
+    if (have_pilot_input()) {
+        last_pilot_input_ms = now;
+    }
+
+    SwitchPos sw_pos_tune = SwitchPos::MID;
+    SwitchPos sw_pos_save = SwitchPos::HIGH;
+    if ((options & OPTIONS_TWO_POSITION) != 0) {
+        sw_pos_tune = SwitchPos::HIGH;
+        sw_pos_save = SwitchPos::NONE;
+    }
+
+    const auto &vehicle = *AP::vehicle();
+
+    if (sw_pos == sw_pos_tune &&
+        (!hal.util->get_soft_armed() || !vehicle.get_likely_flying())) {
+        if (now - last_warning_ms > 5000) {
+            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Quicktune: Must be flying to tune");
+            last_warning_ms = now;
+        }
+        return;
+    }
+    if (sw_pos == SwitchPos::LOW || !hal.util->get_soft_armed() || !vehicle.get_likely_flying()) {
+        // Abort, revert parameters
+        if (need_restore) {
+            need_restore = false;
+            restore_all_params();
+            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Quicktune: Reverted");
+            tune_done_time_ms = 0;
+        }
+        reset_axes_done();
+        return;
+    }
+    if (sw_pos == sw_pos_save) {
+        // Save all params
+        if (need_restore) {
+            need_restore = false;
+            save_all_params();
+        }
+    }
+    if (sw_pos != sw_pos_tune) {
+        return;
+    }
+
+    if (now - last_stage_change_ms < STAGE_DELAY) {
+        // Update slew gain
+        if (slew_parm != Param::END) {
+            const float P = get_param_value(slew_parm);
+            const AxisName axis = get_axis(slew_parm);
+            adjust_gain(slew_parm, P+slew_delta);
+            slew_steps = slew_steps - 1;
+            Write_QWIK(get_slew_rate(axis), P, slew_parm);
+            if (slew_steps == 0) {
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %.4f", get_param_name(slew_parm), P);
+                slew_parm = Param::END;
+                if (get_current_axis() == AxisName::DONE) {
+                    GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: DONE");
+                    tune_done_time_ms = now;
+                }
+            }
+        }
+        return;
+    }
+
+    const AxisName axis = get_current_axis();
+
+    if (axis == AxisName::DONE) {
+        // Nothing left to do, check autosave time
+        if (tune_done_time_ms != 0 && auto_save > 0) {
+            if (now - tune_done_time_ms > (auto_save*1000)) {
+                need_restore = false;
+                save_all_params();
+                tune_done_time_ms = 0;
+            }
+        }
+        return;
+    }
+
+    if (!need_restore) {
+        need_restore = true;
+        // We are just starting tuning, get current values
+        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: Starting tune");
+        // Get all params
+        for (uint8_t p = 0; p < uint8_t(Param::END); p++) {
+            param_saved[p] = get_param_value(Param(p));
+        }
+        // Set up SMAX
+        const Param is[3] { Param::RLL_SMAX, Param::PIT_SMAX, Param::YAW_SMAX };
+        for (const auto p : is) {
+            const float smax = get_param_value(p);
+            if (smax <= 0) {
+                adjust_gain(p, DEFAULT_SMAX);
+            }
+        }
+    }
+
+    if (now - last_pilot_input_ms < PILOT_INPUT_DELAY) {
+        return;
+    }
+
+    if (!BIT_IS_SET(filters_done, uint8_t(axis))) {
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Starting %s tune", get_axis_name(axis));
+        setup_filters(axis);
+    }
+
+    const Param pname = get_pname(axis, current_stage);
+    const float pval = get_param_value(pname);
+    const float limit = gain_limit(pname);
+    const bool limited = (limit > 0.0 && pval >= limit);
+    const float srate = get_slew_rate(axis);
+    const bool oscillating = srate > osc_smax;
+    
+    // Check if reached limit
+    if (limited || oscillating) {
+        float reduction = (100.0-gain_margin)*0.01;
+        if (!oscillating) {
+            reduction = 1.0;
+        }
+        float new_gain = pval * reduction;
+        if (limit > 0.0 && new_gain > limit) {
+            new_gain = limit;
+        }
+        float old_gain = param_saved[uint8_t(pname)];
+        if (new_gain < old_gain && (pname == Param::PIT_D || pname == Param::RLL_D)) {
+            // We are lowering a D gain from the original gain. Also
+            // lower the P gain by the same amount so that we don't
+            // trigger P oscillation. We don't drop P by more than a
+            // factor of 2
+            const float ratio = fmaxf(new_gain / old_gain, 0.5);
+            const uint8_t P_TO_D_OFS = uint8_t(Param::RLL_D) - uint8_t(Param::RLL_P);
+            const Param P_name = Param(uint8_t(pname)-P_TO_D_OFS); //from D to P
+            const float old_pval = get_param_value(P_name);;
+            const float new_pval = old_pval * ratio;
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Adjusting %s %.3f -> %.3f", get_param_name(P_name), old_pval, new_pval);
+            adjust_gain_limited(P_name, new_pval);
+        }
+        // Set up slew gain
+        slew_parm = pname;
+        const float slew_target = limit_gain(pname, new_gain);
+        slew_steps = UPDATE_RATE_HZ/2;
+        slew_delta = (slew_target - get_param_value(pname)) / slew_steps;
+
+        Write_QWIK(srate, pval, pname);
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Quicktune: %s done", get_param_name(pname));
+        advance_stage(axis);
+        last_stage_change_ms = now;
+    } else {
+        float new_gain = pval*get_gain_mul();
+        // cope with the gain starting at zero (some users with have a zero D gain)
+        new_gain = MAX(new_gain, 0.0001);
+        adjust_gain_limited(pname, new_gain);
+        Write_QWIK(srate, pval, pname);
+        if (now - last_gain_report_ms > 3000U) {
+            last_gain_report_ms = now;
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %.4f sr:%.2f", get_param_name(pname), new_gain, srate);
+        }
+    }
+}
+
+/*
+  abort the tune if it has started
+ */
+void AP_Quicktune::abort_tune()
+{
+    if (need_restore) {
+        need_restore = false;
+        restore_all_params();
+    }
+    tune_done_time_ms = 0;
+    reset_axes_done();
+    sw_pos = SwitchPos::LOW;
+}
+
+void AP_Quicktune::update_switch_pos(const  RC_Channel::AuxSwitchPos ch_flag) 
+{
+    sw_pos = SwitchPos(ch_flag);
+}
+
+void AP_Quicktune::reset_axes_done()
+{
+    axes_done = 0;
+    filters_done = 0;
+    current_stage = Stage::D;
+}
+
+void AP_Quicktune::setup_filters(AP_Quicktune::AxisName axis)
+{
+    if (auto_filter <= 0) {
+        BIT_SET(filters_done, uint8_t(axis));
+        return;
+    }
+    AP_InertialSensor *imu = AP_InertialSensor::get_singleton();
+    if (imu == nullptr) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        return;
+    }
+    const float gyro_filter = imu->get_gyro_filter_hz();
+    adjust_gain(get_pname(axis, Stage::FLTT), gyro_filter * FLTT_MUL);
+    adjust_gain(get_pname(axis, Stage::FLTD), gyro_filter * FLTD_MUL);
+
+    if (axis == AxisName::YAW) {
+        const float FLTE = get_param_value(Param::YAW_FLTE);
+        if (FLTE < 0.0 || FLTE > YAW_FLTE_MAX) {
+            adjust_gain(Param::YAW_FLTE, YAW_FLTE_MAX);
+        }
+    }
+    BIT_SET(filters_done, uint8_t(axis));
+}
+
+// Check for pilot input to pause tune
+bool AP_Quicktune::have_pilot_input() const
+{
+    auto &RC = rc();
+    const float roll = RC.get_roll_channel().norm_input_dz();
+    const float pitch = RC.get_pitch_channel().norm_input_dz();
+    const float yaw = RC.get_yaw_channel().norm_input_dz();
+
+    if (fabsf(roll) > 0 || fabsf(pitch) > 0 || fabsf(yaw) > 0) {
+        return true;
+    }
+    return false;
+}
+
+// Get the axis name we are working on, or DONE for all done 
+AP_Quicktune::AxisName AP_Quicktune::get_current_axis() const
+{
+    for (int8_t i = 0; i < int8_t(AxisName::DONE); i++) {
+        if (BIT_IS_SET(axes_enabled, i) && !BIT_IS_SET(axes_done, i)) {
+            return AxisName(i);
+        }
+    }
+    return AxisName::DONE;
+}
+
+// get the AC_PID object for an axis
+AC_PID *AP_Quicktune::get_axis_pid(AP_Quicktune::AxisName axis) const
+{
+    auto &attitude_control = *AC_AttitudeControl::get_singleton();
+    switch(axis) {
+    case AxisName::RLL:
+        return &attitude_control.get_rate_roll_pid();
+    case AxisName::PIT:
+        return &attitude_control.get_rate_pitch_pid();
+    case AxisName::YAW:
+        return &attitude_control.get_rate_yaw_pid();
+    default:
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        break;
+    }
+    return nullptr;
+}
+
+// get slew rate parameter for an axis
+float AP_Quicktune::get_slew_rate(AP_Quicktune::AxisName axis) const
+{
+    auto *pid = get_axis_pid(axis);
+    if (pid == nullptr) {
+        return 0;
+    }
+    return pid->get_pid_info().slew_rate;
+}
+
+// Move to next stage of tune
+void AP_Quicktune::advance_stage(AP_Quicktune::AxisName axis)
+{
+    if (current_stage == Stage::D) {
+        current_stage = Stage::P;
+    } else {
+        BIT_SET(axes_done, uint8_t(axis));
+        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: %s done", get_axis_name(axis));
+        current_stage = Stage::D;
+    }
+}
+
+void AP_Quicktune::adjust_gain(AP_Quicktune::Param param, float value)
+{
+    need_restore = true;
+    BIT_SET(param_changed, uint8_t(param));
+    set_param_value(param, value);
+
+    if (get_stage(param) == Stage::P) {
+        // Also change I gain
+        const uint8_t P_TO_I_OFS = uint8_t(Param::RLL_I) - uint8_t(Param::RLL_P);
+        const uint8_t P_TO_FF_OFS = uint8_t(Param::RLL_FF) - uint8_t(Param::RLL_P);
+        const Param iname = Param(uint8_t(param)+P_TO_I_OFS);
+        const Param ffname = Param(uint8_t(param)+P_TO_FF_OFS);
+        float FF = get_param_value(ffname);
+        if (FF > 0) {
+            // If we have any FF on an axis then we don't couple I to P,
+            // usually we want I = FF for a one second time constant for trim
+            return;
+        }
+        BIT_SET(param_changed, uint8_t(iname));
+
+        // Work out ratio of P to I that we want
+        float pi_ratio = rp_pi_ratio;
+        if (get_axis(param) == AxisName::YAW) {
+            pi_ratio = y_pi_ratio;
+        }
+        if (pi_ratio >= 1) {
+            set_param_value(iname, value/pi_ratio);
+        }
+    }
+
+}
+
+void AP_Quicktune::adjust_gain_limited(AP_Quicktune::Param param, float value)
+{
+    adjust_gain(param, limit_gain(param, value));
+}
+
+float AP_Quicktune::limit_gain(AP_Quicktune::Param param, float value)
+{
+    const float saved_value = param_saved[uint8_t(param)];
+    if (reduce_max >= 0 && reduce_max < 100 && saved_value > 0) {
+        // Check if we exceeded gain reduction
+        const float reduction_pct = 100.0 * (saved_value - value) / saved_value;
+        if (reduction_pct > reduce_max) {
+            const float new_value = saved_value * (100 - reduce_max) * 0.01;
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Limiting %s %.3f -> %.3f", get_param_name(param), value, new_value);
+            value = new_value;
+        }
+    }
+    return value;
+}
+
+const char* AP_Quicktune::get_param_name(AP_Quicktune::Param param) const
+{
+    switch (param)
+    {
+        case Param::RLL_P:
+            return "Roll P";
+        case Param::RLL_I:
+            return "Roll I";
+        case Param::RLL_D:
+            return "Roll D";
+        case Param::PIT_P:
+            return "Pitch P";
+        case Param::PIT_I:
+            return "Pitch I";
+        case Param::PIT_D:
+            return "Pitch D";
+        case Param::YAW_P:
+            return "Yaw P";
+        case Param::YAW_I:
+            return "Yaw I";
+        case Param::YAW_D:
+            return "Yaw D";
+        default:
+            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+            return "UNK";
+    }
+}
+
+float AP_Quicktune::get_gain_mul() const
+{
+    return expf(logf(2.0)/(UPDATE_RATE_HZ*MAX(1,double_time)));
+}
+
+void AP_Quicktune::restore_all_params()
+{
+    for (uint8_t p = 0; p < uint8_t(Param::END); p++) {
+        const auto param = Param(p);
+        if (BIT_IS_SET(param_changed, p)) {
+            set_param_value(param, param_saved[p]);
+            BIT_CLEAR(param_changed, p);
+        }
+    }
+}
+
+void AP_Quicktune::save_all_params()
+{
+    for (uint8_t p = 0; p < uint8_t(Param::END); p++) {
+        const auto param = Param(p);
+        set_and_save_param_value(param, get_param_value(param));
+        param_saved[p] = get_param_value(param);
+        BIT_CLEAR(param_changed, p);
+    }
+    GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: Saved");
+}
+
+AP_Quicktune::Param AP_Quicktune::get_pname(AP_Quicktune::AxisName axis, AP_Quicktune::Stage stage) const
+{
+    const uint8_t axis_offset = uint8_t(axis) * param_per_axis;
+    switch (stage)
+    {
+    case Stage::P:
+        return Param(uint8_t(Param::RLL_P) + axis_offset);
+    case Stage::D:
+        return Param(uint8_t(Param::RLL_D) + axis_offset);
+    case Stage::FLTT:
+        return Param(uint8_t(Param::RLL_FLTT) + axis_offset);
+    case Stage::FLTD:
+        return Param(uint8_t(Param::RLL_FLTD) + axis_offset);
+    default:
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        return Param::END;
+    }
+}
+
+AP_Quicktune::Stage AP_Quicktune::get_stage(AP_Quicktune::Param param) const
+{
+    if (param == Param::RLL_P || param == Param::PIT_P || param == Param::YAW_P) {
+        return Stage::P;
+    } else {
+        return Stage::END; //Means "anything but P gain"
+    }
+}
+
+AP_Float *AP_Quicktune::get_param_pointer(AP_Quicktune::Param param) const
+{
+    const AxisName axis = get_axis(param);
+    auto *pid = get_axis_pid(axis);
+    if (pid == nullptr) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        return nullptr;
+    }
+    const Param roll_param = Param(uint8_t(param) % param_per_axis);
+    switch (roll_param)
+    {
+        case Param::RLL_P:
+            return &pid->kP();
+        case Param::RLL_I:
+            return &pid->kI();
+        case Param::RLL_D:
+            return &pid->kD();
+        case Param::RLL_SMAX:
+            return &pid->slew_limit();
+        case Param::RLL_FLTT:
+            return &pid->filt_T_hz();
+        case Param::RLL_FLTD:
+            return &pid->filt_D_hz();
+        case Param::RLL_FLTE:
+            return &pid->filt_E_hz();
+        case Param::RLL_FF:
+            return &pid->ff();
+        default:
+            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+            return nullptr;
+    }
+}
+
+float AP_Quicktune::get_param_value(AP_Quicktune::Param param) const
+{
+    AP_Float *ptr = get_param_pointer(param);
+    if (ptr != nullptr) {
+        return ptr->get();
+    }
+    return 0.0;
+}
+
+void AP_Quicktune::set_param_value(AP_Quicktune::Param param, float value)
+{
+    AP_Float *ptr = get_param_pointer(param);
+    if (ptr != nullptr) {
+       ptr->set(value);
+    }
+}
+
+void AP_Quicktune::set_and_save_param_value(AP_Quicktune::Param param, float value)
+{
+    AP_Float *ptr = get_param_pointer(param);
+    if (ptr != nullptr) {
+       ptr->set_and_save(value);
+    }
+}
+
+AP_Quicktune::AxisName AP_Quicktune::get_axis(AP_Quicktune::Param param) const
+{
+    if (param >= Param::END) {
+        return AxisName::END;
+    }
+    return AxisName(uint8_t(param) / param_per_axis);
+}
+
+const char* AP_Quicktune::get_axis_name(AP_Quicktune::AxisName axis) const
+{
+    switch (axis)
+    {
+        case AxisName::RLL:
+            return "Roll";
+        case AxisName::PIT:
+            return "Pitch";
+        case AxisName::YAW:
+            return "Yaw";
+        default:
+            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+            return "UNK";
+    }
+}
+
+float AP_Quicktune::gain_limit(AP_Quicktune::Param param) const
+{
+    if (get_axis(param) == AxisName::YAW) {
+        if (param == Param::YAW_P) {
+            return yaw_p_max;
+        }
+        if (param == Param::YAW_D) {
+            return yaw_d_max;
+        }
+    }
+    return 0.0;
+}
+
+
+// @LoggerMessage: QWIK
+// @Description: Quicktune
+// @Field: TimeUS: Time since system startup
+// @Field: ParamNo: number of parameter being tuned
+// @Field: SRate: slew rate
+// @Field: Gain: test gain for current axis and PID element
+// @Field: Param: name of parameter being being tuned
+void AP_Quicktune::Write_QWIK(float srate, float gain, AP_Quicktune::Param param)
+{
+#if HAL_LOGGING_ENABLED
+    AP::logger().WriteStreaming("QWIK","TimeUS,ParamNo,SRate,Gain,Param", "s#---", "-----", "QBffN",
+                                AP_HAL::micros64(),
+                                unsigned(param),
+                                srate,
+                                gain,
+                                get_param_name(param));
+#endif
+}
+
+#endif //AP_QUICKTUNE_ENABLED
diff --git a/libraries/AP_Quicktune/AP_Quicktune.h b/libraries/AP_Quicktune/AP_Quicktune.h
new file mode 100644
index 00000000000000..5c495a9e010acd
--- /dev/null
+++ b/libraries/AP_Quicktune/AP_Quicktune.h
@@ -0,0 +1,167 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#ifndef AP_QUICKTUNE_ENABLED
+#define AP_QUICKTUNE_ENABLED 1 // NOTE: may be disabled by vehicle header
+#endif
+
+#if AP_QUICKTUNE_ENABLED
+
+#include <AC_AttitudeControl/AC_AttitudeControl.h>
+#include <AP_Param/AP_Param.h>
+#include <RC_Channel/RC_Channel.h>
+
+class AP_Quicktune {
+public:
+    AP_Quicktune()
+    {
+        AP_Param::setup_object_defaults(this, var_info);
+    }
+
+    // Empty destructor to suppress compiler warning
+    virtual ~AP_Quicktune() {}
+
+    /* Do not allow copies */
+    CLASS_NO_COPY(AP_Quicktune);
+
+    // Parameter block
+    static const struct AP_Param::GroupInfo var_info[];
+
+    void update(bool mode_supports_quicktune);
+    void update_switch_pos(const RC_Channel::AuxSwitchPos ch_flag);
+
+private:
+
+    // Parameters
+    AP_Int8 enable;
+    AP_Int8 axes_enabled;
+    AP_Float double_time;
+    AP_Float gain_margin;
+    AP_Float osc_smax;
+    AP_Float yaw_p_max;
+    AP_Float yaw_d_max;
+    AP_Float rp_pi_ratio;
+    AP_Float y_pi_ratio;
+    AP_Int8 auto_filter;
+    AP_Float auto_save;
+    AP_Float reduce_max;
+    AP_Int16 options;
+    AP_Int8 angle_max;
+
+    // Low, Mid and High must be in the same positions as they are in RC_Channel::AuxSwitchPos
+    enum class SwitchPos : uint8_t {
+        LOW,
+        MID,
+        HIGH,
+        NONE,
+    };
+
+
+    enum class AxisName : uint8_t {
+        RLL = 0,
+        PIT,
+        YAW,
+        DONE,
+        END,
+    };
+
+    /*
+      note! we rely on the enum being in the same order between axes
+     */
+    enum class Param : uint8_t {
+        RLL_P = 0,
+        RLL_I,
+        RLL_D,
+        RLL_SMAX,
+        RLL_FLTT,
+        RLL_FLTD,
+        RLL_FLTE,
+        RLL_FF,
+
+        PIT_P,
+        PIT_I,
+        PIT_D,
+        PIT_SMAX,
+        PIT_FLTT,
+        PIT_FLTD,
+        PIT_FLTE,
+        PIT_FF,
+
+        YAW_P,
+        YAW_I,
+        YAW_D,
+        YAW_SMAX,
+        YAW_FLTT,
+        YAW_FLTD,
+        YAW_FLTE,
+        YAW_FF,
+        END,
+    };
+
+    static const uint8_t param_per_axis = uint8_t(Param::PIT_P) - uint8_t(Param::RLL_P);
+    static_assert(uint8_t(Param::END) == 3*param_per_axis, "AP_Quicktune Param error");
+
+    // Also the gains
+    enum class Stage : uint8_t {
+        D,
+        P,
+        DONE,
+        I,
+        FF,
+        SMAX,
+        FLTT,
+        FLTD,
+        FLTE,
+        END,
+    };
+
+    // Time keeping
+    uint32_t last_stage_change_ms;
+    uint32_t last_gain_report_ms;
+    uint32_t last_pilot_input_ms;
+    uint32_t last_warning_ms;
+    uint32_t tune_done_time_ms;
+
+    // Bitmasks
+    uint32_t axes_done;
+    uint32_t filters_done;
+    uint32_t param_changed; //Bitmask of changed parameters
+
+    Stage current_stage = Stage::D;
+    Param slew_parm = Param::END;
+    uint8_t slew_steps;
+    float slew_delta;
+    SwitchPos sw_pos; //Switch pos to be set by aux func
+    bool need_restore;
+    float param_saved[uint8_t(Param::END)]; //Saved values of the parameters
+
+    void reset_axes_done();
+    void setup_filters(AxisName axis);
+    bool have_pilot_input() const;
+    AxisName get_current_axis() const;
+    float get_slew_rate(AxisName axis) const;
+    void advance_stage(AxisName axis);
+    void adjust_gain(Param param, float value);
+    void adjust_gain_limited(Param param, float value);
+    float get_gain_mul() const;
+    void restore_all_params();
+    void save_all_params();
+    Param get_pname(AxisName axis, Stage stage) const;
+    AP_Float *get_param_pointer(Param param) const;
+    float get_param_value(Param param) const;
+    void set_param_value(Param param, float value);
+    void set_and_save_param_value(Param param, float value);
+    float gain_limit(Param param) const;
+    AxisName get_axis(Param param) const;
+    float limit_gain(Param param, float value);
+    const char* get_param_name(Param param) const;
+    Stage get_stage(Param param) const;
+    const char* get_axis_name(AxisName axis) const;
+    AC_PID *get_axis_pid(AxisName axis) const;
+    void Write_QWIK(float SRate, float Gain, Param param);
+
+    void abort_tune(void);
+};
+
+#endif  // AP_QUICKTUNE_ENABLED
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp
index 879344e3236cee..80b7cf9fdc537d 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp
@@ -14,15 +14,11 @@ extern const AP_HAL::HAL& hal;
 AP_RCProtocol_DroneCAN::Registry AP_RCProtocol_DroneCAN::registry;
 AP_RCProtocol_DroneCAN *AP_RCProtocol_DroneCAN::_singleton;
 
-void AP_RCProtocol_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_RCProtocol_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_rcinput, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("rc_sub");
-    }
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_rcinput, driver_index) != nullptr);
 }
 
 AP_RCProtocol_DroneCAN* AP_RCProtocol_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id)
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.h b/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.h
index d2c81025d76c63..aa4693020d9695 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.h
@@ -18,7 +18,7 @@ class AP_RCProtocol_DroneCAN : public AP_RCProtocol_Backend {
         _singleton = this;
     }
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
 
     void update() override;
 
diff --git a/libraries/AP_RCProtocol/spm_srxl.cpp b/libraries/AP_RCProtocol/spm_srxl.cpp
index 8dfcf495b58d29..4c8653a3f2e931 100644
--- a/libraries/AP_RCProtocol/spm_srxl.cpp
+++ b/libraries/AP_RCProtocol/spm_srxl.cpp
@@ -1304,7 +1304,7 @@ void srxlOnFrameError(uint8_t busIndex)
 
 SrxlFullID srxlGetTelemetryEndpoint(void)
 {
-    SrxlFullID retVal = {0};
+    SrxlFullID retVal = {{0}};
     if(srxlRx.pTelemRcvr)
     {
         retVal.deviceID = srxlRx.pTelemRcvr->deviceID;
diff --git a/libraries/AP_RCProtocol/spm_srxl.h b/libraries/AP_RCProtocol/spm_srxl.h
index c1aecc3abdc22a..799abfdcdedfd8 100644
--- a/libraries/AP_RCProtocol/spm_srxl.h
+++ b/libraries/AP_RCProtocol/spm_srxl.h
@@ -55,27 +55,6 @@ typedef enum
     SrxlDevType_Broadcast           = 15
 } SrxlDevType;
 
-// Default device ID list used by master when polling
-static const uint8_t SRXL_DEFAULT_ID_OF_TYPE[16] =
-{
-    [SrxlDevType_None]              = 0x00,
-    [SrxlDevType_RemoteReceiver]    = 0x10,
-    [SrxlDevType_Receiver]          = 0x21,
-    [SrxlDevType_FlightController]  = 0x30,
-    [SrxlDevType_ESC]               = 0x40,
-    [5]                             = 0x60,
-    [SrxlDevType_SRXLServo1]        = 0x60,
-    [SrxlDevType_SRXLServo2]        = 0x70,
-    [SrxlDevType_VTX]               = 0x81,
-    [9]                             = 0xFF,
-    [10]                            = 0xFF,
-    [11]                            = 0xFF,
-    [12]                            = 0xFF,
-    [13]                            = 0xFF,
-    [14]                            = 0xFF,
-    [SrxlDevType_Broadcast]         = 0xFF,
-};
-
 // Set SRXL_CRC_OPTIMIZE_MODE in spm_srxl_config.h to one of the following values
 #define SRXL_CRC_OPTIMIZE_EXTERNAL  (0)  // Uses an external function defined by SRXL_CRC_EXTERNAL_FN for CRC
 #define SRXL_CRC_OPTIMIZE_SPEED     (1)  // Uses table lookup for CRC computation (requires 512 const bytes for CRC table)
diff --git a/libraries/AP_RPM/RPM_DroneCAN.cpp b/libraries/AP_RPM/RPM_DroneCAN.cpp
index 4657ff7714be65..fffc87c0b11bdc 100644
--- a/libraries/AP_RPM/RPM_DroneCAN.cpp
+++ b/libraries/AP_RPM/RPM_DroneCAN.cpp
@@ -34,15 +34,11 @@ AP_RPM_DroneCAN::AP_RPM_DroneCAN(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_
 }
 
 // Subscribe to incoming rpm messages
-void AP_RPM_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_RPM_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_rpm, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("rpm_sub");
-    }
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_rpm, driver_index) != nullptr);
 }
 
 // Receive new CAN message
diff --git a/libraries/AP_RPM/RPM_DroneCAN.h b/libraries/AP_RPM/RPM_DroneCAN.h
index c9d8e8c0c511a9..ae6924071346b4 100644
--- a/libraries/AP_RPM/RPM_DroneCAN.h
+++ b/libraries/AP_RPM/RPM_DroneCAN.h
@@ -27,7 +27,7 @@ class AP_RPM_DroneCAN : public AP_RPM_Backend
     AP_RPM_DroneCAN(AP_RPM &_ap_rpm, uint8_t instance, AP_RPM::RPM_State &_state);
 
     // Subscribe to incoming rpm messages
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
 
     // update state
     void update(void) override;
diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp
index a57c3242852fd5..46c5df2d9b1369 100644
--- a/libraries/AP_RTC/AP_RTC.cpp
+++ b/libraries/AP_RTC/AP_RTC.cpp
@@ -9,6 +9,12 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Common/time.h>
 
+#define DEBUG_RTC_SHIFT 0
+
+#if DEBUG_RTC_SHIFT
+#include <AP_Logger/AP_Logger.h>
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 AP_RTC::AP_RTC()
@@ -47,9 +53,19 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type)
 {
     const uint64_t oldest_acceptable_date_us = 1640995200ULL*1000*1000; // 2022-01-01 0:00
 
-    if (type >= rtc_source_type) {
-        // e.g. system-time message when we've been set by the GPS
-        return;
+    // only allow time to be moved forward from the same sourcetype
+    // while the vehicle is disarmed:
+    if (hal.util->get_soft_armed()) {
+        if (type >= rtc_source_type) {
+            // e.g. system-time message when we've been set by the GPS
+            return;
+        }
+    } else {
+        // vehicle is disarmed; accept (e.g.) GPS time source updates
+        if (type > rtc_source_type) {
+            // e.g. system-time message when we've been set by the GPS
+            return;
+        }
     }
 
     // check it's from an allowed sources:
@@ -70,6 +86,11 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type)
     }
     WITH_SEMAPHORE(rsem);
 
+#if DEBUG_RTC_SHIFT
+    uint64_t old_utc = 0;
+    UNUSED_RESULT(get_utc_usec(old_utc));
+#endif
+
     rtc_shift = tmp;
 
     // update hardware clock:
@@ -83,6 +104,31 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type)
     // update signing timestamp
     GCS_MAVLINK::update_signing_timestamp(time_utc_usec);
 #endif
+
+#if DEBUG_RTC_SHIFT
+    uint64_t new_utc = 0;
+    UNUSED_RESULT(get_utc_usec(new_utc));
+    if (old_utc != new_utc) {
+        if (AP::logger().should_log(0xFFFF)){
+            // log to AP_Logger
+            // @LoggerMessage: RTC
+            // @Description: Information about RTC clock resets
+            // @Field: TimeUS: Time since system startup
+            // @Field: old_utc: old time
+            // @Field: new_utc: new time
+            AP::logger().WriteStreaming(
+                "RTC",
+                "TimeUS,old_utc,new_utc",
+                "sss",
+                "FFF",
+                "QQQ",
+                AP_HAL::micros64(),
+                old_utc,
+                new_utc
+                );
+        }
+    }
+#endif
 }
 
 bool AP_RTC::get_utc_usec(uint64_t &usec) const
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp
index 94c3aead5a1be6..37c623b53166aa 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp
@@ -13,14 +13,11 @@ extern const AP_HAL::HAL& hal;
 #define debug_range_finder_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { hal.console->printf(fmt, ##args); }} while (0)
 
 //links the rangefinder uavcan message to this backend
-void AP_RangeFinder_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_RangeFinder_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("measurement_sub");
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
+
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, driver_index) != nullptr);
 }
 
 //Method to find the backend relating to the node id
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h
index 6d763b2b97fba8..6f575db8391144 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h
@@ -16,7 +16,7 @@ class AP_RangeFinder_DroneCAN : public AP_RangeFinder_Backend {
 
     void update() override;
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
     static AP_RangeFinder_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t address, bool create_new);
     static AP_RangeFinder_Backend* detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
 
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
index ae6812021db6e1..72630f95f3949c 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp
@@ -14,6 +14,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Rangefinder type
     // @Description: Type of connected rangefinder
+    // @SortValues: AlphabeticalZeroAtTop
     // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,43:RDS02UF,100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),
diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp
index fe3a73ab2d984c..2a64e19f59b75c 100644
--- a/libraries/AP_Relay/AP_Relay.cpp
+++ b/libraries/AP_Relay/AP_Relay.cpp
@@ -353,7 +353,11 @@ void AP_Relay::init()
             continue;
         }
 
-        if (function == AP_Relay_Params::FUNCTION::RELAY) {
+        bool use_default_param = (function == AP_Relay_Params::FUNCTION::RELAY);
+#ifdef HAL_BUILD_AP_PERIPH
+        use_default_param |= (function >= AP_Relay_Params::FUNCTION::DroneCAN_HARDPOINT_0 && function <= AP_Relay_Params::FUNCTION::DroneCAN_HARDPOINT_15);
+#endif
+        if (use_default_param) {
             // relay by instance number, set the state to match our output
             const AP_Relay_Params::DefaultState default_state = _params[instance].default_state;
             if ((default_state == AP_Relay_Params::DefaultState::OFF) ||
diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp
index 7085e05541d220..0c3ae78f87e2b2 100644
--- a/libraries/AP_Scheduler/AP_Scheduler.cpp
+++ b/libraries/AP_Scheduler/AP_Scheduler.cpp
@@ -447,6 +447,11 @@ void AP_Scheduler::update_logging()
 // Write a performance monitoring packet
 void AP_Scheduler::Log_Write_Performance()
 {
+    uint64_t rtc = 0;
+#if AP_RTC_ENABLED
+    UNUSED_RESULT(AP::rtc().get_utc_usec(rtc));
+#endif
+
     const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
     struct log_Performance pkt = {
         LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
@@ -464,6 +469,7 @@ void AP_Scheduler::Log_Write_Performance()
         i2c_count        : pd.i2c_count,
         i2c_isr_count    : pd.i2c_isr_count,
         extra_loop_us    : extra_loop_us,
+        rtc              : rtc,
     };
     AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
 }
diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp
index b4056d7139b04e..1d19108bd1d438 100644
--- a/libraries/AP_Scripting/AP_Scripting.cpp
+++ b/libraries/AP_Scripting/AP_Scripting.cpp
@@ -94,6 +94,7 @@ const AP_Param::GroupInfo AP_Scripting::var_info[] = {
     // @Bitmask: 3: log runtime memory usage and execution time
     // @Bitmask: 4: Disable pre-arm check
     // @Bitmask: 5: Save CRC of current scripts to loaded and running checksum parameters enabling pre-arm
+    // @Bitmask: 6: Disable heap expansion on allocation failure
     // @User: Advanced
     AP_GROUPINFO("DEBUG_OPTS", 4, AP_Scripting, _debug_options, 0),
 
@@ -380,7 +381,7 @@ void AP_Scripting::thread(void) {
                 GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Scripting: %s", "restarted");
                 break;
             }
-            if ((_debug_options.get() & uint8_t(lua_scripts::DebugLevel::NO_SCRIPTS_TO_RUN)) != 0) {
+            if (option_is_set(DebugOption::NO_SCRIPTS_TO_RUN)) {
                 GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Scripting: %s", "stopped");
             }
         }
@@ -420,7 +421,7 @@ void AP_Scripting::handle_mission_command(const AP_Mission::Mission_Command& cmd
 
 bool AP_Scripting::arming_checks(size_t buflen, char *buffer) const
 {
-    if (!enabled() || ((_debug_options.get() & uint8_t(lua_scripts::DebugLevel::DISABLE_PRE_ARM)) != 0)) {
+    if (!enabled() || option_is_set(DebugOption::DISABLE_PRE_ARM)) {
         return true;
     }
 
@@ -518,9 +519,7 @@ void AP_Scripting::update() {
 // Check if DEBUG_OPTS bit has been set to save current checksum values to params
 void AP_Scripting::save_checksum() {
 
-    const uint8_t opts = _debug_options.get();
-    const uint8_t save_bit = uint8_t(lua_scripts::DebugLevel::SAVE_CHECKSUM);
-    if ((opts & save_bit) == 0) {
+    if (!option_is_set(DebugOption::SAVE_CHECKSUM)) {
         // Bit not set, nothing to do
         return;
     }
@@ -530,7 +529,7 @@ void AP_Scripting::save_checksum() {
     _required_running_checksum.set_and_save(lua_scripts::get_running_checksum() & checksum_param_mask);
 
     // Un-set debug option bit
-    _debug_options.set_and_save(opts & ~save_bit);
+    option_clear(DebugOption::SAVE_CHECKSUM);
 
     GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Scripting: %s", "saved checksums");
 
diff --git a/libraries/AP_Scripting/AP_Scripting.h b/libraries/AP_Scripting/AP_Scripting.h
index 367354227dd637..869e11df547ef8 100644
--- a/libraries/AP_Scripting/AP_Scripting.h
+++ b/libraries/AP_Scripting/AP_Scripting.h
@@ -152,6 +152,16 @@ class AP_Scripting
         AP_Scripting_SerialDevice _serialdevice;
     #endif
 
+    enum class DebugOption : uint8_t {
+        NO_SCRIPTS_TO_RUN = 1U << 0,
+        RUNTIME_MSG = 1U << 1,
+        SUPPRESS_SCRIPT_LOG = 1U << 2,
+        LOG_RUNTIME = 1U << 3,
+        DISABLE_PRE_ARM = 1U << 4,
+        SAVE_CHECKSUM = 1U << 5,
+        DISABLE_HEAP_EXPANSION = 1U << 6,
+    };
+
 private:
 
     void thread(void); // main script execution thread
@@ -185,6 +195,14 @@ class AP_Scripting
 
     AP_Enum<ThreadPriority> _thd_priority;
 
+    bool option_is_set(DebugOption option) const {
+        return (uint8_t(_debug_options.get()) & uint8_t(option)) != 0;
+    }
+
+    void option_clear(DebugOption option) {
+        _debug_options.set_and_save(_debug_options.get() & ~uint8_t(option));
+    }
+
     bool _thread_failed; // thread allocation failed
     bool _init_failed;  // true if memory allocation failed
     bool _restart; // true if scripts should be restarted
diff --git a/libraries/AP_Scripting/applets/copter-deadreckon-home.lua b/libraries/AP_Scripting/applets/copter-deadreckon-home.lua
index 152b752a0040c6..31d5f377859087 100644
--- a/libraries/AP_Scripting/applets/copter-deadreckon-home.lua
+++ b/libraries/AP_Scripting/applets/copter-deadreckon-home.lua
@@ -31,8 +31,8 @@
 --
 -- Testing in SITL:
 --   a. set map setshowsimpos 1 (to allow seeing where vehicle really is in simulator even with GPS disabled)
---   b. set SIM_GPS_DISABLE = 1 to disable GPS (confirm dead reckoning begins)
---   c. set SIM_GPS_DISABLE = 0 to re-enable GPS
+--   b. set SIM_GPS1_ENABLE = 0 to disable GPS (confirm dead reckoning begins)
+--   c. set SIM_GPS1_ENABLE = 1 to re-enable GPS
 --   d. set SIM_GPS_NUMSAT = 3 to lower simulated satellite count to confirm script triggers
 --   e. set DR_GPS_SACC_MAX = 0.01 to lower the threshold and trigger below the simulator value which is 0.04 (remember to set this back after testing!)
 --
diff --git a/libraries/AP_Scripting/applets/copter-deadreckon-home.md b/libraries/AP_Scripting/applets/copter-deadreckon-home.md
index c45ae3080ccd2f..b750eb4d2bf895 100644
--- a/libraries/AP_Scripting/applets/copter-deadreckon-home.md
+++ b/libraries/AP_Scripting/applets/copter-deadreckon-home.md
@@ -39,8 +39,8 @@ Deadreckoning will only be activated while the vehicle is in autonomous modes (e
 ## Testing in SITL
 
   - set map setshowsimpos 1 (to allow seeing where vehicle really is in simulator even with GPS disabled)
-  - set SIM_GPS_DISABLE = 1 to disable GPS (confirm dead reckoning begins)
-  - set SIM_GPS_DISABLE = 0 to re-enable GPS
+  - set SIM_GPS1_ENABLE = 0 to disable GPS (confirm dead reckoning begins)
+  - set SIM_GPS1_ENABLE = 1 to re-enable GPS
   - set SIM_GPS_NUMSAT = 3 to lower simulated satellite count to confirm script triggers
   - set DR_GPS_SACC_MAX = 0.01 to lower the threshold and trigger below the simulator value which is 0.04 (remember to set this back after testing!)
 
diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua
index 3a34534316de80..0a689dd344e750 100644
--- a/libraries/AP_Scripting/docs/docs.lua
+++ b/libraries/AP_Scripting/docs/docs.lua
@@ -2826,6 +2826,25 @@ function vehicle:is_landing() end
 ---@return boolean -- true on success
 function vehicle:set_crosstrack_start(new_start_location) end
 
+-- Register a custom mode. This behaves like guided mode but will report with a custom number and name
+---@param number integer -- mode number to use, should be over 100
+---@param full_name string -- Full mode name
+---@param short_name string -- Short mode name upto 4 characters
+---@return AP_Vehicle__custom_mode_state_ud|nil -- Returns custom mode state which allows customisation of behavior, nil if mode register fails
+function vehicle:register_custom_mode(number, full_name, short_name) end
+
+-- Custom mode state, allows customisation of mode behavior
+---@class (exact) AP_Vehicle__custom_mode_state_ud
+local AP_Vehicle__custom_mode_state_ud = {}
+
+-- get allow_entry, if true the vehicle is allowed to enter this custom mode
+---@return boolean
+function AP_Vehicle__custom_mode_state_ud:allow_entry() end
+
+-- set allow_entry, if true the vehicle is allowed to enter this custom mode
+---@param value boolean
+function AP_Vehicle__custom_mode_state_ud:allow_entry(value) end
+
 -- desc
 onvif = {}
 
diff --git a/libraries/AP_Scripting/examples/Flip_Mode.lua b/libraries/AP_Scripting/examples/Flip_Mode.lua
new file mode 100644
index 00000000000000..6e80799d1a683a
--- /dev/null
+++ b/libraries/AP_Scripting/examples/Flip_Mode.lua
@@ -0,0 +1,246 @@
+-- This script replicates the behavior of flip mode
+local MODE_NUMBER = 100
+
+-- Register flip as mode 100
+local FLIP_MODE_STATE = assert(vehicle:register_custom_mode(MODE_NUMBER, "Flip 2", "FLI2"))
+
+-- Get input channels
+local THROTTLE_CHAN = math.floor(assert(param:get("RCMAP_THROTTLE")))
+local pilot_throttle = assert(rc:get_channel(THROTTLE_CHAN))
+local pilot_pitch = assert(rc:get_channel(assert(param:get("RCMAP_PITCH"))))
+local pilot_roll = assert(rc:get_channel(assert(param:get("RCMAP_ROLL"))))
+
+local HOVER_THROTTLE = Parameter("MOT_THST_HOVER")
+local THROTTLE_MIN = Parameter("RC" .. THROTTLE_CHAN .. "_MIN")
+local THROTTLE_MAX = Parameter("RC" .. THROTTLE_CHAN .. "_MAX")
+local THROTTLE_DZ = Parameter("RC" .. THROTTLE_CHAN .. "_DZ")
+
+-- Replication of the copter function
+local function get_throttle_mid()
+    local r_in = (THROTTLE_MIN:get() + THROTTLE_MAX:get()) * 0.5
+
+    local radio_trim_low = THROTTLE_MIN:get() + THROTTLE_DZ:get()
+
+    return 1000.0 * ((r_in - radio_trim_low) / (THROTTLE_MAX:get() - radio_trim_low))
+end
+
+-- Replication of the copter function
+local function get_pilot_desired_throttle()
+
+    local thr_mid = HOVER_THROTTLE:get()
+    local throttle_control = (pilot_throttle:norm_input_ignore_trim() + 1.0) * (1000.0 / 2.0)
+
+    local mid_stick = get_throttle_mid()
+
+    local throttle_in
+    if throttle_control < mid_stick then
+        throttle_in = throttle_control * 0.5 / mid_stick
+
+    else
+        throttle_in = 0.5 + ((throttle_control - mid_stick) * 0.5 / (1000.0 - mid_stick))
+
+    end
+
+    local expo = -(thr_mid - 0.5) / 0.375
+    expo = math.max(expo, -0.5)
+    expo = math.min(expo,  1.0)
+
+    return throttle_in * (1.0-expo) + expo*throttle_in*throttle_in*throttle_in
+end
+
+-- Used to check for mode changes
+local last_mode_number
+
+local MODES = {
+    STABILIZE = 0,
+    ALT_HOLD = 2,
+}
+
+-- Check if we should be allowed to enter flip mode
+local function allow_enter(previous_mode)
+
+    -- Only allow flip from stabilize and alt hold
+    if (previous_mode ~= MODES.STABILIZE) and (previous_mode ~= MODES.ALT_HOLD) then
+        return false
+    end
+
+    -- Must be armed and flying
+    if (not arming:is_armed()) or (not vehicle:get_likely_flying()) then
+        return false
+    end
+
+    return true
+end
+
+local FLIP_STATE = {
+    START = 0,
+    ROLL = 1,
+    PITCH_A = 2,
+    PITCH_B = 3,
+    RECOVER = 4,
+}
+local state
+local start_ms
+local roll_dir
+local pitch_dir
+local start_attitude = {
+    roll = 0,
+    pitch = 0,
+    yaw = 0,
+}
+
+-- Init on first call
+local previous_mode
+local function init()
+    -- Record the previous mode, this is returned to on completion of the flip
+    previous_mode = last_mode_number
+
+    -- Return to previous mode immediately if flip cannot be performed
+    if not allow_enter(previous_mode) then
+        vehicle:set_mode(previous_mode)
+        return
+    end
+
+    state = FLIP_STATE.START
+    start_ms = millis()
+
+    roll_dir = 0.0
+    pitch_dir = 0.0
+
+    -- choose direction based on pilot's roll and pitch sticks
+    if pilot_pitch:norm_input_dz() > 0.1 then
+        pitch_dir = 1.0
+    elseif pilot_pitch:norm_input_dz() < -0.1 then
+        pitch_dir = -1.0
+    elseif pilot_roll:norm_input_dz() >= 0 then
+        roll_dir = 1.0
+    else
+        roll_dir = -1.0
+    end
+
+    -- Record start attitude to be used in recovery stage
+    start_attitude.roll = math.deg(ahrs:get_roll())
+    start_attitude.pitch = math.deg(ahrs:get_pitch())
+    start_attitude.yaw = math.deg(ahrs:get_yaw())
+
+end
+
+local FLIP_THR_INC = 0.2
+local FLIP_THR_DEC = 0.24
+local FLIP_ROTATION_RATE = 400
+
+local function run()
+    local NOW = millis()
+
+    -- Disarmed, pilot input or timeout then return to previous mode
+    local PILOT_INPUT = (math.abs(pilot_roll:norm_input_dz()) > 0.85) or (math.abs(pilot_pitch:norm_input_dz()) > 0.85)
+    if (not arming:is_armed()) or PILOT_INPUT or ((NOW - start_ms) > 2500) then
+        vehicle:set_mode(previous_mode)
+        return
+    end
+
+    local roll_deg = math.deg(ahrs:get_roll())
+    local pitch_deg = math.deg(ahrs:get_pitch())
+
+    if state == FLIP_STATE.RECOVER then
+        -- Target original attitude with 0 climb rate
+        vehicle:set_target_angle_and_climbrate(start_attitude.roll, start_attitude.pitch, start_attitude.yaw, 0.0, false, 0.0)
+
+        -- See if we have returned to the desired angle
+        local recovery_angle = math.abs((roll_deg - start_attitude.roll) * roll_dir) + math.abs((pitch_deg - start_attitude.pitch) * pitch_dir)
+
+        if recovery_angle < 5.0 then
+            -- Complete, return to original mode
+            vehicle:set_mode(previous_mode)
+        end
+        return
+    end
+
+    local throttle_out = get_pilot_desired_throttle()
+
+    local flip_angle = roll_deg * roll_dir + pitch_deg * pitch_dir
+
+    if state == FLIP_STATE.START then
+        -- Increase throttle
+        throttle_out = math.min(throttle_out + FLIP_THR_INC, 1.0)
+
+        -- Check for next stage
+        if flip_angle >= 45.0 then
+            if roll_dir ~= 0 then
+                -- Rolling flip
+                state = FLIP_STATE.ROLL
+            else
+                -- Pitching flip
+                state = FLIP_STATE.PITCH_A
+            end
+        end
+
+    elseif state == FLIP_STATE.ROLL then
+        -- decrease throttle
+        throttle_out = math.max(throttle_out - FLIP_THR_DEC, 0.0)
+
+        -- beyond 90deg move on to recovery
+        if (flip_angle < 45.0) and (flip_angle > -90.0) then
+            state = FLIP_STATE.RECOVER
+        end
+
+    elseif state == FLIP_STATE.PITCH_A then
+        -- decrease throttle
+        throttle_out = math.max(throttle_out - FLIP_THR_DEC, 0.0)
+
+        -- check roll for inversion
+        if (math.abs(roll_deg) > 90.0) and (flip_angle > 45.0) then
+            state = FLIP_STATE.PITCH_B
+        end
+
+    elseif state == FLIP_STATE.PITCH_B then
+        -- decrease throttle
+        throttle_out = math.max(throttle_out - FLIP_THR_DEC, 0.0)
+
+        -- check roll for un-inversion
+        if (math.abs(roll_deg) < 90.0) and (flip_angle > -45.0) then
+            state = FLIP_STATE.RECOVER
+        end
+
+    end
+
+    -- Send rate and throttle command to vehicle
+    local roll_rate = FLIP_ROTATION_RATE * roll_dir
+    local pitch_rate = FLIP_ROTATION_RATE * pitch_dir
+
+    vehicle:set_target_rate_and_throttle(roll_rate, pitch_rate, 0.0, throttle_out)
+
+end
+
+local function exit()
+    -- Nothing to do here
+end
+
+local function update()
+
+    -- Only allow entry into flip mode if armed and flying
+    local armed = arming:is_armed()
+    local flying = vehicle:get_likely_flying()
+    FLIP_MODE_STATE:allow_entry(armed and flying)
+
+    local mode = vehicle:get_mode()
+    if mode == MODE_NUMBER then
+        if last_mode_number ~= MODE_NUMBER then
+            -- Fist call after entering
+            init()
+        else
+            -- Runtime function
+            run()
+        end
+
+    elseif last_mode_number == MODE_NUMBER then
+        -- Exit mode
+        exit()
+    end
+    last_mode_number = mode
+
+    -- Run at 100Hz
+    return update, 10
+end
+
+return update()
diff --git a/libraries/AP_Scripting/examples/plane-callout-alt.lua b/libraries/AP_Scripting/examples/plane-callout-alt.lua
new file mode 100644
index 00000000000000..4a7181dbad1260
--- /dev/null
+++ b/libraries/AP_Scripting/examples/plane-callout-alt.lua
@@ -0,0 +1,219 @@
+--[[
+
+   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 <http://www.gnu.org/licenses/>.
+
+--]]
+
+SCRIPT_VERSION = "4.6.0-003"
+SCRIPT_NAME = "Plane Altitude Callouts"
+SCRIPT_NAME_SHORT = "Callout"
+
+-- This script calls out altitude if the user is near (WARN) or above (MAX)
+-- Allows for units to be feet even if you do everything else in metric because the laws typically specify 400 feet for UAV/RPAS in most countries
+-- all of the settings are stored in parameters:
+-- CALLOUT_ALT_UNITS 1 = metric, 2 (default) = imperial
+-- CALLOUT_ALT_MAX max allowed altitude (its still a message there is no action)
+-- CALLOUT_ALT_STEP callout (via GC message) when altitude changes by this amount or more
+-- CALLOUT_ALT_CALL seconds between callout of flying altitude
+-- CALLOUT_ALT_WARN seconds between callouts that you are less than ALT_STEP below ALT_MAX
+-- CALLOUT_ALT_HIGH seconds between callouts that you have exceeded ALT_MAX
+
+REFRESH_RATE      = 1000	    --check every 1 second
+
+MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
+
+local PARAM_TABLE_KEY = 102
+local PARAM_TABLE_PREFIX = "ZPC_"
+
+-- add a parameter and bind it to a variable
+local function bind_add_param(name, idx, default_value)
+   assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
+   return Parameter(PARAM_TABLE_PREFIX .. name)
+end
+
+-- setup follow mode specific parameters
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 10), 'could not add param table')
+
+
+-- Add these ZPC_ parameters specifically for this script
+--[[
+  // @Param: ZPC_ALT_UNITS
+  // @DisplayName: Plane Callouts Units - defaults to feet
+  // @Description: 1: Metric/meters or 2: Imperial/feet
+  // @User: Standard
+--]]
+ZPC_ALT_UNITS = bind_add_param('ALT_UNITS', 1, 2)
+
+--[[
+    // @Param: ZPC_ALT_MAX
+    // @DisplayName: Plane Callouts max altitude
+    // @Description: Maximum altitude in ALT_UNITS
+    // @Range: 0 30
+    // @Units: seconds
+--]]
+ZPC_ALT_MAX = bind_add_param("ALT_MAX", 2, 400)
+
+--[[
+    // @Param: ZPC_ALT_STEP
+    // @DisplayName: Plane Callouts altitude steps
+    // @Description: Altitude steps for callouts in ALT_UNITS
+    // @Range: 0 100
+    // @Units: ALT_UNIT
+--]]
+ZPC_ALT_STEP = bind_add_param("ALT_STEP", 3, 25)
+
+--[[
+    // @Param: ZPC_ALT_CALL
+    // @DisplayName: Plane Callouts frequency
+    // @Description: How often to callout altitude when flying normally
+    // @Range: 0 30
+    // @Units: seconds
+--]]
+ZPC_ALT_CALL = bind_add_param("ALT_CALL", 4, 25)
+
+--[[
+    // @Param: ZPC_ALT_WARN
+    // @DisplayName: Plane altitude warning frequency
+    // @Description: How often to nag about almost hitting MAX
+    // @Range: 0 30
+    // @Units: seconds
+--]]
+ZPC_ALT_WARN = bind_add_param("ALT_WARN", 5, 25)
+
+--[[
+    // @Param: ZPC_ALT_HIGH
+    // @DisplayName: Plane altitude max altitude callout frequency
+    // @Description: How often to nag about exceeding MAX
+    // @Range: 0 30
+    // @Units: seconds
+--]]
+ZPC_ALT_HIGH = bind_add_param("ALT_HIGH", 6, 10)
+
+local alt_units = ZPC_ALT_UNITS:get() or 2  -- default to feet because "thats what it is"
+local alt_max = ZPC_ALT_MAX:get() or 400    -- most places this is the legal limit, if you have a different limit, change this
+local alt_step = ZPC_ALT_STEP:get() or 25
+local alt_call_sec = ZPC_ALT_CALL:get() or 25
+local alt_warn_sec = ZPC_ALT_WARN:get() or 25
+local alt_high_sec = ZPC_ALT_HIGH:get() or 10
+
+local alt_last = 0
+local alt_warn = alt_max - alt_step
+
+local unit = "meters"
+if (alt_units == 2) then
+    unit = "feet"
+end
+local altitude_max = string.format("%i %s", math.floor(alt_max+0.5), unit )
+
+local time_last_warn_s = millis() / 1000
+local time_last_max_s = millis() / 1000
+local time_last_update_s = millis() / 1000
+local alt_max_exceeded = false
+local alt_warn_exceeded = false
+
+local function update() -- this is the loop which periodically runs
+    local current_time_s = millis() / 1000
+    -- setting the height/altitude variables like this means all the code below works without change for either metric or Imperial units
+    local terrain_height = terrain:height_above_terrain(true)
+
+    -- if terrain height is not available use height above home
+    if terrain_height == nil then
+            -- override terrain height with home height (TODO: parameterize this)
+        local pos = ahrs:get_relative_position_NED_home()
+        if pos == nil then
+            return
+        else
+            terrain_height = -pos:z()
+        end
+    end
+
+    --- allow these parameters to be changed at runtime.
+    alt_units = ZPC_ALT_UNITS:get() or 2
+    alt_max = ZPC_ALT_MAX:get() or 400    -- most places this is the legal limit, if you have a different limit, change this
+    alt_step = ZPC_ALT_STEP:get() or 25
+    alt_call_sec = ZPC_ALT_CALL:get() or 25
+    alt_warn_sec = ZPC_ALT_WARN:get() or 25
+    alt_high_sec = ZPC_ALT_HIGH:get() or 10
+    if (alt_units == 2) then
+        unit = "feet"
+    else
+        unit = "meters"
+    end
+
+    if (alt_units == 2) then
+        terrain_height = terrain_height * 3.28084
+    end
+    local altitude = string.format("%i %s", math.floor(terrain_height+0.5), unit )
+
+    if terrain_height > alt_max then
+        if (time_last_max_s < current_time_s - alt_high_sec) then
+            gcs:send_text(MAV_SEVERITY.ERROR, string.format("Altitude %s too high max %s", altitude,  altitude_max ))
+            time_last_max_s = current_time_s
+            alt_max_exceeded = true
+        end
+    elseif terrain_height > alt_warn then
+            if (time_last_warn_s < current_time_s - alt_warn_sec) then
+                gcs:send_text(MAV_SEVERITY.WARNING, string.format("Warning altitude is %s", altitude ))
+                time_last_warn_s = current_time_s
+                alt_warn_exceeded = true
+            end
+	else
+	    -- we are fine now, but maybe we were not fine before. 
+        -- So if we previously displayed altitude warn/error messages, let the pilot know we are now fine
+        if(alt_max_exceeded or alt_warn_exceeded) then
+            gcs:send_text(MAV_SEVERITY.WARNING, string.format("Altitude %s is Ok", altitude ))
+        else
+            -- nothing else important happened, so see if our altitude has gone up or down by more than ALT_STEP
+		    -- in which case we call it out
+            if (time_last_update_s < current_time_s - alt_call_sec) then
+                local alt_diff = (terrain_height - alt_last) 
+                if (math.abs(alt_diff) > alt_step) then
+                    gcs:send_text(MAV_SEVERITY.WARNING, string.format("Altitude is %s", altitude ))
+                    alt_last = math.floor(terrain_height / alt_step + 0.5) * alt_step
+                end
+                time_last_update_s = current_time_s
+            end
+        end
+        alt_max_exceeded = false
+        alt_warn_exceeded = false
+    end
+end
+
+local displayed_banner = false
+-- wrapper around update(). This calls update() at 1Hz,
+-- and if update faults then an error is displayed, but the script is not
+-- stopped
+local function protected_wrapper()
+
+    if not displayed_banner then
+        gcs:send_text(MAV_SEVERITY.INFO, string.format("%s %s script loaded callouts in %s", SCRIPT_NAME, SCRIPT_VERSION, unit) )
+        displayed_banner = true
+    end
+
+    if not arming:is_armed() then return protected_wrapper, REFRESH_RATE * 10 end
+
+    local success, err = pcall(update)
+    if not success then
+        gcs:send_text(MAV_SEVERITY.ALERT, SCRIPT_NAME_SHORT .. "Internal Error: " .. err)
+        -- when we fault we run the update function again after 1s, slowing it
+        -- down a bit so we don't flood the console with errors
+        return protected_wrapper, 1000
+    end
+    return protected_wrapper, REFRESH_RATE
+end
+
+
+  -- start running update loop - wait 20 seconds before starting up
+return protected_wrapper, 20000
+
diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc
index ae6ba0a48f426e..0a2653a55b280a 100644
--- a/libraries/AP_Scripting/generator/description/bindings.desc
+++ b/libraries/AP_Scripting/generator/description/bindings.desc
@@ -309,6 +309,9 @@ singleton AP_ONVIF method set_absolutemove boolean float'skip_check float'skip_c
 singleton AP_ONVIF method get_pan_tilt_limit_min Vector2f
 singleton AP_ONVIF method get_pan_tilt_limit_max Vector2f
 
+ap_object AP_Vehicle::custom_mode_state depends (!defined(HAL_BUILD_AP_PERIPH))
+ap_object AP_Vehicle::custom_mode_state field allow_entry boolean read write
+
 include AP_Vehicle/AP_Vehicle.h
 singleton AP_Vehicle depends (!defined(HAL_BUILD_AP_PERIPH))
 singleton AP_Vehicle rename vehicle
@@ -352,6 +355,7 @@ singleton AP_Vehicle method reboot void boolean
 singleton AP_Vehicle method is_landing boolean
 singleton AP_Vehicle method is_taking_off boolean
 singleton AP_Vehicle method set_crosstrack_start boolean Location
+singleton AP_Vehicle method register_custom_mode AP_Vehicle::custom_mode_state uint8_t'skip_check string string
 
 
 include AP_SerialLED/AP_SerialLED.h
diff --git a/libraries/AP_Scripting/generator/src/main.c b/libraries/AP_Scripting/generator/src/main.c
index e22f03d05d07d6..92278a34a1273c 100644
--- a/libraries/AP_Scripting/generator/src/main.c
+++ b/libraries/AP_Scripting/generator/src/main.c
@@ -1222,8 +1222,11 @@ void handle_ap_object(void) {
   } else if (strcmp(type, keyword_manual) == 0) {
     handle_manual(node, ALIAS_TYPE_MANUAL);
 
+  } else if (strcmp(type, keyword_field) == 0) {
+    handle_userdata_field(node);
+
   } else {
-    error(ERROR_SINGLETON, "AP_Objects only support renames, methods, semaphore or manual keywords (got %s)", type);
+    error(ERROR_SINGLETON, "AP_Objects only support renames, methods, field, semaphore or manual keywords (got %s)", type);
   }
 
   // check that we didn't just add 2 singleton flags
@@ -1830,6 +1833,28 @@ void emit_singleton_fields() {
   }
 }
 
+void emit_ap_object_field(const struct userdata *data, const struct userdata_field *field) {
+  fprintf(source, "static int %s_%s(lua_State *L) {\n", data->sanatized_name, field->name);
+  fprintf(source, "    %s *ud = *check_%s(L, 1);\n", data->name, data->sanatized_name);
+  emit_field(field, "ud", "->");
+}
+
+void emit_ap_object_fields() {
+  struct userdata * node = parsed_ap_objects;
+  while(node) {
+    struct userdata_field *field = node->fields;
+    if (field) {
+      start_dependency(source, node->dependency);
+      while(field) {
+        emit_ap_object_field(node, field);
+        field = field->next;
+      }
+      end_dependency(source, node->dependency);
+    }
+    node = node->next;
+  }
+}
+
 // emit refences functions for a call, return the number of arduments added
 int emit_references(const struct argument *arg, const char * tab) {
   int arg_index = NULLABLE_ARG_COUNT_BASE + 2;
@@ -3162,7 +3187,7 @@ int main(int argc, char **argv) {
   emit_methods(parsed_userdata);
   emit_index(parsed_userdata);
 
-
+  emit_ap_object_fields();
   emit_methods(parsed_ap_objects);
   emit_index(parsed_ap_objects);
 
diff --git a/libraries/AP_Scripting/lua/src/lundump.c b/libraries/AP_Scripting/lua/src/lundump.c
index ee86a91ed61214..432db3c337210b 100644
--- a/libraries/AP_Scripting/lua/src/lundump.c
+++ b/libraries/AP_Scripting/lua/src/lundump.c
@@ -241,7 +241,7 @@ static void fchecksize (LoadState *S, size_t size, const char *tname) {
 #define checksize(S,t)	fchecksize(S,sizeof(t),#t)
 
 static void checkHeader (LoadState *S) {
-  checkliteral(S, LUA_SIGNATURE + 1, "not a");  /* 1st char already checked */
+  checkliteral(S, &LUA_SIGNATURE[1], "not a");  /* 1st char already checked */
   if (LoadByte(S) != LUAC_VERSION)
     error(S, "version mismatch in");
   if (LoadByte(S) != LUAC_FORMAT)
diff --git a/libraries/AP_Scripting/lua_scripts.cpp b/libraries/AP_Scripting/lua_scripts.cpp
index 03c94921136f9d..b635d1d4dfeb8e 100644
--- a/libraries/AP_Scripting/lua_scripts.cpp
+++ b/libraries/AP_Scripting/lua_scripts.cpp
@@ -40,11 +40,12 @@ uint32_t lua_scripts::loaded_checksum;
 uint32_t lua_scripts::running_checksum;
 HAL_Semaphore lua_scripts::crc_sem;
 
-lua_scripts::lua_scripts(const AP_Int32 &vm_steps, const AP_Int32 &heap_size, const AP_Int8 &debug_options)
+lua_scripts::lua_scripts(const AP_Int32 &vm_steps, const AP_Int32 &heap_size, AP_Int8 &debug_options)
     : _vm_steps(vm_steps),
       _debug_options(debug_options)
 {
-    _heap.create(heap_size, 4);
+    const bool allow_heap_expansion = !option_is_set(AP_Scripting::DebugOption::DISABLE_HEAP_EXPANSION);
+    _heap.create(heap_size, 10, allow_heap_expansion, 20*1024);
 }
 
 lua_scripts::~lua_scripts() {
@@ -129,14 +130,14 @@ int lua_scripts::atpanic(lua_State *L) {
 // helper for print and log of runtime stats
 void lua_scripts::update_stats(const char *name, uint32_t run_time, int total_mem, int run_mem)
 {
-    if ((_debug_options.get() & uint8_t(DebugLevel::RUNTIME_MSG)) != 0) {
+    if (option_is_set(AP_Scripting::DebugOption::RUNTIME_MSG)) {
         GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Lua: Time: %u Mem: %d + %d",
                                             (unsigned int)run_time,
                                             (int)total_mem,
                                             (int)run_mem);
     }
 #if HAL_LOGGING_ENABLED
-    if ((_debug_options.get() & uint8_t(DebugLevel::LOG_RUNTIME)) != 0) {
+    if (option_is_set(AP_Scripting::DebugOption::LOG_RUNTIME)) {
         struct log_Scripting pkt {
             LOG_PACKET_HEADER_INIT(LOG_SCRIPTING_MSG),
             time_us      : AP_HAL::micros64(),
@@ -295,7 +296,7 @@ void lua_scripts::load_all_scripts_in_dir(lua_State *L, const char *dirname) {
         reschedule_script(script);
 
 #if HAL_LOGGER_FILE_CONTENTS_ENABLED
-        if ((_debug_options.get() & uint8_t(DebugLevel::SUPPRESS_SCRIPT_LOG)) == 0) {
+        if (!option_is_set(AP_Scripting::DebugOption::SUPPRESS_SCRIPT_LOG)) {
             AP::logger().log_file_content(filename);
         }
 #endif
@@ -539,6 +540,8 @@ void lua_scripts::run(void) {
     succeeded_initial_load = true;
 #endif // __clang_analyzer__
 
+    uint32_t expansion_size = 0;
+
     while (AP_Scripting::get_singleton()->should_run()) {
 #if defined(AP_SCRIPTING_CHECKS) && AP_SCRIPTING_CHECKS >= 1
         if (lua_gettop(L) != 0) {
@@ -564,7 +567,7 @@ void lua_scripts::run(void) {
                 hal.scheduler->delay(scripts->next_run_ms - now_ms);
             }
 
-            if ((_debug_options.get() & uint8_t(DebugLevel::RUNTIME_MSG)) != 0) {
+            if (option_is_set(AP_Scripting::DebugOption::RUNTIME_MSG)) {
                 GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Lua: Running %s", scripts->name);
             }
             // take a copy of the script name for the purposes of
@@ -600,12 +603,23 @@ void lua_scripts::run(void) {
             lua_gc(L, LUA_GCCOLLECT, 0);
 
         } else {
-            if ((_debug_options.get() & uint8_t(DebugLevel::NO_SCRIPTS_TO_RUN)) != 0) {
+            if (option_is_set(AP_Scripting::DebugOption::NO_SCRIPTS_TO_RUN)) {
                 GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Lua: No scripts to run");
             }
             hal.scheduler->delay(1000);
         }
 
+        /*
+          report a warning if SCR_HEAP_SIZE wasn't adequate and we
+          expanded at runtime, so the user can fix it for future
+          flights
+         */
+        const uint32_t new_expansion_size = _heap.get_expansion_size();
+        if (new_expansion_size > expansion_size) {
+            expansion_size = new_expansion_size;
+            set_and_print_new_error_message(MAV_SEVERITY_WARNING, "Required SCR_HEAP_SIZE over %u", unsigned(expansion_size));
+        }
+
         // re-print the latest error message every 10 seconds 10 times
         const uint8_t error_prints = 10;
         if ((print_error_count < error_prints) && (AP_HAL::millis() - last_print_ms > 10000)) {
diff --git a/libraries/AP_Scripting/lua_scripts.h b/libraries/AP_Scripting/lua_scripts.h
index 437b8d2035368a..d08bb6fe59a9a5 100644
--- a/libraries/AP_Scripting/lua_scripts.h
+++ b/libraries/AP_Scripting/lua_scripts.h
@@ -26,7 +26,7 @@
 #include <AP_Scripting/AP_Scripting.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_HAL/Semaphores.h>
-#include <AP_Common/MultiHeap.h>
+#include <AP_MultiHeap/AP_MultiHeap.h>
 #include "lua_common_defs.h"
 
 #include "lua/src/lua.hpp"
@@ -34,7 +34,7 @@
 class lua_scripts
 {
 public:
-    lua_scripts(const AP_Int32 &vm_steps, const AP_Int32 &heap_size, const AP_Int8 &debug_options);
+    lua_scripts(const AP_Int32 &vm_steps, const AP_Int32 &heap_size, AP_Int8 &debug_options);
 
     ~lua_scripts();
 
@@ -48,15 +48,6 @@ class lua_scripts
 
     static bool overtime; // script exceeded it's execution slot, and we are bailing out
 
-    enum class DebugLevel {
-        NO_SCRIPTS_TO_RUN = 1U << 0,
-        RUNTIME_MSG = 1U << 1,
-        SUPPRESS_SCRIPT_LOG = 1U << 2,
-        LOG_RUNTIME = 1U << 3,
-        DISABLE_PRE_ARM = 1U << 4,
-        SAVE_CHECKSUM = 1U << 5,
-    };
-
 private:
 
     void create_sandbox(lua_State *L);
@@ -96,7 +87,11 @@ class lua_scripts
     lua_State *lua_state;
 
     const AP_Int32 & _vm_steps;
-    const AP_Int8 & _debug_options;
+    AP_Int8 & _debug_options;
+
+    bool option_is_set(AP_Scripting::DebugOption option) const {
+        return (uint8_t(_debug_options.get()) & uint8_t(option)) != 0;
+    }
 
     static void *alloc(void *ud, void *ptr, size_t osize, size_t nsize);
 
diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h
index 4d13ef8e797d67..f9dd8b17c8073f 100644
--- a/libraries/AP_Soaring/AP_Soaring.h
+++ b/libraries/AP_Soaring/AP_Soaring.h
@@ -23,10 +23,10 @@
 #include "Variometer.h"
 #include "SpeedToFly.h"
 
-#define INITIAL_THERMAL_RADIUS 80.0
-#define INITIAL_STRENGTH_COVARIANCE 0.0049
-#define INITIAL_RADIUS_COVARIANCE 400.0
-#define INITIAL_POSITION_COVARIANCE 400.0
+static constexpr float INITIAL_THERMAL_RADIUS = 80.0;
+static constexpr float INITIAL_STRENGTH_COVARIANCE = 0.0049;
+static constexpr float INITIAL_RADIUS_COVARIANCE = 400.0;
+static constexpr float INITIAL_POSITION_COVARIANCE = 400.0;
 
 
 class SoaringController {
diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp
index f25cf5294e6f93..49277cfce07ba7 100644
--- a/libraries/AP_TECS/AP_TECS.cpp
+++ b/libraries/AP_TECS/AP_TECS.cpp
@@ -427,6 +427,9 @@ void AP_TECS::_update_speed(float DT)
 
     // Get measured airspeed or default to trim speed and constrain to range between min and max if
     // airspeed sensor data cannot be used
+
+    // Equivalent airspeed
+    float _EAS;
     if (!use_airspeed || !_ahrs.airspeed_estimate(_EAS)) {
         // If no airspeed available use average of min and max
         _EAS = constrain_float(aparm.airspeed_cruise.get(), (float)aparm.airspeed_min.get(), (float)aparm.airspeed_max.get());
diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h
index 5bc757c0b79a72..57748edba1a073 100644
--- a/libraries/AP_TECS/AP_TECS.h
+++ b/libraries/AP_TECS/AP_TECS.h
@@ -271,9 +271,6 @@ class AP_TECS {
     float _vel_dot;
     float _vel_dot_lpf;
 
-    // Equivalent airspeed
-    float _EAS;
-
     // True airspeed limits
     float _TASmax;
     float _TASmin;
diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.cpp
index 0092939a06566b..172d10700e9fab 100644
--- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.cpp
+++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.cpp
@@ -53,15 +53,11 @@ AP_TemperatureSensor_DroneCAN::AP_TemperatureSensor_DroneCAN(AP_TemperatureSenso
 }
 
 // Subscript to incoming temperature messages
-void AP_TemperatureSensor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
+bool AP_TemperatureSensor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
 {
-    if (ap_dronecan == nullptr) {
-        return;
-    }
+    const auto driver_index = ap_dronecan->get_driver_index();
 
-    if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, ap_dronecan->get_driver_index()) == nullptr) {
-        AP_BoardConfig::allocation_error("temp_sub");
-    }
+    return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, driver_index) != nullptr);
 }
 
 void AP_TemperatureSensor_DroneCAN::handle_temperature(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_device_Temperature &msg)
diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.h
index fa2a3f086f0098..92b6d8ec310089 100644
--- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.h
+++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.h
@@ -28,7 +28,7 @@ class AP_TemperatureSensor_DroneCAN : public AP_TemperatureSensor_Backend {
 public:
     AP_TemperatureSensor_DroneCAN(AP_TemperatureSensor &front, AP_TemperatureSensor::TemperatureSensor_State &state, AP_TemperatureSensor_Params &params);
 
-    static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
+    static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
 
     // Don't do anything in update, but still need to override the pure virtual method.
     void update(void) override {};
diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp
index 48966bfbe161c7..79fc81a41f9a6f 100644
--- a/libraries/AP_Terrain/AP_Terrain.cpp
+++ b/libraries/AP_Terrain/AP_Terrain.cpp
@@ -154,19 +154,17 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected)
     }
 
     // hXY are the heights of the 4 surrounding grid points
-    int16_t h00, h01, h10, h11;
-
-    h00 = grid.height[info.idx_x+0][info.idx_y+0];
-    h01 = grid.height[info.idx_x+0][info.idx_y+1];
-    h10 = grid.height[info.idx_x+1][info.idx_y+0];
-    h11 = grid.height[info.idx_x+1][info.idx_y+1];
+    const auto h00 = grid.height[info.idx_x+0][info.idx_y+0];
+    const auto h01 = grid.height[info.idx_x+0][info.idx_y+1];
+    const auto h10 = grid.height[info.idx_x+1][info.idx_y+0];
+    const auto h11 = grid.height[info.idx_x+1][info.idx_y+1];
 
     // do a simple dual linear interpolation. We could do something
     // fancier, but it probably isn't worth it as long as the
     // grid_spacing is kept small enough
-    float avg1 = (1.0f-info.frac_x) * h00  + info.frac_x * h10;
-    float avg2 = (1.0f-info.frac_x) * h01  + info.frac_x * h11;
-    float avg  = (1.0f-info.frac_y) * avg1 + info.frac_y * avg2;
+    const float avg1 = (1.0f-info.frac_x) * h00  + info.frac_x * h10;
+    const float avg2 = (1.0f-info.frac_x) * h01  + info.frac_x * h11;
+    const float avg  = (1.0f-info.frac_y) * avg1 + info.frac_y * avg2;
 
     height = avg;
 
@@ -567,7 +565,7 @@ void AP_Terrain::update_reference_offset(void)
     if (!reference_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) {
         return;
     }
-    float adjustment = alt_cm*0.01 - height;
+    const float adjustment = alt_cm*0.01 - height;
     reference_offset = constrain_float(adjustment, -offset_max, offset_max);
     if (fabsf(adjustment) > offset_max.get()+0.5) {
         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Terrain: clamping offset %.0f to %.0f",
diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
index 02895d1bc27407..f56262e0c33116 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.h
+++ b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -172,6 +172,8 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     virtual bool is_crashed() const;
 
 #if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
+    // Method to takeoff for use by external control
+    virtual bool start_takeoff(const float alt) { return false; }
     // Method to control vehicle position for use by external control
     virtual bool set_target_location(const Location& target_loc) { return false; }
 #endif // AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED
@@ -179,7 +181,6 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     /*
       methods to control vehicle for use by scripting
     */
-    virtual bool start_takeoff(float alt) { return false; }
     virtual bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) { return false; }
     virtual bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) { return false; }
     virtual bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) { return false; }
@@ -246,6 +247,12 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     // returns true on success and control_value is set to a value in the range -1 to +1
     virtual bool get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value) { return false; }
 
+    // Register a custom mode with given number and names, return a structure which the script can edit
+    struct custom_mode_state {
+        bool allow_entry;
+    };
+    virtual custom_mode_state* register_custom_mode(const uint8_t number, const char* full_name, const char* short_name) { return nullptr; }
+
 #endif // AP_SCRIPTING_ENABLED
 
     // returns true if vehicle is in the process of landing
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index 8bb1108de3d189..649a12a6ad6337 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -1139,6 +1139,7 @@ class GCS_MAVLINK
         uint8_t next_index;
     } available_modes;
     bool send_available_modes();
+    bool send_available_mode_monitor();
 
 };
 
@@ -1301,6 +1302,11 @@ class GCS
     MAV_RESULT lua_command_int_packet(const mavlink_command_int_t &packet);
 #endif
 
+    // Sequence number should be incremented when available modes changes
+    // Sent in AVAILABLE_MODES_MONITOR msg
+    uint8_t get_available_modes_sequence() const { return available_modes_sequence; }
+    void available_modes_changed() { available_modes_sequence += 1; }
+
 protected:
 
     virtual GCS_MAVLINK *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
@@ -1378,6 +1384,10 @@ class GCS
     // GCS::update_send is called so we don't starve later links of
     // time in which they are permitted to send messages.
     uint8_t first_backend_to_send;
+
+    // Sequence number should be incremented when available modes changes
+    // Sent in AVAILABLE_MODES_MONITOR msg
+    uint8_t available_modes_sequence;
 };
 
 GCS &gcs();
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 9922a4f8377b07..e670339e7b756b 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -1153,6 +1153,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
         { MAVLINK_MSG_ID_AIRSPEED, MSG_AIRSPEED},
 #endif
         { MAVLINK_MSG_ID_AVAILABLE_MODES, MSG_AVAILABLE_MODES},
+        { MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR, MSG_AVAILABLE_MODES_MONITOR},
             };
 
     for (uint8_t i=0; i<ARRAY_SIZE(map); i++) {
@@ -3210,6 +3211,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_message(const mavlink_command_int
         available_modes.should_send = true;
         available_modes.next_index = 1;
         available_modes.requested_index = (uint8_t)packet.param2;
+
+        // After the first request sequnece is streamed in the AVAILABLE_MODES_MONITOR message
+        // This allows the GCS to re-request modes if there is a change
+        set_ap_message_interval(MSG_AVAILABLE_MODES_MONITOR, 5000);
         break;
 
     default:
@@ -5502,7 +5507,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
 #endif
 
     case MAV_CMD_DO_SET_ROI_NONE: {
-        const Location zero_loc = Location();
+        const Location zero_loc;
         return handle_command_do_set_roi(zero_loc);
     }
 
@@ -6137,6 +6142,18 @@ bool GCS_MAVLINK::send_available_modes()
     return true;
 }
 
+bool GCS_MAVLINK::send_available_mode_monitor()
+{
+    CHECK_PAYLOAD_SIZE(AVAILABLE_MODES_MONITOR);
+
+    mavlink_msg_available_modes_monitor_send(
+        chan,
+        gcs().get_available_modes_sequence()
+    );
+
+    return true;
+}
+
 bool GCS_MAVLINK::try_send_message(const enum ap_message id)
 {
     bool ret = true;
@@ -6571,6 +6588,10 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         ret = send_available_modes();
         break;
 
+    case MSG_AVAILABLE_MODES_MONITOR:
+        ret = send_available_mode_monitor();
+        break;
+
     default:
         // try_send_message must always at some stage return true for
         // a message, or we will attempt to infinitely retry the
diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h
index 2c8800a959c256..8a297bb003f1ec 100644
--- a/libraries/GCS_MAVLink/GCS_config.h
+++ b/libraries/GCS_MAVLink/GCS_config.h
@@ -68,7 +68,7 @@
 // ArduPilot 4.7 stops compiling them in by default
 // ArduPilot 4.8 removes the code entirely
 #ifndef AP_MAVLINK_RALLY_POINT_PROTOCOL_ENABLED
-#define AP_MAVLINK_RALLY_POINT_PROTOCOL_ENABLED HAL_GCS_ENABLED && HAL_RALLY_ENABLED
+#define AP_MAVLINK_RALLY_POINT_PROTOCOL_ENABLED 0
 #endif
 
 // CODE_REMOVAL
diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp
index fed0560e07831f..fdb3d11e6c9791 100644
--- a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp
+++ b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp
@@ -19,7 +19,7 @@ bool MissionItemProtocol_Fence::get_item_as_mission_item(uint16_t seq,
     if (fence == nullptr) {
         return false;
     }
-    const uint8_t num_stored_items = fence->polyfence().num_stored_items();
+    const auto num_stored_items = fence->polyfence().num_stored_items();
     if (seq > num_stored_items) {
         return false;
     }
@@ -66,6 +66,7 @@ bool MissionItemProtocol_Fence::get_item_as_mission_item(uint16_t seq,
     ret_packet.x = fenceitem.loc.x;
     ret_packet.y = fenceitem.loc.y;
     ret_packet.z = 0;
+    ret_packet.mission_type = MAV_MISSION_TYPE_FENCE;
 
     return true;
 }
@@ -75,7 +76,7 @@ MAV_MISSION_RESULT MissionItemProtocol_Fence::get_item(const GCS_MAVLINK &_link,
                                                        const mavlink_mission_request_int_t &packet,
                                                        mavlink_mission_item_int_t &ret_packet)
 {
-    const uint8_t num_stored_items = _fence.polyfence().num_stored_items();
+    const auto num_stored_items = _fence.polyfence().num_stored_items();
     if (packet.seq > num_stored_items) {
         return MAV_MISSION_INVALID_SEQUENCE;
     }
@@ -109,10 +110,16 @@ MAV_MISSION_RESULT MissionItemProtocol_Fence::convert_MISSION_ITEM_INT_to_AC_Pol
     switch (mission_item_int.command) {
     case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
         ret.type = AC_PolyFenceType::POLYGON_INCLUSION;
+        if (mission_item_int.param1 > 255) {
+            return MAV_MISSION_INVALID_PARAM1;
+        }
         ret.vertex_count = mission_item_int.param1;
         break;
     case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION:
         ret.type = AC_PolyFenceType::POLYGON_EXCLUSION;
+        if (mission_item_int.param1 > 255) {
+            return MAV_MISSION_INVALID_PARAM1;
+        }
         ret.vertex_count = mission_item_int.param1;
         break;
     case MAV_CMD_NAV_FENCE_RETURN_POINT:
@@ -218,7 +225,7 @@ MAV_MISSION_RESULT MissionItemProtocol_Fence::allocate_receive_resources(const u
         return MAV_MISSION_ERROR;
     }
 
-    const uint16_t allocation_size = count * sizeof(AC_PolyFenceItem);
+    const uint32_t allocation_size = count * sizeof(AC_PolyFenceItem);
     if (allocation_size != 0) {
         _new_items = (AC_PolyFenceItem*)malloc(allocation_size);
         if (_new_items == nullptr) {
diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp
index ee33e55188e35c..ca33979c3e2cfd 100644
--- a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp
+++ b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp
@@ -143,6 +143,7 @@ bool MissionItemProtocol_Rally::get_item_as_mission_item(uint16_t seq,
     ret_packet.x = rallypoint.lat;
     ret_packet.y = rallypoint.lng;
     ret_packet.z = rallypoint.alt;
+    ret_packet.mission_type = MAV_MISSION_TYPE_RALLY;
 
     return true;
 }
diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h
index b82e64611f9dc4..d55a29a31db2ed 100644
--- a/libraries/GCS_MAVLink/ap_message.h
+++ b/libraries/GCS_MAVLink/ap_message.h
@@ -104,5 +104,6 @@ enum ap_message : uint8_t {
 #endif
     MSG_AIRSPEED,
     MSG_AVAILABLE_MODES,
+    MSG_AVAILABLE_MODES_MONITOR,
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index 2868030a828be7..7194fa092b80e3 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -260,6 +260,7 @@ class RC_Channel {
         FLIGHTMODE_PAUSE =   178,  // e.g. pause movement towards waypoint
         ICE_START_STOP =     179, // AP_ICEngine start stop
         AUTOTUNE_TEST_GAINS = 180, // auto tune tuning switch to test or revert gains
+        QUICKTUNE =          181,  //quicktune 3 position switch
 
 
         // inputs from 200 will eventually used to replace RCMAP
diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp
index d51521cf3bff92..6ca425a5ea6966 100644
--- a/libraries/SITL/SIM_GPS.cpp
+++ b/libraries/SITL/SIM_GPS.cpp
@@ -30,6 +30,121 @@
 
 #include <GCS_MAVLink/GCS.h>
 
+namespace SITL {
+// user settable parameters for GNSS sensors
+const AP_Param::GroupInfo SIM::GPSParms::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: GPS enable
+    // @Description: Enable simulated GPS
+    // @Values: 0:Disable, 1:Enable
+    // @User: Advanced
+    AP_GROUPINFO_FLAGS("ENABLE",    1, GPSParms, enabled, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: LAG_MS
+    // @DisplayName: GPS Lag
+    // @Description: GPS lag
+    // @Units: ms
+    // @User: Advanced
+    AP_GROUPINFO("LAG_MS",     2, GPSParms, delay_ms, 100),
+
+    // @Param: TYPE
+    // @DisplayName: GPS type
+    // @Description: Sets the type of simulation used for GPS
+    // @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP2, 11:Trimble, 19:MSP
+    // @User: Advanced
+    AP_GROUPINFO("TYPE",       3, GPSParms, type,  GPS::Type::UBLOX),
+
+    // @Param: BYTELOS
+    // @DisplayName: GPS Byteloss
+    // @Description: Percent of bytes lost from GPS
+    // @Units: %
+    // @User: Advanced
+    AP_GROUPINFO("BYTELOS",   4, GPSParms, byteloss,  0),
+
+    // @Param: NUMSATS
+    // @DisplayName: GPS Num Satellites
+    // @Description: Number of satellites GPS has in view
+    AP_GROUPINFO("NUMSATS",    5, GPSParms, numsats,   10),
+
+    // @Param: GLTCH
+    // @DisplayName: GPS Glitch
+    // @Description: Glitch offsets of simulated GPS sensor
+    // @Vector3Parameter: 1
+    // @User: Advanced
+    AP_GROUPINFO("GLTCH",     6, GPSParms, glitch,  0),
+
+    // @Param: HZ
+    // @DisplayName: GPS Hz
+    // @Description: GPS Update rate
+    // @Units: Hz
+    AP_GROUPINFO("HZ",         7, GPSParms, hertz,  5),
+
+    // @Param: DRFTALT
+    // @DisplayName: GPS Altitude Drift
+    // @Description: GPS altitude drift error
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("DRFTALT",   8, GPSParms, drift_alt, 0),
+
+    // @Param: POS
+    // @DisplayName: GPS Position
+    // @Description: GPS antenna phase center position relative to the body frame origin
+    // @Units: m
+    // @Vector3Parameter: 1
+    AP_GROUPINFO("POS",        9, GPSParms, pos_offset, 0),
+
+    // @Param: NOISE
+    // @DisplayName: GPS Noise
+    // @Description: Amplitude of the GPS altitude error
+    // @Units: m
+    // @User: Advanced
+    AP_GROUPINFO("NOISE",     10, GPSParms, noise, 0),
+
+    // @Param: LCKTIME
+    // @DisplayName: GPS Lock Time
+    // @Description: Delay in seconds before GPS acquires lock
+    // @Units: s
+    // @User: Advanced
+    AP_GROUPINFO("LCKTIME",  11, GPSParms, lock_time, 0),
+
+    // @Param: ALT_OFS
+    // @DisplayName: GPS Altitude Offset
+    // @Description: GPS Altitude Error
+    // @Units: m
+    AP_GROUPINFO("ALT_OFS",   12, GPSParms, alt_offset, 0),
+
+    // @Param: HDG
+    // @DisplayName: GPS Heading
+    // @Description: Enable GPS output of NMEA heading HDT sentence or UBLOX_RELPOSNED
+    // @Values: 0:Disabled, 1:Emit HDT, 2:Emit THS, 3:KSXT, 4:Be Moving Baseline Base
+    // @User: Advanced
+    AP_GROUPINFO("HDG",       13, GPSParms, hdg_enabled, SIM::GPS_HEADING_NONE),
+
+    // @Param: ACC
+    // @DisplayName: GPS Accuracy
+    // @Description: GPS Accuracy
+    // @User: Advanced
+    AP_GROUPINFO("ACC",       14, GPSParms, accuracy, 0.3),
+
+    // @Param: VERR
+    // @DisplayName: GPS Velocity Error
+    // @Description: GPS Velocity Error Offsets in NED
+    // @Vector3Parameter: 1
+    // @User: Advanced
+    AP_GROUPINFO("VERR",      15, GPSParms, vel_err, 0),
+
+    // @Param: JAM
+    // @DisplayName: GPS jamming enable
+    // @Description: Enable simulated GPS jamming
+    // @User: Advanced
+    // @Values: 0:Disabled, 1:Enabled
+    AP_GROUPINFO("JAM",       16, GPSParms, jam, 0),
+
+    AP_GROUPEND
+};
+}
+
 // the number of GPS leap seconds - copied from AP_GPS.h
 #define GPS_LEAPSECONDS_MILLIS 18000ULL
 
@@ -43,6 +158,13 @@ GPS_Backend::GPS_Backend(GPS &_front, uint8_t _instance) :
     front{_front}
 {
     _sitl = AP::sitl();
+
+#if HAL_SIM_GPS_ENABLED && AP_SIM_MAX_GPS_SENSORS > 0
+    // default the first backend to enabled:
+    if (_instance == 0 && !_sitl->gps[0].enabled.configured()) {
+        _sitl->gps[0].enabled.set(1);
+    }
+#endif
 }
 
 ssize_t GPS_Backend::write_to_autopilot(const char *p, size_t size) const
@@ -78,11 +200,11 @@ ssize_t GPS::write_to_autopilot(const char *p, size_t size) const
     // the first will start sending back 3 satellites, the second just
     // stops responding when disabled.  This is not necessarily a good
     // thing.
-    if (instance == 1 && _sitl->gps_disable[instance]) {
+    if (instance == 1 && !_sitl->gps[instance].enabled) {
         return -1;
     }
 
-    const float byteloss = _sitl->gps_byteloss[instance];
+    const float byteloss = _sitl->gps[instance].byteloss;
 
     // shortcut if we're not doing byteloss:
     if (!is_positive(byteloss)) {
@@ -217,7 +339,7 @@ GPS_Backend::GPS_TOW GPS_Backend::gps_time()
 
 void GPS::check_backend_allocation()
 {
-    const Type configured_type = Type(_sitl->gps_type[instance].get());
+    const Type configured_type = Type(_sitl->gps[instance].type.get());
     if (allocated_type == configured_type) {
         return;
     }
@@ -328,7 +450,7 @@ void GPS::update()
 
     //Capture current position as basestation location for
     if (!_gps_has_basestation_position &&
-        now_ms >= _sitl->gps_lock_time[0]*1000UL) {
+        now_ms >= _sitl->gps[0].lock_time*1000UL) {
         _gps_basestation_data.latitude = latitude;
         _gps_basestation_data.longitude = longitude;
         _gps_basestation_data.altitude = altitude;
@@ -338,15 +460,15 @@ void GPS::update()
         _gps_has_basestation_position = true;
     }
 
-    const uint8_t idx = instance;  // alias to avoid code churn
+    const auto &params = _sitl->gps[instance];
 
     struct GPS_Data d {};
 
     // simulate delayed lock times
-    bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL);
+    bool have_lock = (params.enabled && now_ms >= params.lock_time*1000UL);
 
     // Only let physics run and GPS write at configured GPS rate (default 5Hz).
-    if ((now_ms - last_write_update_ms) < (uint32_t)(1000/_sitl->gps_hertz[instance])) {
+    if ((now_ms - last_write_update_ms) < (uint32_t)(1000/params.hertz)) {
         // Reading runs every iteration.
         // Beware- physics don't update every iteration with this approach.
         // Currently, none of the drivers rely on quickly updated physics.
@@ -356,7 +478,7 @@ void GPS::update()
 
     last_write_update_ms = now_ms;
 
-    d.num_sats = _sitl->gps_numsats[idx];
+    d.num_sats = params.numsats;
     d.latitude = latitude;
     d.longitude = longitude;
     d.yaw_deg = _sitl->state.yawDeg;
@@ -364,10 +486,10 @@ void GPS::update()
     d.pitch_deg = _sitl->state.pitchDeg;
 
     // add an altitude error controlled by a slow sine wave
-    d.altitude = altitude + _sitl->gps_noise[idx] * sinf(now_ms * 0.0005f) + _sitl->gps_alt_offset[idx];
+    d.altitude = altitude + params.noise * sinf(now_ms * 0.0005f) + params.alt_offset;
 
     // Add offset to c.g. velocity to get velocity at antenna and add simulated error
-    Vector3f velErrorNED = _sitl->gps_vel_err[idx];
+    Vector3f velErrorNED = params.vel_err;
     d.speedN = speedN + (velErrorNED.x * rand_float());
     d.speedE = speedE + (velErrorNED.y * rand_float());
     d.speedD = speedD + (velErrorNED.z * rand_float());
@@ -375,18 +497,18 @@ void GPS::update()
     d.have_lock = have_lock;
 
     // fill in accuracies
-    d.horizontal_acc = _sitl->gps_accuracy[idx];
-    d.vertical_acc = _sitl->gps_accuracy[idx];
-    d.speed_acc = _sitl->gps_vel_err[instance].get().xy().length();
+    d.horizontal_acc = params.accuracy;
+    d.vertical_acc = params.accuracy;
+    d.speed_acc = params.vel_err.get().xy().length();
 
-    if (_sitl->gps_drift_alt[idx] > 0) {
+    if (params.drift_alt > 0) {
         // add slow altitude drift controlled by a slow sine wave
-        d.altitude += _sitl->gps_drift_alt[idx]*sinf(now_ms*0.001f*0.02f);
+        d.altitude += params.drift_alt*sinf(now_ms*0.001f*0.02f);
     }
 
     // correct the latitude, longitude, height and NED velocity for the offset between
     // the vehicle c.g. and GPS antenna
-    Vector3f posRelOffsetBF = _sitl->gps_pos_offset[idx];
+    Vector3f posRelOffsetBF = params.pos_offset;
     if (!posRelOffsetBF.is_zero()) {
         // get a rotation matrix following DCM conventions (body to earth)
         Matrix3f rotmat;
@@ -418,18 +540,18 @@ void GPS::update()
 
     // get delayed data
     d.timestamp_ms = now_ms;
-    d = interpolate_data(d, _sitl->gps_delay_ms[instance]);
+    d = interpolate_data(d, params.delay_ms);
 
     // Applying GPS glitch
     // Using first gps glitch
-    Vector3f glitch_offsets = _sitl->gps_glitch[idx];
+    Vector3f glitch_offsets = params.glitch;
     d.latitude += glitch_offsets.x;
     d.longitude += glitch_offsets.y;
     d.altitude += glitch_offsets.z;
 
-        if (_sitl->gps_jam[idx] == 1) {
-            simulate_jamming(d);
-        }
+    if (params.jam == 1) {
+        simulate_jamming(d);
+    }
 
     backend->publish(&d);
 }
diff --git a/libraries/SITL/SIM_GPS_NMEA.cpp b/libraries/SITL/SIM_GPS_NMEA.cpp
index 23d2a91ad45f98..a4e19fb40b4688 100644
--- a/libraries/SITL/SIM_GPS_NMEA.cpp
+++ b/libraries/SITL/SIM_GPS_NMEA.cpp
@@ -99,12 +99,12 @@ void GPS_NMEA::publish(const GPS_Data *d)
                      ground_track_deg,
                      dstring);
 
-    if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_HDT) {
+    if (_sitl->gps[instance].hdg_enabled == SITL::SIM::GPS_HEADING_HDT) {
         nmea_printf("$GPHDT,%.2f,T", d->yaw_deg);
     }
-    else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_THS) {
+    else if (_sitl->gps[instance].hdg_enabled == SITL::SIM::GPS_HEADING_THS) {
         nmea_printf("$GPTHS,%.2f,%c,T", d->yaw_deg, d->have_lock ? 'A' : 'V');
-    } else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_KSXT) {
+    } else if (_sitl->gps[instance].hdg_enabled == SITL::SIM::GPS_HEADING_KSXT) {
         // Unicore support
         // $KSXT,20211016083433.00,116.31296102,39.95817066,49.4911,223.57,-11.32,330.19,0.024,,1,3,28,27,,,,-0.012,0.021,0.020,,*2D
         nmea_printf("$KSXT,%04u%02u%02u%02u%02u%02u.%02u,%.8f,%.8f,%.4f,%.2f,%.2f,%.2f,%.2f,%.3f,%u,%u,%u,%u,,,,%.3f,%.3f,%.3f,,",
diff --git a/libraries/SITL/SIM_GPS_UBLOX.cpp b/libraries/SITL/SIM_GPS_UBLOX.cpp
index ce77a44d284a1a..3f3ef5da683b80 100644
--- a/libraries/SITL/SIM_GPS_UBLOX.cpp
+++ b/libraries/SITL/SIM_GPS_UBLOX.cpp
@@ -35,6 +35,47 @@ void GPS_UBlox::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
     write_to_autopilot((char*)chk, sizeof(chk));
 }
 
+void GPS_UBlox::update_relposned(ubx_nav_relposned &relposned, uint32_t tow_ms, float yaw_deg)
+{
+    Vector3f ant1_pos { NaNf, NaNf, NaNf };
+
+    // find our partner:
+    for (uint8_t i=0; i<ARRAY_SIZE(_sitl->gps); i++) {
+        if (i == instance) {
+            // this shouldn't matter as our heading type should never be base
+            continue;
+        }
+        if (_sitl->gps[i].hdg_enabled != SITL::SIM::GPS_HEADING_BASE) {
+            continue;
+        }
+        ant1_pos = _sitl->gps[i].pos_offset.get();
+        break;
+    }
+    if (ant1_pos.is_nan()) {
+        return;
+    }
+
+    const Vector3f ant2_pos = _sitl->gps[instance].pos_offset.get();
+    Vector3f rel_antenna_pos = ant2_pos - ant1_pos;
+    Matrix3f rot;
+    // project attitude back using gyros to get antenna orientation at time of GPS sample
+    Vector3f gyro(radians(_sitl->state.rollRate),
+                  radians(_sitl->state.pitchRate),
+                  radians(_sitl->state.yawRate));
+    rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(yaw_deg));
+    const float lag = _sitl->gps[instance].delay_ms * 0.001;
+    rot.rotate(gyro * (-lag));
+    rel_antenna_pos = rot * rel_antenna_pos;
+    relposned.version = 1;
+    relposned.iTOW = tow_ms;
+    relposned.relPosN = rel_antenna_pos.x * 100;
+    relposned.relPosE = rel_antenna_pos.y * 100;
+    relposned.relPosD = rel_antenna_pos.z * 100;
+    relposned.relPosLength = rel_antenna_pos.length() * 100;
+    relposned.relPosHeading = degrees(Vector2f(rel_antenna_pos.x, rel_antenna_pos.y).angle()) * 1.0e5;
+    relposned.flags = gnssFixOK | diffSoln | carrSolnFixed | isMoving | relPosValid | relPosHeadingValid;
+}
+
 /*
   send a new set of GPS UBLOX packets
  */
@@ -139,44 +180,7 @@ void GPS_UBlox::publish(const GPS_Data *d)
             int32_t prRes;
         } sv[SV_COUNT];
     } svinfo {};
-    enum RELPOSNED {
-        gnssFixOK          = 1U << 0,
-        diffSoln           = 1U << 1,
-        relPosValid        = 1U << 2,
-        carrSolnFloat      = 1U << 3,
-
-        carrSolnFixed      = 1U << 4,
-        isMoving           = 1U << 5,
-        refPosMiss         = 1U << 6,
-        refObsMiss         = 1U << 7,
-
-        relPosHeadingValid = 1U << 8,
-        relPosNormalized   = 1U << 9
-    };
-    struct PACKED ubx_nav_relposned {
-        uint8_t version;
-        uint8_t reserved1;
-        uint16_t refStationId;
-        uint32_t iTOW;
-        int32_t relPosN;
-        int32_t relPosE;
-        int32_t relPosD;
-        int32_t relPosLength;
-        int32_t relPosHeading;
-        uint8_t reserved2[4];
-        int8_t relPosHPN;
-        int8_t relPosHPE;
-        int8_t relPosHPD;
-        int8_t relPosHPLength;
-        uint32_t accN;
-        uint32_t accE;
-        uint32_t accD;
-        uint32_t accLength;
-        uint32_t accHeading;
-        uint8_t reserved3[4];
-        uint32_t flags;
-    } relposned {};
-
+    ubx_nav_relposned relposned {};
     const uint8_t MSG_POSLLH = 0x2;
     const uint8_t MSG_STATUS = 0x3;
     const uint8_t MSG_DOP = 0x4;
@@ -267,27 +271,15 @@ void GPS_UBlox::publish(const GPS_Data *d)
     pvt.headVeh = 0;
     memset(pvt.reserved2, '\0', ARRAY_SIZE(pvt.reserved2));
 
-    if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) {
-        const Vector3f ant1_pos = _sitl->gps_pos_offset[instance^1].get();
-        const Vector3f ant2_pos = _sitl->gps_pos_offset[instance].get();
-        Vector3f rel_antenna_pos = ant2_pos - ant1_pos;
-        Matrix3f rot;
-        // project attitude back using gyros to get antenna orientation at time of GPS sample
-        Vector3f gyro(radians(_sitl->state.rollRate),
-                      radians(_sitl->state.pitchRate),
-                      radians(_sitl->state.yawRate));
-        rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(d->yaw_deg));
-        const float lag = _sitl->gps_delay_ms[instance] * 0.001;
-        rot.rotate(gyro * (-lag));
-        rel_antenna_pos = rot * rel_antenna_pos;
-        relposned.version = 1;
-        relposned.iTOW = gps_tow.ms;
-        relposned.relPosN = rel_antenna_pos.x * 100;
-        relposned.relPosE = rel_antenna_pos.y * 100;
-        relposned.relPosD = rel_antenna_pos.z * 100;
-        relposned.relPosLength = rel_antenna_pos.length() * 100;
-        relposned.relPosHeading = degrees(Vector2f(rel_antenna_pos.x, rel_antenna_pos.y).angle()) * 1.0e5;
-        relposned.flags = gnssFixOK | diffSoln | carrSolnFixed | isMoving | relPosValid | relPosHeadingValid;
+    switch (_sitl->gps[instance].hdg_enabled) {
+    case SITL::SIM::GPS_HEADING_NONE:
+    case SITL::SIM::GPS_HEADING_BASE:
+        break;
+    case SITL::SIM::GPS_HEADING_THS:
+    case SITL::SIM::GPS_HEADING_KSXT:
+    case SITL::SIM::GPS_HEADING_HDT:
+        update_relposned(relposned, gps_tow.ms, d->yaw_deg);
+        break;
     }
 
     send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
@@ -296,7 +288,7 @@ void GPS_UBlox::publish(const GPS_Data *d)
     send_ubx(MSG_SOL,    (uint8_t*)&sol, sizeof(sol));
     send_ubx(MSG_DOP,    (uint8_t*)&dop, sizeof(dop));
     send_ubx(MSG_PVT,    (uint8_t*)&pvt, sizeof(pvt));
-    if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) {
+    if (_sitl->gps[instance].hdg_enabled > SITL::SIM::GPS_HEADING_NONE) {
         send_ubx(MSG_RELPOSNED,    (uint8_t*)&relposned, sizeof(relposned));
     }
 
diff --git a/libraries/SITL/SIM_GPS_UBLOX.h b/libraries/SITL/SIM_GPS_UBLOX.h
index 6ab5d01eea76fc..06f03cf5d8e60b 100644
--- a/libraries/SITL/SIM_GPS_UBLOX.h
+++ b/libraries/SITL/SIM_GPS_UBLOX.h
@@ -15,6 +15,45 @@ class GPS_UBlox : public GPS_Backend {
     void publish(const GPS_Data *d) override;
 
 private:
+    enum RELPOSNED {
+        gnssFixOK          = 1U << 0,
+        diffSoln           = 1U << 1,
+        relPosValid        = 1U << 2,
+        carrSolnFloat      = 1U << 3,
+
+        carrSolnFixed      = 1U << 4,
+        isMoving           = 1U << 5,
+        refPosMiss         = 1U << 6,
+        refObsMiss         = 1U << 7,
+
+        relPosHeadingValid = 1U << 8,
+        relPosNormalized   = 1U << 9
+    };
+    struct PACKED ubx_nav_relposned {
+        uint8_t version;
+        uint8_t reserved1;
+        uint16_t refStationId;
+        uint32_t iTOW;
+        int32_t relPosN;
+        int32_t relPosE;
+        int32_t relPosD;
+        int32_t relPosLength;
+        int32_t relPosHeading;
+        uint8_t reserved2[4];
+        int8_t relPosHPN;
+        int8_t relPosHPE;
+        int8_t relPosHPD;
+        int8_t relPosHPLength;
+        uint32_t accN;
+        uint32_t accE;
+        uint32_t accD;
+        uint32_t accLength;
+        uint32_t accHeading;
+        uint8_t reserved3[4];
+        uint32_t flags;
+    };
+
+    void update_relposned(ubx_nav_relposned &relposned, uint32_t tow_ms, float yaw_deg);
     void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size);
 };
 
diff --git a/libraries/SITL/SIM_Precland.cpp b/libraries/SITL/SIM_Precland.cpp
index 818d652bad8db0..a4d97022466d6c 100644
--- a/libraries/SITL/SIM_Precland.cpp
+++ b/libraries/SITL/SIM_Precland.cpp
@@ -85,7 +85,7 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("TYPE", 6, SIM_Precland, _type, SIM_Precland::PRECLAND_TYPE_CYLINDER),
 
-    // @Param: ALT_LIMIT
+    // @Param: ALT_LMT
     // @DisplayName: Precland device alt range
     // @Description: Precland device maximum range altitude
     // @Units: m
@@ -93,7 +93,7 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ALT_LMT", 7, SIM_Precland, _alt_limit, 15),
 
-    // @Param: DIST_LIMIT
+    // @Param: DIST_LMT
     // @DisplayName: Precland device lateral range
     // @Description: Precland device maximum lateral range
     // @Units: m
diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h
index 91956ad90c2aef..557c3077ed9dea 100644
--- a/libraries/SITL/SIM_config.h
+++ b/libraries/SITL/SIM_config.h
@@ -3,6 +3,8 @@
 #include <AP_HAL/AP_HAL_Boards.h>
 #include <GCS_MAVLink/GCS_config.h>
 
+#define AP_SIM_MAX_GPS_SENSORS 4
+
 #ifndef HAL_SIM_ADSB_ENABLED
 #define HAL_SIM_ADSB_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 1ff14520b72df9..e329ed232a9609 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -721,181 +721,38 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
 #if HAL_SIM_GPS_ENABLED
 // GPS SITL parameters
 const AP_Param::GroupInfo SIM::var_gps[] = {
-    // @Param: GPS_DISABLE
-    // @DisplayName: GPS 1 disable
-    // @Description: Disables GPS 1
-    // @Values: 0:Enable, 1:GPS Disabled
-    // @User: Advanced
-    AP_GROUPINFO("GPS_DISABLE",    1, SIM,  gps_disable[0], 0),
-    // @Param: GPS_LAG_MS
-    // @DisplayName: GPS 1 Lag
-    // @Description: GPS 1 lag
-    // @Units: ms
-    // @User: Advanced
-    AP_GROUPINFO("GPS_LAG_MS",     2, SIM,  gps_delay_ms[0], 100),
-    // @Param: GPS_TYPE
-    // @DisplayName: GPS 1 type
-    // @Description: Sets the type of simulation used for GPS 1
-    // @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP2, 11:Trimble, 19:MSP
-    // @User: Advanced
-    AP_GROUPINFO("GPS_TYPE",       3, SIM,  gps_type[0],  GPS::Type::UBLOX),
-    // @Param: GPS_BYTELOSS
-    // @DisplayName: GPS Byteloss
-    // @Description: Percent of bytes lost from GPS 1
-    // @Units: %
-    // @User: Advanced
-    AP_GROUPINFO("GPS_BYTELOSS",   4, SIM,  gps_byteloss[0],  0),
-    // @Param: GPS_NUMSATS
-    // @DisplayName: GPS 1 Num Satellites
-    // @Description: Number of satellites GPS 1 has in view
-    AP_GROUPINFO("GPS_NUMSATS",    5, SIM,  gps_numsats[0],   10),
-    // @Param: GPS_GLITCH
-    // @DisplayName: GPS 1 Glitch
-    // @Description: Glitch offsets of simulated GPS 1 sensor
-    // @Vector3Parameter: 1
-    // @User: Advanced
-    AP_GROUPINFO("GPS_GLITCH",     6, SIM,  gps_glitch[0],  0),
-    // @Param: GPS_HZ
-    // @DisplayName: GPS 1 Hz
-    // @Description: GPS 1 Update rate
-    // @Units: Hz
-    AP_GROUPINFO("GPS_HZ",         7, SIM,  gps_hertz[0],  5),
-    // @Param: GPS_DRIFTALT
-    // @DisplayName: GPS 1 Altitude Drift
-    // @Description: GPS 1 altitude drift error
-    // @Units: m
-    // @User: Advanced
-    AP_GROUPINFO("GPS_DRIFTALT",   8, SIM,  gps_drift_alt[0], 0),
-    // @Param: GPS_POS
-    // @DisplayName: GPS 1 Position
-    // @Description: GPS 1 antenna phase center position relative to the body frame origin
-    // @Units: m
-    // @Vector3Parameter: 1
-    AP_GROUPINFO("GPS_POS",        9, SIM,  gps_pos_offset[0], 0),
-    // @Param: GPS_NOISE
-    // @DisplayName: GPS 1 Noise
-    // @Description: Amplitude of the GPS1 altitude error
-    // @Units: m
-    // @User: Advanced
-    AP_GROUPINFO("GPS_NOISE",     10, SIM,  gps_noise[0], 0),
-    // @Param: GPS_LOCKTIME
-    // @DisplayName: GPS 1 Lock Time
-    // @Description: Delay in seconds before GPS1 acquires lock
-    // @Units: s
-    // @User: Advanced
-    AP_GROUPINFO("GPS_LOCKTIME",  11, SIM,  gps_lock_time[0], 0),
-    // @Param: GPS_ALT_OFS
-    // @DisplayName: GPS 1 Altitude Offset
-    // @Description: GPS 1 Altitude Error
-    // @Units: m
-    AP_GROUPINFO("GPS_ALT_OFS",   12, SIM,  gps_alt_offset[0], 0),
-    // @Param: GPS_HDG
-    // @DisplayName: GPS 1 Heading
-    // @Description: Enable GPS1 output of NMEA heading HDT sentence or UBLOX_RELPOSNED
-    // @Values: 0:Disabled, 1:Enabled
-    // @User: Advanced
-    AP_GROUPINFO("GPS_HDG",       13, SIM,  gps_hdg_enabled[0], SIM::GPS_HEADING_NONE),
-    // @Param: GPS_ACC
-    // @DisplayName: GPS 1 Accuracy
-    // @Description: GPS 1 Accuracy
-    // @User: Advanced
-    AP_GROUPINFO("GPS_ACC",       14, SIM,  gps_accuracy[0], 0.3),
-    // @Param: GPS_VERR
-    // @DisplayName: GPS 1 Velocity Error
-    // @Description: GPS 1 Velocity Error Offsets in NED
-    // @Vector3Parameter: 1
-    // @User: Advanced
-    AP_GROUPINFO("GPS_VERR",      15, SIM,  gps_vel_err[0], 0),
-    // @Param: GPS_JAM
-    // @DisplayName: GPS jamming enable
-    // @Description: Enable simulated GPS jamming
-    // @User: Advanced
-    // @Values: 0:Disabled, 1:Enabled
-    AP_GROUPINFO("GPS_JAM",       16, SIM,  gps_jam[0], 0),
-    // @Param: GPS2_DISABLE
-    // @DisplayName: GPS 2 disable
-    // @Description: Disables GPS 2
-    // @Values: 0:Enable, 1:GPS Disabled
-    // @User: Advanced
-    AP_GROUPINFO("GPS2_DISABLE",  30, SIM,  gps_disable[1], 1),
-    // @Param: GPS2_LAG_MS
-    // @DisplayName: GPS 2 Lag
-    // @Description: GPS 2 lag in ms
-    // @Units: ms
-    // @User: Advanced
-    AP_GROUPINFO("GPS2_LAG_MS",   31, SIM,  gps_delay_ms[1], 100),
-    // @Param: GPS2_TYPE
-    // @CopyFieldsFrom: SIM_GPS_TYPE
-    // @DisplayName: GPS 2 type
-    // @Description: Sets the type of simulation used for GPS 2
-    AP_GROUPINFO("GPS2_TYPE",     32, SIM,  gps_type[1],  GPS::Type::UBLOX),
-    // @Param: GPS2_BYTELOS
-    // @DisplayName: GPS 2 Byteloss
-    // @Description: Percent of bytes lost from GPS 2
-    // @Units: %
-    // @User: Advanced
-    AP_GROUPINFO("GPS2_BYTELOS",  33, SIM,  gps_byteloss[1],  0),
-    // @Param: GPS2_NUMSATS
-    // @DisplayName: GPS 2 Num Satellites
-    // @Description: Number of satellites GPS 2 has in view
-    AP_GROUPINFO("GPS2_NUMSATS",  34, SIM,  gps_numsats[1],   10),
-    // @Param: GPS2_GLTCH
-    // @DisplayName: GPS 2 Glitch
-    // @Description: Glitch offsets of simulated GPS 2 sensor
-    // @Vector3Parameter: 1
-    // @User: Advanced
-    AP_GROUPINFO("GPS2_GLTCH",    35, SIM,  gps_glitch[1],  0),
-    // @Param: GPS2_HZ
-    // @DisplayName: GPS 2 Hz
-    // @Description: GPS 2 Update rate
-    // @Units: Hz
-    AP_GROUPINFO("GPS2_HZ",       36, SIM,  gps_hertz[1],  5),
-    // @Param: GPS2_DRFTALT
-    // @DisplayName: GPS 2 Altitude Drift
-    // @Description: GPS 2 altitude drift error
-    // @Units: m
-    // @User: Advanced
-    AP_GROUPINFO("GPS2_DRFTALT",  37, SIM,  gps_drift_alt[1], 0),
-    // @Param: GPS2_POS
-    // @DisplayName: GPS 2 Position
-    // @Description: GPS 2 antenna phase center position relative to the body frame origin
-    // @Units: m
-    // @Vector3Parameter: 1
-    AP_GROUPINFO("GPS2_POS",      38, SIM,  gps_pos_offset[1], 0),
-    // @Param: GPS2_NOISE
-    // @DisplayName: GPS 2 Noise
-    // @Description: Amplitude of the GPS2 altitude error
-    // @Units: m
-    // @User: Advanced
-    AP_GROUPINFO("GPS2_NOISE",    39, SIM,  gps_noise[1], 0),
-    // @Param: GPS2_LCKTIME
-    // @DisplayName: GPS 2 Lock Time
-    // @Description: Delay in seconds before GPS2 acquires lock
-    // @Units: s
-    // @User: Advanced
-    AP_GROUPINFO("GPS2_LCKTIME",  40, SIM,  gps_lock_time[1], 0),
-    // @Param: GPS2_ALT_OFS
-    // @DisplayName: GPS 2 Altitude Offset
-    // @Description: GPS 2 Altitude Error
-    // @Units: m
-    AP_GROUPINFO("GPS2_ALT_OFS",  41, SIM,  gps_alt_offset[1], 0),
-    // @Param: GPS2_HDG
-    // @DisplayName: GPS 2 Heading
-    // @Description: Enable GPS2 output of NMEA heading HDT sentence or UBLOX_RELPOSNED
-    // @Values: 0:Disabled, 1:Enabled
-    // @User: Advanced
-    AP_GROUPINFO("GPS2_HDG",      42, SIM,  gps_hdg_enabled[1], SIM::GPS_HEADING_NONE),
-    // @Param: GPS2_ACC
-    // @DisplayName: GPS 2 Accuracy
-    // @Description: GPS 2 Accuracy
-    // @User: Advanced
-    AP_GROUPINFO("GPS2_ACC",      43, SIM,  gps_accuracy[1], 0.3),
-    // @Param: GPS2_VERR
-    // @DisplayName: GPS 2 Velocity Error
-    // @Description: GPS 2 Velocity Error Offsets in NED
-    // @Vector3Parameter: 1
-    // @User: Advanced
-    AP_GROUPINFO("GPS2_VERR",     44, SIM,  gps_vel_err[1], 0),
+    //  1 was GPS_DISABLE
+    //  2 was GPS_LAG_MS
+    //  3 was GPS_TYPE
+    //  4 was GPS_BYTELOSS
+    //  5 was GPS_NUMSATS
+    //  6 was GPS_GLITCH
+    //  7 was GPS_HZ
+    //  8 was GPS_DRIFTALT
+    //  9 was GPS_POS
+    // 10 was GPS_NOISE
+    // 11 was GPS_LOCKTIME
+    // 12 was GPS_ALT_OFS
+    // 13 was GPS_HDG
+    // 14 was GPS_ACC
+    // 15 was GPS_VERR
+    // 16 was GPS_JAM
+
+    // 30 was GPS2_DISABLE
+    // 31 was GPS2_LAG_MS
+    // 32 was GPS2_TYPE
+    // 33 was GPS2_BYTELOSS
+    // 34 was GPS2_NUMSATS
+    // 35 was GPS2_GLITCH
+    // 36 was GPS2_HZ
+    // 37 was GPS2_DRIFTALT
+    // 38 was GPS2_POS
+    // 39 was GPS2_NOISE
+    // 40 was GPS2_LOCKTIME
+    // 41 was GPS2_ALT_OFS
+    // 42 was GPS2_HDG
+    // 43 was GPS2_ACC
+    // 44 was GPS2_VERR
 
     // @Param: INIT_LAT_OFS
     // @DisplayName: Initial Latitude Offset
@@ -915,14 +772,30 @@ const AP_Param::GroupInfo SIM::var_gps[] = {
     // @Description: Log number for GPS:update_file()
     AP_GROUPINFO("GPS_LOG_NUM",   48, SIM,  gps_log_num, 0),
 
-    // @Param: GPS2_JAM
-    // @DisplayName: GPS jamming enable
-    // @Description: Enable simulated GPS jamming
-    // @User: Advanced
-    // @Values: 0:Disabled, 1:Enabled
-    AP_GROUPINFO("GPS2_JAM",      49, SIM,  gps_jam[1], 0),
+    // 49 was GPS2_JAM
 
-    AP_GROUPEND
+#if AP_SIM_MAX_GPS_SENSORS > 0
+     // @Group: GPS1_
+    // @Path: ./SIM_GPS.cpp
+    AP_SUBGROUPINFO(gps[0], "GPS1_", 50, SIM, GPSParms),
+#endif
+#if AP_SIM_MAX_GPS_SENSORS > 1
+    // @Group: GPS2_
+    // @Path: ./SIM_GPS.cpp
+    AP_SUBGROUPINFO(gps[1], "GPS2_", 51, SIM, GPSParms),
+#endif
+#if AP_SIM_MAX_GPS_SENSORS > 2
+    // @Group: GPS3_
+    // @Path: ./SIM_GPS.cpp
+    AP_SUBGROUPINFO(gps[2], "GPS3_", 52, SIM, GPSParms),
+#endif
+#if AP_SIM_MAX_GPS_SENSORS > 3
+    // @Group: GPS4_
+    // @Path: ./SIM_GPS.cpp
+    AP_SUBGROUPINFO(gps[3], "GPS4_", 53, SIM, GPSParms),
+#endif
+
+   AP_GROUPEND
 };
 #endif  // HAL_SIM_GPS_ENABLED
 
diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
index e23b2f6144791b..4edda5efd21e48 100644
--- a/libraries/SITL/SITL.h
+++ b/libraries/SITL/SITL.h
@@ -155,6 +155,7 @@ class SIM {
         GPS_HEADING_HDT  = 1,
         GPS_HEADING_THS  = 2,
         GPS_HEADING_KSXT = 3,
+        GPS_HEADING_BASE = 4,  // act as an RTK base
     };
 
     struct sitl_fdm state;
@@ -197,23 +198,6 @@ class SIM {
     AP_Float engine_mul;  // engine multiplier
     AP_Int8  engine_fail; // engine servo to fail (0-7)
 
-    AP_Float gps_noise[2]; // amplitude of the gps altitude error
-    AP_Int16 gps_lock_time[2]; // delay in seconds before GPS gets lock
-    AP_Int16 gps_alt_offset[2]; // gps alt error
-    AP_Int8  gps_disable[2]; // disable simulated GPS
-    AP_Int16 gps_delay_ms[2];   // delay in milliseconds
-    AP_Int8  gps_type[2]; // see enum SITL::GPS::Type
-    AP_Float gps_byteloss[2];// byte loss as a percent
-    AP_Int8  gps_numsats[2]; // number of visible satellites
-    AP_Vector3f gps_glitch[2];  // glitch offsets in lat, lon and altitude
-    AP_Int8  gps_hertz[2];   // GPS update rate in Hz
-    AP_Int8 gps_hdg_enabled[2]; // enable the output of a NMEA heading HDT sentence or UBLOX RELPOSNED
-    AP_Float gps_drift_alt[2]; // altitude drift error
-    AP_Vector3f gps_pos_offset[2];  // XYZ position of the GPS antenna phase centre relative to the body frame origin (m)
-    AP_Float gps_accuracy[2];
-    AP_Vector3f gps_vel_err[2]; // Velocity error offsets in NED (x = N, y = E, z = D)
-    AP_Int8 gps_jam[2]; // jamming simulation enable
-
     // initial offset on GPS lat/lon, used to shift origin
     AP_Float gps_init_lat_ofs;
     AP_Float gps_init_lon_ofs;
@@ -311,7 +295,33 @@ class SIM {
         AP_Float servo_filter; // servo 2p filter in Hz
     };
     ServoParams servo;
-    
+
+    class GPSParms {
+    public:
+        GPSParms(void) {
+            AP_Param::setup_object_defaults(this, var_info);
+        }
+        static const struct AP_Param::GroupInfo var_info[];
+
+        AP_Float noise; // amplitude of the gps altitude error
+        AP_Int16 lock_time; // delay in seconds before GPS gets lock
+        AP_Int16 alt_offset; // gps alt error
+        AP_Int8  enabled; // enable simulated GPS
+        AP_Int16 delay_ms;   // delay in milliseconds
+        AP_Int8  type; // see enum SITL::GPS::Type
+        AP_Float byteloss;// byte loss as a percent
+        AP_Int8  numsats; // number of visible satellites
+        AP_Vector3f glitch;  // glitch offsets in lat, lon and altitude
+        AP_Int8  hertz;   // GPS update rate in Hz
+        AP_Int8 hdg_enabled; // enable the output of a NMEA heading HDT sentence or UBLOX RELPOSNED
+        AP_Float drift_alt; // altitude drift error
+        AP_Vector3f pos_offset;  // XYZ position of the GPS antenna phase centre relative to the body frame origin (m)
+        AP_Float accuracy;
+        AP_Vector3f vel_err; // Velocity error offsets in NED (x = N, y = E, z = D)
+        AP_Int8 jam; // jamming simulation enable
+    };
+    GPSParms gps[AP_SIM_MAX_GPS_SENSORS];
+
     // physics model parameters
     class ModelParm {
     public:
diff --git a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param
index 4a349fd3e72c42..98d5e0b7a06e42 100644
--- a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param
+++ b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param
@@ -620,16 +620,9 @@ SIM_FLOW_POS_X,0
 SIM_FLOW_POS_Y,0
 SIM_FLOW_POS_Z,0
 SIM_FLOW_RATE,10
-SIM_GP2_GLITCH_X,0
-SIM_GP2_GLITCH_Y,0
-SIM_GP2_GLITCH_Z,0
 SIM_GPS_ALT_OFS,0
 SIM_GPS_BYTELOSS,0
-SIM_GPS_DISABLE,0
 SIM_GPS_DRIFTALT,0
-SIM_GPS_GLITCH_X,0
-SIM_GPS_GLITCH_Y,0
-SIM_GPS_GLITCH_Z,0
 SIM_GPS_HZ,20
 SIM_GPS_LOCKTIME,0
 SIM_GPS_NOISE,0
diff --git a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param
index bfaf562bcfabe3..960c2c713c2c90 100644
--- a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param
+++ b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param
@@ -640,21 +640,14 @@ SIM_FLOW_POS_X,0
 SIM_FLOW_POS_Y,0
 SIM_FLOW_POS_Z,0
 SIM_FLOW_RATE,10
-SIM_GP2_GLITCH_X,0
-SIM_GP2_GLITCH_Y,0
-SIM_GP2_GLITCH_Z,0
-SIM_GPS_ALT_OFS,0
-SIM_GPS_BYTELOSS,0
-SIM_GPS_DISABLE,0
-SIM_GPS_DRIFTALT,0
-SIM_GPS_GLITCH_X,0
-SIM_GPS_GLITCH_Y,0
-SIM_GPS_GLITCH_Z,0
-SIM_GPS_HZ,20
-SIM_GPS_LOCKTIME,0
-SIM_GPS_NOISE,0
-SIM_GPS_NUMSATS,10
-SIM_GPS_TYPE,1
+SIM_GPS1_ALT_OFS,0
+SIM_GPS1_BYTELOSS,0
+SIM_GPS1_DRIFTALT,0
+SIM_GPS1_HZ,20
+SIM_GPS1_LOCKTIME,0
+SIM_GPS1_NOISE,0
+SIM_GPS1_NUMSATS,10
+SIM_GPS1_TYPE,1
 SIM_GPS2_ENABLE,0
 SIM_GPS2_TYPE,1
 SIM_GRPE_ENABLE,0
diff --git a/wscript b/wscript
index f400c74cdb7ee3..fff0a563f21111 100644
--- a/wscript
+++ b/wscript
@@ -429,6 +429,11 @@ configuration in order to save typing.
                  default=0,
                  help='zero time on boot in microseconds')
 
+    g.add_option('--enable-iomcu-profiled-support',
+                    action='store_true',
+                    default=False,
+                    help='enable iomcu profiled support')
+
     g.add_option('--enable-new-checking',
         action='store_true',
         default=False,