Merge branch 'master' into ardupilot-guru

.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

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

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

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

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

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

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

.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

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

ArduCopter/Copter.h

@@ -668,12 +668,12 @@ private:
 #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 @@ private:
     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 @@ private:
 #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

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();
+    const char* name;
-    uint8_t mode_number = (uint8_t)modes[index_zero]->mode_number();
+    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

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;
+            return &mode_acro;
-            break;
 #endif
 
         case Mode::Number::STABILIZE:
-            ret = &mode_stabilize;
+            return &mode_stabilize;
-            break;
 
         case Mode::Number::ALT_HOLD:
-            ret = &mode_althold;
+            return &mode_althold;
-            break;
 
 #if MODE_AUTO_ENABLED
         case Mode::Number::AUTO:
-            ret = &mode_auto;
+            return &mode_auto;
-            break;
 #endif
 
 #if MODE_CIRCLE_ENABLED
         case Mode::Number::CIRCLE:
-            ret = &mode_circle;
+            return &mode_circle;
-            break;
 #endif
 
 #if MODE_LOITER_ENABLED
         case Mode::Number::LOITER:
-            ret = &mode_loiter;
+            return &mode_loiter;
-            break;
 #endif
 
 #if MODE_GUIDED_ENABLED
         case Mode::Number::GUIDED:
-            ret = &mode_guided;
+            return &mode_guided;
-            break;
 #endif
 
         case Mode::Number::LAND:
-            ret = &mode_land;
+            return &mode_land;
-            break;
 
 #if MODE_RTL_ENABLED
         case Mode::Number::RTL:
-            ret = &mode_rtl;
+            return &mode_rtl;
-            break;
 #endif
 
 #if MODE_DRIFT_ENABLED
         case Mode::Number::DRIFT:
-            ret = &mode_drift;
+            return &mode_drift;
-            break;
 #endif
 
 #if MODE_SPORT_ENABLED
         case Mode::Number::SPORT:
-            ret = &mode_sport;
+            return &mode_sport;
-            break;
 #endif
 
 #if MODE_FLIP_ENABLED
         case Mode::Number::FLIP:
-            ret = &mode_flip;
+            return &mode_flip;
-            break;
 #endif
 
 #if AUTOTUNE_ENABLED
         case Mode::Number::AUTOTUNE:
-            ret = &mode_autotune;
+            return &mode_autotune;
-            break;
 #endif
 
 #if MODE_POSHOLD_ENABLED
         case Mode::Number::POSHOLD:
-            ret = &mode_poshold;
+            return &mode_poshold;
-            break;
 #endif
 
 #if MODE_BRAKE_ENABLED
         case Mode::Number::BRAKE:
-            ret = &mode_brake;
+            return &mode_brake;
-            break;
 #endif
 
 #if MODE_THROW_ENABLED
         case Mode::Number::THROW:
-            ret = &mode_throw;
+            return &mode_throw;
-            break;
 #endif
 
 #if HAL_ADSB_ENABLED
         case Mode::Number::AVOID_ADSB:
-            ret = &mode_avoid_adsb;
+            return &mode_avoid_adsb;
-            break;
 #endif
 
 #if MODE_GUIDED_NOGPS_ENABLED
         case Mode::Number::GUIDED_NOGPS:
-            ret = &mode_guided_nogps;
+            return &mode_guided_nogps;
-            break;
 #endif
 
 #if MODE_SMARTRTL_ENABLED
         case Mode::Number::SMART_RTL:
-            ret = &mode_smartrtl;
+            return &mode_smartrtl;
-            break;
 #endif
 
 #if MODE_FLOWHOLD_ENABLED
         case Mode::Number::FLOWHOLD:
-            ret = (Mode *)g2.mode_flowhold_ptr;
+            return (Mode *)g2.mode_flowhold_ptr;
-            break;
 #endif
 
 #if MODE_FOLLOW_ENABLED
         case Mode::Number::FOLLOW:
-            ret = &mode_follow;
+            return &mode_follow;
-            break;
 #endif
 
 #if MODE_ZIGZAG_ENABLED
         case Mode::Number::ZIGZAG:
