AP_Periph: prepare for 1.3.2

not enough ram to uncompress at runtime

.github/workflows/test_size.yml

@@ -1,180 +0,0 @@
-name: test size
-
-on: [push, pull_request, workflow_dispatch]
-# paths:
-# - "*"
-# - "!README.md" <-- don't rebuild on doc change
-concurrency:
-  group: ci-${{github.workflow}}-${{ github.ref }}
-  cancel-in-progress: true
-
-jobs:
-  build:
-    runs-on: ubuntu-20.04
-    container: ardupilot/ardupilot-dev-chibios:latest
-    strategy:
-      fail-fast: false  # don't cancel if a job from the matrix fails
-      matrix:
-        toolchain: [
-            base,  # GCC
-        ]
-        config: [
-            Durandal,
-            MatekF405,
-            Pixhawk1-1M,
-            Hitec-Airspeed,  # see special code for Periph below (3 places!)
-            f103-GPS  # see special code for Periph below (3 places!)
-        ]
-    steps:
-      - uses: actions/checkout@v2
-        with:
-          ref: 'master'
-          path: 'master'
-          submodules: 'recursive'
-      # Put ccache into github cache for faster build
-      - name: Prepare ccache timestamp
-        id: ccache_cache_timestamp
-        run: |
-          NOW=$(date -u +"%F-%T")
-          echo "::set-output name=timestamp::${NOW}"
-      - name: ccache cache files
-        uses: actions/cache@v2
-        with:
-          path: ~/.ccache
-          key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}}
-          restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-  # restore ccache from either previous build on this branch or on master
-      - name: setup ccache
-        run: |
-          mkdir -p ~/.ccache
-          echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
-          echo "compression = true" >> ~/.ccache/ccache.conf
-          echo "compression_level = 6" >> ~/.ccache/ccache.conf
-          echo "max_size = 400M" >> ~/.ccache/ccache.conf
-          ccache -s
-          ccache -z
-
-      - name: Build master ${{matrix.config}} ${{ matrix.toolchain }}
-        env:
-          CI_BUILD_TARGET: ${{matrix.config}}
-        shell: bash
-        run: |
-          PATH="/github/home/.local/bin:$PATH"
-          cd master
-          ./waf configure --board ${{matrix.config}}
-          if [ "${{matrix.config}}" = "Hitec-Airspeed" ] ||
-             [ "${{matrix.config}}" = "f103-GPS" ]; then
-            ./waf AP_Periph
-          else
-            ./waf
-          fi
-          mkdir -p $GITHUB_WORKSPACE/master_bin
-          cp -r build/${{matrix.config}}/bin/* $GITHUB_WORKSPACE/master_bin/
-
-          # build a set of binaries without symbols so we can check if
-          #  the binaries have changed.
-          echo [`date`] Building master with no versions
-
-          NO_VERSIONS_DIR="$GITHUB_WORKSPACE/master_bin_no_versions"
-          mkdir "$NO_VERSIONS_DIR"
-
-          if [ "${{matrix.config}}" = "Hitec-Airspeed" ] ||
-             [ "${{matrix.config}}" = "f103-GPS" ]; then
-            CHIBIOS_GIT_VERSION="12345678" GIT_VERSION="abcdef" ./waf AP_Periph
-          else
-            CHIBIOS_GIT_VERSION="12345678" GIT_VERSION="abcdef" ./waf
-          fi
-          cp -r build/${{matrix.config}}/bin/* "$NO_VERSIONS_DIR"
-
-          echo [`date`] Built master with no versions
-
-      - uses: actions/checkout@v2
-        with:
-          fetch-depth: 0
-          path: 'pr'
-
-      - name: Build PR rebased ${{matrix.config}} ${{ matrix.toolchain }}
-        env:
-          CI_BUILD_TARGET: ${{matrix.config}}
-        shell: bash
-        run: |
-          PATH="/github/home/.local/bin:$PATH"
-          cd pr/
-          git config user.email "ardupilot-ci@ardupilot.org"
-          git config user.name "ArduPilot CI"
-          git remote add ardupilot https://github.com/ArduPilot/ardupilot.git
-          git fetch --no-tags --prune --progress ardupilot master
-          git rebase ardupilot/master
-          git submodule update --init --recursive --depth=1
-          ./waf configure --board ${{matrix.config}}
-          if [ "${{matrix.config}}" = "Hitec-Airspeed" ] ||
-             [ "${{matrix.config}}" = "f103-GPS" ]; then
-            ./waf AP_Periph
-          else
-            ./waf
-          fi
-          mkdir $GITHUB_WORKSPACE/pr_bin
-          cp -r build/${{matrix.config}}/bin/* $GITHUB_WORKSPACE/pr_bin/
-
-          # build a set of binaries without symbols so we can check if
-          #  the binaries have changed.
-          echo [`date`] Building PR with no versions
-
-          NO_VERSIONS_DIR="$GITHUB_WORKSPACE/pr_bin_no_versions"
-          mkdir "$NO_VERSIONS_DIR"
-
-          if [ "${{matrix.config}}" = "Hitec-Airspeed" ] ||
-             [ "${{matrix.config}}" = "f103-GPS" ]; then
-            CHIBIOS_GIT_VERSION="12345678" GIT_VERSION="abcdef" ./waf AP_Periph
-          else
-            CHIBIOS_GIT_VERSION="12345678" GIT_VERSION="abcdef" ./waf
-          fi
-          cp -r build/${{matrix.config}}/bin/* "$NO_VERSIONS_DIR"
-
-          echo [`date`] Built PR with no versions
-
-          # build MatekF405 Plane without quadplane
-          if [ "${{matrix.config}}" = "MatekF405" ]; then
-            PLANE_BINARY="build/MatekF405/bin/arduplane.bin"
-            echo "normal size"
-            ls -l "$PLANE_BINARY"
-            EXTRA_HWDEF="/tmp/extra-options.def"
-            echo "define HAL_QUADPLANE_ENABLED 0" >"$EXTRA_HWDEF"
-            ./waf configure --board ${{matrix.config}} --extra-hwdef="$EXTRA_HWDEF"
-            ./waf plane
-            rm "$EXTRA_HWDEF"
-            echo "non-quadplane size:"
-            ls -l "$PLANE_BINARY"
-          fi
-
-      - name: Full size compare with Master
-        shell: bash
-        run: |
-          cd pr/
-          python3 -m pip install -U tabulate
-          Tools/scripts/pretty_diff_size.py -m $GITHUB_WORKSPACE/master_bin -s $GITHUB_WORKSPACE/pr_bin
-
-      - name: Checksum compare with Master
-        shell: bash
-        run: |
-          diff -r $GITHUB_WORKSPACE/master_bin_no_versions $GITHUB_WORKSPACE/pr_bin_no_versions --exclude=*.elf --exclude=*.apj || true
-
-      - name: elf_diff compare with Master
-        shell: bash
-        run: |
-          python3 -m pip install -U weasyprint elf_diff anytree
-          mkdir elf_diff
-          if [ "${{matrix.config}}" = "Hitec-Airspeed" ] ||
-             [ "${{matrix.config}}" = "f103-GPS" ]; then
-             python3 -m elf_diff --bin_prefix=arm-none-eabi- --html_dir=elf_diff/AP_Periph $GITHUB_WORKSPACE/master_bin/AP_Periph $GITHUB_WORKSPACE/pr_bin/AP_Periph
-          else
-             python3 -m elf_diff --bin_prefix=arm-none-eabi- --html_dir=elf_diff/plane $GITHUB_WORKSPACE/master_bin/arduplane $GITHUB_WORKSPACE/pr_bin/arduplane
-             python3 -m elf_diff --bin_prefix=arm-none-eabi- --html_dir=elf_diff/copter $GITHUB_WORKSPACE/master_bin/arducopter $GITHUB_WORKSPACE/pr_bin/arducopter
-          fi
-          zip -r elf_diff.zip elf_diff
-
-      - name: Archive elf_diff output
-        uses: actions/upload-artifact@v2
-        with:
-           name: ELF_DIFF_${{matrix.config}}
-           path: elf_diff.zip
-           retention-days: 14

ArduCopter/GCS_Mavlink.cpp

@@ -995,30 +995,24 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
 
 MAV_RESULT GCS_MAVLINK_Copter::handle_command_pause_continue(const mavlink_command_int_t &packet)
 {
-#if MODE_AUTO_ENABLED
-    if (copter.flightmode->mode_number() != Mode::Number::AUTO) {
-        // only supported in AUTO mode
+    // requested pause
+    if ((uint8_t) packet.param1 == 0) {
+        if (copter.flightmode->pause()) {
+            return MAV_RESULT_ACCEPTED;
+        }
+        send_text(MAV_SEVERITY_INFO, "Failed to pause");
         return MAV_RESULT_FAILED;
     }
 
-    // requested pause from GCS
-    if ((int8_t) packet.param1 == 0) {
-        copter.mode_auto.mission.stop();
-        copter.mode_auto.loiter_start();
-        gcs().send_text(MAV_SEVERITY_INFO, "Paused mission");
-        return MAV_RESULT_ACCEPTED;
-    }
-
-    // requested resume from GCS
-    if ((int8_t) packet.param1 == 1) {
-        copter.mode_auto.mission.resume();
-        gcs().send_text(MAV_SEVERITY_INFO, "Resumed mission");
-        return MAV_RESULT_ACCEPTED;
+    // requested resume
+    if ((uint8_t) packet.param1 == 1) {
+        if (copter.flightmode->resume()) {
+            return MAV_RESULT_ACCEPTED;
+        }
+        send_text(MAV_SEVERITY_INFO, "Failed to resume");
+        return MAV_RESULT_FAILED;
     }
-#endif
-
-    // fail pause or continue
-    return MAV_RESULT_FAILED;
+    return MAV_RESULT_DENIED;
 }
 
 void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg)

ArduCopter/mode.cpp

@@ -431,6 +431,39 @@ void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, floa
     // roll_out and pitch_out are returned
 }
 
+// transform pilot's roll or pitch input into a desired velocity
+Vector2f Mode::get_pilot_desired_velocity(float vel_max) const
+{
+    Vector2f vel;
+
+    // throttle failsafe check
+    if (copter.failsafe.radio || !copter.ap.rc_receiver_present) {
+        return vel;
+    }
+    // fetch roll and pitch inputs
+    float roll_out = channel_roll->get_control_in();
+    float pitch_out = channel_pitch->get_control_in();
+
+    // convert roll and pitch inputs to -1 to +1 range
+    float scaler = 1.0 / (float)ROLL_PITCH_YAW_INPUT_MAX;
+    roll_out *= scaler;
+    pitch_out *= scaler;
+
+    // convert roll and pitch inputs into velocity in NE frame
+    vel = Vector2f(-pitch_out, roll_out);
+    if (vel.is_zero()) {
+        return vel;
+    }
+    copter.rotate_body_frame_to_NE(vel.x, vel.y);
+
+    // Transform square input range to circular output
+    // vel_scaler is the vector to the edge of the +- 1.0 square in the direction of the current input
+    Vector2f vel_scaler = vel / MAX(fabsf(vel.x), fabsf(vel.y));
+    // We scale the output by the ratio of the distance to the square to the unit circle and multiply by vel_max
+    vel *= vel_max / vel_scaler.length();
+    return vel;
+}
+
 bool Mode::_TakeOff::triggered(const float target_climb_rate) const
 {
     if (!copter.ap.land_complete) {
@@ -598,13 +631,12 @@ void Mode::land_run_vertical_control(bool pause_descent)
 
 void Mode::land_run_horizontal_control()
 {
-    float target_roll = 0.0f;
-    float target_pitch = 0.0f;
+    Vector2f vel_correction;
     float target_yaw_rate = 0;
 
     // relax loiter target if we might be landed
     if (copter.ap.land_complete_maybe) {
-        loiter_nav->soften_for_landing();
+        pos_control->soften_for_landing_xy();
     }
 
     // process pilot inputs
@@ -621,11 +653,14 @@ void Mode::land_run_horizontal_control()
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();
 
-            // convert pilot input to lean angles
-            get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
+            // convert pilot input to reposition velocity
+            // use half maximum acceleration as the maximum velocity to ensure aircraft will
+            // stop from full reposition speed in less than 1 second.
+            const float max_pilot_vel = wp_nav->get_wp_acceleration() * 0.5;
+            vel_correction = get_pilot_desired_velocity(max_pilot_vel);
 
             // record if pilot has overridden roll or pitch
-            if (!is_zero(target_roll) || !is_zero(target_pitch)) {
+            if (!vel_correction.is_zero()) {
                 if (!copter.ap.land_repo_active) {
                     AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE);
                 }
@@ -641,11 +676,11 @@ void Mode::land_run_horizontal_control()
     }
 
     // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle
-    bool doing_precision_landing = false;
+    copter.ap.prec_land_active = false;
 #if PRECISION_LANDING == ENABLED
-    doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired();
+    copter.ap.prec_land_active = !copter.ap.land_repo_active && copter.precland.target_acquired();
     // run precision landing
-    if (doing_precision_landing) {
+    if (copter.ap.prec_land_active) {
         Vector2f target_pos, target_vel;
         if (!copter.precland.get_target_position_cm(target_pos)) {
             target_pos = inertial_nav.get_position_xy_cm();
@@ -657,29 +692,16 @@ void Mode::land_run_horizontal_control()
         Vector2p landing_pos = target_pos.topostype();
         // target vel will remain zero if landing target is stationary
         pos_control->input_pos_vel_accel_xy(landing_pos, target_vel, zero);
-        // run pos controller
-        pos_control->update_xy_controller();
     }
 #endif
 
-    if (!doing_precision_landing) {
-        if (copter.ap.prec_land_active) {
-            // precland isn't active anymore but was active a while back
-            // lets run an init again
-            // set target to stopping point
-            Vector2f stopping_point;
-            loiter_nav->get_stopping_point_xy(stopping_point);
-            loiter_nav->init_target(stopping_point);
-        }
-        // process roll, pitch inputs
-        loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
-
-        // run loiter controller
-        loiter_nav->update();
+    if (!copter.ap.prec_land_active) {
+        Vector2f accel;
+        pos_control->input_vel_accel_xy(vel_correction, accel);
     }
 
-    copter.ap.prec_land_active = doing_precision_landing;
-
+    // run pos controller
+    pos_control->update_xy_controller();
     Vector3f thrust_vector = pos_control->get_thrust_vector();
 
     if (g2.wp_navalt_min > 0) {

ArduCopter/mode.h

@@ -90,6 +90,7 @@ public:
 
     // pilot input processing
     void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const;
+    Vector2f get_pilot_desired_velocity(float vel_max) const;
     float get_pilot_desired_yaw_rate(float yaw_in);
     float get_pilot_desired_throttle() const;
 
@@ -109,6 +110,10 @@ public:
     // returns true if pilot's yaw input should be used to adjust vehicle's heading
     virtual bool use_pilot_yaw() const {return true; }
 
+    // pause and resume a mode
+    virtual bool pause() { return false; };
+    virtual bool resume() { return false; };
+
 protected:
 
     // helper functions
@@ -196,15 +201,23 @@ protected:
 
     virtual bool do_user_takeoff_start(float takeoff_alt_cm);
 
-    // method shared by both Guided and Auto for takeoff.  This is
-    // waypoint navigation but the user can control the yaw.
+    // method shared by both Guided and Auto for takeoff.
+    // position controller controls vehicle but the user can control the yaw.
     void auto_takeoff_run();
-    void auto_takeoff_set_start_alt(void);
+    void auto_takeoff_start(float complete_alt_cm, bool terrain_alt);
+    bool auto_takeoff_get_position(Vector3p& completion_pos);
 
     // altitude above-ekf-origin below which auto takeoff does not control horizontal position
     static bool auto_takeoff_no_nav_active;
     static float auto_takeoff_no_nav_alt_cm;
 
+    // auto takeoff variables
+    static float auto_take_off_start_alt_cm;    // start altitude expressed as cm above ekf origin
+    static float auto_take_off_complete_alt_cm; // completion altitude expressed as cm above ekf origin
+    static bool auto_takeoff_terrain_alt;       // true if altitudes are above terrain
+    static bool auto_takeoff_complete;          // true when takeoff is complete
+    static Vector3p auto_takeoff_complete_pos;  // target takeoff position as offset from ekf origin in cm
+
 public:
     // Navigation Yaw control
     class AutoYaw {
@@ -410,12 +423,15 @@ public:
     // Auto
     SubMode mode() const { return _mode; }
 
+    // pause continue in auto mode
+    bool pause() override;
+    bool resume() override;
+
     bool loiter_start();
     void rtl_start();
     void takeoff_start(const Location& dest_loc);
     void wp_start(const Location& dest_loc);
     void land_start();
-    void land_start(const Vector2f& destination);
     void circle_movetoedge_start(const Location &circle_center, float radius_m);
     void circle_start();
     void nav_guided_start();
@@ -486,7 +502,6 @@ private:
 
     Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const;
 
-    void payload_place_start(const Vector2f& destination);
     void payload_place_run();
     bool payload_place_run_should_run();
     void payload_place_run_loiter();
@@ -939,6 +954,8 @@ public:
 
     bool is_taking_off() const override;
 
+    // initialises position controller to implement take-off
+    // takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available
     bool do_user_takeoff_start(float takeoff_alt_cm) override;
 
     enum class SubMode {
@@ -961,6 +978,10 @@ public:
 
     bool use_pilot_yaw() const override;
 
+    // pause continue in guided mode
+    bool pause() override;
+    bool resume() override;
+
 protected:
 
     const char *name() const override { return "GUIDED"; }
@@ -996,6 +1017,7 @@ private:
     void pos_control_run();
     void accel_control_run();
     void velaccel_control_run();
+    void pause_control_run();
     void posvelaccel_control_run();
     void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
 
@@ -1003,6 +1025,9 @@ private:
     SubMode guided_mode = SubMode::TakeOff;
     bool send_notification;     // used to send one time notification to ground station
     bool takeoff_complete;      // true once takeoff has completed (used to trigger retracting of landing gear)
+
+    // guided mode is paused or not
+    bool _paused;
 };
 
 

ArduCopter/mode_auto.cpp

@@ -262,39 +262,43 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
 {
     _mode = SubMode::TAKEOFF;
 
-    Location dest(dest_loc);
-
     if (!copter.current_loc.initialised()) {
         // vehicle doesn't know where it is ATM.  We should not
         // initialise our takeoff destination without knowing this!
         return;
     }
 
-    // set horizontal target
-    dest.lat = copter.current_loc.lat;
-    dest.lng = copter.current_loc.lng;
+    // calculate current and target altitudes
+    // by default current_alt_cm and alt_target_cm are alt-above-EKF-origin
+    int32_t alt_target_cm;
+    bool alt_target_terrain = false;
+    float current_alt_cm = inertial_nav.get_position_z_up_cm();
+    float terrain_offset;   // terrain's altitude in cm above the ekf origin
+    if ((dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) && wp_nav->get_terrain_offset(terrain_offset)) {
+        // subtract terrain offset to convert vehicle's alt-above-ekf-origin to alt-above-terrain
+        current_alt_cm -= terrain_offset;
 
-    // get altitude target
-    int32_t alt_target;
-    if (!dest.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_target)) {
-        // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
-        AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
-        // fall back to altitude above current altitude
-        alt_target = copter.current_loc.alt + dest.alt;
+        // specify alt_target_cm as alt-above-terrain
+        alt_target_cm = dest_loc.alt;
+        alt_target_terrain = true;
+    } else {
+        // set horizontal target
+        Location dest(dest_loc);
+        dest.lat = copter.current_loc.lat;
+        dest.lng = copter.current_loc.lng;
+
+        // get altitude target above EKF origin
+        if (!dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) {
+            // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
+            AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
+            // fall back to altitude above current altitude
+            alt_target_cm = current_alt_cm + dest.alt;
+        }
     }
 
     // sanity check target
-    int32_t alt_target_min_cm = copter.current_loc.alt + (copter.ap.land_complete ? 100 : 0);
-    if (alt_target < alt_target_min_cm ) {
-        dest.set_alt_cm(alt_target_min_cm , Location::AltFrame::ABOVE_HOME);
-    }
-
-    // set waypoint controller target
-    if (!wp_nav->set_wp_destination_loc(dest)) {
-        // failure to set destination can only be because of missing terrain data
-        copter.failsafe_terrain_on_event();
-        return;
-    }
+    int32_t alt_target_min_cm = current_alt_cm + (copter.ap.land_complete ? 100 : 0);
+    alt_target_cm = MAX(alt_target_cm, alt_target_min_cm);
 
     // initialise yaw
     auto_yaw.set_mode(AUTO_YAW_HOLD);
@@ -302,13 +306,25 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
     // clear i term when we're taking off
     set_throttle_takeoff();
 
-    // get initial alt for WP_NAVALT_MIN
-    auto_takeoff_set_start_alt();
+    // initialise alt for WP_NAVALT_MIN and set completion alt
+    auto_takeoff_start(alt_target_cm, alt_target_terrain);
 }
 
 // auto_wp_start - initialises waypoint controller to implement flying to a particular destination
 void ModeAuto::wp_start(const Location& dest_loc)
 {
+    // init wpnav and set origin if transitioning from takeoff
+    if (!wp_nav->is_active()) {
+        Vector3f stopping_point;
+        if (_mode == SubMode::TAKEOFF) {
+            Vector3p takeoff_complete_pos;
+            if (auto_takeoff_get_position(takeoff_complete_pos)) {
+                stopping_point = takeoff_complete_pos.tofloat();
+            }
+        }
+        wp_nav->wp_and_spline_init(0, stopping_point);
+    }
+
     // send target to waypoint controller
     if (!wp_nav->set_wp_destination_loc(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
@@ -328,21 +344,20 @@ void ModeAuto::wp_start(const Location& dest_loc)
 // auto_land_start - initialises controller to implement a landing
 void ModeAuto::land_start()
 {
-    // set target to stopping point
-    Vector2f stopping_point;
-    loiter_nav->get_stopping_point_xy(stopping_point);
+    _mode = SubMode::LAND;
 
-    // call location specific land start function
-    land_start(stopping_point);
-}
+    // set horizontal speed and acceleration limits
+    pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
+    pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
 
-// auto_land_start - initialises controller to implement a landing
-void ModeAuto::land_start(const Vector2f& destination)
-{
-    _mode = SubMode::LAND;
+    // initialise the vertical position controller
+    if (!pos_control->is_active_xy()) {
+        pos_control->init_xy_controller();
+    }
 
-    // initialise loiter target destination
-    loiter_nav->init_target(destination);
+    // set vertical speed and acceleration limits
+    pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
+    pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
 
     // initialise the vertical position controller
     if (!pos_control->is_active_z()) {
@@ -469,12 +484,29 @@ bool ModeAuto::is_taking_off() const
 // auto_payload_place_start - initialises controller to implement a placing
 void ModeAuto::payload_place_start()
 {
-    // set target to stopping point
-    Vector2f stopping_point;
-    loiter_nav->get_stopping_point_xy(stopping_point);
+    _mode = SubMode::NAV_PAYLOAD_PLACE;
+    nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
+
+    // set horizontal speed and acceleration limits
+    pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
+    pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
+
+    // initialise the vertical position controller
+    if (!pos_control->is_active_xy()) {
+        pos_control->init_xy_controller();
+    }
+
+    // set vertical speed and acceleration limits
+    pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
+    pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
+
+    // initialise the vertical position controller
+    if (!pos_control->is_active_z()) {
+        pos_control->init_z_controller();
+    }
 
-    // call location specific place start function
-    payload_place_start(stopping_point);
+    // initialise yaw
+    auto_yaw.set_mode(AUTO_YAW_HOLD);
 }
 
 // returns true if pilot's yaw input should be used to adjust vehicle's heading
@@ -892,8 +924,6 @@ void ModeAuto::land_run()
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
         make_safe_ground_handling();
-        loiter_nav->clear_pilot_desired_acceleration();
-        loiter_nav->init_target();
         return;
     }
 
@@ -999,8 +1029,14 @@ void ModeAuto::loiter_to_alt_run()
     }
 
     if (!loiter_to_alt.loiter_start_done) {
-        loiter_nav->clear_pilot_desired_acceleration();
-        loiter_nav->init_target();
+        // set horizontal speed and acceleration limits
+        pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
+        pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
+
+        if (!pos_control->is_active_xy()) {
+            pos_control->init_xy_controller();
+        }
+
         _mode = SubMode::LOITER_TO_ALT;
         loiter_to_alt.loiter_start_done = true;
     }
@@ -1038,31 +1074,12 @@ void ModeAuto::loiter_to_alt_run()
     pos_control->update_z_controller();
 }
 
-// auto_payload_place_start - initialises controller to implement placement of a load
-void ModeAuto::payload_place_start(const Vector2f& destination)
-{
-    _mode = SubMode::NAV_PAYLOAD_PLACE;
-    nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
-
-    // initialise loiter target destination
-    loiter_nav->init_target(destination);
-
-    // initialise the vertical position controller
-    pos_control->init_z_controller();
-
-    // initialise yaw
-    auto_yaw.set_mode(AUTO_YAW_HOLD);
-}
-
 // auto_payload_place_run - places an object in auto mode
 //      called by auto_run at 100hz or more
 void ModeAuto::payload_place_run()
 {
     if (!payload_place_run_should_run()) {
         zero_throttle_and_relax_ac();
-        // set target to current position
-        loiter_nav->clear_pilot_desired_acceleration();
-        loiter_nav->init_target();
         return;
     }
 
@@ -1194,6 +1211,18 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
         }
     }
 
+    // init wpnav and set origin if transitioning from takeoff
+    if (!wp_nav->is_active()) {
+        Vector3f stopping_point;
+        if (_mode == SubMode::TAKEOFF) {
+            Vector3p takeoff_complete_pos;
+            if (auto_takeoff_get_position(takeoff_complete_pos)) {
+                stopping_point = takeoff_complete_pos.tofloat();
+            }
+        }
+        wp_nav->wp_and_spline_init(0, stopping_point);
+    }
+
     // get waypoint's location from command and send to wp_nav
     const Location dest_loc = loc_from_cmd(cmd, default_loc);
     if (!wp_nav->set_wp_destination_loc(dest_loc)) {
@@ -1381,6 +1410,7 @@ void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
 
     // set vertical speed and acceleration limits
     pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
+    pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
 }
 
 // do_spline_wp - initiate move to next waypoint
@@ -1637,18 +1667,15 @@ void ModeAuto::do_RTL(void)
 // verify_takeoff - check if we have completed the takeoff
 bool ModeAuto::verify_takeoff()
 {
-    // have we reached our target altitude?
-    const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
-
 #if LANDING_GEAR_ENABLED == ENABLED
     // if we have reached our destination
-    if (reached_wp_dest) {
+    if (auto_takeoff_complete) {
         // retract the landing gear
         copter.landinggear.retract_after_takeoff();
     }
 #endif
 
-    return reached_wp_dest;
+    return auto_takeoff_complete;
 }
 
 // verify_land - returns true if landing has been completed
@@ -1660,11 +1687,8 @@ bool ModeAuto::verify_land()
         case State::FlyToLocation:
             // check if we've reached the location
             if (copter.wp_nav->reached_wp_destination()) {
-                // get destination so we can use it for loiter target
-                const Vector2f& dest = copter.wp_nav->get_wp_destination().xy();
-
                 // initialise landing controller
-                land_start(dest);
+                land_start();
 
                 // advance to next state
                 state = State::Descending;
@@ -2042,4 +2066,28 @@ bool ModeAuto::verify_nav_script_time()
 }
 #endif
 
+// pause - Prevent aircraft from progressing along the track
+bool ModeAuto::pause()
+{
+    // do not pause if already paused or not in the WP sub mode or already reached to the destination
+    if(wp_nav->paused() || _mode != SubMode::WP || wp_nav->reached_wp_destination()) {
+        return false;
+    }
+
+    wp_nav->set_pause();
+    return true;
+}
+
+// resume - Allow aircraft to progress along the track
+bool ModeAuto::resume()
+{
+    // do not resume if not paused before
+    if(!wp_nav->paused()) {
+        return false;
+    }
+
+    wp_nav->set_resume();
+    return true;
+}
+
 #endif

ArduCopter/mode_brake.cpp

@@ -47,7 +47,7 @@ void ModeBrake::run()
 
     // relax stop target if we might be landed
     if (copter.ap.land_complete_maybe) {
-        loiter_nav->soften_for_landing();
+        pos_control->soften_for_landing_xy();
     }
 
     // use position controller to stop

ArduCopter/mode_guided.cpp

@@ -40,6 +40,10 @@ bool ModeGuided::init(bool ignore_checks)
     guided_vel_target_cms.zero();
     guided_accel_target_cmss.zero();
     send_notification = false;
+
+    // clear pause state when entering guided mode
+    _paused = false;
+
     return true;
 }
 
@@ -47,6 +51,12 @@ bool ModeGuided::init(bool ignore_checks)
 // should be called at 100hz or more
 void ModeGuided::run()
 {
+    // run pause control if the vehicle is paused
+    if (_paused) {
+        pause_control_run();
+        return;
+    }
+
     // call the correct auto controller
     switch (guided_mode) {
 
@@ -98,14 +108,15 @@ bool ModeGuided::allows_arming(AP_Arming::Method method) const
     return (copter.g2.guided_options & (uint32_t)Options::AllowArmingFromTX) != 0;
 };
 
-// do_user_takeoff_start - initialises waypoint controller to implement take-off
+// initialises position controller to implement take-off
+// takeoff_alt_cm is interpreted as alt-above-home (in cm) or alt-above-terrain if a rangefinder is available
 bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
 {
     guided_mode = SubMode::TakeOff;
 
-    // initialise wpnav destination
-    Location target_loc = copter.current_loc;
-    Location::AltFrame frame = Location::AltFrame::ABOVE_HOME;
+    // calculate target altitude and frame (either alt-above-ekf-origin or alt-above-terrain)
+    int32_t alt_target_cm;
+    bool alt_target_terrain = false;
     if (wp_nav->rangefinder_used_and_healthy() &&
         wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER &&
         takeoff_alt_cm < copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270)) {
@@ -113,15 +124,19 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
         if (takeoff_alt_cm <= copter.rangefinder_state.alt_cm) {
             return false;
         }
-        frame = Location::AltFrame::ABOVE_TERRAIN;
-    }
-    target_loc.set_alt_cm(takeoff_alt_cm, frame);
+        // provide target altitude as alt-above-terrain
+        alt_target_cm = takeoff_alt_cm;
+        alt_target_terrain = true;
+    } else {
+        // interpret altitude as alt-above-home
+        Location target_loc = copter.current_loc;
+        target_loc.set_alt_cm(takeoff_alt_cm, Location::AltFrame::ABOVE_HOME);
 
-    if (!wp_nav->set_wp_destination_loc(target_loc)) {
-        // failure to set destination can only be because of missing terrain data
-        AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
-        // failure is propagated to GCS with NAK
-        return false;
+        // provide target altitude as alt-above-ekf-origin
+        if (!target_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) {
+            // this should never happen but we reject the command just in case
+            return false;
+        }
     }
 
     // initialise yaw
@@ -130,8 +145,8 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
     // clear i term when we're taking off
     set_throttle_takeoff();
 
-    // get initial alt for WP_NAVALT_MIN
-    auto_takeoff_set_start_alt();
+    // initialise alt for WP_NAVALT_MIN and set completion alt
+    auto_takeoff_start(alt_target_cm, alt_target_terrain);
 
     // record takeoff has not completed
     takeoff_complete = false;
@@ -629,7 +644,7 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_
 void ModeGuided::takeoff_run()
 {
     auto_takeoff_run();
-    if (!takeoff_complete && wp_nav->reached_wp_destination()) {
+    if (auto_takeoff_complete && !takeoff_complete) {
         takeoff_complete = true;
 #if LANDING_GEAR_ENABLED == ENABLED
         // optionally retract landing gear
@@ -844,6 +859,36 @@ void ModeGuided::velaccel_control_run()
     }
 }
 
+// pause_control_run - runs the guided mode pause controller
+// called from guided_run
+void ModeGuided::pause_control_run()
+{
+    // if not armed set throttle to zero and exit immediately
+    if (is_disarmed_or_landed()) {
+        // do not spool down tradheli when on the ground with motor interlock enabled
+        make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
+        return;
+    }
+
+    // set motors to full range
+    motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+
+    // set the horizontal velocity and acceleration targets to zero
+    Vector2f vel_xy, accel_xy;
+    pos_control->input_vel_accel_xy(vel_xy, accel_xy, false);
+
+    // set the vertical velocity and acceleration targets to zero
+    float vel_z = 0.0;
+    pos_control->input_vel_accel_z(vel_z, 0.0, false, false);
+
+    // call velocity controller which includes z axis controller
+    pos_control->update_xy_controller();
+    pos_control->update_z_controller();
+
+    // call attitude controller
+    attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0);
+}
+
 // posvelaccel_control_run - runs the guided position, velocity and acceleration controller
 // called from guided_run
 void ModeGuided::posvelaccel_control_run()
@@ -1159,4 +1204,18 @@ uint32_t ModeGuided::get_timeout_ms() const
     return MAX(copter.g2.guided_timeout, 0.1) * 1000;
 }
 
+// pause guide mode
+bool ModeGuided::pause()
+{
+    _paused = true;
+    return true;
+}
+
+// resume guided mode
+bool ModeGuided::resume()
+{
+    _paused = false;
+    return true;
+}
+
 #endif

ArduCopter/mode_land.cpp

@@ -5,17 +5,19 @@ bool ModeLand::init(bool ignore_checks)
 {
     // check if we have GPS and decide which LAND we're going to do
     control_position = copter.position_ok();
-    if (control_position) {
-        // set target to stopping point
-        Vector2f stopping_point;
-        loiter_nav->get_stopping_point_xy(stopping_point);
-        loiter_nav->init_target(stopping_point);
+
+    // set horizontal speed and acceleration limits
+    pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
+    pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
+
+    // initialise the horizontal position controller
+    if (control_position && !pos_control->is_active_xy()) {
+        pos_control->init_xy_controller();
     }
 
     // set vertical speed and acceleration limits
     pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
     pos_control->set_correction_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
-    pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
 
     // initialise the vertical position controller
     if (!pos_control->is_active_z()) {
@@ -76,8 +78,6 @@ void ModeLand::gps_run()
     // Land State Machine Determination
     if (is_disarmed_or_landed()) {
         make_safe_ground_handling();
-        loiter_nav->clear_pilot_desired_acceleration();
-        loiter_nav->init_target();
     } else {
         // set motors to full range
         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

ArduCopter/mode_rtl.cpp

@@ -273,9 +273,6 @@ void ModeRTL::descent_start()
     _state = SubMode::FINAL_DESCENT;
     _state_complete = false;
 
-    // Set wp navigation target to above home
-    loiter_nav->init_target(wp_nav->get_wp_destination().xy());
-
     // initialise altitude target to stopping point
     pos_control->init_z_controller_stopping_point();
 
@@ -297,8 +294,7 @@ void ModeRTL::descent_start()
 //      called by rtl_run at 100hz or more
 void ModeRTL::descent_run()
 {
-    float target_roll = 0.0f;
-    float target_pitch = 0.0f;
+    Vector2f vel_correction;
     float target_yaw_rate = 0.0f;
 
     // if not armed set throttle to zero and exit immediately
@@ -321,11 +317,11 @@ void ModeRTL::descent_run()
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();
 
-            // convert pilot input to lean angles
-            get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
+            // convert pilot input to reposition velocity
+            vel_correction = get_pilot_desired_velocity(wp_nav->get_wp_acceleration() * 0.5);
 
             // record if pilot has overridden roll or pitch
-            if (!is_zero(target_roll) || !is_zero(target_pitch)) {
+            if (!vel_correction.is_zero()) {
                 if (!copter.ap.land_repo_active) {
                     AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE);
                 }
@@ -342,11 +338,9 @@ void ModeRTL::descent_run()
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
-    // process roll, pitch inputs
-    loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
-
-    // run loiter controller
-    loiter_nav->update();
+    Vector2f accel;
+    pos_control->input_vel_accel_xy(vel_correction, accel);
+    pos_control->update_xy_controller();
 
     // WP_Nav has set the vertical position control targets
     // run the vertical position controller and set output throttle
@@ -354,7 +348,7 @@ void ModeRTL::descent_run()
     pos_control->update_z_controller();
 
     // roll & pitch from waypoint controller, yaw rate from pilot
-    attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
+    attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
 
     // check if we've reached within 20cm of final altitude
     _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20;
@@ -366,8 +360,14 @@ void ModeRTL::land_start()
     _state = SubMode::LAND;
     _state_complete = false;
 
-    // Set wp navigation target to above home
-    loiter_nav->init_target(wp_nav->get_wp_destination().xy());
+    // set horizontal speed and acceleration limits
+    pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
+    pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
+
+    // initialise loiter target destination
+    if (!pos_control->is_active_xy()) {
+        pos_control->init_xy_controller();
+    }
 
     // initialise the vertical position controller
     if (!pos_control->is_active_z()) {
@@ -408,8 +408,6 @@ void ModeRTL::land_run(bool disarm_on_land)
     // if not armed set throttle to zero and exit immediately
     if (is_disarmed_or_landed()) {
         make_safe_ground_handling();
-        loiter_nav->clear_pilot_desired_acceleration();
-        loiter_nav->init_target();
         return;
     }
 

ArduCopter/takeoff.cpp

@@ -4,6 +4,11 @@ Mode::_TakeOff Mode::takeoff;
 
 bool Mode::auto_takeoff_no_nav_active = false;
 float Mode::auto_takeoff_no_nav_alt_cm = 0;
+float Mode::auto_take_off_start_alt_cm = 0;
+float Mode::auto_take_off_complete_alt_cm = 0;
+bool Mode::auto_takeoff_terrain_alt = false;
+bool Mode::auto_takeoff_complete = false;
+Vector3p Mode::auto_takeoff_complete_pos;
 
 // This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes.
 //   The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude
@@ -93,13 +98,21 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
     }
 }
 
+// auto_takeoff_run - controls the vertical position controller during the process of taking off in auto modes
+// auto_takeoff_complete set to true when target altitude is within 10% of the take off altitude and less than 50% max climb rate
 void Mode::auto_takeoff_run()
 {
     // if not armed set throttle to zero and exit immediately
     if (!motors->armed() || !copter.ap.auto_armed) {
         // do not spool down tradheli when on the ground with motor interlock enabled
         make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
-        wp_nav->shift_wp_origin_and_destination_to_current_pos_xy();
+        return;
+    }
+
+    // get terrain offset
+    float terr_offset = 0.0f;
+    if (auto_takeoff_terrain_alt && !wp_nav->get_terrain_offset(terr_offset)) {
+        gcs().send_text(MAV_SEVERITY_CRITICAL, "auto takeoff: failed to get terrain offset");
         return;
     }
 
@@ -116,17 +129,18 @@ void Mode::auto_takeoff_run()
         }
     }
 
-    // aircraft stays in landed state until rotor speed runup has finished
+    // aircraft stays in landed state until rotor speed run up has finished
     if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
         set_land_complete(false);
     } else {
         // motors have not completed spool up yet so relax navigation and position controllers
-        wp_nav->shift_wp_origin_and_destination_to_current_pos_xy();
+        pos_control->relax_velocity_controller_xy();
+        pos_control->update_xy_controller();
         pos_control->relax_z_controller(0.0f);   // forces throttle output to decay to zero
         pos_control->update_z_controller();
         attitude_control->reset_yaw_target_and_rate();
         attitude_control->reset_rate_controller_I_terms();
-        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
+        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
         return;
     }
 
@@ -135,51 +149,74 @@ void Mode::auto_takeoff_run()
         // check if vehicle has reached no_nav_alt threshold
         if (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm) {
             auto_takeoff_no_nav_active = false;
-            wp_nav->shift_wp_origin_and_destination_to_stopping_point_xy();
-        } else {
-            // shift the navigation target horizontally to our current position
-            wp_nav->shift_wp_origin_and_destination_to_current_pos_xy();
         }
-        // tell the position controller that we have limited roll/pitch demand to prevent integrator buildup
-        pos_control->set_externally_limited_xy();
-    }
-
-    // run waypoint controller
-    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
-
-    Vector3f thrustvector{0, 0, -GRAVITY_MSS * 100.0f};
-    if (!auto_takeoff_no_nav_active) {
-        thrustvector = wp_nav->get_thrust_vector();
+        pos_control->relax_velocity_controller_xy();
+    } else {
+        Vector2f vel;
+        Vector2f accel;
+        pos_control->input_vel_accel_xy(vel, accel);
     }
+    pos_control->update_xy_controller();
 
-    // WP_Nav has set the vertical position control targets
+    // command the aircraft to the take off altitude
+    float pos_z = auto_take_off_complete_alt_cm + terr_offset;
+    float vel_z = 0.0;
+    copter.pos_control->input_pos_vel_accel_z(pos_z, vel_z, 0.0);
+    
     // run the vertical position controller and set output throttle
-    copter.pos_control->update_z_controller();
+    pos_control->update_z_controller();
 
     // call attitude controller
     if (auto_yaw.mode() == AUTO_YAW_HOLD) {
         // roll & pitch from position controller, yaw rate from pilot
-        attitude_control->input_thrust_vector_rate_heading(thrustvector, target_yaw_rate);
+        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
     } else if (auto_yaw.mode() == AUTO_YAW_RATE) {
         // roll & pitch from position controller, yaw rate from mavlink command or mission item
-        attitude_control->input_thrust_vector_rate_heading(thrustvector, auto_yaw.rate_cds());
+        attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
     } else {
         // roll & pitch from position controller, yaw heading from GCS or auto_heading()
-        attitude_control->input_thrust_vector_heading(thrustvector, auto_yaw.yaw(), auto_yaw.rate_cds());
+        attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
+    }
+
+    // handle takeoff completion
+    bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_take_off_start_alt_cm) >= ((auto_take_off_complete_alt_cm  - auto_take_off_start_alt_cm) * 0.90);
+    bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * 0.1;
+    auto_takeoff_complete = reached_altitude && reached_climb_rate;
+
+    // calculate completion for location in case it is needed for a smooth transition to wp_nav
+    if (auto_takeoff_complete) {
+        const Vector3p& complete_pos = copter.pos_control->get_pos_target_cm();
+        auto_takeoff_complete_pos = Vector3p{complete_pos.x, complete_pos.y, pos_z};
     }
 }
 
-void Mode::auto_takeoff_set_start_alt(void)
+void Mode::auto_takeoff_start(float complete_alt_cm, bool terrain_alt)
 {
+    auto_take_off_start_alt_cm = inertial_nav.get_position_z_up_cm();
+    auto_take_off_complete_alt_cm = complete_alt_cm;
+    auto_takeoff_terrain_alt = terrain_alt;
+    auto_takeoff_complete = false;
     if ((g2.wp_navalt_min > 0) && (is_disarmed_or_landed() || !motors->get_interlock())) {
         // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min
-        auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
+        auto_takeoff_no_nav_alt_cm = auto_take_off_start_alt_cm + g2.wp_navalt_min * 100;
         auto_takeoff_no_nav_active = true;
     } else {
         auto_takeoff_no_nav_active = false;
     }
 }
 
+// return takeoff final position if takeoff has completed successfully
+bool Mode::auto_takeoff_get_position(Vector3p& complete_pos)
+{
+    // only provide location if takeoff has completed
+    if (!auto_takeoff_complete) {
+        return false;
+    }
+
+    complete_pos = auto_takeoff_complete_pos;
+    return true;
+}
+
 bool Mode::is_taking_off() const
 {
     if (!has_user_takeoff(false)) {

ArduCopter/version.h

@@ -6,14 +6,14 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ArduCopter V4.2.0-dev"
+#define THISFIRMWARE "ArduCopter V4.2.0-beta1"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,2,0,FIRMWARE_VERSION_TYPE_DEV
+#define FIRMWARE_VERSION 4,2,0,FIRMWARE_VERSION_TYPE_BETA
 
 #define FW_MAJOR 4
 #define FW_MINOR 2
 #define FW_PATCH 0
-#define FW_TYPE FIRMWARE_VERSION_TYPE_DEV
+#define FW_TYPE FIRMWARE_VERSION_TYPE_BETA
 
 #include <AP_Common/AP_FWVersionDefine.h>

ArduPlane/AP_Arming.cpp

@@ -174,6 +174,16 @@ bool AP_Arming_Plane::quadplane_checks(bool display_failure)
         }
     }
 
+    /*
+      Q_ASSIST_SPEED really should be enabled for all quadplanes except tailsitters
+     */
+    if (check_enabled(ARMING_CHECK_PARAMETERS) &&
+        is_zero(plane.quadplane.assist_speed) &&
+        !plane.quadplane.tailsitter.enabled()) {
+        check_failed(display_failure,"Q_ASSIST_SPEED is not set");
+        ret = false;
+    }
+
     return ret;
 }
 #endif // HAL_QUADPLANE_ENABLED
@@ -275,12 +285,15 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
 bool AP_Arming_Plane::disarm(const AP_Arming::Method method, bool do_disarm_checks)
 {
     if (do_disarm_checks &&
-        method == AP_Arming::Method::RUDDER) {
-        // don't allow rudder-disarming in flight:
+        (method == AP_Arming::Method::MAVLINK ||
+         method == AP_Arming::Method::RUDDER)) {
         if (plane.is_flying()) {
-            // obviously this could happen in-flight so we can't warn about it
+            // don't allow mavlink or rudder disarm while flying
             return false;
         }
+    }
+    
+    if (do_disarm_checks && method == AP_Arming::Method::RUDDER) {
         // option must be enabled:
         if (get_rudder_arming_type() != AP_Arming::RudderArming::ARMDISARM) {
             gcs().send_text(MAV_SEVERITY_INFO, "Rudder disarm: disabled");
@@ -348,3 +361,16 @@ void AP_Arming_Plane::update_soft_armed()
     }
 }
 
+/*
+  extra plane mission checks
+ */
+bool AP_Arming_Plane::mission_checks(bool report)
+{
+    // base checks
+    bool ret = AP_Arming::mission_checks(report);
+    if (plane.mission.get_landing_sequence_start() > 0 && plane.g.rtl_autoland.get() == 0) {
+        ret = false;
+        check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled");
+    }
+    return ret;
+}

ArduPlane/AP_Arming.h

@@ -34,6 +34,7 @@ protected:
     bool ins_checks(bool report) override;
 
     bool quadplane_checks(bool display_failure);
+    bool mission_checks(bool report) override;
 
 private:
     void change_arm_state(void);

ArduPlane/ArduPlane.cpp

@@ -738,6 +738,7 @@ bool Plane::get_target_location(Location& target_loc)
     case Mode::Number::GUIDED:
     case Mode::Number::AUTO:
     case Mode::Number::LOITER:
+    case Mode::Number::TAKEOFF:
 #if HAL_QUADPLANE_ENABLED
     case Mode::Number::QLOITER:
     case Mode::Number::QLAND:
@@ -751,6 +752,39 @@ bool Plane::get_target_location(Location& target_loc)
     }
     return false;
 }
+
+/*
+  update_target_location() works in all auto navigation modes
+ */
+bool Plane::update_target_location(const Location &old_loc, const Location &new_loc)
+{
+    if (!old_loc.same_latlon_as(next_WP_loc)) {
+        return false;
+    }
+    ftype alt_diff;
+    if (!old_loc.get_alt_distance(next_WP_loc, alt_diff) ||
+        !is_zero(alt_diff)) {
+        return false;
+    }
+    next_WP_loc = new_loc;
+    next_WP_loc.change_alt_frame(old_loc.get_alt_frame());
+
+    return true;
+}
+
+// allow for velocity matching in VTOL
+bool Plane::set_velocity_match(const Vector2f &velocity)
+{
+#if HAL_QUADPLANE_ENABLED
+    if (quadplane.in_vtol_mode() || quadplane.in_vtol_land_sequence()) {
+        quadplane.poscontrol.velocity_match = velocity;
+        quadplane.poscontrol.last_velocity_match_ms = AP_HAL::millis();
+        return true;
+    }
+#endif
+    return false;
+}
+
 #endif // AP_SCRIPTING_ENABLED
 
 #if OSD_ENABLED

ArduPlane/Attitude.cpp