-            ret = &mode_zigzag;
+            return &mode_zigzag;
-            break;
 #endif
 
 #if MODE_SYSTEMID_ENABLED
         case Mode::Number::SYSTEMID:
-            ret = (Mode *)g2.mode_systemid_ptr;
+            return (Mode *)g2.mode_systemid_ptr;
-            break;
 #endif
 
 #if MODE_AUTOROTATE_ENABLED
         case Mode::Number::AUTOROTATE:
-            ret = &mode_autorotate;
+            return &mode_autorotate;
-            break;
 #endif
 
 #if MODE_TURTLE_ENABLED
         case Mode::Number::TURTLE:
-            ret = &mode_turtle;
+            return &mode_turtle;
-            break;
 #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;
 }
 
 

ArduCopter/mode.h

@@ -1193,14 +1193,37 @@ private:
     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;
+    static SubMode guided_mode;
-    bool send_notification;     // used to send one time notification to ground station
+    static 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 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 {
 

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

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)

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

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

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

ArduPlane/Parameters.h

@@ -362,6 +362,7 @@ public:
         k_param_takeoff_throttle_idle,
 
         k_param_pullup = 270,
+        k_param_quicktune,
     };
 
     AP_Int16 format_version;

ArduPlane/Plane.h

@@ -330,6 +330,10 @@ private:
     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 @@ private:
     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);

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

ArduPlane/RC_Channel.h

@@ -30,7 +30,6 @@ private:
     void do_aux_function_soaring_3pos(AuxSwitchPos ch_flag);
 
     void do_aux_function_flare(AuxSwitchPos ch_flag);
-
 };
 
 class RC_Channels_Plane : public RC_Channels

ArduPlane/altitude.cpp

@@ -44,7 +44,7 @@ void Plane::check_home_alt_change(void)
             next_WP_loc.alt += alt_change_cm;
         }
         // reset TECS to force the field elevation estimate to reset
-        TECS_controller.reset();
+        TECS_controller.offset_altitude(alt_change_cm * 0.01f);
     }
     auto_state.last_home_alt_cm = home_alt_cm;
 }

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 @@ public:
     // 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 @@ protected:
 
     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 @@ public:
 protected:
 
     bool _enter() override;
+#if AP_QUICKTUNE_ENABLED
+    bool supports_quicktune() const override { return true; }
+#endif
 };
 
 class ModeQLoiter : public Mode
@@ -688,6 +705,10 @@ protected:
 
     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

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

ArduPlane/wscript

@@ -27,6 +27,7 @@ def build(bld):
             'AP_Follow',
             'AC_PrecLand',
             'AP_IRLock',
+            'AP_Quicktune',
         ],
     )
 

Tools/AP_Bootloader/board_types.txt

@@ -334,6 +334,8 @@ AP_HW_GEPRCF745BTHD                  1501
 
 AP_HW_HGLRCF405V4                    1524
 
+AP_HW_F4BY_F427                      1530
+
 AP_HW_MFT-SEMA100                    2000
 
 AP_HW_AET-H743-Basic                 2024
@@ -364,6 +366,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

Tools/autotest/arduplane.py