@@ -61,6 +61,10 @@ float Plane::calc_speed_scaler(void)
  */
 bool Plane::stick_mixing_enabled(void)
 {
+    if (!rc().has_valid_input()) {
+        // never stick mix without valid RC
+        return false;
+    }
 #if AC_FENCE == ENABLED
     const bool stickmixing = fence_stickmixing();
 #else
@@ -81,10 +85,7 @@ bool Plane::stick_mixing_enabled(void)
         // we're in an auto mode. Check the stick mixing flag
         if (g.stick_mixing != StickMixing::NONE &&
             g.stick_mixing != StickMixing::VTOL_YAW &&
-            stickmixing &&
-            failsafe.state == FAILSAFE_NONE &&
-            !rc_failsafe_active()) {
-            // we're in an auto mode, and haven't triggered failsafe
+            stickmixing) {
             return true;
         } else {
             return false;

ArduPlane/GCS_Mavlink.cpp

@@ -172,20 +172,23 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const
     }
 #if HAL_QUADPLANE_ENABLED
     const QuadPlane &quadplane = plane.quadplane;
-    if (quadplane.show_vtol_view()) {
+    if (quadplane.show_vtol_view() && quadplane.using_wp_nav()) {
         const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
-        bool wp_nav_valid = quadplane.using_wp_nav();
+
+        const Vector2f& curr_pos = quadplane.inertial_nav.get_position_xy_cm();
+        const Vector2f& target_pos = quadplane.pos_control->get_pos_target_cm().xy().tofloat();
+        const Vector2f error = (target_pos - curr_pos) * 0.01;
 
         mavlink_msg_nav_controller_output_send(
             chan,
             targets.x * 0.01,
             targets.y * 0.01,
             targets.z * 0.01,
-            wp_nav_valid ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0,
-            wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination() * 0.01, UINT16_MAX) : 0,
+            degrees(error.angle()),
+            MIN(error.length(), UINT16_MAX),
             (plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_pos_error_z_cm() * 0.01 : 0,
             plane.airspeed_error * 100,  // incorrect units; see PR#7933
-            wp_nav_valid ? quadplane.wp_nav->crosstrack_error() : 0);
+            quadplane.wp_nav->crosstrack_error());
         return;
     }
 #endif
@@ -428,6 +431,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
     // @Units: Hz
     // @Range: 0 50
     // @Increment: 1
+    // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0],  1),
 
@@ -437,6 +441,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
     // @Units: Hz
     // @Range: 0 50
     // @Increment: 1
+    // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1],  1),
 
@@ -446,6 +451,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
     // @Units: Hz
     // @Range: 0 50
     // @Increment: 1
+    // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("RC_CHAN",  2, GCS_MAVLINK_Parameters, streamRates[2],  1),
 
@@ -455,6 +461,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
     // @Units: Hz
     // @Range: 0 50
     // @Increment: 1
+    // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3],  1),
 
@@ -464,6 +471,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
     // @Units: Hz
     // @Range: 0 50
     // @Increment: 1
+    // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4],  1),
 
@@ -473,6 +481,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
     // @Units: Hz
     // @Range: 0 50
     // @Increment: 1
+    // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("EXTRA1",   5, GCS_MAVLINK_Parameters, streamRates[5],  1),
 
@@ -482,6 +491,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
     // @Units: Hz
     // @Range: 0 50
     // @Increment: 1
+    // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("EXTRA2",   6, GCS_MAVLINK_Parameters, streamRates[6],  1),
 
@@ -491,6 +501,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
     // @Units: Hz
     // @Range: 0 50
     // @Increment: 1
+    // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("EXTRA3",   7, GCS_MAVLINK_Parameters, streamRates[7],  1),
 
@@ -500,6 +511,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
     // @Units: Hz
     // @Range: 0 50
     // @Increment: 1
+    // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK_Parameters, streamRates[8],  10),
 
@@ -509,6 +521,7 @@ const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
     // @Units: Hz
     // @Range: 0 50
     // @Increment: 1
+    // @RebootRequired: True
     // @User: Advanced
     AP_GROUPINFO("ADSB",   9, GCS_MAVLINK_Parameters, streamRates[9],  5),
     AP_GROUPEND
@@ -642,6 +655,10 @@ void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status,
 {
 #if HAL_ADSB_ENABLED
     plane.avoidance_adsb.handle_msg(msg);
+#endif
+#if AP_SCRIPTING_ENABLED
+    // pass message to follow library
+    plane.g2.follow.handle_msg(msg);
 #endif
     GCS_MAVLINK::packetReceived(status, msg);
 }
@@ -874,6 +891,16 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in
     case MAV_CMD_GUIDED_CHANGE_HEADING:
         return handle_command_int_guided_slew_commands(packet);
 
+    case MAV_CMD_DO_FOLLOW:
+#if AP_SCRIPTING_ENABLED
+        // param1: sysid of target to follow
+        if ((packet.param1 > 0) && (packet.param1 <= 255)) {
+            plane.g2.follow.set_target_sysid((uint8_t)packet.param1);
+            return MAV_RESULT_ACCEPTED;
+        }
+#endif
+        return MAV_RESULT_FAILED;
+        
     default:
         return GCS_MAVLINK::handle_command_int_packet(packet);
     }
@@ -1075,6 +1102,16 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
         }
         return MAV_RESULT_ACCEPTED;
 
+#if AP_SCRIPTING_ENABLED
+    case MAV_CMD_DO_FOLLOW:
+        // param1: sysid of target to follow
+        if ((packet.param1 > 0) && (packet.param1 <= 255)) {
+            plane.g2.follow.set_target_sysid((uint8_t)packet.param1);
+            return MAV_RESULT_ACCEPTED;
+        }
+        return MAV_RESULT_FAILED;
+#endif
+        
     default:
         return GCS_MAVLINK::handle_command_long_packet(packet);
     }

ArduPlane/Log.cpp

@@ -23,7 +23,7 @@ void Plane::Log_Write_Attitude(void)
     } else {
         ahrs.Write_Attitude(targets);
     }
-    if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) {
+    if (AP_HAL::millis() - quadplane.last_att_control_ms < 100) {
         // log quadplane PIDs separately from fixed wing PIDs
         logger.Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info());
         logger.Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info());

ArduPlane/Parameters.cpp

@@ -1229,6 +1229,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15
     AP_GROUPINFO("ONESHOT_MASK", 32, ParametersG2, oneshot_mask, 0),
+
+#if AP_SCRIPTING_ENABLED
+    // @Group: FOLL
+    // @Path: ../libraries/AP_Follow/AP_Follow.cpp
+    AP_SUBGROUPINFO(follow, "FOLL", 33, ParametersG2, AP_Follow),
+#endif
     
     AP_GROUPEND
 };

ArduPlane/Parameters.h

@@ -545,6 +545,9 @@ public:
     AC_PID guidedHeading{5000.0,  0.0,   0.0, 0 ,  10.0,   5.0,  5.0 ,  5.0  , 0.2};
 #endif
 
+#if AP_SCRIPTING_ENABLED
+    AP_Follow follow;
+#endif
 
     AP_Float        fs_ekf_thresh;
 

ArduPlane/Plane.h

@@ -82,6 +82,7 @@
 #include <AP_Gripper/AP_Gripper.h>
 #include <AP_Landing/AP_Landing.h>
 #include <AP_LandingGear/AP_LandingGear.h>     // Landing Gear library
+#include <AP_Follow/AP_Follow.h>
 
 #include "GCS_Mavlink.h"
 #include "GCS_Plane.h"
@@ -1209,6 +1210,8 @@ public:
 #if AP_SCRIPTING_ENABLED
     bool set_target_location(const Location& target_loc) override;
     bool get_target_location(Location& target_loc) override;
+    bool update_target_location(const Location &old_loc, const Location &new_loc) override;
+    bool set_velocity_match(const Vector2f &velocity) override;
 #endif // AP_SCRIPTING_ENABLED
 
 };

ArduPlane/ReleaseNotes.txt

@@ -1,3 +1,111 @@
+Release 4.2.0beta3 18th March 2022
+----------------------------------
+
+This is the 3rd beta of the 4.2.0 major release. This beta should be
+close to the final 4.2.0 release.
+
+ - fixed pitch envelope constraint after AIRBRAKE
+ - improvements to POSITION1 quadplane landing controller
+ - added arming check for Q_ASSIST_SPEED
+ - added warning if arming with ARMING_CHECK=0
+ - added arming check for DO_LAND_START with RTL_AUTOLAND=0
+ - improved throttle mix for quadplane autoland
+ - added fence breach message on GCS
+ - constrain indexing on declination tables
+
+Happy flying!
+
+Release 4.2.0beta2 10th March 2022
+----------------------------------
+
+This is the 2nd beta of the 4.2.0 major release. This beta should be
+close to the final 4.2.0 release.
+
+ - fixed EKF3 constrain bug
+ - added quadplane ship landing support
+ - added Q_LAND_ALTCHG to adjust landing detection sensitivity
+ - fixed a bug in terrain home compensation
+ - changed quaplane POSITION1 landing controller to use jerk and accel limiting
+ - fixed default gain for weathervaning on quadplanes
+ - fixed health check of airspeed sensor for dual sensors
+ - fixed arming check on servos for GPIO outputs
+ - disable stick mixing when RC input is unavailable
+ - fixed stick mixing during GCS failsafe
+ - disallow mavlink disarm while flying
+ - adjust frequency selection for DShot
+ - fixed logging bug for MCU voltage
+ - added BeastH7v2 support
+ - improvement to AC_WPNav for quadplane waypoint control
+ - simplify RC failsafe messages sent to GCS
+
+Happy flying!
+
+Release 4.2.0beta1 2nd March 2022
+---------------------------------
+
+This is a major release with a lot of changes. We expect at least 4
+weeks of beta testing and several beta updates before 4.2.0 final is
+released.
+
+Changes include:
+
+ - EKF startup messages reduced
+ - LORD Microstrain CX5/GX5 external AHRS support
+ - Auto mode supports up to 100 DO_JUMP commands on high memory boards
+ - Auto support for NAV_SCRIPT_TIME commands (Lua within Auto)
+ - aerobatic scripting from any mode via RC switches
+ - new boards AirLink, BeastF7v2, BeastH7v2, JHEMCU GSF405A, KakuteH7, KakuteH7Mini, MambaF405US-I2C, MatekF405-TE, ModalAI fc-v1, PixC4-Jetson, Pixhawk5X, QioTekZealotH743, RPI-Zero2W, SPRacingH7 Extreme, Swan-K1
+ - Parachute option to leave servo in open position (see CHUTE_OPTIONS parameter)
+ - Parachute released arming check added
+ - Pre-arm check of IMU heater temp
+ - Pre-arm check of rangefinder health
+ - Pre-arm check of throttle position skipped if PILOT_THR_BHV is "Feedback from mid stick"
+ - ADIS16470, ADIS16507 and BMI270 IMU support
+ - AK09918 compass support
+ - Battery monitor supports voltage offset (see BATTx_VLT_OFFSET)
+ - Benewake TFMiniPlus I2C address defaults correctly
+ - Buzzer can be connected to any PWM on any board
+ - Compass calibration (in-flight) uses GSF for better accuracy
+ - CRSFv3 support, CSRF telemetry link reports link quality in RSSI
+ - Cybot D1 Lidar support
+ - DroneCan (aka UAVCAN) battery monitors support scaling (see BATTx_CURR_MULT)
+ - DroneCan (aka UAVCAN) GPS-for-yaw support
+ - Electronic Fuel Injection support incl Lutan EFI
+ - FETtecOneWire resyncs if EMI causes lost bytes
+ - IMU heater params renamed to BRD_HEAT_xxx
+ - Landing gear enable parameter added (see LGR_ENABLE)
+ - Lightware SF40C ver 0.9 support removed (ver 1.0 and higher still supported)
+ - Maxbotix serial sonar driver support RNGFNDx_SCALING parameter to support for varieties of sensor
+ - MPPT solar charge controller support
+ - MTK GPS driver removed
+ - Optical flow in-flight calibration
+ - Ping200x ADSB support
+ - Proximity sensor min and max range (see PRX_MIN, PRX_MAX)
+ - QSPI external flash support
+ - uLanding (aka USD1) radar provides average of last few samples
+ - Unicore NMEA GPS support for yaw and 3D velocity
+ - Board ID sent in AUTOPILOT_VERSION mavlink message
+ - DO_SET_CAM_TRIG_DIST supports instantly triggering camera
+ - DJI FPV OSD multi screen and stats support
+ - GPIO pins configured by setting SERVOx_FUNCTION to -1 (also see SERVO_GPIO_MASK. BRD_PWM_COUNT removed)
+ - GPIO pin support on main outputs on boards with IOMCU
+ - GyroFlow logging (see LOG_BITMASK's "VideoStabilization" option)
+ - Firmware version logged in VER message
+ - SD card format via MAVLink
+ - Serial port option to disable changes to stream rate (see SERIALx_OPTIONS)
+ - VIBE logging units to m/s/s
+ - BLHeli passthrough reliability improvements
+ - Compass learning (inflight) fixed to ignore unused compasses (e.g. those with COMPASS_USE = 0)
+ - EKF optical flow terrain alt reset fixed (large changes in rangefinder alt might never be fused)
+ - EKF resets due to bad IMU data occur at most once per second
+ - GPIO pin fix on CubeOrange, F4BY, mRoControlZeroF7, R9Pilot
+ - MAVlink2 serial ports always send MAVLink2 messages (previously waited until other side sent MAVLink2)
+ - Omnibusf4pro bi-directional dshot fix
+ - Real-Time-Clock (RTC) oldest possible date updated to Jan-2022
+
+Happy flying!
+
+
 Release 4.1.7 12th February 2022
 --------------------------------
 

ArduPlane/altitude.cpp

@@ -628,8 +628,8 @@ void Plane::rangefinder_terrain_correction(float &height)
         return;
     }
     float terrain_amsl1, terrain_amsl2;
-    if (!terrain.height_amsl(current_loc, terrain_amsl1, false) ||
-        !terrain.height_amsl(next_WP_loc, terrain_amsl2, false)) {
+    if (!terrain.height_amsl(current_loc, terrain_amsl1) ||
+        !terrain.height_amsl(next_WP_loc, terrain_amsl2)) {
         return;
     }
     float correction = (terrain_amsl1 - terrain_amsl2);

ArduPlane/control_modes.cpp

@@ -107,9 +107,8 @@ void Plane::read_control_switch()
     // If we get this value we do not want to change modes.
     if(switchPosition == 255) return;
 
-    if (failsafe.rc_failsafe || failsafe.throttle_counter > 0) {
-        // when we are in rc_failsafe mode then RC input is not
-        // working, and we need to ignore the mode switch channel
+    if (!rc().has_valid_input()) {
+        // ignore the mode switch channel if there is no valid RC input
         return;
     }
 

ArduPlane/events.cpp

@@ -21,7 +21,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
     failsafe.state = fstype;
     failsafe.short_timer_ms = millis();
     failsafe.saved_mode_number = control_mode->mode_number();
-    gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on: type=%u/reason=%u", fstype, static_cast<unsigned>(reason));
+    gcs().send_text(MAV_SEVERITY_WARNING, "RC Short Failsafe On");
     switch (control_mode->mode_number())
     {
     case Mode::Number::MANUAL:
@@ -102,7 +102,12 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype, ModeReason reaso
 void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason)
 {
     // This is how to handle a long loss of control signal failsafe.
-    gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on: type=%u/reason=%u", fstype, static_cast<unsigned>(reason));
+    if (reason == ModeReason:: GCS_FAILSAFE) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe On");
+    }
+    else {
+        gcs().send_text(MAV_SEVERITY_WARNING, "RC Long Failsafe On");
+    }
     //  If the GCS is locked up we allow control to revert to RC
     RC_Channels::clear_overrides();
     failsafe.state = fstype;
@@ -186,7 +191,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason
 void Plane::failsafe_short_off_event(ModeReason reason)
 {
     // We're back in radio contact
-    gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off: reason=%u", static_cast<unsigned>(reason));
+    gcs().send_text(MAV_SEVERITY_WARNING, "Short Failsafe Cleared");
     failsafe.state = FAILSAFE_NONE;
     //restore entry mode if desired but check that our current mode is still due to failsafe
     if ( _last_reason == ModeReason::RADIO_FAILSAFE) { 
@@ -197,8 +202,13 @@ void Plane::failsafe_short_off_event(ModeReason reason)
 
 void Plane::failsafe_long_off_event(ModeReason reason)
 {
-    // We're back in radio contact
-    gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event off: reason=%u", static_cast<unsigned>(reason));
+    // We're back in radio contact with RC or GCS
+    if (reason == ModeReason:: GCS_FAILSAFE) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Off");
+    }
+    else {
+        gcs().send_text(MAV_SEVERITY_WARNING, "RC Long Failsafe Cleared");
+    }
     failsafe.state = FAILSAFE_NONE;
 }
 

ArduPlane/fence.cpp

@@ -50,6 +50,10 @@ void Plane::fence_check()
         // user disables/re-enables using the fence channel switch
         return;
     }
+    
+     if(new_breaches && plane.is_flying()) {
+         GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence Breached");
+     }
 
     if (new_breaches || orig_breaches) {
         // if the user wants some kind of response and motors are armed

ArduPlane/mode.cpp

@@ -80,6 +80,10 @@ bool Mode::enter()
     // initialize speed variable used in AUTO and GUIDED for DO_CHANGE_SPEED commands
     plane.new_airspeed_cm = -1;
 
+#if HAL_QUADPLANE_ENABLED
+    quadplane.mode_enter();
+#endif
+
     bool enter_result = _enter();
 
     if (enter_result) {

ArduPlane/quadplane.cpp

@@ -103,7 +103,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
 
     // @Param: ASSIST_SPEED
     // @DisplayName: Quadplane assistance speed
-    // @Description: This is the speed below which the quad motors will provide stability and lift assistance in fixed wing modes. Zero means no assistance except during transition
+    // @Description: This is the speed below which the quad motors will provide stability and lift assistance in fixed wing modes. Zero means no assistance except during transition. Note that if this is set to zero then other Q_ASSIST features are also disabled. A higher value will lead to more false positives which can waste battery. A lower value will result in less false positive, but will result in assistance taking longer to trigger. If unsure then set to 3 m/s below the minimum airspeed you will fly at. If you don't have an airspeed sensor then use 5 m/s below the minimum airspeed you fly at. If you want to disable the arming check Q_ASSIST_SPEED then set to -1.
     // @Units: m/s
     // @Range: 0 100
     // @Increment: 0.1
@@ -434,6 +434,15 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @Path: ../libraries/AC_AttitudeControl/AC_WeatherVane.cpp
     AP_SUBGROUPPTR(weathervane, "WVANE_", 30, QuadPlane, AC_WeatherVane),
 
+    // @Param: LAND_ALTCHG
+    // @DisplayName: Land detection altitude change threshold
+    // @Description: The maximum altitude change allowed during land detection. You can raise this value if you find that landing detection takes a long time to complete. It is the maximum change in altitude over a period of 4 seconds for landing to be detected
+    // @Units: m
+    // @Range: 0.1 0.6
+    // @Increment: 0.05
+    // @User: Standard
+    AP_GROUPINFO("LAND_ALTCHG", 31, QuadPlane, landing_detect.detect_alt_change, 0.2),
+    
     AP_GROUPEND
 };
 
@@ -461,6 +470,7 @@ static const struct AP_Param::defaults_table_struct defaults_table[] = {
     { "Q_LOIT_SPEED",     500 },
     { "Q_WP_SPEED",       500 },
     { "Q_WP_ACCEL",       100 },
+    { "Q_P_JERK_XY",      2   },
 };
 
 /*
@@ -1224,7 +1234,8 @@ float QuadPlane::get_desired_yaw_rate_cds(bool should_weathervane)
 // get pilot desired climb rate in cm/s
 float QuadPlane::get_pilot_desired_climb_rate_cms(void) const
 {
-    if (plane.failsafe.rc_failsafe || plane.failsafe.throttle_counter > 0) {
+    if (!rc().has_valid_input()) {
+        // no valid input means no sensible pilot desired climb rate.
         // descend at 0.5m/s for now
         return -50;
     }
@@ -1623,16 +1634,14 @@ void SLT_Transition::update()
             quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
             motors->output();
         }
-        last_fw_mode_ms = now;
-        last_fw_nav_pitch_cd = plane.nav_pitch_cd;
+        set_last_fw_pitch();
         in_forced_transition = false;
         return;
     }
 
     quadplane.motors_output();
 
-    last_fw_mode_ms = now;
-    last_fw_nav_pitch_cd = plane.nav_pitch_cd;
+    set_last_fw_pitch();
 }
 
 void SLT_Transition::VTOL_update()
@@ -1704,7 +1713,7 @@ void QuadPlane::update(void)
     }
 
     const uint32_t now = AP_HAL::millis();
-    if (!in_vtol_mode()) {
+    if (!in_vtol_mode() && !in_vtol_airbrake()) {
         // we're in a fixed wing mode, cope with transitions and check
         // for assistance needed
         if (plane.control_mode == &plane.mode_manual ||
@@ -1723,7 +1732,7 @@ void QuadPlane::update(void)
 
     } else {
 
-        assisted_flight = false;
+        assisted_flight = in_vtol_airbrake();
 
         // output to motors
         motors_output();
@@ -1735,8 +1744,7 @@ void QuadPlane::update(void)
     // disable throttle_wait when throttle rises above 10%
     if (throttle_wait &&
         (plane.get_throttle_input() > 10 ||
-         plane.failsafe.rc_failsafe ||
-         plane.failsafe.throttle_counter>0)) {
+         !rc().has_valid_input())) {
         throttle_wait = false;
     }
 
@@ -2002,6 +2010,9 @@ bool QuadPlane::in_vtol_auto(void) const
 /*
   are we in a VTOL mode? This is used to decide if we run the
   transition handling code or not
+
+  note that AIRBRAKE is not considered in_vtol_mode even though the
+  VTOL motors are running
  */
 bool QuadPlane::in_vtol_mode(void) const
 {
@@ -2009,7 +2020,7 @@ bool QuadPlane::in_vtol_mode(void) const
         return false;
     }
     if (in_vtol_land_sequence()) {
-        return poscontrol.get_state() != QPOS_APPROACH;
+        return poscontrol.get_state() != QPOS_APPROACH && poscontrol.get_state() != QPOS_AIRBRAKE;
     }
     if (plane.control_mode->is_vtol_mode()) {
         return true;
@@ -2024,7 +2035,7 @@ bool QuadPlane::in_vtol_mode(void) const
         return true;
     }
     if (in_vtol_auto()) {
-        if (!plane.auto_state.vtol_loiter || poscontrol.get_state() > QPOS_APPROACH) {
+        if (!plane.auto_state.vtol_loiter || poscontrol.get_state() > QPOS_AIRBRAKE) {
             return true;
         }
     }
@@ -2073,7 +2084,8 @@ void QuadPlane::update_land_positioning(void)
     poscontrol.target_vel_cms = Vector3f(-pitch_in, roll_in, 0) * speed_max_cms;
     poscontrol.target_vel_cms.rotate_xy(ahrs_view->yaw);
 
-    poscontrol.target_cm += (poscontrol.target_vel_cms * dt).topostype();
+    // integrate our corrected position
+    poscontrol.xy_correction += poscontrol.target_vel_cms.xy() * dt * 0.01;
 
     poscontrol.pilot_correction_active = (!is_zero(roll_in) || !is_zero(pitch_in));
     if (poscontrol.pilot_correction_active) {
@@ -2084,13 +2096,20 @@ void QuadPlane::update_land_positioning(void)
 /*
   run (and possibly init) xy controller
  */
-void QuadPlane::run_xy_controller(void)
+void QuadPlane::run_xy_controller(float accel_limit)
 {
+    float accel_cmss = wp_nav->get_wp_acceleration();
+    if (is_positive(accel_limit)) {
+        // allow for accel limit override
+        accel_cmss = MAX(accel_cmss, accel_limit*100);
+    }
+    const float speed_cms = wp_nav->get_default_speed_xy();
+    pos_control->set_max_speed_accel_xy(speed_cms, accel_cmss);
+    pos_control->set_correction_speed_accel_xy(speed_cms, accel_cmss);
     if (!pos_control->is_active_xy()) {
-        pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
-        pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
         pos_control->init_xy_controller();
     }
+    pos_control->set_lean_angle_max_cd(MIN(4500, MAX(accel_to_angle(accel_limit)*100, aparm.angle_max)));
     pos_control->update_xy_controller();
 }
 
@@ -2111,8 +2130,13 @@ void QuadPlane::poscontrol_init_approach(void)
             if (tailsitter.enabled() || motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
                 gcs().send_text(MAV_SEVERITY_INFO,"VTOL Position1 d=%.1f", dist);
                 poscontrol.set_state(QPOS_POSITION1);
+                transition->set_last_fw_pitch();
             } else {
-                gcs().send_text(MAV_SEVERITY_INFO,"VTOL short d=%.1f", dist);
+                gcs().send_text(MAV_SEVERITY_INFO,"VTOL airbrake v=%.1f d=%.0f sd=%.0f h=%.1f",
+                                plane.ahrs.groundspeed(),
+                                dist,
+                                stopping_distance(),
+                                plane.relative_ground_altitude(plane.g.rangefinder_landing));
                 poscontrol.set_state(QPOS_AIRBRAKE);
             }
         } else {
@@ -2121,6 +2145,8 @@ void QuadPlane::poscontrol_init_approach(void)
         }
         poscontrol.thrust_loss_start_ms = 0;
     }
+    poscontrol.pilot_correction_done = false;
+    poscontrol.xy_correction.zero();
 }
 
 /*
@@ -2128,10 +2154,12 @@ void QuadPlane::poscontrol_init_approach(void)
  */
 void QuadPlane::log_QPOS(void)
 {
-    AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist", "QBf",
+    AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist,TSpd,TAcc", "QBfff",
                                 AP_HAL::micros64(),
                                 poscontrol.get_state(),
-                                plane.auto_state.wp_distance);
+                                plane.auto_state.wp_distance,
+                                poscontrol.target_speed,
+                                poscontrol.target_accel);
 }
 
 /*
@@ -2139,6 +2167,7 @@ void QuadPlane::log_QPOS(void)
  */
 void QuadPlane::PosControlState::set_state(enum position_control_state s)
 {
+    const uint32_t now = AP_HAL::millis();
     if (state != s) {
         auto &qp = plane.quadplane;
         pilot_correction_done = false;
@@ -2148,29 +2177,30 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
             // never do a rate reset, if attitude control is not active it will be automaticaly reset before running, see: last_att_control_ms
             // if it is active then the rate control should not be reset at all
             qp.attitude_control->reset_yaw_target_and_rate(false);
-            pos1_start_speed = plane.ahrs.groundspeed_vector().length();
-        } else if (s == QPOS_POSITION2) {
-            // POSITION2 changes target speed, so we need to change it
-            // back to normal
-            qp.pos_control->set_max_speed_accel_xy(qp.wp_nav->get_default_speed_xy(),
-                                                   qp.wp_nav->get_wp_acceleration());
-            qp.pos_control->set_correction_speed_accel_xy(qp.wp_nav->get_default_speed_xy(),
-                                                   qp.wp_nav->get_wp_acceleration());
+            pos1_speed_limit = plane.ahrs.groundspeed_vector().length();
+            done_accel_init = false;
         } else if (s == QPOS_AIRBRAKE) {
             // start with zero integrator on vertical throttle
             qp.pos_control->get_accel_z_pid().set_integrator(0);
         } else if (s == QPOS_LAND_DESCEND) {
             // reset throttle descent control
             qp.thr_ctrl_land = false;
+        } else if (s == QPOS_LAND_FINAL) {
+            // remember last pos reset to handle GPS glitch in LAND_FINAL
+            Vector2f rpos;
+            last_pos_reset_ms = plane.ahrs.getLastPosNorthEastReset(rpos);
         }
+        // double log to capture the state change
+        qp.log_QPOS();
+        state = s;
         qp.log_QPOS();
+        last_log_ms = now;
     }
-    state = s;
-    last_state_change_ms = AP_HAL::millis();
+    last_state_change_ms = now;
 
     // we consider setting the state to be equivalent to running to
     // prevent code from overriding the state as stale
-    last_run_ms = last_state_change_ms;
+    last_run_ms = now;
 }
 
 /*
@@ -2186,10 +2216,10 @@ void QuadPlane::vtol_position_controller(void)
     uint32_t now_ms = AP_HAL::millis();
 
     // distance that we switch to QPOS_POSITION2
-    const float position2_dist_threshold = 5.0;
+    const float position2_dist_threshold = 10.0;
 
     // target speed when we reach position2 threshold
-    const float position2_target_speed = 2.0;
+    const float position2_target_speed = 3.0;
 
     if (hal.util->get_soft_armed()) {
         poscontrol.last_run_ms = now_ms;
@@ -2199,6 +2229,11 @@ void QuadPlane::vtol_position_controller(void)
     // and tilt is more than tilt max
     bool suppress_z_controller = false;
 
+    Vector2f landing_velocity;
+    if (now_ms - poscontrol.last_velocity_match_ms < 1000) {
+        landing_velocity = poscontrol.velocity_match;
+    }
+
     // horizontal position control
     switch (poscontrol.get_state()) {
 
@@ -2288,6 +2323,7 @@ void QuadPlane::vtol_position_controller(void)
                                 stop_distance,
                                 plane.relative_ground_altitude(plane.g.rangefinder_landing));
                 poscontrol.set_state(QPOS_POSITION1);
+                transition->set_last_fw_pitch();
             } else {
                 gcs().send_text(MAV_SEVERITY_INFO,"VTOL airbrake v=%.1f d=%.0f sd=%.0f h=%.1f",
                                 groundspeed,
@@ -2312,6 +2348,7 @@ void QuadPlane::vtol_position_controller(void)
         if (poscontrol.get_state() == QPOS_AIRBRAKE &&
             poscontrol.time_since_state_start_ms() > min_airbrake_ms &&
             (aspeed < aspeed_threshold ||
+             fabsf(degrees(closing_vel.angle(desired_closing_vel))) > 60 ||
              closing_speed > MAX(desired_closing_speed*1.2, desired_closing_speed+2) ||
              labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd ||
              labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) > attitude_error_threshold_cd)) {
@@ -2321,6 +2358,7 @@ void QuadPlane::vtol_position_controller(void)
                             plane.relative_ground_altitude(plane.g.rangefinder_landing),
                             desired_closing_speed);
             poscontrol.set_state(QPOS_POSITION1);
+            transition->set_last_fw_pitch();
 
             // switch to vfwd for throttle control
             vel_forward.integrator = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
@@ -2344,6 +2382,7 @@ void QuadPlane::vtol_position_controller(void)
                     gcs().send_text(MAV_SEVERITY_INFO,"VTOL pos1 thrust loss as=%.1f at=%.1f",
                                     aspeed, aspeed_threshold);
                     poscontrol.set_state(QPOS_POSITION1);
+                    transition->set_last_fw_pitch();
                 }
             } else {
                 poscontrol.thrust_loss_start_ms = 0;
@@ -2355,6 +2394,7 @@ void QuadPlane::vtol_position_controller(void)
                 gcs().send_text(MAV_SEVERITY_INFO,"VTOL pos1 low speed as=%.1f at=%.1f",
                                 aspeed, aspeed_threshold);
                 poscontrol.set_state(QPOS_POSITION1);
+                transition->set_last_fw_pitch();
             }
         }
 
@@ -2374,6 +2414,8 @@ void QuadPlane::vtol_position_controller(void)
 
         const Vector2f diff_wp = plane.current_loc.get_distance_NE(loc);
         const float distance = diff_wp.length();
+        const Vector2f rel_groundspeed_vector = landing_closing_velocity();
+        const float rel_groundspeed_sq = rel_groundspeed_vector.length_squared();
 
         // calculate speed we should be at to reach the position2
         // target speed at the position2 distance threshold, assuming
@@ -2383,15 +2425,16 @@ void QuadPlane::vtol_position_controller(void)
         float target_speed = stopping_speed;
 
         // maximum configured VTOL speed
-        const float wp_speed = wp_nav->get_default_speed_xy() * 0.01;
-        const float current_speed_sq = plane.ahrs.groundspeed_vector().length_squared();
+        const float wp_speed = MAX(1.0, wp_nav->get_default_speed_xy() * 0.01);
         const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle()));
 
-        // limit target speed to initial pos1 speed, but at least twice the Q_WP_SPEED
-        target_speed = MIN(MAX(poscontrol.pos1_start_speed, 2*wp_speed), target_speed);
+        // limit target speed to a the pos1 speed limit, which starts out at the initial speed
+        // but is adjusted if we start putting our nose down. We always allow at least twice
+        // the WP speed
+        target_speed = MIN(MAX(poscontrol.pos1_speed_limit, 2*wp_speed), target_speed);
 
         if (poscontrol.reached_wp_speed ||
-            current_speed_sq < sq(wp_speed) ||
+            rel_groundspeed_sq < sq(wp_speed) ||
             wp_speed > 1.35*scaled_wp_speed) {
             // once we get below the Q_WP_SPEED then we don't want to
             // speed up again. At that point we should fly within the
@@ -2407,26 +2450,46 @@ void QuadPlane::vtol_position_controller(void)
         plane.nav_controller->update_waypoint(plane.current_loc, loc);
 
         Vector2f target_speed_xy_cms;
+        Vector2f target_accel_cms;
+        const float target_accel = accel_needed(distance, rel_groundspeed_sq);
         if (distance > 0.1) {
-            target_speed_xy_cms = diff_wp.normalized() * target_speed * 100;
+            Vector2f diff_wp_norm = diff_wp.normalized();
+            target_speed_xy_cms = diff_wp_norm * target_speed * 100;
+            target_accel_cms = diff_wp_norm * (-target_accel*100);
         }
-        if (!tailsitter.enabled()) {
-            // this method ignores pos-control velocity/accel limtis
-            pos_control->set_vel_desired_xy_cms(target_speed_xy_cms);
-
-            // reset position controller xy target to current position
-            // because we only want velocity control (no position control)
-            const Vector2f& curr_pos = inertial_nav.get_position_xy_cm();
-            pos_control->set_pos_target_xy_cm(curr_pos.x, curr_pos.y);
-            pos_control->set_accel_desired_xy_cmss(Vector2f());
-        } else {
-            // tailsitters use input shaping and abide by velocity limits
-            pos_control->input_vel_accel_xy(target_speed_xy_cms, Vector2f());
+        const float target_speed_ms = target_speed_xy_cms.length() * 0.01;
+
+        target_speed_xy_cms += landing_velocity * 100;
+        poscontrol.target_speed = target_speed_ms;
+        poscontrol.target_accel = target_accel;
+
+        if (!poscontrol.reached_wp_speed &&
+            rel_groundspeed_sq < sq(target_speed_ms) &&
+            rel_groundspeed_sq > sq(2*wp_speed) &&
+            plane.nav_pitch_cd < 0) {
+            // we have slowed down more than expected, likely due to
+            // drag from the props and we're starting to put our nose
+            // down as a result. We want to accept the slowdown and
+            // re-calculate the target speed profile
+            poscontrol.pos1_speed_limit = sqrtf(rel_groundspeed_sq);
         }
 
+        // use input shaping and abide by accel and jerk limits
+        pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel_cms);
+
         // run horizontal velocity controller
-        run_xy_controller();
+        run_xy_controller(MAX(target_accel, transition_decel)*1.5);
 
+        if (!poscontrol.done_accel_init) {
+            /*
+              the pos controller init assumes zero accel, we need to
+              override that so that we can start decelerating more
+              quickly at the start of POSITION1
+             */
+            poscontrol.done_accel_init = true;
+            pos_control->set_accel_desired_xy_cmss(target_accel_cms);
+        }
+        
         // nav roll and pitch are controller by position controller
         plane.nav_roll_cd = pos_control->get_roll_cd();
         plane.nav_pitch_cd = pos_control->get_pitch_cd();
@@ -2452,36 +2515,20 @@ void QuadPlane::vtol_position_controller(void)
 
     case QPOS_POSITION2:
     case QPOS_LAND_DESCEND: {
+        setup_target_position();
         /*
           for final land repositioning and descent we run the position controller
          */
-        if (poscontrol.pilot_correction_done) {
-            // if the pilot has repositioned the vehicle then we switch to velocity control.  This prevents the vehicle
-            // shifting position in the event of GPS glitches.
-            Vector2f zero;
-            pos_control->input_vel_accel_xy(poscontrol.target_vel_cms.xy(), zero);
-        } else {
-            Vector2f zero;
-            pos_control->input_pos_vel_accel_xy(poscontrol.target_cm.xy(), zero, zero);
-        }
+        Vector2f zero;
+        Vector2f vel_cms = poscontrol.target_vel_cms.xy() + landing_velocity*100;
+        pos_control->input_pos_vel_accel_xy(poscontrol.target_cm.xy(), vel_cms, zero);
 
         // also run fixed wing navigation
         plane.nav_controller->update_waypoint(plane.current_loc, loc);
 
         update_land_positioning();
 
-        /*
-          apply the same asymmetric speed limits from POSITION1, so we
-          don't suddenly speed up when we change to POSITION2 and
-          LAND_DESCEND
-         */
-        const Vector2f diff_wp = plane.current_loc.get_distance_NE(loc);
-        const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle()));
-
-        pos_control->set_max_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration());
-        pos_control->set_correction_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration());
-
-        run_xy_controller();
+        run_xy_controller(transition_decel*1.5);
 
         // nav roll and pitch are controlled by position controller
         plane.nav_roll_cd = pos_control->get_roll_cd();
@@ -2505,9 +2552,23 @@ void QuadPlane::vtol_position_controller(void)
         if (should_relax()) {
             pos_control->relax_velocity_controller_xy();
         } else {
-            // we use velocity control in QPOS_LAND_FINAL to allow for GPS glitch handling
             Vector2f zero;
-            pos_control->input_vel_accel_xy(poscontrol.target_vel_cms.xy(), zero);
+            Vector2f vel_cms = poscontrol.target_vel_cms.xy() + landing_velocity*100;
+            Vector2f rpos;
+            const uint32_t last_reset_ms = plane.ahrs.getLastPosNorthEastReset(rpos);
+            /* we use velocity control when we may be touching the
+              ground or if we've had a position reset from AHRS. This
+              helps us handle a GPS glitch in the final land phase,
+              and also prevents trying to reposition after touchdown
+            */
+            if (motors->limit.throttle_lower ||
+                motors->get_throttle() < 0.5*motors->get_throttle_hover() ||
+                last_reset_ms != poscontrol.last_pos_reset_ms) {
+                pos_control->input_vel_accel_xy(vel_cms, zero);
+            } else {
+                // otherwise use full pos control
+                pos_control->input_pos_vel_accel_xy(poscontrol.target_cm.xy(), vel_cms, zero);
+            }
         }
 
         run_xy_controller();
@@ -2668,20 +2729,12 @@ void QuadPlane::setup_target_position(void)
         set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     }
 
-    const Vector2f diff2d = origin.get_distance_NE(loc);
+    Vector2f diff2d = origin.get_distance_NE(loc);
+    diff2d += poscontrol.xy_correction;
     poscontrol.target_cm.x = diff2d.x * 100;
     poscontrol.target_cm.y = diff2d.y * 100;
     poscontrol.target_cm.z = plane.next_WP_loc.alt - origin.alt;
 
-    const uint32_t now = AP_HAL::millis();
-    if (!loc.same_latlon_as(last_auto_target) ||
-        plane.next_WP_loc.alt != last_auto_target.alt ||
-        now - last_loiter_ms > 500) {
-        wp_nav->set_wp_destination(poscontrol.target_cm.tofloat());
-        last_auto_target = loc;
-    }
-    last_loiter_ms = now;
-
     // set vertical speed and acceleration limits
     pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
     pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
@@ -2692,18 +2745,31 @@ void QuadPlane::setup_target_position(void)
  */
 void QuadPlane::takeoff_controller(void)
 {
+    if (!hal.util->get_soft_armed()) {
+        return;
+    }
+
+    if (plane.control_mode == &plane.mode_guided && guided_takeoff
+        && tiltrotor.enabled() && !tiltrotor.fully_up()) {
+        // waiting for motors to tilt up
+        return;
+    }
+
     /*
       for takeoff we use the position controller
     */
     setup_target_position();
 
-    // set position controller desired velocity and acceleration to zero
-    pos_control->set_vel_desired_xy_cms(Vector2f());
-    pos_control->set_accel_desired_xy_cmss(Vector2f());
-
     // set position control target and update
-    Vector2f zero;
-    pos_control->input_vel_accel_xy(zero, zero);
+
+    Vector2f vel, zero;
+    if (AP_HAL::millis() - poscontrol.last_velocity_match_ms < 1000) {
+        vel = poscontrol.velocity_match * 100;
+    }
+
+    pos_control->set_accel_desired_xy_cmss(zero);
+    pos_control->set_vel_desired_xy_cms(vel);
+    pos_control->input_vel_accel_xy(vel, zero);
 
     run_xy_controller();
 
@@ -2716,7 +2782,7 @@ void QuadPlane::takeoff_controller(void)
                                                                   get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
 
     float vel_z = wp_nav->get_default_speed_up();
-    if (guided_takeoff) {
+    if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
         // for guided takeoff we aim for a specific height with zero
         // velocity at that height
         Location origin;
@@ -2744,6 +2810,16 @@ void QuadPlane::waypoint_controller(void)
 {
     setup_target_position();
 
+    const Location &loc = plane.next_WP_loc;
+    const uint32_t now = AP_HAL::millis();
+    if (!loc.same_latlon_as(last_auto_target) ||
+        plane.next_WP_loc.alt != last_auto_target.alt ||
+        now - last_loiter_ms > 500) {
+        wp_nav->set_wp_destination(poscontrol.target_cm.tofloat());
+        last_auto_target = loc;
+    }
+    last_loiter_ms = now;
+
     /*
       this is full copter control of auto flight
     */
@@ -2991,7 +3067,7 @@ bool QuadPlane::land_detector(uint32_t timeout_ms)
     // we only consider the vehicle landed when the motors have been
     // at minimum for timeout_ms+1000 and the vertical position estimate has not
     // changed by more than 20cm for timeout_ms
-    if (fabsf(height - landing_detect.vpos_start_m) > 0.2) {
+    if (fabsf(height - landing_detect.vpos_start_m) > landing_detect.detect_alt_change) {
         // height has changed, call off landing detection
         landing_detect.land_start_ms = 0;
         return false;
@@ -3073,10 +3149,19 @@ bool QuadPlane::verify_vtol_land(void)
             const float dist = (inertial_nav.get_position_neu_cm().topostype() - poscontrol.target_cm).xy().length() * 0.01;
             reached_position = dist < descend_dist_threshold;
         }
+        Vector2f target_vel;
+        if (AP_HAL::millis() - poscontrol.last_velocity_match_ms < 1000) {
+            target_vel = poscontrol.velocity_match;
+        }
+        Vector3f vel_ned;
+        UNUSED_RESULT(plane.ahrs.get_velocity_NED(vel_ned));
+        
         if (reached_position &&
-            plane.ahrs.groundspeed() < descend_speed_threshold) {
+            (vel_ned.xy() - target_vel).length() < descend_speed_threshold) {
             poscontrol.set_state(QPOS_LAND_DESCEND);
             poscontrol.pilot_correction_done = false;
+            pos_control->set_lean_angle_max_cd(0);
+            poscontrol.xy_correction.zero();
 #if AC_FENCE == ENABLED
             plane.fence.auto_disable_fence_for_landing();
 #endif
@@ -3419,12 +3504,6 @@ bool QuadPlane::using_wp_nav(void) const
     if (plane.control_mode == &plane.mode_qloiter || plane.control_mode == &plane.mode_qland) {
         return true;
     }
-    if (plane.control_mode == &plane.mode_qrtl && poscontrol.get_state() >= QPOS_POSITION2) {
-        return true;
-    }
-    if (plane.control_mode == &plane.mode_auto && poscontrol.get_state() > QPOS_APPROACH) {
-        return true;
-    }
     return false;
 }
 
@@ -3484,7 +3563,7 @@ bool QuadPlane::in_transition(void) const
 /*
   calculate current stopping distance for a quadplane in fixed wing flight
  */
-float QuadPlane::stopping_distance(float ground_speed_squared)
+float QuadPlane::stopping_distance(float ground_speed_squared) const
 {
     // use v^2/(2*accel). This is only quite approximate as the drag
     // varies with pitch, but it gives something for the user to
@@ -3492,6 +3571,14 @@ float QuadPlane::stopping_distance(float ground_speed_squared)
     return ground_speed_squared / (2 * transition_decel);
 }
 
+/*
+  calculate acceleration needed to stop in the given distance given current speed
+ */
+float QuadPlane::accel_needed(float stop_distance, float ground_speed_squared) const
+{
+    return ground_speed_squared / (2 * MAX(1,stop_distance));
+}
+
 /*
   calculate current stopping distance for a quadplane in fixed wing flight
  */
@@ -3556,7 +3643,17 @@ void QuadPlane::update_throttle_mix(void)
         // check for requested descent
         bool descent_not_demanded = pos_control->get_vel_desired_cms().z >= 0.0f;
 
-        if (large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
+        bool use_mix_max = large_angle_request || large_angle_error || accel_moving || descent_not_demanded;
+
+        /*
+          special case for auto landing, we want a high degree of
+          attitude control until LAND_FINAL
+         */
+        if (in_vtol_land_sequence()) {
+            use_mix_max = !in_vtol_land_final();
+        }
+
+        if (use_mix_max) {
             attitude_control->set_throttle_mix_max(1.0);
         } else {
             attitude_control->set_throttle_mix_min();
@@ -3629,6 +3726,23 @@ bool QuadPlane::in_vtol_land_poscontrol(void) const
     return false;
 }
 
+/*
+  see if we are in the airbrake phase of a VTOL landing
+ */
+bool QuadPlane::in_vtol_airbrake(void) const
+{
+    if (plane.control_mode == &plane.mode_qrtl &&
+        poscontrol.get_state() == QPOS_AIRBRAKE) {
+        return true;
+    }
+    if (plane.control_mode == &plane.mode_auto &&
+        is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
+        poscontrol.get_state() == QPOS_AIRBRAKE) {
+        return true;
+    }
+    return false;
+}
+
 // return true if we should show VTOL view
 bool QuadPlane::show_vtol_view() const
 {
@@ -3670,14 +3784,16 @@ bool QuadPlane::use_fw_attitude_controllers(void) const
 }
 
 /*
-  calculate our closing velocity vector on the landing point. In the
-  future this will take account of the landing point having a
-  velocity
+  calculate our closing velocity vector on the landing point, taking
+  into account target velocity
 */
 Vector2f QuadPlane::landing_closing_velocity()
 {
-    Vector2f vel = ahrs.groundspeed_vector();
-    return vel;
+    Vector2f landing_velocity;
+    if (AP_HAL::millis() - poscontrol.last_velocity_match_ms < 1000) {
+        landing_velocity = poscontrol.velocity_match;
+    }
+    return ahrs.groundspeed_vector() - landing_velocity;
 }
 
 /*
@@ -3784,24 +3900,77 @@ bool SLT_Transition::active() const
     return quadplane.assisted_flight && ((transition_state == TRANSITION_AIRSPEED_WAIT) || (transition_state == TRANSITION_TIMER));
 }
 
+/*
+  limit VTOL roll/pitch in POSITION1, POSITION2 and waypoint controller. This serves three roles:
+   1) an expanding envelope limit on pitch to prevent sudden pitch at the start of a back transition
+
+   2) limiting roll and pitch down to the Q_ANGLE_MAX, as the accel limits may push us beyond that for pitch up.
+      This is needed as the position controller doesn't have separate limits for pitch and roll
+
+   3) preventing us pitching up a lot when our airspeed may be low
+      enough that the real airspeed may be negative, which would result
+      in reversed control surfaces
+ */
 bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_cd)
 {
+    bool ret = false;
+    const int16_t angle_max = quadplane.aparm.angle_max;
+
+    /*
+      we always limit roll to Q_ANGLE_MAX
+     */
+    int32_t new_roll_cd = constrain_int32(roll_cd, -angle_max, angle_max);
+    if (new_roll_cd != roll_cd) {
+        roll_cd = new_roll_cd;
+        ret = true;
+    }
+
+    /*
+      always limit pitch down to Q_ANGLE_MAX. We need to do this as
+      the position controller accel limits may exceed this limit
+     */
+    if (pitch_cd < -angle_max) {
+        pitch_cd = -angle_max;
+        ret = true;
+    }
+
+    /*
+      prevent trying to fly backwards (negative airspeed) at high
+      pitch angles, which can result in a high degree of instability
+      in SLT aircraft. This can happen with a tailwind in a back
+      transition, where the position controller (which is unaware of
+      airspeed) demands high pitch to hit the desired landing point
+     */
+    float airspeed;
+    if (pitch_cd > angle_max &&
+        plane.ahrs.airspeed_estimate(airspeed) && airspeed < 0.5 * plane.aparm.airspeed_min) {
+        const float max_limit_cd = linear_interpolate(angle_max, 4500,
+                                                      airspeed,
+                                                      0, 0.5 * plane.aparm.airspeed_min);
+        if (pitch_cd > max_limit_cd) {
+            pitch_cd = max_limit_cd;
+            ret = true;
+        }
+    }
+
     if (quadplane.back_trans_pitch_limit_ms <= 0) {
-        // disabled
-        return false;
+        // time based pitch envelope disabled
+        return ret;
     }
-    uint32_t limit_time_ms = quadplane.back_trans_pitch_limit_ms;
 
-    const uint32_t now = AP_HAL::millis();
-    if (now - last_fw_mode_ms > limit_time_ms) {
-        // past transition period, nothing to do
-        return false;
+    const uint32_t limit_time_ms = quadplane.back_trans_pitch_limit_ms;
+
+    const uint32_t dt = AP_HAL::millis() - last_fw_mode_ms;
+    if (last_fw_mode_ms == 0 || dt > limit_time_ms) {
+        // we are beyond the time limit, don't apply envelope
+        last_fw_mode_ms = 0;
+        return ret;
     }
 
     // we limit pitch during initial transition
-    float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(quadplane.aparm.angle_max,plane.aparm.pitch_limit_max_cd),
-                                                now,
-                                                last_fw_mode_ms, last_fw_mode_ms+limit_time_ms);
+    const float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(angle_max,plane.aparm.pitch_limit_max_cd),
+                                            dt,
+                                            0, limit_time_ms);
 
     if (pitch_cd > max_limit_cd) {
         pitch_cd = max_limit_cd;
@@ -3819,16 +3988,25 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_
         to prevent inability to progress to position if moving from a loiter
         to landing
     */
-    float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-quadplane.aparm.angle_max,plane.aparm.pitch_limit_min_cd),
-                                            now,
-                                            last_fw_mode_ms, last_fw_mode_ms+limit_time_ms);
+    const float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-angle_max,plane.aparm.pitch_limit_min_cd),
+                                                  dt,
+                                                  0, limit_time_ms);
 
     if (plane.nav_pitch_cd < min_limit_cd) {
         plane.nav_pitch_cd = min_limit_cd;
         return true;
     }
 
-    return false;
+    return ret;
+}
+
+/*
+  remember last fixed wing pitch for pitch envelope in back transition
+ */
+void SLT_Transition::set_last_fw_pitch()
+{
+    last_fw_mode_ms = AP_HAL::millis();
+    last_fw_nav_pitch_cd = plane.nav_pitch_cd;
 }
 
 void SLT_Transition::force_transistion_complete() {
@@ -3836,9 +4014,8 @@ void SLT_Transition::force_transistion_complete() {
     in_forced_transition = false;
     transition_start_ms = 0;
     transition_low_airspeed_ms = 0;
-    last_fw_mode_ms = AP_HAL::millis();
-    last_fw_nav_pitch_cd = plane.nav_pitch_cd;
-};
+    set_last_fw_pitch();
+}
 
 MAV_VTOL_STATE SLT_Transition::get_mav_vtol_state() const
 {
@@ -3864,4 +4041,26 @@ MAV_VTOL_STATE SLT_Transition::get_mav_vtol_state() const
     return MAV_VTOL_STATE_UNDEFINED;
 }
 