@@ -5,6 +5,7 @@ AP_FLAKE8_CLEAN
 '''
 
 from __future__ import print_function
+import copy
 import math
 import os
 import signal
@@ -6190,10 +6191,43 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
             higher_home = home
             higher_home.alt += 40
             self.set_home(higher_home)
+            self.change_mode(mode)
             self.wait_altitude(15, 25, relative=True, minimum_duration=10)
             self.disarm_vehicle(force=True)
             self.reboot_sitl()
 
+    def SetHomeAltChange2(self):
+        '''ensure TECS operates predictably as home altitude changes continuously'''
+        '''
+        This can happen when performing a ship landing, where the home
+        coordinates are continuously set by the ship GNSS RX.
+        '''
+        self.set_parameter('TRIM_THROTTLE', 70)
+        self.wait_ready_to_arm()
+        home = self.home_position_as_mav_location()
+        target_alt = 20
+        self.takeoff(target_alt, mode="TAKEOFF")
+        self.change_mode("LOITER")
+        self.delay_sim_time(20) # Let the plane settle.
+
+        tstart = self.get_sim_time()
+        test_time = 10 # Run the test for 10s.
+        pub_freq = 10
+        for i in range(test_time*pub_freq):
+            tnow = self.get_sim_time()
+            higher_home = copy.copy(home)
+            # Produce 1Hz sine waves in home altitude change.
+            higher_home.alt += 40*math.sin((tnow-tstart)*(2*math.pi))
+            self.set_home(higher_home)
+            if tnow-tstart > test_time:
+                break
+            self.delay_sim_time(1.0/pub_freq)
+
+        # Test if the altitude is still within bounds.
+        self.wait_altitude(home.alt+target_alt-5, home.alt+target_alt+5, relative=False, minimum_duration=1, timeout=2)
+        self.disarm_vehicle(force=True)
+        self.reboot_sitl()
+
     def ForceArm(self):
         '''check force-arming functionality'''
         self.set_parameter("SIM_GPS1_ENABLE", 0)
@@ -6462,6 +6496,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
             self.ScriptStats,
             self.GPSPreArms,
             self.SetHomeAltChange,
+            self.SetHomeAltChange2,
             self.ForceArm,
             self.MAV_CMD_EXTERNAL_WIND_ESTIMATE,
             self.GliderPullup,

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_RLL_P 0.108
-Q_A_RAT_PIT_P 0.15
+Q_A_RAT_RLL_I 0.108
-Q_A_RAT_RLL_D 0.002
+Q_A_RAT_RLL_D 0.001
-Q_A_RAT_PIT_D 0.002
+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

Tools/autotest/logger_metadata/parse.py

@@ -182,7 +182,7 @@ class LoggerDocco(object):
                 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)):

Tools/autotest/quadplane.py

@@ -363,6 +363,20 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
     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 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
 
         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'''
 
@@ -2104,6 +2193,7 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
             self.LoiterAltQLand,
             self.VTOLLandSpiral,
             self.VTOLQuicktune,