+/*
+  see if we are in a VTOL takeoff
+ */
+bool QuadPlane::in_vtol_takeoff(void) const
+{
+    if (in_vtol_auto() && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) {
+        return true;
+    }
+    return false;
+}
+
+// called when we change mode (for any mode, not just Q modes)
+void QuadPlane::mode_enter(void)
+{
+    if (available()) {
+        pos_control->set_lean_angle_max_cd(0);
+    }
+    poscontrol.xy_correction.zero();
+    poscontrol.velocity_match.zero();
+    poscontrol.last_velocity_match_ms = 0;
+}
+
 #endif  // HAL_QUADPLANE_ENABLED

ArduPlane/quadplane.h

@@ -108,6 +108,7 @@ public:
     bool verify_vtol_land(void);
     bool in_vtol_auto(void) const;
     bool in_vtol_mode(void) const;
+    bool in_vtol_takeoff(void) const;
     bool in_vtol_posvel_mode(void) const;
     void update_throttle_hover();
     bool show_vtol_view() const;
@@ -163,6 +164,9 @@ public:
     };
     void set_q_assist_state(Q_ASSIST_STATE_ENUM state) {q_assist_state = state;};
 
+    // called when we change mode (for any mode, not just Q modes)
+    void mode_enter(void);
+
 private:
     AP_AHRS &ahrs;
     AP_Vehicle::MultiCopter aparm;
@@ -261,12 +265,13 @@ private:
     void update_throttle_suppression(void);
 
     void run_z_controller(void);
-    void run_xy_controller(void);
+    void run_xy_controller(float accel_limit=0.0);
 
     void setup_defaults(void);
 
     // calculate a stopping distance for fixed-wing to vtol transitions
-    float stopping_distance(float ground_speed_squared);
+    float stopping_distance(float ground_speed_squared) const;
+    float accel_needed(float stop_distance, float ground_speed_squared) const;
     float stopping_distance(void);
 
     // distance below which we don't do approach, based on stopping
@@ -402,6 +407,9 @@ private:
         uint32_t lower_limit_start_ms;
         uint32_t land_start_ms;
         float vpos_start_m;
+
+        // landing detection threshold in meters
+        AP_Float detect_alt_change;
     } landing_detect;
 
     // throttle mix acceleration filter
@@ -430,6 +438,7 @@ private:
             return AP_HAL::millis() - last_state_change_ms;
         }
         Vector3p target_cm;
+        Vector2f xy_correction;
         Vector3f target_vel_cms;
         bool slow_descent:1;
         bool pilot_correction_active;
@@ -438,7 +447,13 @@ private:
         uint32_t last_log_ms;
         bool reached_wp_speed;
         uint32_t last_run_ms;
-        float pos1_start_speed;
+        float pos1_speed_limit;
+        bool done_accel_init;
+        Vector2f velocity_match;
+        uint32_t last_velocity_match_ms;
+        float target_speed;
+        float target_accel;
+        uint32_t last_pos_reset_ms;
     private:
         uint32_t last_state_change_ms;
         enum position_control_state state;
@@ -561,6 +576,11 @@ private:
       see if we are in the VTOL position control phase of a landing
     */
     bool in_vtol_land_poscontrol(void) const;
+
+    /*
+      are we in the airbrake phase of a VTOL landing?
+     */
+    bool in_vtol_airbrake(void) const;
     
     // Q assist state, can be enabled, disabled or force. Default to enabled
     Q_ASSIST_STATE_ENUM q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED;

ArduPlane/radio.cpp

@@ -280,7 +280,7 @@ void Plane::control_failsafe()
         // throttle has dropped below the mark
         failsafe.throttle_counter++;
         if (failsafe.throttle_counter == 10) {
-            gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe on");
+            gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe %s", "on");
             failsafe.rc_failsafe = true;
             AP_Notify::flags.failsafe_radio = true;
         }
@@ -295,7 +295,7 @@ void Plane::control_failsafe()
             failsafe.throttle_counter = 3;
         }
         if (failsafe.throttle_counter == 1) {
-            gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe off");
+            gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe %s", "off");
         } else if(failsafe.throttle_counter == 0) {
             failsafe.rc_failsafe = false;
             AP_Notify::flags.failsafe_radio = false;

ArduPlane/servos.cpp

@@ -561,7 +561,7 @@ void Plane::set_servos_controlled(void)
                control_mode == &mode_fbwa ||
                control_mode == &mode_autotune) {
         // a manual throttle mode
-        if (failsafe.throttle_counter) {
+        if (!rc().has_valid_input()) {
             SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
         } else if (g.throttle_passthru_stabilize) {
             // manual pass through of throttle while in FBWA or
@@ -615,7 +615,7 @@ void Plane::set_servos_flaps(void)
     int8_t manual_flap_percent = 0;
 
     // work out any manual flap input
-    if (channel_flap != nullptr && !failsafe.rc_failsafe && failsafe.throttle_counter == 0) {
+    if (channel_flap != nullptr && rc().has_valid_input()) {
         manual_flap_percent = channel_flap->percent_input();
     }
 

ArduPlane/tailsitter.cpp

@@ -312,6 +312,11 @@ void Tailsitter::output(void)
 
             // in assisted flight this is done in the normal motor output path
             if (!quadplane.assisted_flight) {
+
+                // keep attitude control throttle level upto date, this value should never be output to motors
+                // it is used to re-set the accel Z integrator term allowing for a smooth transfer of control
+                quadplane.attitude_control->set_throttle_out(throttle, false, 0);
+
                 // convert the hover throttle to the same output that would result if used via AP_Motors
                 // apply expo, battery scaling and SPIN min/max.
                 throttle = motors->thrust_to_actuator(throttle);

ArduPlane/tiltrotor.cpp

@@ -479,6 +479,17 @@ bool Tiltrotor::fully_fwd(void) const
     return (current_tilt >= get_fully_forward_tilt());
 }
 
+/*
+  return true if the rotors are fully tilted up
+ */
+bool Tiltrotor::fully_up(void) const
+{
+    if (!enabled() || (tilt_mask == 0)) {
+        return false;
+    }
+    return (current_tilt <= 0);
+}
+
 /*
   control vectoring for tilt multicopters
  */

ArduPlane/tiltrotor.h

@@ -49,6 +49,7 @@ public:
     }
 
     bool fully_fwd() const;
+    bool fully_up() const;
     float tilt_max_change(bool up, bool in_flap_range = false) const;
     float get_fully_forward_tilt() const;
     float get_forward_flight_tilt() const;

ArduPlane/transition.h

@@ -54,6 +54,8 @@ public:
 
     virtual bool allow_weathervane() { return true; }
 
+    virtual void set_last_fw_pitch(void) {}
+
 protected:
 
     // refences for convenience
@@ -93,6 +95,8 @@ public:
 
     bool set_VTOL_roll_pitch_limit(int32_t& nav_roll_cd, int32_t& nav_pitch_cd) override;
 
+    void set_last_fw_pitch(void) override;
+
 protected:
 
     enum {

ArduPlane/version.h

@@ -6,14 +6,14 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ArduPlane V4.2.0dev"
+#define THISFIRMWARE "ArduPlane V4.2.0beta3"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,2,0,FIRMWARE_VERSION_TYPE_DEV
+#define FIRMWARE_VERSION 4,2,0,FIRMWARE_VERSION_TYPE_BETA+2
 
 #define FW_MAJOR 4
 #define FW_MINOR 2
 #define FW_PATCH 0
-#define FW_TYPE FIRMWARE_VERSION_TYPE_DEV
+#define FW_TYPE FIRMWARE_VERSION_TYPE_BETA+2
 
 #include <AP_Common/AP_FWVersionDefine.h>

ArduPlane/wscript

@@ -29,6 +29,7 @@ def build(bld):
             'AP_OSD',
             'AC_AutoTune',
             'AP_KDECAN',
+            'AP_Follow',
         ],
     )
 

Rover/mode_loiter.cpp

@@ -56,8 +56,20 @@ void ModeLoiter::update()
         _desired_speed *= yaw_error_ratio;
     }
 
+    // 0 turn rate is no limit
+    float turn_rate = 0.0;
+
+    // make sure sailboats don't try and sail directly into the wind
+    if (g2.sailboat.use_indirect_route(_desired_yaw_cd)) {
+        _desired_yaw_cd = g2.sailboat.calc_heading(_desired_yaw_cd);
+        if (g2.sailboat.tacking()) {
+            // use pivot turn rate for tacks
+            turn_rate = g2.wp_nav.get_pivot_rate();
+        }
+    }
+
     // run steering and throttle controllers
-    calc_steering_to_heading(_desired_yaw_cd);
+    calc_steering_to_heading(_desired_yaw_cd, turn_rate);
     calc_throttle(_desired_speed, true);
 }
 

Rover/version.h

@@ -6,14 +6,14 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ArduRover V4.2.0-dev"
+#define THISFIRMWARE "ArduRover V4.2.0-beta1"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,2,0,FIRMWARE_VERSION_TYPE_DEV
+#define FIRMWARE_VERSION 4,2,0,FIRMWARE_VERSION_TYPE_BETA
 
 #define FW_MAJOR 4
 #define FW_MINOR 2
 #define FW_PATCH 0
-#define FW_TYPE FIRMWARE_VERSION_TYPE_DEV
+#define FW_TYPE FIRMWARE_VERSION_TYPE_BETA
 
 #include <AP_Common/AP_FWVersionDefine.h>

Tools/AP_Bootloader/board_types.txt

@@ -132,7 +132,7 @@ AP_HW_KAKUTEF4_MINI                  1030
 AP_HW_H31_PIXC4_PI                   1031
 AP_HW_H31_PIXC4_JETSON               1032
 AP_HW_CUBEORANGE_JOEY                1033
-AP_HW_SIERRAF9PGPS_PERIPH            1034
+AP_HW_SierraF9P                      1034
 AP_HW_HolybroGPS                     1035
 AP_HW_QioTekZealotH743               1036
 AP_HW_HEREPRO                        1037
@@ -159,6 +159,7 @@ AP_HW_KakuteH7Mini                   1058
 AP_HW_JHEMCUGSF405A                  1059
 AP_HW_SPRACINGH7                     1060
 AP_HW_DEVEBOXH7                      1061
+AP_HW_MatekL431                      1062
 
 AP_HW_CUBEORANGE_PERIPH              1400
 AP_HW_CUBEBLACK_PERIPH               1401

Tools/AP_Periph/Parameters.cpp

@@ -173,7 +173,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @RebootRequired: True
     GSCALAR(gps_port, "GPS_PORT", HAL_PERIPH_GPS_PORT_DEFAULT),
 
-#if HAL_NUM_CAN_IFACES >= 2
+#if GPS_MOVING_BASELINE
     // @Param: MB_CAN_PORT
     // @DisplayName: Moving Baseline CAN Port option
     // @Description: Autoselect dedicated CAN port on which moving baseline data will be transmitted.

Tools/AP_Periph/Parameters.h

@@ -95,7 +95,7 @@ public:
 
 #ifdef HAL_PERIPH_ENABLE_GPS
     AP_Int8 gps_port;
-#if HAL_NUM_CAN_IFACES >= 2
+#if GPS_MOVING_BASELINE
     AP_Int8 gps_mb_only_can_port;
 #endif
 #endif

Tools/AP_Periph/ReleaseNotes.txt

@@ -0,0 +1,102 @@
+Release 1.3.2 28th Apr 2022
+---------------------------
+
+This is a minor release with the following changes:
+
+ - added bootloader for Holybro_G4 GPS
+ - uncompress bootloader on Holybro_G4 GPS to handle low memory
+ - fixed RM3100 mask on Sierra-L431
+ - disable watchdog on DMA errors
+
+Release 1.3.1 18th Apr 2022
+---------------------------
+
+This is a minor release with two changes:
+
+ - fixed intermittent loss of GPS packets on F4 and L4 GPS nodes which caused loss of GPS lock on the flight controller
+ - add support for the MatekL431-Rangefinder target
+
+Release 1.3.0 18th Mar 2022
+---------------------------
+
+This is a major release with several significant bug fixes and
+improvements:
+
+ - added new peripherals: BirdCANdy, MatekL431, CubeOrange-periph,
+   G4-ESC, HerePro, Hitec-Airspeed, HolybroG4GPS, HolybroGPS,
+   Sierra-F405, Sierra-F421, Sierra-F9P, Sierra-L431,
+   f103-QiotekPeriph, f405-MatekGPS, f405-MatekAirspeed, mRo-M10095,
+   ARK_GPS, HitecMosaic, MatekH743-periph, Pixracer-periph
+
+ - support dshot for CAN ESC outputs
+
+ - support a wider range of notify options
+
+ - numerous small bug fixes
+
+ - support lua scripting in peripherals
+
+ - switched to DroneCAN compiler and libraries
+
+ - support logging in peripherals
+
+ - support dual CAN bus
+
+ - support BLHeli monitoring of ESC telemetry
+
+ - support mavlink in peripherals
+
+ - support moving baseline yaw dual-GPS on dual-CAN GPS
+
+ - support MPPT battery driver
+
+ - fixed MSP GPS yaw
+
+Note that the next major release will add CANFD support.
+
+Release 1.2.0 6th Jan 2020
+--------------------------
+
+This is a major release with several significant bug fixes and
+improvements:
+
+ - support for battery monitor nodes
+
+ - support for testing in SITL
+
+ - improvements in error reporting to the flight controller
+
+ - fixes to stack sizes
+
+ - MSP output support
+
+ - support for BGR NCP5623 LEDs
+
+ - switched to common CAN stack with main ArduPilot vehicle code
+
+ - added several new board types
+
+
+Release 1.1.0 14th May 2020
+---------------------------
+
+This is a major release with several significant bug fixes:
+
+ - fixed initial GPS timestamp which could cause ArduPilot to get bad
+   time sync
+
+ - fixed airspeed pressure wrap
+
+ - fixed rangefinder to send the RNGFND1_ADDR as sensor_id
+
+ - added distinctive LED blink pattern when waiting for UAVCAN node ID
+   allocation
+
+ - added HWESC build targets for HobbyWing ESC telemetry
+
+ - fixed RM3100 compass scaling bug
+
+Release 1.0.0 9th November 2019
+-------------------------------
+
+Initial stable release

Tools/AP_Periph/can.cpp

@@ -59,7 +59,7 @@ extern AP_Periph_FW periph;
 #if defined(STM32H7)
 #define HAL_PERIPH_LOOP_DELAY_US 64
 #else
-#define HAL_PERIPH_LOOP_DELAY_US 512
+#define HAL_PERIPH_LOOP_DELAY_US 1024
 #endif
 #endif
 

Tools/AP_Periph/release-notes.txt

@@ -1,46 +0,0 @@
-Release 1.2.0 6th Jan 2020
---------------------------
-
-This is a major release with several significant bug fixes and
-improvements:
-
- - support for battery monitor nodes
-
- - support for testing in SITL
-
- - improvements in error reporting to the flight controller
-
- - fixes to stack sizes
-
- - MSP output support
-
- - support for BGR NCP5623 LEDs
-
- - switched to common CAN stack with main ArduPilot vehicle code
-
- - added several new board types
-
-
-Release 1.1.0 14th May 2020
----------------------------
-
-This is a major release with several significant bug fixes:
-
- - fixed initial GPS timestamp which could cause ArduPilot to get bad
-   time sync
-
- - fixed airspeed pressure wrap
-
- - fixed rangefinder to send the RNGFND1_ADDR as sensor_id
-
- - added distinctive LED blink pattern when waiting for UAVCAN node ID
-   allocation
-
- - added HWESC build targets for HobbyWing ESC telemetry
-
- - fixed RM3100 compass scaling bug
-
-Release 1.0.0 9th November 2019
--------------------------------
-
-Initial stable release

Tools/AP_Periph/version.h

@@ -1,14 +1,11 @@
 #pragma once
 
-#define THISFIRMWARE "AP_Periph V1.3dev"
+#define THISFIRMWARE "AP_Periph V1.3.2"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 1,3,0,FIRMWARE_VERSION_TYPE_DEV
+#define FIRMWARE_VERSION 1,3,2,FIRMWARE_VERSION_TYPE_OFFICIAL
 
 #define FW_MAJOR 1
 #define FW_MINOR 3
-#define FW_PATCH 0
-#define FW_TYPE FIRMWARE_VERSION_TYPE_DEV
-
-
-
+#define FW_PATCH 2
+#define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL

Tools/ardupilotwaf/chibios.py

@@ -266,6 +266,18 @@ class build_abin(Task.Task):
     def __str__(self):
         return self.outputs[0].path_from(self.generator.bld.bldnode)
 
+class build_normalized_bins(Task.Task):
+    '''Move external flash binaries to regular location if regular bin is zero length'''
+    color='CYAN'
+    always_run = True
+    def run(self):
+        if self.env.HAS_EXTERNAL_FLASH_SECTIONS and os.path.getsize(self.inputs[0].abspath()) == 0:
+                os.remove(self.inputs[0].abspath())
+                shutil.move(self.inputs[1].abspath(), self.inputs[0].abspath())
+
+    def keyword(self):
+        return "bin cleanup"
+
 class build_intel_hex(Task.Task):
     '''build an intel hex file for upload with DFU'''
     color='CYAN'
@@ -302,12 +314,15 @@ def chibios_firmware(self):
         abin_task = self.create_task('build_abin', src=link_output, tgt=abin_target)
         abin_task.set_run_after(generate_apj_task)
 
+    cleanup_task = self.create_task('build_normalized_bins', src=bin_target)
+    cleanup_task.set_run_after(generate_apj_task)
+
     bootloader_bin = self.bld.srcnode.make_node("Tools/bootloaders/%s_bl.bin" % self.env.BOARD)
     if self.bld.env.HAVE_INTEL_HEX:
         if os.path.exists(bootloader_bin.abspath()):
             hex_target = self.bld.bldnode.find_or_declare('bin/' + link_output.change_ext('.hex').name)
             hex_task = self.create_task('build_intel_hex', src=[bin_target[0], bootloader_bin], tgt=hex_target)
-            hex_task.set_run_after(generate_bin_task)
+            hex_task.set_run_after(cleanup_task)
         else:
             print("Not embedding bootloader; %s does not exist" % bootloader_bin)
 

Tools/autotest/arducopter.py

@@ -3961,7 +3961,7 @@ class AutoTestCopter(AutoTest):
 
         if not (m.type_mask == (target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE) or m.type_mask == target_typemask):
             raise NotAchievedException("Did not receive proper mask: expected=%u or %u, got=%u" %
-                  ((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask))
+                                       ((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask))
 
         if x - m.x > 0.1:
             raise NotAchievedException("Did not receive proper target position x: wanted=%f got=%f" % (x, m.x))
@@ -4012,7 +4012,7 @@ class AutoTestCopter(AutoTest):
         # Check the last received message
         if not (m.type_mask == (target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE) or m.type_mask == target_typemask):
             raise NotAchievedException("Did not receive proper mask: expected=%u or %u, got=%u" %
-                  ((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask))
+                                       ((target_typemask | MAV_POS_TARGET_TYPE_MASK.LAST_BYTE), target_typemask, m.type_mask))
 
         if vx - m.vx > 0.1:
             raise NotAchievedException("Did not receive proper target velocity vx: wanted=%f got=%f" % (vx, m.vx))

Tools/autotest/arduplane.py

@@ -570,9 +570,12 @@ class AutoTestPlane(AutoTest):
 
     def fly_deepstall_absolute(self):
         self.start_subtest("DeepStall Relative Absolute")
-        self.set_parameter("LAND_TYPE", 1)
         deepstall_elevator_pwm = 1661
-        self.set_parameter("LAND_DS_ELEV_PWM", deepstall_elevator_pwm)
+        self.set_parameters({
+            "LAND_TYPE": 1,
+            "LAND_DS_ELEV_PWM": deepstall_elevator_pwm,
+            "RTL_AUTOLAND": 1,
+        })
         self.load_mission("plane-deepstall-mission.txt")
         self.change_mode("AUTO")
         self.wait_ready_to_arm()
@@ -593,9 +596,12 @@ class AutoTestPlane(AutoTest):
 
     def fly_deepstall_relative(self):
         self.start_subtest("DeepStall Relative")
-        self.set_parameter("LAND_TYPE", 1)
         deepstall_elevator_pwm = 1661
-        self.set_parameter("LAND_DS_ELEV_PWM", deepstall_elevator_pwm)
+        self.set_parameters({
+            "LAND_TYPE": 1,
+            "LAND_DS_ELEV_PWM": deepstall_elevator_pwm,
+            "RTL_AUTOLAND": 1,
+        })
         self.load_mission("plane-deepstall-relative-mission.txt")
         self.change_mode("AUTO")
         self.wait_ready_to_arm()
@@ -766,6 +772,7 @@ class AutoTestPlane(AutoTest):
                 "RC%u_OPTION" % flaps_ch: 208, # Flaps RCx_OPTION
                 "LAND_FLAP_PERCNT": 50,
                 "LOG_DISARMED": 1,
+                "RTL_AUTOLAND": 1,
 
                 "RC%u_MIN" % flaps_ch: flaps_ch_min,
                 "RC%u_MAX" % flaps_ch: flaps_ch_max,
@@ -898,7 +905,7 @@ class AutoTestPlane(AutoTest):
             raise NotAchievedException("Bad absalt (want=%f vs got=%f)" % (original_alt+30, x.alt_msl))
         self.fly_home_land_and_disarm()
 
-    def test_throttle_failsafe(self):
+    def ThrottleFailsafe(self):
         self.change_mode('MANUAL')
         m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
         receiver_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_RC_RECEIVER
@@ -1000,11 +1007,11 @@ class AutoTestPlane(AutoTest):
                 "FS_SHORT_ACTN": 3, # 3 means disabled
                 "SIM_RC_FAIL": 1,
             })