+            self.VTOLQuicktune_CPP,
             self.PrecisionLanding,
             self.ShipLanding,
             Test(self.MotorTest, kwargs={  # tests motors 4 and 2

Tools/autotest/rover.py

@@ -6876,6 +6876,75 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         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 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
             self.DataFlash,
             self.SkidSteer,
             self.PolyFence,
+            self.SDPolyFence,
             self.PolyFenceAvoidance,
             self.PolyFenceObjectAvoidanceAuto,
             self.PolyFenceObjectAvoidanceGuided,

Tools/autotest/vehicle_test_suite.py

@@ -2711,8 +2711,6 @@ class TestSuite(ABC):
             "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 @@ class TestSuite(ABC):
             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
@@ -9430,6 +9428,20 @@ Also, ignores heartbeats not from our target system'''
                                 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:

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

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

Tools/ros2/ardupilot_dds_tests/setup.py

@@ -27,6 +27,7 @@ setup(
             "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",
         ],
     },
 )

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
 )

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

Tools/scripts/build_ci.sh

@@ -5,7 +5,13 @@
 
 XOLDPWD=$PWD  # profile changes directory :-(
 
+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

Tools/scripts/build_options.py

@@ -224,6 +224,7 @@ BUILD_OPTIONS = [
     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

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

Tools/scripts/build_tests/test_ccache.py

@@ -65,11 +65,22 @@ subprocess.run(["ccache", "-C", "-z"])
 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)

Tools/scripts/extract_features.py

@@ -281,6 +281,7 @@ class ExtractFeatures(object):
 
             ('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):

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

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,

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -29,6 +29,9 @@
 #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"
@@ -915,6 +918,35 @@ 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;

libraries/AP_DDS/AP_DDS_Service_Table.h

@@ -11,6 +11,9 @@ enum class ServiceIndex: uint8_t {
 #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
@@ -66,6 +69,18 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
         .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),

libraries/AP_DDS/AP_DDS_config.h

@@ -141,6 +141,10 @@
 #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

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

libraries/AP_DDS/README.md

@@ -210,6 +210,7 @@ $ ros2 service list
 /ap/arm_motors
 /ap/mode_switch
 /ap/prearm_check
+/ap/experimental/takeoff
 ---
 ```
 
@@ -236,6 +237,7 @@ $ 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:
@@ -272,6 +274,16 @@ 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.

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

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

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

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

libraries/AP_HAL_ChibiOS/hwdef/CubeNode-ETH/hwdef.dat

@@ -3,7 +3,7 @@ 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
 
@@ -11,12 +11,17 @@ undef HAL_GCS_ENABLED
 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
@@ -29,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

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.
+

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

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

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

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
 

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
 

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

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
 

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

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:

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

libraries/AP_HAL_ESP32/Scheduler.h

@@ -85,16 +85,16 @@ public:
     static const int IO_PRIO      = 5;
     static const int STORAGE_PRIO = 4;
 
-    static const int TIMER_SS 	  = 4096;
+    static const int TIMER_SS     = 1024*3;
-    static const int MAIN_SS      = 8192;
+    static const int MAIN_SS      = 1024*5;
-    static const int RCIN_SS      = 4096;
+    static const int RCIN_SS      = 1024*3;
-    static const int RCOUT_SS     = 4096;
+    static const int RCOUT_SS     = 1024*1.5;
-    static const int WIFI_SS1     = 6192;
+    static const int WIFI_SS1     = 1024*2.25;
-    static const int WIFI_SS2     = 6192;
+    static const int WIFI_SS2     = 1024*2.25;
-    static const int UART_SS      = 2048;
+    static const int UART_SS      = 1024*2.25;
-    static const int DEVICE_SS    = 4096;
+    static const int DEVICE_SS    = 1024*4;     // DEVICEBUS/s
-    static const int IO_SS        = 4096;
+    static const int IO_SS        = 1024*3.5;   // APM_IO
-    static const int STORAGE_SS   = 8192;
+    static const int STORAGE_SS   = 1024*2;     // APM_STORAGE
 
 private:
     AP_HAL::HAL::Callbacks *callbacks;

libraries/AP_HAL_ESP32/boards/esp32empty.h

@@ -78,8 +78,10 @@
 //SPI Devices
 #define HAL_ESP32_SPI_DEVICES {}
 
-//RCIN
+//RMT pin number
-#define HAL_ESP32_RCIN GPIO_NUM_36
+#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,

libraries/AP_Logger/LogStructure.h

@@ -800,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
@@ -897,7 +897,7 @@ struct PACKED log_VER {
 // @Field: Mem: Free memory available
 // @Field: Load: System processor load
 // @Field: InE: Internal error mask; which internal errors have been detected
-// @FieldBitmaskEnum: IntE: AP_InternalError::error_t
+// @FieldBitmaskEnum: InE: AP_InternalError::error_t
 // @Field: ErrL: Internal error line number; last line number on which a internal error was detected
 // @Field: ErC: Internal error count; how many internal errors have been detected
 // @Field: SPIC: Number of SPI transactions processed

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 (get_command_id(i) == uint16_t(MAV_CMD_DO_RETURN_PATH_START)) {
-        if (read_cmd_from_storage(i, tmp) && (tmp.id == 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 @@ reset_do_jump_tracking:
 
 // 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)) {

libraries/AP_Mission/AP_Mission.h

@@ -869,7 +869,7 @@ private:
 
     // 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);

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;

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

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

libraries/AP_RTC/AP_RTC.cpp

@@ -114,9 +114,8 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type)
             // @LoggerMessage: RTC
             // @Description: Information about RTC clock resets
             // @Field: TimeUS: Time since system startup
-            // @Field: old: old time
+            // @Field: old_utc: old time
-            // @Field: new: new time
+            // @Field: new_utc: new time
-            // @Field: in: new argument time
             AP::logger().WriteStreaming(
                 "RTC",
                 "TimeUS,old_utc,new_utc",

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

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

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

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
+

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

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

libraries/AP_Soaring/AP_Soaring.h

@@ -23,10 +23,10 @@
 #include "Variometer.h"
 #include "SpeedToFly.h"
 
-#define INITIAL_THERMAL_RADIUS 80.0
+static constexpr float INITIAL_THERMAL_RADIUS = 80.0;
-#define INITIAL_STRENGTH_COVARIANCE 0.0049
+static constexpr float INITIAL_STRENGTH_COVARIANCE = 0.0049;
-#define INITIAL_RADIUS_COVARIANCE 400.0
+static constexpr float INITIAL_RADIUS_COVARIANCE = 400.0;
-#define INITIAL_POSITION_COVARIANCE 400.0
+static constexpr float INITIAL_POSITION_COVARIANCE = 400.0;
 
 
 class SoaringController {

libraries/AP_TECS/AP_TECS.cpp

@@ -1124,7 +1124,6 @@ void AP_TECS::_initialise_states(float hgt_afe)
         _integKE              = 0.0f;
         _last_throttle_dem    = aparm.throttle_cruise * 0.01f;
         _last_pitch_dem       = _ahrs.get_pitch();
-        _hgt_afe              = hgt_afe;
         _hgt_dem_in_prev      = hgt_afe;
         _hgt_dem_lpf          = hgt_afe;
         _hgt_dem_rate_ltd     = hgt_afe;
@@ -1502,3 +1501,31 @@ void AP_TECS::_update_pitch_limits(const int32_t ptchMinCO_cd) {
     // don't allow max pitch to go below min pitch
     _PITCHmaxf = MAX(_PITCHmaxf, _PITCHminf);
 }
+
+void AP_TECS::offset_altitude(const float alt_offset)
+{
+    // Convention: When alt_offset is positive it means that the altitude of
+    // home has increased. Thus, the relative altitude of the vehicle has
+    // decreased.
+    //
+    // Assumption: This method is called more often and before
+    // `update_pitch_throttle()`. This is necessary to ensure that new height
+    // demands which incorporate the home change are compatible with the
+    // (now updated) internal height state.
+
+    _flare_hgt_dem_ideal    -= alt_offset;
+    _flare_hgt_dem_adj      -= alt_offset;
+    _hgt_at_start_of_flare  -= alt_offset;
+    _hgt_dem_in_prev        -= alt_offset;
+    _hgt_dem_lpf            -= alt_offset;
+    _hgt_dem_rate_ltd       -= alt_offset;
+    _hgt_dem_prev           -= alt_offset;
+    _height_filter.height   -= alt_offset;
+
+    // The following variables are updated anew in every call of
+    // `update_pitch_throttle()`. There's no need to update those.
+    // _hgt_dem
+    // _hgt_dem_in_raw
+    // _hgt_dem_in
+    // Energies
+}

libraries/AP_TECS/AP_TECS.h

@@ -154,6 +154,9 @@ public:
         _need_reset = true;
     }
 
+    // Apply an altitude offset, to compensate for changes in home alt.
+    void offset_altitude(const float alt_offset);
+
     // this supports the TECS_* user settable parameters
     static const struct AP_Param::GroupInfo var_info[];
 

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;
+    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];
-    h00 = grid.height[info.idx_x+0][info.idx_y+0];
+    const auto h10 = grid.height[info.idx_x+1][info.idx_y+0];
-    h01 = grid.height[info.idx_x+0][info.idx_y+1];
+    const auto h11 = grid.height[info.idx_x+1][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];
 
     // 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;
+    const float avg1 = (1.0f-info.frac_x) * h00  + info.frac_x * h10;
-    float avg2 = (1.0f-info.frac_x) * h01  + info.frac_x * h11;
+    const 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 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",

libraries/AP_Vehicle/AP_Vehicle.h

@@ -172,6 +172,8 @@ public:
     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 @@ public:
     /*
       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 @@ public:
     // 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

libraries/GCS_MAVLink/GCS.h

@@ -1139,6 +1139,7 @@ private:
         uint8_t next_index;
     } available_modes;
     bool send_available_modes();
+    bool send_available_mode_monitor();
 
 };
 
@@ -1301,6 +1302,11 @@ public:
     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 @@ private:
     // 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();

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

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

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

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

libraries/RC_Channel/RC_Channel.h

@@ -260,6 +260,7 @@ public:
         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

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