-            self.wait_statustext("Long event on", check_context=True)
+            self.wait_statustext("Long failsafe on", check_context=True)
             self.wait_mode("RTL")
 #            self.context_clear_collection("STATUSTEXT")
             self.set_parameter("SIM_RC_FAIL", 0)
-            self.wait_text("Long event off", check_context=True)
+            self.wait_text("Long Failsafe Cleared", check_context=True)
             self.change_mode("MANUAL")
 
             self.progress("Trying again with THR_FS_VALUE")
@@ -1012,7 +1019,7 @@ class AutoTestPlane(AutoTest):
                 "THR_FS_VALUE": 960,
                 "SIM_RC_FAIL": 2,
             })
-            self.wait_statustext("Long event on", check_context=True)
+            self.wait_statustext("Long Failsafe on", check_context=True)
             self.wait_mode("RTL")
         except Exception as e:
             self.print_exception_caught(e)
@@ -1021,6 +1028,27 @@ class AutoTestPlane(AutoTest):
         if ex is not None:
             raise ex
 
+        self.start_subtest("Not use RC throttle input when THR_FAILSAFE==2")
+        self.takeoff(100)
+        self.set_rc(3, 1800)
+        self.set_rc(1, 2000)
+        self.wait_attitude(desroll=45, timeout=1)
+        self.context_push()
+        self.set_parameters({
+            "THR_FAILSAFE": 2,
+            "SIM_RC_FAIL": 1,  # no pulses
+        })
+        self.delay_sim_time(1)
+        self.wait_attitude(desroll=0, timeout=5)
+        self.assert_servo_channel_value(3, self.get_parameter("RC3_MIN"))
+        self.set_parameters({
+            "SIM_RC_FAIL": 0,  # fix receiver
+        })
+        self.zero_throttle()
+        self.disarm_vehicle(force=True)
+        self.context_pop()
+        self.reboot_sitl()
+
     def test_throttle_failsafe_fence(self):
         fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
 
@@ -1074,6 +1102,7 @@ class AutoTestPlane(AutoTest):
         self.context_push()
         ex = None
         try:
+            self.set_parameter("RTL_AUTOLAND", 1)
             self.load_mission("plane-gripper-mission.txt")
             self.set_current_waypoint(1)
             self.change_mode('AUTO')
@@ -1620,7 +1649,10 @@ class AutoTestPlane(AutoTest):
 
     def airspeed_autocal(self):
         self.progress("Ensure no AIRSPEED_AUTOCAL on ground")
-        self.set_parameter("ARSPD_AUTOCAL", 1)
+        self.set_parameters({
+            "ARSPD_AUTOCAL": 1,
+            "RTL_AUTOLAND": 1,
+        })
         m = self.mav.recv_match(type='AIRSPEED_AUTOCAL',
                                 blocking=True,
                                 timeout=5)
@@ -1718,6 +1750,7 @@ class AutoTestPlane(AutoTest):
         self.set_parameters({
             "FLIGHT_OPTIONS": 0,
             "ALT_HOLD_RTL": 8000,
+            "RTL_AUTOLAND": 1,
         })
         takeoff_alt = 10
         self.takeoff(alt=takeoff_alt)
@@ -1836,6 +1869,7 @@ class AutoTestPlane(AutoTest):
 
             '''ensure rangefinder gives height-above-ground'''
             self.load_mission("plane-gripper-mission.txt") # borrow this
+            self.set_parameter("RTL_AUTOLAND", 1)
             self.set_current_waypoint(1)
             self.change_mode('AUTO')
             self.wait_ready_to_arm()
@@ -2247,7 +2281,7 @@ function'''
         self.wait_waypoint(4, 4, timeout=1200, max_dist=120)
 
         # Disarm
-        self.disarm_vehicle()
+        self.disarm_vehicle_expect_fail()
 
         self.progress("Mission OK")
 
@@ -2344,7 +2378,7 @@ function'''
             raise NotAchievedException("Airspeed did not reduce with lower SOAR_VSPEED")
 
         # Disarm
-        self.disarm_vehicle()
+        self.disarm_vehicle_expect_fail()
 
         self.progress("Mission OK")
 
@@ -2851,7 +2885,7 @@ function'''
             self.context_clear_collection("STATUSTEXT")
             ###################################################################
 
-            self.disarm_vehicle()
+            self.disarm_vehicle(force=True)
 
         except Exception as e:
             self.print_exception_caught(e)
@@ -3504,7 +3538,7 @@ function'''
 
             ("ThrottleFailsafe",
              "Fly throttle failsafe",
-             self.test_throttle_failsafe),
+             self.ThrottleFailsafe),
 
             ("NeedEKFToArm",
              "Ensure we need EKF to be healthy to arm",

Tools/autotest/common.py

@@ -4243,6 +4243,22 @@ class AutoTest(ABC):
                      timeout=timeout)
         return self.wait_disarmed()
 
+    def disarm_vehicle_expect_fail(self):
+        '''disarm, checking first that non-forced disarm fails, then doing a forced disarm'''
+        self.progress("Disarm - expect to fail")
+        self.run_cmd(mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+                     0,  # DISARM
+                     0,
+                     0,
+                     0,
+                     0,
+                     0,
+                     0,
+                     timeout=10,
+                     want_result=mavutil.mavlink.MAV_RESULT_FAILED)
+        self.progress("Disarm - forced")
+        self.disarm_vehicle(force=True)
+
     def wait_disarmed_default_wait_time(self):
         return 30
 
@@ -5822,6 +5838,21 @@ class AutoTest(ABC):
             if comparator(m_value, value):
                 return m_value
 
+    def assert_servo_channel_value(self, channel, value, comparator=operator.eq):
+        """assert channel value (default condition is equality)"""
+        channel_field = "servo%u_raw" % channel
+        opstring = ("%s" % comparator)[-3:-1]
+        m = self.assert_receive_message('SERVO_OUTPUT_RAW', timeout=1)
+        m_value = getattr(m, channel_field, None)
+        if m_value is None:
+            raise ValueError("message (%s) has no field %s" %
+                             (str(m), channel_field))
+        self.progress("assert SERVO_OUTPUT_RAW.%s=%u %s %u" %
+                      (channel_field, m_value, opstring, value))
+        if comparator(m_value, value):
+            return m_value
+        raise NotAchievedException("Wrong value")
+
     def get_rc_channel_value(self, channel, timeout=2):
         """wait for channel to hit value"""
         channel_field = "chan%u_raw" % channel
@@ -5854,6 +5885,16 @@ class AutoTest(ABC):
             if value == m_value:
                 return
 
+    def assert_rc_channel_value(self, channel, value):
+        channel_field = "chan%u_raw" % channel
+
+        m_value = self.get_rc_channel_value(channel, timeout=1)
+        self.progress("RC_CHANNELS.%s=%u want=%u" %
+                      (channel_field, m_value, value))
+        if value != m_value:
+            raise NotAchievedException("Expected %s to be %u got %u" %
+                                       (channel, value, m_value))
+
     def wait_location(self,
                       loc,
                       accuracy=5.0,
@@ -11012,7 +11053,7 @@ switch value'''
         # ok done, stop the engine
         if self.is_plane():
             self.zero_throttle()
-            if not self.disarm_vehicle():
+            if not self.disarm_vehicle(force=True):
                 raise NotAchievedException("Failed to DISARM")
 
     def test_frsky_d(self):

Tools/autotest/locations.txt

@@ -92,3 +92,5 @@ RF_AirStadium=36.893328,-2.720371,1434,0
 RF_BuenaVista=37.093686,-2.890969,2390,0
 RF_Castle=37.090662,-3.074557,2736,0
 RF_Garage=37.621798,-2.646596,788,0
+SFO_Bay=37.62561973,-122.33235387,0.0,0
+

Tools/autotest/quadplane.py

@@ -248,13 +248,12 @@ class AutoTestQuadPlane(AutoTest):
             self.progress("Waiting for Motor1 to speed up")
             self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge)
 
-            self.progress("Disarm/rearm with GCS")
-            self.disarm_vehicle()
+            self.disarm_vehicle_expect_fail()
             self.arm_vehicle()
 
             self.progress("Verify that airmode is still on")
             self.wait_servo_channel_value(5, spin_min_pwm, comparator=operator.ge)
-            self.disarm_vehicle()
+            self.disarm_vehicle(force=True)
             self.wait_ready_to_arm()
 
     def test_motor_mask(self):

Tools/scripts/CAN/fix2_gap.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+'''
+test script to check if all CAN GPS nodes are producing Fix2 frames at the expected rate
+'''
+
+import dronecan, time
+from dronecan import uavcan
+
+# get command line arguments
+from argparse import ArgumentParser
+parser = ArgumentParser(description='Fix2 gap example')
+parser.add_argument("--bitrate", default=1000000, type=int, help="CAN bit rate")
+parser.add_argument("--node-id", default=100, type=int, help="CAN node ID")
+parser.add_argument("--max-gap", default=0.25, type=int, help="max gap in seconds")
+parser.add_argument("port", default=None, type=str, help="serial port or mavcan URI")
+args = parser.parse_args()
+
+# Initializing a DroneCAN node instance.
+node = dronecan.make_node(args.port, node_id=args.node_id, bitrate=args.bitrate)
+
+# Initializing a node monitor
+node_monitor = dronecan.app.node_monitor.NodeMonitor(node)
+
+last_fix2 = {}
+
+def handle_fix2(msg):
+    nodeid = msg.transfer.source_node_id
+    tstamp = msg.transfer.ts_real
+    if not nodeid in last_fix2:
+        last_fix2[nodeid] = tstamp
+        return
+    dt = tstamp - last_fix2[nodeid]
+    last_fix2[nodeid] = tstamp
+    if dt > args.max_gap:
+        print("Node %u gap=%.3f" % (nodeid, dt))
+
+# callback for printing ESC status message to stdout in human-readable YAML format.
+node.add_handler(dronecan.uavcan.equipment.gnss.Fix2, handle_fix2)
+
+while True:
+    try:
+        node.spin()
+    except Exception as ex:
+        print(ex)

Tools/scripts/runplanetest.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 import pexpect, time, sys
 from pymavlink import mavutil
@@ -25,6 +25,20 @@ def wait_mode(mav, modes, timeout=10):
     print("Failed to get mode from %s" % modes)
     sys.exit(1)
 
+def wait_prearm_ok(mav, timeout=30):
+    '''wait for pre-arm OK'''
+    start_time = time.time()
+    last_mode = None
+    while time.time() < start_time+timeout:
+        m = mav.recv_match(type='SYS_STATUS', blocking=True, timeout=2)
+        if m is None:
+            return
+        if m.onboard_control_sensors_health & mavutil.mavlink.MAV_SYS_STATUS_PREARM_CHECK != 0:
+            print("Prearm OK")
+            return
+    print("Failed to get pre-arm OK")
+    sys.exit(1)
+    
 def wait_time(mav, simtime):
     '''wait for simulation time to pass'''
     imu = mav.recv_match(type='RAW_IMU', blocking=True)
@@ -36,17 +50,24 @@ def wait_time(mav, simtime):
             break
 
 cmd = '../Tools/autotest/sim_vehicle.py -D -f quadplane'
-mavproxy = pexpect.spawn(cmd, logfile=sys.stdout, timeout=30)
+mavproxy = pexpect.spawn(cmd, logfile=sys.stdout.buffer, timeout=30)
 mavproxy.expect("ArduPilot Ready")
 
 mav = mavutil.mavlink_connection('127.0.0.1:14550')
 
 mavproxy.send('speedup 40\n')
-mavproxy.expect('using GPS')
-mavproxy.expect('using GPS')
-mavproxy.expect('using GPS')
-mavproxy.expect('using GPS')
-mavproxy.send('auto\n')
+wait_prearm_ok(mav)
+mavproxy.send('mode guided\n')
+wait_mode(mav, ['GUIDED'])
 mavproxy.send('arm throttle\n')
-wait_mode(mav, ['AUTO'])
-mavproxy.expect("Mission: 5 WP")
+mavproxy.send('takeoff 40\n')
+wait_time(mav, 30)
+mavproxy.send('mode cruise\n')
+wait_mode(mav, ['CRUISE'])
+wait_time(mav, 10)
+mavproxy.send('mode qrtl\n')
+wait_mode(mav, ['QRTL'])
+mavproxy.send('module load console\n')
+mavproxy.send('module load map\n')
+mavproxy.logfile = None
+mavproxy.interact()

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -458,14 +458,23 @@ void AC_PosControl::relax_velocity_controller_xy()
 
     // decay resultant acceleration and therefore current attitude target to zero
     float decay = 1.0 - _dt / (_dt + POSCONTROL_RELAX_TC);
-
     _accel_target.xy() *= decay;
     _pid_vel_xy.set_integrator(_accel_target - _accel_desired);
 }
 
+/// reduce response for landing
+void AC_PosControl::soften_for_landing_xy()
+{
+    // decay position error to zero
+    _pos_target.xy() += (_inav.get_position_xy_cm().topostype() - _pos_target.xy()) * (_dt / (_dt + POSCONTROL_RELAX_TC));
+
+    // Prevent I term build up in xy velocity controller.
+    // Note that this flag is reset on each loop in update_xy_controller()
+    set_externally_limited_xy();
+}
+
 /// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
 ///     This function is the default initialisation for any position control that provides position, velocity and acceleration.
-///     This function is private and contains all the shared xy axis initialisation functions
 void AC_PosControl::init_xy_controller()
 {
     // set roll, pitch lean angle targets to current attitude
@@ -474,6 +483,7 @@ void AC_PosControl::init_xy_controller()
     _pitch_target = att_target_euler_cd.y;
     _yaw_target = att_target_euler_cd.z; // todo: this should be thrust vector heading, not yaw.
     _yaw_rate_target = 0.0f;
+    _angle_max_override_cd = 0.0;
 
     _pos_target.xy() = _inav.get_position_xy_cm().topostype();
 
@@ -488,7 +498,7 @@ void AC_PosControl::init_xy_controller()
 
     // limit acceleration using maximum lean angles
     float angle_max = MIN(_attitude_control.get_althold_lean_angle_max_cd(), get_lean_angle_max_cd());
-    float accel_max = GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f));
+    float accel_max = angle_to_accel(angle_max * 0.01) * 100.0;
     _accel_target.xy().limit_length(accel_max);
 
     // initialise I terms from lean angles
@@ -625,7 +635,7 @@ void AC_PosControl::update_xy_controller()
 
     // limit acceleration using maximum lean angles
     float angle_max = MIN(_attitude_control.get_althold_lean_angle_max_cd(), get_lean_angle_max_cd());
-    float accel_max = GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f));
+    float accel_max = angle_to_accel(angle_max * 0.01) * 100;
     // Define the limit vector before we constrain _accel_target 
     _limit_vector.xy() = _accel_target.xy();
     if (!limit_accel_xy(_vel_desired.xy(), _accel_target.xy(), accel_max)) {
@@ -977,6 +987,9 @@ void AC_PosControl::update_z_controller()
 /// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
 float AC_PosControl::get_lean_angle_max_cd() const
 {
+    if (is_positive(_angle_max_override_cd)) {
+        return _angle_max_override_cd;
+    }
     if (!is_positive(_lean_angle_max)) {
         return _attitude_control.lean_angle_max_cd();
     }
@@ -1157,9 +1170,9 @@ void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss,
     const float accel_right = -accel_x_cmss * _ahrs.sin_yaw() + accel_y_cmss * _ahrs.cos_yaw();
 
     // update angle targets that will be passed to stabilize controller
-    pitch_target = atanf(-accel_forward / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
+    pitch_target = accel_to_angle(-accel_forward * 0.01) * 100;
     float cos_pitch_target = cosf(pitch_target * M_PI / 18000.0f);
-    roll_target = atanf(accel_right * cos_pitch_target / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
+    roll_target = accel_to_angle((accel_right * cos_pitch_target)*0.01) * 100;
 }
 
 // lean_angles_to_accel_xy - convert roll, pitch lean target angles to NE frame accelerations in cm/s/s

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -96,6 +96,9 @@ public:
     ///     This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
     void relax_velocity_controller_xy();
 
+    /// reduce response for landing
+    void soften_for_landing_xy();
+
     // init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
     ///     This function is the default initialisation for any position control that provides position, velocity and acceleration.
     ///     This function is private and contains all the shared xy axis initialisation functions
@@ -340,6 +343,12 @@ public:
     /// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
     float get_lean_angle_max_cd() const;
 
+    /*
+      set_lean_angle_max_cd - set the maximum lean angle. A value of zero means to use the ANGLE_MAX parameter.
+      This is reset to zero on init_xy_controller()
+    */
+    void set_lean_angle_max_cd(float angle_max_cd) { _angle_max_override_cd = angle_max_cd; }
+    
 
     /// Other
 
@@ -463,6 +472,9 @@ protected:
     // high vibration handling
     bool        _vibe_comp_enabled;     // true when high vibration compensation is on
 
+    // angle max override, if zero then use ANGLE_MAX parameter
+    float       _angle_max_override_cd;
+
     // return true if on a real vehicle or SITL with lock-step scheduling
     bool has_good_timing(void) const;
 };

libraries/AC_AttitudeControl/AC_WeatherVane.cpp

@@ -10,10 +10,12 @@
     #define WVANE_PARAM_ENABLED 1
     #define WVANE_PARAM_SPD_MAX_DEFAULT 0
     #define WVANE_PARAM_VELZ_MAX_DEFAULT 0
+    #define WVANE_PARAM_GAIN_DEFAULT 0
 #else
     #define WVANE_PARAM_ENABLED 0
     #define WVANE_PARAM_SPD_MAX_DEFAULT 2
     #define WVANE_PARAM_VELZ_MAX_DEFAULT 1
+    #define WVANE_PARAM_GAIN_DEFAULT 1
 #endif
 
 
@@ -32,7 +34,7 @@ const AP_Param::GroupInfo AC_WeatherVane::var_info[] = {
     // @Range: 0.5 4
     // @Increment: 0.1
     // @User: Standard
-    AP_GROUPINFO("GAIN", 2, AC_WeatherVane, _gain, 1.0),
+    AP_GROUPINFO("GAIN", 2, AC_WeatherVane, _gain, WVANE_PARAM_GAIN_DEFAULT),
 
     // @Param: ANG_MIN
     // @DisplayName: Weathervaning min angle
@@ -98,8 +100,8 @@ AC_WeatherVane::AC_WeatherVane(void)
 bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg, const bool is_takeoff, const bool is_landing)
 {
     Direction dir = (Direction)_direction.get();
-    if ((dir == Direction::OFF) || !allowed || (pilot_yaw != 0)) {
-        // parameter disabled
+    if ((dir == Direction::OFF) || !allowed || (pilot_yaw != 0) || !is_positive(_gain)) {
+        // parameter disabled, or 0 gain
         // disabled temporarily
         // dont't override pilot
         reset();

libraries/AC_Autorotation/AC_Autorotation.cpp

@@ -366,7 +366,7 @@ void AC_Autorotation::update_forward_speed_controller(void)
     _accel_out_last = _accel_out;
 
     // update angle targets that will be passed to stabilize controller
-    _pitch_target = atanf(-_accel_out/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI);
+    _pitch_target = accel_to_angle(-_accel_out*0.01) * 100;
 
 }
 

libraries/AC_WPNav/AC_Loiter.cpp

@@ -132,14 +132,7 @@ void AC_Loiter::init_target()
 /// reduce response for landing
 void AC_Loiter::soften_for_landing()
 {
-    const Vector3f& curr_pos = _inav.get_position_neu_cm();
-
-    // set target position to current position
-    _pos_control.set_pos_target_xy_cm(curr_pos.x, curr_pos.y);
-
-    // also prevent I term build up in xy velocity controller. Note
-    // that this flag is reset on each loop, in update_xy_controller()
-    _pos_control.set_externally_limited_xy();
+    _pos_control.soften_for_landing_xy();
 }
 
 /// set pilot desired acceleration in centi-degrees
@@ -218,7 +211,7 @@ void AC_Loiter::calc_desired_velocity(float nav_dt, bool avoidance_on)
     float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f);
     gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, LOITER_SPEED_MIN);
 
-    float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_angle_max_cd()*0.01f));
+    float pilot_acceleration_max = angle_to_accel(get_angle_max_cd()*0.01) * 100;
 
     // range check nav_dt
     if (nav_dt < 0) {

libraries/AC_WPNav/AC_WPNav.cpp

@@ -143,14 +143,15 @@ AC_WPNav::TerrainSource AC_WPNav::get_terrain_source() const
 
 /// wp_and_spline_init - initialise straight line and spline waypoint controllers
 ///     speed_cms should be a positive value or left at zero to use the default speed
-///     updates target roll, pitch targets and I terms based on vehicle lean angles
+///     stopping_point should be the vehicle's stopping point (equal to the starting point of the next segment) if know or left as zero
 ///     should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
-void AC_WPNav::wp_and_spline_init(float speed_cms)
+void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point)
 {
     
     // sanity check parameters
     // check _wp_accel_cmss is reasonable
-    const float wp_accel_cmss = MIN(_wp_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max_cd() * 0.01f)));
+    _scurve_accel_corner = angle_to_accel(_pos_control.get_lean_angle_max_cd() * 0.01) * 100;
+    const float wp_accel_cmss = MIN(_wp_accel_cmss, _scurve_accel_corner);
     _wp_accel_cmss.set_and_save_ifchanged((_wp_accel_cmss <= 0) ? WPNAV_ACCELERATION : wp_accel_cmss);
     
     // check _wp_radius_cm is reasonable
@@ -177,27 +178,31 @@ void AC_WPNav::wp_and_spline_init(float speed_cms)
     if (!is_positive(_wp_jerk)) {
         _wp_jerk = _wp_accel_cmss;
     }
-    calc_scurve_jerk_and_jerk_time();
+    calc_scurve_jerk_and_snap();
 
     _scurve_prev_leg.init();
     _scurve_this_leg.init();
     _scurve_next_leg.init();
     _track_scalar_dt = 1.0f;
 
-    // set flag so get_yaw() returns current heading target
-    _flags.reached_destination = false;
+    _flags.reached_destination = true;
     _flags.fast_waypoint = false;
 
     // initialise origin and destination to stopping point
-    Vector3f stopping_point;
-    get_wp_stopping_point(stopping_point);
+    if (stopping_point.is_zero()) {
+        get_wp_stopping_point(stopping_point);
+    }
     _origin = _destination = stopping_point;
     _terrain_alt = false;
     _this_leg_is_spline = false;
 
     // initialise the terrain velocity to the current maximum velocity
-    _terrain_vel = _wp_desired_speed_xy_cms;
-    _terrain_accel = 0.0;
+    _offset_vel = _wp_desired_speed_xy_cms;
+    _offset_accel = 0.0;
+    _paused = false;
+
+    // mark as active
+    _wp_last_update = AP_HAL::millis();
 }
 
 /// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation
@@ -205,8 +210,8 @@ void AC_WPNav::set_speed_xy(float speed_cms)
 {
     // range check target speed and protect against divide by zero
     if (speed_cms >= WPNAV_WP_SPEED_MIN && is_positive(_wp_desired_speed_xy_cms)) {
-        // update terrain following speed scalar
-        _terrain_vel = speed_cms * _terrain_vel / _wp_desired_speed_xy_cms;
+        // update horizontal velocity speed offset scalar
+        _offset_vel = speed_cms * _offset_vel / _wp_desired_speed_xy_cms;
 
         // initialize the desired wp speed
         _wp_desired_speed_xy_cms = speed_cms;
@@ -334,7 +339,7 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
         _scurve_this_leg.calculate_track(_origin, _destination,
                                          _pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(),
                                          _wp_accel_cmss, _wp_accel_z_cmss,
-                                         _scurve_jerk_time, _scurve_jerk * 100.0f);
+                                         _scurve_snap * 100.0f, _scurve_jerk * 100.0f);
         if (!is_zero(origin_speed)) {
             // rebuild start of scurve if we have a non-zero origin speed
             _scurve_this_leg.set_origin_speed_max(origin_speed);
@@ -362,7 +367,7 @@ bool AC_WPNav::set_wp_destination_next(const Vector3f& destination, bool terrain
     _scurve_next_leg.calculate_track(_destination, destination,
                                      _pos_control.get_max_speed_xy_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(),
                                      _wp_accel_cmss, _wp_accel_z_cmss,
-                                     _scurve_jerk_time, _scurve_jerk * 100.0f);
+                                     _scurve_snap * 100.0f, _scurve_jerk * 100.0);
     if (_this_leg_is_spline) {
         const float this_leg_dest_speed_max = _spline_this_leg.get_destination_speed_max();
         const float next_leg_origin_speed_max = _scurve_next_leg.set_origin_speed_max(this_leg_dest_speed_max);
@@ -459,14 +464,14 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
 
     // get current position and adjust altitude to origin and destination's frame (i.e. _frame)
     const Vector3f &curr_pos = _inav.get_position_neu_cm() - Vector3f{0, 0, terr_offset};
-
-    // Use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of aircraft
     Vector3f curr_target_vel = _pos_control.get_vel_desired_cms();
     curr_target_vel.z -= _pos_control.get_vel_offset_z_cms();
 
+    // Use _track_scalar_dt to slow down progression of the position target moving too far in front of aircraft
+    // _track_scalar_dt does not scale the velocity or acceleration
     float track_scaler_dt = 1.0f;
     // check target velocity is non-zero
-    if (is_positive(curr_target_vel.length())) {
+    if (is_positive(curr_target_vel.length_squared())) {
         Vector3f track_direction = curr_target_vel.normalized();
         const float track_error = _pos_control.get_pos_error_cm().dot(track_direction);
         const float track_velocity = _inav.get_velocity_neu_cms().dot(track_direction);
@@ -474,13 +479,15 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
         track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_xy_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f);
     }
 
-    float vel_time_scalar = 1.0;
+    // Use vel_scaler_dt to slow down the trajectory time
+    // vel_scaler_dt scales the velocity and acceleration to be kinematically constent
+    float vel_scaler_dt = 1.0;
     if (is_positive(_wp_desired_speed_xy_cms)) {
-        update_vel_accel(_terrain_vel, _terrain_accel, dt, 0.0, 0.0);
-        shape_vel_accel( _wp_desired_speed_xy_cms * offset_z_scaler, 0.0,
-            _terrain_vel, _terrain_accel,
-            -_wp_accel_cmss, _wp_accel_cmss, _pos_control.get_shaping_jerk_xy_cmsss(), dt, true);
-        vel_time_scalar = _terrain_vel / _wp_desired_speed_xy_cms;
+        update_vel_accel(_offset_vel, _offset_accel, dt, 0.0, 0.0);
+        const float vel_input = !_paused ? _wp_desired_speed_xy_cms * offset_z_scaler : 0.0;
+        shape_vel_accel(vel_input, 0.0, _offset_vel, _offset_accel, -_wp_accel_cmss, _wp_accel_cmss,
+                        _pos_control.get_shaping_jerk_xy_cmsss(), dt, true);
+        vel_scaler_dt = _offset_vel / _wp_desired_speed_xy_cms;
     }
 
     // change s-curve time speed with a time constant of maximum acceleration / maximum jerk
@@ -497,16 +504,23 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
     if (!_this_leg_is_spline) {
         // update target position, velocity and acceleration
         target_pos = _origin;
-        s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _flags.fast_waypoint, _track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel, target_accel);
+        s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, _scurve_accel_corner, _flags.fast_waypoint, _track_scalar_dt * vel_scaler_dt * dt, target_pos, target_vel, target_accel);
     } else {
         // splinetarget_vel
         target_vel = curr_target_vel;
-        _spline_this_leg.advance_target_along_track(_track_scalar_dt * vel_time_scalar * dt, target_pos, target_vel);
+        _spline_this_leg.advance_target_along_track(_track_scalar_dt * vel_scaler_dt * dt, target_pos, target_vel);
         s_finished = _spline_this_leg.reached_destination();
     }
 
-    target_vel *= vel_time_scalar;
-    target_accel *= sq(vel_time_scalar);
+    Vector3f accel_offset;
+    if (is_positive(target_vel.length_squared())) {
+        Vector3f track_direction = target_vel.normalized();
+        accel_offset = track_direction * _offset_accel * target_vel.length() / _wp_desired_speed_xy_cms;
+    }
+
+    target_vel *= vel_scaler_dt;
+    target_accel *= sq(vel_scaler_dt);
+    target_accel += accel_offset;
 
     // convert final_target.z to altitude above the ekf origin
     target_pos.z += _pos_control.get_pos_offset_z_cm();
@@ -857,8 +871,8 @@ bool AC_WPNav::get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_
 }
 
 // helper function to calculate scurve jerk and jerk_time values
-// updates _scurve_jerk and _scurve_jerk_time
-void AC_WPNav::calc_scurve_jerk_and_jerk_time()
+// updates _scurve_jerk and _scurve_snap
+void AC_WPNav::calc_scurve_jerk_and_snap()
 {
     // calculate jerk
     _scurve_jerk = MIN(_attitude_control.get_ang_vel_roll_max_rads() * GRAVITY_MSS, _attitude_control.get_ang_vel_pitch_max_rads() * GRAVITY_MSS);
@@ -868,14 +882,14 @@ void AC_WPNav::calc_scurve_jerk_and_jerk_time()
         _scurve_jerk = MIN(_scurve_jerk, _wp_jerk);
     }
 
-    // calculate jerk time
-    // Jounce (the rate of change of jerk) uses the attitude control input time constant because multicopters
+    // calculate maximum snap
+    // Snap (the rate of change of jerk) uses the attitude control input time constant because multicopters
     // lean to accelerate. This means the change in angle is equivalent to the change in acceleration
-    const float jounce = MIN(_attitude_control.get_accel_roll_max_radss() * GRAVITY_MSS, _attitude_control.get_accel_pitch_max_radss() * GRAVITY_MSS);
-    if (is_positive(jounce)) {
-        _scurve_jerk_time = MAX(_attitude_control.get_input_tc(), 0.5f * _scurve_jerk * M_PI / jounce);
-    } else {
-        _scurve_jerk_time = MAX(_attitude_control.get_input_tc(), 0.1f);
+    _scurve_snap = (_scurve_jerk * M_PI) / (2.0 * MAX(_attitude_control.get_input_tc(), 0.1f));
+    const float snap = MIN(_attitude_control.get_accel_roll_max_radss(), _attitude_control.get_accel_pitch_max_radss()) * GRAVITY_MSS;
+    if (is_positive(snap)) {
+        _scurve_snap = MIN(_scurve_snap, snap);
     }
-    _scurve_jerk_time *= 2.0f;
+    // reduce maximum snap by a factor of two from what the aircraft is capable of
+    _scurve_snap *= 0.5;
 }

libraries/AC_WPNav/AC_WPNav.h

@@ -52,11 +52,18 @@ public:
     ///     speed_cms is the desired max speed to travel between waypoints.  should be a positive value or omitted to use the default speed
     ///     updates target roll, pitch targets and I terms based on vehicle lean angles
     ///     should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
-    void wp_and_spline_init(float speed_cms = 0.0f);
+    void wp_and_spline_init(float speed_cms = 0.0f, Vector3f stopping_point = Vector3f{});
 
     /// set current target horizontal speed during wp navigation
     void set_speed_xy(float speed_cms);
 
+    /// set pause or resume during wp navigation
+    void set_pause() { _paused = true; }
+    void set_resume() { _paused = false; }
+
+    /// get paused status
+    bool paused() { return _paused; }
+
     /// set current target climb or descent rate during wp navigation
     void set_speed_up(float speed_up_cms);
     void set_speed_down(float speed_down_cms);
@@ -213,8 +220,8 @@ protected:
     } _flags;
 
     // helper function to calculate scurve jerk and jerk_time values
-    // updates _scurve_jerk and _scurve_jerk_time
-    void calc_scurve_jerk_and_jerk_time();
+    // updates _scurve_jerk and _scurve_snap
+    void calc_scurve_jerk_and_snap();
 
     // references and pointers to external libraries
     const AP_InertialNav&   _inav;
@@ -240,8 +247,9 @@ protected:
     SCurve _scurve_prev_leg;            // previous scurve trajectory used to blend with current scurve trajectory
     SCurve _scurve_this_leg;            // current scurve trajectory
     SCurve _scurve_next_leg;            // next scurve trajectory used to blend with current scurve trajectory
+    float _scurve_accel_corner;         // scurve maximum corner acceleration in m/s/s
     float _scurve_jerk;                 // scurve jerk max in m/s/s/s
-    float _scurve_jerk_time;            // scurve jerk time (time in seconds for jerk to increase from zero _scurve_jerk)
+    float _scurve_snap;                 // scurve snap in m/s/s/s/s
 
     // spline curves
     SplineCurve _spline_this_leg;      // spline curve for current segment
@@ -257,8 +265,9 @@ protected:
     Vector3f    _origin;                // starting point of trip to next waypoint in cm from ekf origin
     Vector3f    _destination;           // target destination in cm from ekf origin
     float       _track_scalar_dt;       // time compression multiplier to slow the progress along the track
-    float       _terrain_vel;            // maximum horizontal velocity used to ensure the aircraft can maintain height above terrain
-    float       _terrain_accel;          // acceleration value used to change _terrain_vel
+    float       _offset_vel;            // horizontal velocity reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain
+    float       _offset_accel;          // horizontal acceleration reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain
+    bool        _paused;                // flag for pausing waypoint controller
 
     // terrain following variables
     bool        _terrain_alt;   // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -150,8 +150,9 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
 
     // @Param: _TUBE_ORDER
     // @DisplayName: Control pitot tube order
-    // @Description: Changes the pitot tube order to specify the dynamic pressure side of the sensor. Accepts either if set to 2. Accepts only one side if set to 0 or 1 and can help detect excessive pressure on the static port without indicating positive airspeed.
+    // @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the first (often the top) connector on the sensor needs to be the stagnation pressure (the pressure at the tip of the pitot tube). If set to 1 then the second (often the bottom) connector needs to be the stagnation pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft is receiving excessive pressure on the static port compared to the stagnation port such as during a stall, which would otherwise be seen as a positive airspeed.
     // @User: Advanced
+    // @Values: 0:Normal,1:Swapped,2:Auto Detect
     AP_GROUPINFO("_TUBE_ORDER",  6, AP_Airspeed, param[0].tube_order, 2),
 
 #ifndef HAL_BUILD_AP_PERIPH
@@ -253,8 +254,9 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
 
     // @Param: 2_TUBE_ORDR
     // @DisplayName: Control pitot tube order of 2nd airspeed sensor
-    // @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the top connector on the sensor needs to be the dynamic pressure. If set to 1 then the bottom connector needs to be the dynamic pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft it receiving excessive pressure on the static port, which would otherwise be seen as a positive airspeed.
+    // @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the first (often the top) connector on the sensor needs to be the stagnation pressure (the pressure at the tip of the pitot tube). If set to 1 then the second (often the bottom) connector needs to be the stagnation pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft is receiving excessive pressure on the static port compared to the stagnation port such as during a stall, which would otherwise be seen as a positive airspeed.
     // @User: Advanced
+    // @Values: 0:Normal,1:Swapped,2:Auto Detect
     AP_GROUPINFO("2_TUBE_ORDR",  17, AP_Airspeed, param[1].tube_order, 2),
 
     // @Param: 2_SKIP_CAL
@@ -689,7 +691,7 @@ void AP_Airspeed::Log_Airspeed()
             offset        : get_offset(i),
             use           : use(i),
             healthy       : healthy(i),
-            health_prob   : get_health_failure_probability(i),
+            health_prob   : get_health_probability(i),
             primary       : get_primary()
         };
         AP::logger().WriteBlock(&pkt, sizeof(pkt));

libraries/AP_Airspeed/AP_Airspeed.h

@@ -266,12 +266,12 @@ private:
     // returns 0 if the sensor is not enabled
     float get_pressure(uint8_t i);
 
-    // get the failure health probability
-    float get_health_failure_probability(uint8_t i) const {
+    // get the health probability
+    float get_health_probability(uint8_t i) const {
         return state[i].failures.health_probability;
     }
-    float get_health_failure_probability(void) const {
-        return get_health_failure_probability(primary);
+    float get_health_probability(void) const {
+        return get_health_probability(primary);
     }
 
     void update_calibration(uint8_t i, float raw_pressure);

libraries/AP_Airspeed/AP_Airspeed_Health.cpp

@@ -28,8 +28,7 @@ void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i)
         return;
     }
 
-    const float aspeed = get_airspeed();
-    if (aspeed <= 0) {
+    if (state[i].airspeed <= 0) {
         // invalid estimate
         return;
     }
@@ -44,7 +43,7 @@ void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i)
         }
         return;
     }
-    const float speed_diff = fabsf(aspeed-gps.ground_speed());
+    const float speed_diff = fabsf(state[i].airspeed-gps.ground_speed());
 
     // update health_probability with LowPassFilter
     if (speed_diff > _wind_max) {

libraries/AP_Arming/AP_Arming.cpp

@@ -796,7 +796,7 @@ bool AP_Arming::servo_checks(bool report) const
     bool check_passed = true;
     for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
         const SRV_Channel *c = SRV_Channels::srv_channel(i);
-        if (c == nullptr || c->get_function() == SRV_Channel::k_none) {
+        if (c == nullptr || c->get_function() <= SRV_Channel::k_none) {
             continue;
         }
 
@@ -1364,6 +1364,10 @@ bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks)
         armed = false;
     }
 
+    if (armed && do_arming_checks && checks_to_perform == 0) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Warning: Arming Checks Disabled");
+    }
+
     return armed;
 }
 

libraries/AP_Arming/AP_Arming.h

@@ -164,7 +164,7 @@ protected:
 
     bool manual_transmitter_checks(bool report);
 
-    bool mission_checks(bool report);
+    virtual bool mission_checks(bool report);
 
     bool rangefinder_checks(bool report);
 

libraries/AP_Common/Location.cpp

@@ -140,7 +140,7 @@ bool Location::get_alt_cm(AltFrame desired_frame, int32_t &ret_alt_cm) const
         if (terrain == nullptr) {
             return false;
         }
-        if (!terrain->height_amsl(*this, alt_terr_cm, true)) {
+        if (!terrain->height_amsl(*this, alt_terr_cm)) {
             return false;
         }
         // convert terrain alt to cm
@@ -374,14 +374,14 @@ bool Location::sanitize(const Location &defaultLoc)
 assert_storage_size<Location, 16> _assert_storage_size_Location;
 
 
-// return bearing in centi-degrees from location to loc2
-int32_t Location::get_bearing_to(const struct Location &loc2) const
+// return bearing in radians from location to loc2, return is 0 to 2*Pi
+ftype Location::get_bearing(const struct Location &loc2) const
 {
     const int32_t off_x = diff_longitude(loc2.lng,lng);
     const int32_t off_y = (loc2.lat - lat) / loc2.longitude_scale((lat+loc2.lat)/2);
-    int32_t bearing = 9000 + atan2F(-off_y, off_x) * DEGX100;
+    ftype bearing = (M_PI*0.5) + atan2F(-off_y, off_x);
     if (bearing < 0) {
-        bearing += 36000;
+        bearing += 2*M_PI;
     }
     return bearing;
 }

libraries/AP_Common/Location.h

@@ -92,10 +92,13 @@ public:
 
     void zero(void);
 
-    // return bearing in centi-degrees from location to loc2
-    int32_t get_bearing_to(const struct Location &loc2) const;
-    // return the bearing in radians
-    ftype get_bearing(const struct Location &loc2) const { return radians(get_bearing_to(loc2) * 0.01); } ;
+    // return the bearing in radians, from 0 to 2*Pi
+    ftype get_bearing(const struct Location &loc2) const;
+
+    // return bearing in centi-degrees from location to loc2, return is 0 to 35999
+    int32_t get_bearing_to(const struct Location &loc2) const {
+        return int32_t(get_bearing(loc2) * DEGX100 + 0.5);
+    }
 
     // check if lat and lng match. Ignore altitude and options
     bool same_latlon_as(const Location &loc2) const;

libraries/AP_Common/tests/test_location.cpp

@@ -311,7 +311,7 @@ TEST(Location, Distance)
     bearing = test_home.get_bearing_to(test_loc);
     EXPECT_EQ(31503, bearing);
     const float bearing_rad = test_home.get_bearing(test_loc);
-    EXPECT_FLOAT_EQ(radians(315.03), bearing_rad);
+    EXPECT_FLOAT_EQ(5.4982867, bearing_rad);
 
 }
 

libraries/AP_Declination/AP_Declination.cpp

@@ -66,8 +66,8 @@ bool AP_Declination::get_mag_field_ef(float latitude_deg, float longitude_deg, f
     }
 
     /* find index of nearest low sampling point */
-    uint32_t min_lat_index = static_cast<uint32_t>((-(SAMPLING_MIN_LAT) + min_lat)  / SAMPLING_RES);
-    uint32_t min_lon_index = static_cast<uint32_t>((-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES);
+    uint32_t min_lat_index = constrain_int32(static_cast<uint32_t>((-(SAMPLING_MIN_LAT) + min_lat)  / SAMPLING_RES), 0, LAT_TABLE_SIZE - 2);
+    uint32_t min_lon_index = constrain_int32(static_cast<uint32_t>((-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES), 0, LON_TABLE_SIZE -2);
 
     /* calculate intensity */
 

libraries/AP_Declination/AP_Declination.h

@@ -35,7 +35,10 @@ private:
     static const float SAMPLING_MIN_LON;
     static const float SAMPLING_MAX_LON;
 
-    static const float declination_table[19][37];
-    static const float inclination_table[19][37];
-    static const float intensity_table[19][37];
+    static const uint32_t LAT_TABLE_SIZE = 19;
+    static const uint32_t LON_TABLE_SIZE = 37;
+
+    static const float declination_table[LAT_TABLE_SIZE][LON_TABLE_SIZE];
+    static const float inclination_table[LAT_TABLE_SIZE][LON_TABLE_SIZE];
+    static const float intensity_table[LAT_TABLE_SIZE][LON_TABLE_SIZE];
 };

libraries/AP_Declination/tables.cpp

@@ -9,7 +9,7 @@ const float AP_Declination::SAMPLING_MAX_LAT = 90;
 const float AP_Declination::SAMPLING_MIN_LON = -180;
 const float AP_Declination::SAMPLING_MAX_LON = 180;
 
-const float AP_Declination::declination_table[19][37] = {
+const float AP_Declination::declination_table[LAT_TABLE_SIZE][LON_TABLE_SIZE] = {
     {149.10950f,139.10950f,129.10950f,119.10950f,109.10949f,99.10950f,89.10950f,79.10950f,69.10950f,59.10950f,49.10950f,39.10950f,29.10950f,19.10950f,9.10950f,-0.89050f,-10.89050f,-20.89050f,-30.89050f,-40.89050f,-50.89050f,-60.89050f,-70.89050f,-80.89050f,-90.89050f,-100.89050f,-110.89050f,-120.89050f,-130.89050f,-140.89050f,-150.89050f,-160.89050f,-170.89050f,179.10950f,169.10950f,159.10950f,149.10950f},
     {129.37759f,117.14583f,106.01898f,95.84726f,86.44522f,77.63150f,69.24826f,61.16874f,53.29825f,45.57105f,37.94414f,30.38880f,22.88112f,15.39339f,7.88854f,0.31945f,-7.36677f,-15.22089f,-23.28322f,-31.57827f,-40.11442f,-48.88906f,-57.89765f,-67.14429f,-76.65158f,-86.46832f,-96.67422f,-107.38079f,-118.72599f,-130.85732f,-143.89431f,-157.86353f,-172.61739f,172.21319f,157.16190f,142.76170f,129.37759f},
     {85.60184f,77.69003f,71.32207f,65.86993f,60.92414f,56.17033f,51.35320f,46.28164f,40.84704f,35.03587f,28.92623f,22.66416f,16.41848f,10.31921f,4.39763f,-1.44271f,-7.40082f,-13.70324f,-20.51470f,-27.87783f,-35.70713f,-43.83304f,-52.06997f,-60.27655f,-68.39086f,-76.44339f,-84.56374f,-93.00460f,-102.21930f,-113.07088f,-127.37057f,-149.05145f,176.63172f,138.21637f,112.07842f,96.22737f,85.60184f},
@@ -31,7 +31,7 @@ const float AP_Declination::declination_table[19][37] = {
     {-177.79784f,-167.79784f,-157.79784f,-147.79784f,-137.79784f,-127.79784f,-117.79784f,-107.79784f,-97.79784f,-87.79784f,-77.79784f,-67.79784f,-57.79784f,-47.79784f,-37.79784f,-27.79784f,-17.79784f,-7.79784f,2.20217f,12.20217f,22.20217f,32.20217f,42.20217f,52.20217f,62.20217f,72.20217f,82.20217f,92.20217f,102.20217f,112.20217f,122.20217f,132.20217f,142.20217f,152.20217f,162.20217f,172.20217f,-177.79784f}
 };
 
-const float AP_Declination::inclination_table[19][37] = {
+const float AP_Declination::inclination_table[LAT_TABLE_SIZE][LON_TABLE_SIZE] = {
     {-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f},
     {-78.33243f,-77.56645f,-76.64486f,-75.60941f,-74.49599f,-73.33711f,-72.16456f,-71.01082f,-69.90877f,-68.88978f,-67.98065f,-67.20063f,-66.55969f,-66.05909f,-65.69426f,-65.45930f,-65.35147f,-65.37404f,-65.53651f,-65.85220f,-66.33408f,-66.99021f,-67.82010f,-68.81276f,-69.94649f,-71.18994f,-72.50361f,-73.84119f,-75.15044f,-76.37388f,-77.45008f,-78.31699f,-78.91913f,-79.21830f,-79.20379f,-78.89480f,-78.33243f},
     {-80.91847f,-79.09801f,-77.26826f,-75.41050f,-73.49957f,-71.51974f,-69.48020f,-67.42760f,-65.44927f,-63.66181f,-62.18407f,-61.10090f,-60.43119f,-60.11709f,-60.04466f,-60.08935f,-60.16521f,-60.25535f,-60.41391f,-60.74312f,-61.35672f,-62.34264f,-63.73840f,-65.52698f,-67.65072f,-70.03207f,-72.58967f,-75.24472f,-77.91857f,-80.52353f,-82.93966f,-84.94483f,-86.05606f,-85.75384f,-84.42566f,-82.72116f,-80.91847f},
@@ -53,7 +53,7 @@ const float AP_Declination::inclination_table[19][37] = {
     {88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f}
 };
 
-const float AP_Declination::intensity_table[19][37] = {
+const float AP_Declination::intensity_table[LAT_TABLE_SIZE][LON_TABLE_SIZE] = {
     {0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f},
     {0.60733f,0.60103f,0.59321f,0.58408f,0.57385f,0.56274f,0.55099f,0.53886f,0.52664f,0.51464f,0.50318f,0.49258f,0.48311f,0.47506f,0.46864f,0.46409f,0.46158f,0.46131f,0.46341f,0.46797f,0.47499f,0.48434f,0.49579f,0.50895f,0.52332f,0.53833f,0.55334f,0.56771f,0.58086f,0.59227f,0.60156f,0.60848f,0.61292f,0.61488f,0.61448f,0.61189f,0.60733f},
     {0.63154f,0.61845f,0.60363f,0.58729f,0.56950f,0.55031f,0.52986f,0.50843f,0.48660f,0.46508f,0.44473f,0.42628f,0.41025f,0.39690f,0.38632f,0.37857f,0.37385f,0.37260f,0.37540f,0.38291f,0.39557f,0.41347f,0.43621f,0.46292f,0.49236f,0.52306f,0.55344f,0.58192f,0.60704f,0.62760f,0.64283f,0.65244f,0.65659f,0.65582f,0.65087f,0.64254f,0.63154f},

libraries/AP_Follow/AP_Follow.cpp

@@ -33,6 +33,14 @@ extern const AP_HAL::HAL& hal;
 
 #define AP_FOLLOW_POS_P_DEFAULT 0.1f    // position error gain default
 
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+#define AP_FOLLOW_ALT_TYPE_DEFAULT 0
+#else
+#define AP_FOLLOW_ALT_TYPE_DEFAULT AP_FOLLOW_ALTITUDE_TYPE_RELATIVE
+#endif
+
+AP_Follow *AP_Follow::_singleton;
+
 // table of user settable parameters
 const AP_Param::GroupInfo AP_Follow::var_info[] = {
 
@@ -117,7 +125,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     // @Description: Follow altitude type
     // @Values: 0:absolute, 1:relative
     // @User: Standard
-    AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALTITUDE_TYPE_RELATIVE),
+    AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
     AP_GROUPEND
@@ -131,6 +139,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
 AP_Follow::AP_Follow() :
         _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
+    _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
@@ -275,13 +284,10 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
     }
 
     // decode global-position-int message
-    if (msg.msgid == MAVLINK_MSG_ID_GLOBAL_POSITION_INT) {
-
-        // get estimated location and velocity (for logging)
-        Location loc_estimate{};
-        Vector3f vel_estimate;
-        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
+    bool updated = false;
 
+    switch (msg.msgid) {
+    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
         // decode message
         mavlink_global_position_int_t packet;
         mavlink_msg_global_position_int_decode(&msg, &packet);
@@ -296,13 +302,11 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
 
         // select altitude source based on FOLL_ALT_TYPE param 
         if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            // relative altitude
-            _target_location.alt = packet.relative_alt / 10;        // convert millimeters to cm
-            _target_location.relative_alt = 1;                // set relative_alt flag
+            // above home alt
+            _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
         } else {
             // absolute altitude
-            _target_location.alt = packet.alt / 10;                 // convert millimeters to cm
-            _target_location.relative_alt = 0;                // reset relative_alt flag
+            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
         }
 
         _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
@@ -320,6 +324,70 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg)
             _sysid.set(msg.sysid);
             _automatic_sysid = true;
         }
+        updated = true;
+        break;
+    }
+    case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // decode message
+        mavlink_follow_target_t packet;
+        mavlink_msg_follow_target_decode(&msg, &packet);
+
+        // ignore message if lat and lon are (exactly) zero
+        if ((packet.lat == 0 && packet.lon == 0)) {
+            return;
+        }
+        // require at least position
+        if ((packet.est_capabilities & (1<<0)) == 0) {
+            return;
+        }
+
+        Location new_loc = _target_location;
+        new_loc.lat = packet.lat;
+        new_loc.lng = packet.lon;
+        new_loc.set_alt_cm(packet.alt*100, Location::AltFrame::ABSOLUTE);
+
+        // FOLLOW_TARGET is always AMSL, change the provided alt to
+        // above home if we are configured for relative alt
+        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE &&
+            !new_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
+            return;
+        }
+        _target_location = new_loc;
+
+        if (packet.est_capabilities & (1<<1)) {
+            _target_velocity_ned.x = packet.vel[0]; // velocity north
+            _target_velocity_ned.y = packet.vel[1]; // velocity east
+            _target_velocity_ned.z = packet.vel[2]; // velocity down
+        } else {
+            _target_velocity_ned.zero();
+        }
+
+        // get a local timestamp with correction for transport jitter
+        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
+
+        if (packet.est_capabilities & (1<<3)) {
+            Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
+            float r, p, y;
+            q.to_euler(r,p,y);
+            _target_heading = degrees(y);
+            _last_heading_update_ms = _last_location_update_ms;
+        }
+
+        // initialise _sysid if zero to sender's id
+        if (_sysid == 0) {
+            _sysid.set(msg.sysid);
+            _automatic_sysid = true;
+        }
+        updated = true;
+        break;
+    }
+    }
+    
+    if (updated) {
+        // get estimated location and velocity
+        Location loc_estimate{};
+        Vector3f vel_estimate;
+        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
 
         // log lead's estimated vs reported position
 // @LoggerMessage: FOLL
@@ -420,3 +488,40 @@ void AP_Follow::clear_dist_and_bearing_to_target()
     _dist_to_target = 0.0f;
     _bearing_to_target = 0.0f;
 }
+
+// get target's estimated location and velocity (in NED), with offsets added
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const
+{
+    Vector3f ofs;
+    if (!get_offsets_ned(ofs) ||
+        !get_target_location_and_velocity(loc, vel_ned)) {
+        return false;
+    }
+    // apply offsets
+    loc.offset(ofs.x, ofs.y);
+    loc.alt -= ofs.z*100;
+    return true;
+}
+
+// return true if we have a target
+bool AP_Follow::have_target(void) const
+{
+    if (!_enabled) {
+        return false;
+    }
+
+    // check for timeout
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        return false;
+    }
+    return true;
+}
+
+namespace AP {
+
+AP_Follow &follow()
+{
+    return *AP_Follow::get_singleton();
+}
+
+}

libraries/AP_Follow/AP_Follow.h

@@ -38,6 +38,11 @@ public:
     // constructor
     AP_Follow();
 
+    // enable as singleton
+    static AP_Follow *get_singleton(void) {
+        return _singleton;
+    }
+
     // returns true if library is enabled
     bool enabled() const { return _enabled; }
 
@@ -57,6 +62,9 @@ public:
     // get target's estimated location and velocity (in NED)
     bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const;
 
+    // get target's estimated location and velocity (in NED), with offsets added
+    bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const;
+    
     // get distance vector to target (in meters), target plus offsets, and target's velocity all in NED frame
     bool get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned);
 
@@ -86,10 +94,14 @@ public:
     // get bearing to target (including offset) in degrees (for reporting purposes)
     float get_bearing_to_target() const { return _bearing_to_target; }
 
+    // get system time of last position update
+    uint32_t get_last_update_ms() const { return _last_location_update_ms; }
+
     // parameter list
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
+    static AP_Follow *_singleton;
 
     // get velocity estimate in m/s in NED frame using dt since last update
     bool get_velocity_ned(Vector3f &vel_ned, float dt) const;
@@ -132,3 +144,7 @@ private:
     // setup jitter correction with max transport lag of 3s
     JitterCorrection _jitter{3000};
 };
+
+namespace AP {
+    AP_Follow &follow();
+};

libraries/AP_HAL/RCOutput.cpp

@@ -86,15 +86,23 @@ uint32_t AP_HAL::RCOutput::calculate_bitrate_prescaler(uint32_t timer_clock, uin
         }
     }
 
-    // find the closest value
     const uint32_t freq = timer_clock / prescaler;
-    const float delta = fabsf(float(freq) - target_frequency);
-    if (freq > target_frequency
-        && delta > fabsf(float(timer_clock / (prescaler+1)) - target_frequency)) {
-        prescaler++;
-    } else if (freq < target_frequency
-        && delta > fabsf(float(timer_clock / (prescaler-1)) - target_frequency)) {
-        prescaler--;
+    // if using dshot then always pick the high value. choosing low seems to not agree with some
+    // ESCs despite the fact that BLHeli32 is supposed not to care what the bitrate is
+    if (is_dshot) {
+        if (freq < target_frequency) {
+            prescaler--;
+        }
+    } else {
+        // find the closest value
+        const float delta = fabsf(float(freq) - target_frequency);
+        if (freq > target_frequency
+            && delta > fabsf(float(timer_clock / (prescaler+1)) - target_frequency)) {
+            prescaler++;
+        } else if (freq < target_frequency
+            && delta > fabsf(float(timer_clock / (prescaler-1)) - target_frequency)) {
+            prescaler--;
+        }
     }
 
     return prescaler;

libraries/AP_HAL/tests/test_prescaler.cpp

@@ -11,7 +11,7 @@ protected:
         const uint32_t prescaler = AP_HAL::RCOutput::calculate_bitrate_prescaler(clock, target_rate, is_dshot);
         // we would like at most a 1% discrepancy in target versus actual
         const float rate_delta = fabsf(float(clock / prescaler) - target_rate) / target_rate;
-        EXPECT_TRUE(rate_delta < 0.13f);
+        EXPECT_TRUE(rate_delta < 0.20f);
     }
 };
 

libraries/AP_HAL_ChibiOS/AnalogIn.cpp

@@ -513,8 +513,9 @@ void AnalogIn::_timer_tick(void)
 
         _mcu_temperature = ((110 - 30) / (TS_CAL2 - TS_CAL1)) * (float(buf_adc3[1]) - TS_CAL1) + 30;
         _mcu_voltage = 3.3 * VREFINT_CAL / float(buf_adc3[2]+0.001);
-        _mcu_voltage_min = 3.3 * VREFINT_CAL / float(min_adc3[2]+0.001);
-        _mcu_voltage_max = 3.3 * VREFINT_CAL / float(max_adc3[2]+0.001);
+        // note min/max swap due to inversion
+        _mcu_voltage_min = 3.3 * VREFINT_CAL / float(max_adc3[2]+0.001);
+        _mcu_voltage_max = 3.3 * VREFINT_CAL / float(min_adc3[2]+0.001);
     }
 #endif
 }

libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/README.md

@@ -0,0 +1,87 @@
+# iFlight Beast H7 v2 55A AIO Flight Controller
+
+https://shop.iflight-rc.com/Beast-H7-V2-55A-AIO-MPU6000-25.5-25.5-pro1588
+
+The Beast H7 AIO is a flight controller produced by [iFlight](https://shop.iflight-rc.com/).
+
+## Features
+
+ - MCU: BGA-STM32H743
+ - Gyro: BMI270
+ - 16Mb Onboard Flash
+ - BEC output: 5V 2.5A 
+ - No Barometer
+ - OSD: AT7456E
+ - 5 UARTS: (UART1, UART2, UART3, UART4, UART7)
+ - I2C for external compass. UART3 pins are used for I2C (BRD_ALT_CONFIG=1)
+ - 5 PWM outputs (4 motors and 1 LED)
+
+## Pinout
+
+![Beast H7 v2 AIO Board](beast_h7v2_pinout.jpg "Beast H7 v2 AIO")
+
+## UART Mapping
+
+The UARTs are marked Rn and Tn in the above pinouts. The Rn pin is the
+receive pin for UARTn. The Tn pin is the transmit pin for UARTn.
+|Name|Pin|Function|
+|:-|:-|:-|
+|SERIAL0|COMPUTER|USB|
+|SERIAL1|RX1/TX1|UART1 (DJI connector)|
+|SERIAL2|TX2/RX2|UART2 (DJI connector, TX is on the back side of board)|
+|SERIAL3|TX3/RX3|UART3|
+|SERIAL4|TX4/RX4|UART4|
+|SERIAL7|TX7/RX7|UART7 (GPS)|
+
+All UARTS support DMA.
+
+## RC Input
+ 
+RC input is configured on the (UART2_RX/UART2_TX) pins which forms part of the DJI connector. It supports all RC protocols.
+  
+## OSD Support
+
+The Beast H7 v2 AIO supports OSD using OSD_TYPE 1 (MAX7456 driver).
+
+## PWM Output
+
+The Beast H7 AIO supports up to 4 PWM outputs. The pads for motor output ESC1 to ESC4 on the above diagram are for the 4 outputs. All 4 outputs support DShot as well as all PWM types.
+
+The PWM are in in two groups.
+
+Channels within the same group need to use the same output rate. If
+any channel in a group uses DShot then all channels in the group need
+to use DShot.
+
+## Battery Monitoring
+
+The board has a builtin voltage sensor. The voltage sensor can handle up to 6S
+LiPo batteries.
+
+The correct battery setting parameters are:
+
+ - BATT_MONITOR 4
+ - BATT_VOLT_PIN 12
+ - BATT_VOLT_MULT around 10.9
+ - BATT_CURR_PIN 13
+ - BATT_CURR_MULT around 28.5
+
+These are set by default in the firmware and shouldn't need to be adjusted
+
+## Compass
+
+The Beast H7 v2 AIO does not have a builtin compass, but you can attach an external compass to I2C pins. Default configuration does not have I2C bus enabled. You need to set BRD_ALT_CONFIG=1 to make I2C use RX3/TX3 pins.
+
+## NeoPixel LED
+
+The board includes a NeoPixel LED on the underside which is pre-configured to output ArduPilot sequences. This is the fifth PWM output.
+
+## Loading Firmware
+
+Initial firmware load can be done with DFU by plugging in USB with the
+bootloader button pressed. Then you should load the "with_bl.hex"
+firmware, using your favourite DFU loading tool.
+
+Once the initial firmware is loaded you can update the firmware using
+any ArduPilot ground station software. Updates should be done with the
+*.apj firmware files.

libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/defaults.parm

@@ -0,0 +1,3 @@
+# setup for LEDs on chan5
+SERVO5_FUNCTION 120
+NTF_LED_TYPES 257

libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/hwdef-bl.dat

@@ -0,0 +1,3 @@
+include ../BeastH7/hwdef-bl.dat
+undef APJ_BOARD_ID
+APJ_BOARD_ID 1056

libraries/AP_HAL_ChibiOS/hwdef/BeastH7v2/hwdef.dat

@@ -0,0 +1,59 @@
+# hw definition file for processing by chibios_pins.py
+# for iFlight Beast H7 v2 hardware, based on Beast H7 v1
+# thanks to betaflight for pin information
+
+include ../BeastH7/hwdef.dat
+undef APJ_BOARD_ID
+undef HAL_SERIAL3_PROTOCOL
+undef IMU
+undef BARO
+undef PD15
+undef PE9 PE11 PB8 PB9 PB11 PB10 PA10 PA9 PA3 PA2
+undef HAL_DEFAULT_INS_FAST_SAMPLE DMA_PRIORITY DMA_NOSHARE
+undef STM32_PWM_USE_ADVANCED
+undef I2C1
+
+# board ID for firmware load
+APJ_BOARD_ID 1056
+
+# V2 has different motor mapping
+PB4 TIM3_CH1 TIM3 PWM(2) GPIO(51) BIDIR   # 2
+PB5 TIM3_CH2 TIM3 PWM(3) GPIO(52)         # 3
+
+# only one I2C bus
+I2C_ORDER I2C2
+
+# order of UARTs (and USB)
+SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 EMPTY EMPTY UART7
+
+# Buzzer - DMA timer channel use by LEDs
+PD2 BUZZER OUTPUT GPIO(80) LOW
+
+# USART1 (DJI)
+PA10 USART1_RX USART1
+PA9  USART1_TX USART1 NODMA
+
+# USART2 (DJI RCIN)
+PA3 USART2_RX USART2
+PA2 USART2_TX USART2 NODMA
+define HAL_SERIAL2_PROTOCOL SerialProtocol_RCIN
+
+# USART3 (RCIN)
+PB11 USART3_RX USART3
+PB10 USART3_TX USART3
+
+# I2C2 for compass. These pins can also be used as USART3
+PB10 I2C2_SCL I2C2 ALT(1)
+PB11 I2C2_SDA I2C2 ALT(1)
+
+# spi devices
+SPIDEV bmi270    SPI1 DEVID1 MPU6000_CS  MODE3 10*MHZ 10*MHZ
+
+# no built-in compass and no external I2C so no compass
+define HAL_PROBE_EXTERNAL_I2C_COMPASSES
+
+# one IMU
+IMU BMI270 SPI:bmi270 ROTATION_ROLL_180_YAW_225
+
+# v2 has no BARO
+define HAL_BARO_ALLOW_INIT_NO_BARO 1

libraries/AP_HAL_ChibiOS/hwdef/CUAV_GPS/hwdef.dat

@@ -152,3 +152,6 @@ define HAL_PERIPH_ENABLE_BARO
 # startup
 define HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY 0
 define HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY 8
+
+# also enable buzzer
+define HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY 1

libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat

@@ -9,3 +9,5 @@ include ../CubeBlack/hwdef.dat
 # pull Solo's default parameters from /Tools/Frame_params
 # these are parameters the Solo requires for proper operation that are different from the 4 standard defaults.
 env DEFAULT_PARAMETERS 'Tools/Frame_params/Solo_Copter-4_GreenCube.param'
+
+AUTOBUILD_TARGETS Copter

libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat

@@ -51,3 +51,5 @@ define HAL_PROBE_EXTERNAL_I2C_COMPASSES
 
 define HAL_OREO_LED_ENABLED 1
 define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 5
+
+AUTOBUILD_TARGETS Copter

libraries/AP_HAL_ChibiOS/hwdef/FreeflyRTK/hwdef.dat

@@ -130,6 +130,7 @@ define AP_PARAM_MAX_EMBEDDED_PARAM 256
 # disable dual GPS and GPS blending to save flash space
 define GPS_MAX_RECEIVERS 1
 define GPS_MAX_INSTANCES 1
+define GPS_MOVING_BASELINE 1
 define HAL_COMPASS_MAX_SENSORS 1
 
 # GPS+MAG+BARO+Buzzer+NeoPixels

libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef-bl.dat

@@ -0,0 +1,86 @@
+# hw definition file for processing by chibios_pins.py
+
+# MCU class and specific type
+MCU STM32G4xx STM32G474xx
+
+FLASH_RESERVE_START_KB 0
+FLASH_BOOTLOADER_LOAD_KB 32
+
+# reserve some space for params
+APP_START_OFFSET_KB 4
+
+# assume 512k flash part
+FLASH_SIZE_KB 512
+
+# board ID for firmware load
+APJ_BOARD_ID 1053
+
+# setup build for a peripheral firmware
+env AP_PERIPH 1
+
+# debug on USART2
+STDOUT_SERIAL SD2
+STDOUT_BAUDRATE 57600
+
+# crystal frequency
+OSCILLATOR_HZ 16000000
+
+define CH_CFG_ST_FREQUENCY 1000000
+
+# order of UARTs
+SERIAL_ORDER USART2
+
+# blue LED
+PC10 LED_BOOTLOADER OUTPUT HIGH
+define HAL_LED_ON 0
+
+PB15 LED_RED OUTPUT HIGH
+PC6  LED_GREEN OUTPUT HIGH
+
+# USART2
+PA2 USART2_TX USART2
+PA3 USART2_RX USART2
+
+# SWD debugging
+PA13 JTMS-SWDIO SWD
+PA14 JTCK-SWCLK SWD
+
+define HAL_USE_SERIAL TRUE
+
+define STM32_SERIAL_USE_USART1 FALSE
+define STM32_SERIAL_USE_USART2 TRUE
+define STM32_SERIAL_USE_USART3 FALSE
+
+define HAL_NO_GPIO_IRQ
+define HAL_USE_EMPTY_IO TRUE
+
+# avoid timer and RCIN threads to save memory
+define HAL_NO_TIMER_THREAD
+define HAL_NO_RCIN_THREAD
+
+define DMA_RESERVE_SIZE 0
+
+define HAL_DISABLE_LOOP_DELAY
+
+# enable CAN support
+
+PA11 CAN1_RX CAN1
+PA12 CAN1_TX CAN1
+PC13 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW
+
+PB12 CAN2_RX CAN2
+PB13 CAN2_TX CAN2
+PB14 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW LOW
+
+define CAN_APP_NODE_NAME "org.ardupilot.HolybroG4_GPS"
+
+# make bl baudrate match debug baudrate for easier debugging
+define BOOTLOADER_BAUDRATE 57600
+
+# use a smaller bootloader timeout
+define HAL_BOOTLOADER_TIMEOUT 2500
+
+# Add CS pins to ensure they are high in bootloader
+PB1 BARO_CS CS
+PC4 GYRO_CS CS
+PC14 ICM_CS CS

libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat

@@ -0,0 +1,161 @@
+# hw definition file for processing by chibios_pins.py
+# MCU class and specific type
+
+# MCU class and specific type
+MCU STM32G4xx STM32G474xx
+
+FLASH_RESERVE_START_KB 36
+
+STORAGE_FLASH_PAGE 16
+define HAL_STORAGE_SIZE 800
+
+# board ID for firmware load
+APJ_BOARD_ID 1053
+
+# setup build for a peripheral firmware
+env AP_PERIPH 1
+
+# crystal frequency
+OSCILLATOR_HZ 16000000
+
+define CH_CFG_ST_FREQUENCY 1000000
+
+# assume 512k flash part
+FLASH_SIZE_KB 512
+
+# debug on USART2
+STDOUT_SERIAL SD2
+STDOUT_BAUDRATE 57600
+
+# order of UARTs
+SERIAL_ORDER USART2 USART3
+
+# sensor power control
+PC11 VDD_3V3_SENSORS_EN OUTPUT HIGH
+
+# LEDs
+PC10 LED OUTPUT HIGH GPIO(2) # blue
+PB15 LED_R OUTPUT HIGH GPIO(0)
+PC6  LED_G OUTPUT HIGH GPIO(1)
+
+define HAL_GPIO_A_LED_PIN 0
+define HAL_GPIO_B_LED_PIN 1
+define HAL_GPIO_C_LED_PIN 2
+
+define HAL_GPIO_LED_ON  0
+define HAL_GPIO_LED_OFF 1
+
+define HAL_HAVE_PIXRACER_LED
+
+# USART3, GPS
+PB10 USART3_TX USART3
+PB11 USART3_RX USART3
+
+# USART2, debug
+PA2 USART2_TX USART2
+PA3 USART2_RX USART2
+
+# SWD debugging
+PA13 JTMS-SWDIO SWD
+PA14 JTCK-SWCLK SWD
+
+# I2C1 bus
+PB7 I2C1_SDA I2C1
+PB8 I2C1_SCL I2C1
+
+# I2C2 bus
+PA8 I2C2_SDA I2C2
+PA9 I2C2_SCL I2C2
+
+define HAL_I2C_INTERNAL_MASK 3
+
+# I2C buses
+I2C_ORDER I2C1 I2C2
+
+# one SPI bus
+PA5 SPI1_SCK SPI1
+PA6 SPI1_MISO SPI1
+PA7 SPI1_MOSI SPI1
+
+# SPI CS
+PC4  GYR_CS CS
+PB1  ACC_CS CS
+PC14 ICM_CS  CS
+
+# GPS PPS
+PA15 GPS_PPS_IN INPUT
+
+# SPI devices
+SPIDEV bmi088_a SPI1 DEVID1  ACC_CS MODE3 10*MHZ 10*MHZ
+SPIDEV bmi088_g SPI1 DEVID2  GYR_CS MODE3 10*MHZ 10*MHZ
+
+# compass
+COMPASS BMM150 I2C:0:0x10 false ROTATION_YAW_90
+
+# baro
+BARO BMP388 I2C:0:0x77
+
+# IMU
+IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_ROLL_180_YAW_90
+
+define HAL_BARO_ALLOW_INIT_NO_BARO
+
+define HAL_USE_ADC FALSE
+define STM32_ADC_USE_ADC1 FALSE
+define HAL_DISABLE_ADC_DRIVER TRUE
+
+define HAL_NO_GPIO_IRQ
+
+# avoid RCIN thread to save memory
+define HAL_NO_RCIN_THREAD
+
+define HAL_USE_RTC FALSE
+define DISABLE_SERIAL_ESC_COMM TRUE
+
+define DMA_RESERVE_SIZE 0
+
+define HAL_DISABLE_LOOP_DELAY
+
+# enable CAN support
+
+PA11 CAN1_RX CAN1
+PA12 CAN1_TX CAN1
+PC13 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW
+
+PB12 CAN2_RX CAN2
+PB13 CAN2_TX CAN2
+PB14 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW LOW
+
+define CAN_APP_NODE_NAME "org.ardupilot.HolybroG4_GPS"
+
+define HAL_NO_LOGGING
+define HAL_NO_MONITOR_THREAD
+
+define HAL_MINIMIZE_FEATURES 0
+
+define HAL_DEVICE_THREAD_STACK 768
+
+# we setup a small defaults.parm
+define AP_PARAM_MAX_EMBEDDED_PARAM 256
+
+# disable dual GPS and GPS blending to save flash space
+define GPS_MAX_RECEIVERS 1
+define GPS_MAX_INSTANCES 1
+define HAL_COMPASS_MAX_SENSORS 1
+
+# GPS+MAG+BARO+LEDs
+define HAL_PERIPH_ENABLE_GPS
+define HAL_PERIPH_ENABLE_MAG
+define HAL_PERIPH_ENABLE_BARO
+define HAL_PERIPH_ENABLE_NOTIFY
+define HAL_PERIPH_ENABLE_RC_OUT
+
+# single baro
+define BARO_MAX_INSTANCES 1
+
+# GPS on 2nd port
+define HAL_PERIPH_GPS_PORT_DEFAULT 1
+
+# keep ROMFS uncompressed as we don't have enough RAM
+# to uncompress the bootloader at runtime
+env ROMFS_UNCOMPRESSED True

libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat

@@ -157,6 +157,7 @@ define HAL_WITH_DSP FALSE
 # --------------------- save flash ----------------------
 define AP_BATTMON_SMBUS_ENABLE 0
 define AP_BATTMON_FUEL_ENABLE 0
+define AP_BATT_MONITOR_MAX_INSTANCES 1
 define HAL_PARACHUTE_ENABLED 0
 define HAL_SPRAYER_ENABLED 0
 define HAL_GENERATOR_ENABLED 0
@@ -169,3 +170,4 @@ define HAL_NMEA_OUTPUT_ENABLED 0
 define HAL_BUTTON_ENABLED 0
 define HAL_OREO_LED_ENABLED 0
 define HAL_PICCOLO_CAN_ENABLE 0
+define BARO_MAX_INSTANCES 1

libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Airspeed/hwdef-bl.dat

@@ -0,0 +1,3 @@
+include ../MatekL431/hwdef-bl.inc
+
+define CAN_APP_NODE_NAME "org.ardupilot.MatekL431-Airspeed"