AP_Scripting: fixed float register save/restore in setjmp/longjmp

the register save must happen before the setjmp() call, which means
outside of the LUAI_TRY() macro. We also should be saving all 32
floating point registers

.github/workflows/ccache.env

@@ -1,6 +1,8 @@
 # common ccache env vars for CI
 export CCACHE_SLOPPINESS=file_stat_matches
 
+git config --global --add safe.directory ${GITHUB_WORKSPACE}
+
 mkdir -p ~/.ccache
 echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
 echo "compression = true" >> ~/.ccache/ccache.conf

.github/workflows/cygwin_build.yml

@@ -13,26 +13,27 @@ jobs:
       - uses: actions/checkout@v2
         with:
           submodules: 'recursive'
-      - name: Install cygwin
+      - uses: cygwin/cygwin-install-action@master
+        with:
+          packages: cygwin64 gcc-g++=10.2.0-1 python37 python37-future python37-lxml python37-pip python37-setuptools python37-wheel git procps gettext
+      - name: Prepare Python environment
         env:
           HOME: ${{ runner.workspace }}/ardupilot
         run: |
-          choco install cygwin --params "/InstallDir:C:\Cygwin /NoStartMenu /NoAdmin"
-          choco install cygwin32-gcc-g++ python37 python37-future python37-lxml python37-pip python37-setuptools python37-wheel git libexpat procps gettext --source cygwin
-          C:\Cygwin\bin\bash --login -c "ln -sf /usr/bin/python3.7 /usr/bin/python && ln -sf /usr/bin/pip3.7 /usr/bin/pip"
-          C:\Cygwin\bin\bash --login -c "python -m pip install empy pexpect"
-          C:\Cygwin\bin\bash --login -c "python -m pip install dronecan --upgrade"
+          bash --login -c "ln -sf /usr/bin/python3.7 /usr/bin/python && ln -sf /usr/bin/pip3.7 /usr/bin/pip"
+          bash --login -c "python -m pip install empy==3.3.4 pexpect"
+          bash --login -c "python -m pip install dronecan --upgrade"
       - name: Build SITL
         env:
           HOME: ${{ runner.workspace }}/ardupilot
         run: |
-          C:\Cygwin\bin\bash --login -c "Tools/scripts/cygwin_build.sh"
+          bash --login -c "Tools/scripts/cygwin_build.sh"
 
       - name: Check build files
         id: check_files
         uses: andstor/file-existence-action@v1
         with:
-          files: "artifacts/ArduPlane.elf.exe, artifacts/ArduCopter.elf.exe, artifacts/ArduHeli.elf.exe, artifacts/ArduRover.elf.exe, artifacts/ArduSub.elf.exe"
+          files: "artifacts/*"
           allow_failure: true
 
       - name: Archive build

.github/workflows/test_size.yml

@@ -1,6 +1,6 @@
 name: test size
 
-on: [push, pull_request, workflow_dispatch]
+on: [pull_request]
 # paths:
 # - "*"
 # - "!README.md" <-- don't rebuild on doc change
@@ -28,8 +28,8 @@ jobs:
     steps:
       - uses: actions/checkout@v2
         with:
-          ref: 'master'
-          path: 'master'
+          ref: ${{ github.event.pull_request.base.ref }}
+          path: base_branch
           submodules: 'recursive'
       # Put ccache into github cache for faster build
       - name: Prepare ccache timestamp
@@ -42,14 +42,14 @@ jobs:
         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: Build master ${{matrix.config}} ${{ matrix.toolchain }}
+          restore-keys: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-  # restore ccache from either previous build on this branch or on base branch
+      - name: Build ${{ github.event.pull_request.base.ref }} ${{matrix.config}} ${{ matrix.toolchain }}
         env:
           CI_BUILD_TARGET: ${{matrix.config}}
         shell: bash
         run: |
           PATH="/github/home/.local/bin:$PATH"
-          cd master
+          cd base_branch
           ./waf configure --board ${{matrix.config}}
           if [ "${{matrix.config}}" = "Hitec-Airspeed" ] ||
              [ "${{matrix.config}}" = "f103-GPS" ]; then
@@ -57,14 +57,14 @@ jobs:
           else
             ./waf
           fi
-          mkdir -p $GITHUB_WORKSPACE/master_bin
-          cp -r build/${{matrix.config}}/bin/* $GITHUB_WORKSPACE/master_bin/
+          mkdir -p $GITHUB_WORKSPACE/base_branch_bin
+          cp -r build/${{matrix.config}}/bin/* $GITHUB_WORKSPACE/base_branch_bin/
 
           # build a set of binaries without symbols so we can check if
           #  the binaries have changed.
-          echo [`date`] Building master with no versions
+          echo [`date`] Building ${{ github.event.pull_request.base.ref }} with no versions
 
-          NO_VERSIONS_DIR="$GITHUB_WORKSPACE/master_bin_no_versions"
+          NO_VERSIONS_DIR="$GITHUB_WORKSPACE/base_branch_bin_no_versions"
           mkdir "$NO_VERSIONS_DIR"
 
           if [ "${{matrix.config}}" = "Hitec-Airspeed" ] ||
@@ -75,7 +75,7 @@ jobs:
           fi
           cp -r build/${{matrix.config}}/bin/* "$NO_VERSIONS_DIR"
 
-          echo [`date`] Built master with no versions
+          echo [`date`] Built ${{ github.event.pull_request.base.ref }} with no versions
 
       - uses: actions/checkout@v2
         with:
@@ -92,8 +92,8 @@ jobs:
           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 fetch --no-tags --prune --progress ardupilot ${{ github.event.pull_request.base.ref }}
+          git rebase ardupilot/${{ github.event.pull_request.base.ref }}
           git submodule update --init --recursive --depth=1
           ./waf configure --board ${{matrix.config}}
           if [ "${{matrix.config}}" = "Hitec-Airspeed" ] ||
@@ -136,29 +136,29 @@ jobs:
             ls -l "$PLANE_BINARY"
           fi
 
-      - name: Full size compare with Master
+      - name: Full size compare with base branch
         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
+          Tools/scripts/pretty_diff_size.py -m $GITHUB_WORKSPACE/base_branch_bin -s $GITHUB_WORKSPACE/pr_bin
 
-      - name: Checksum compare with Master
+      - name: Checksum compare with ${{ github.event.pull_request.base.ref }}
         shell: bash
         run: |
-          diff -r $GITHUB_WORKSPACE/master_bin_no_versions $GITHUB_WORKSPACE/pr_bin_no_versions --exclude=*.elf --exclude=*.apj || true
+          diff -r $GITHUB_WORKSPACE/base_branch_bin_no_versions $GITHUB_WORKSPACE/pr_bin_no_versions --exclude=*.elf --exclude=*.apj || true
 
-      - name: elf_diff compare with Master
+      - name: elf_diff compare with ${{ github.event.pull_request.base.ref }}
         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
+             python3 -m elf_diff --bin_prefix=arm-none-eabi- --html_dir=elf_diff/AP_Periph $GITHUB_WORKSPACE/base_branch_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
+             python3 -m elf_diff --bin_prefix=arm-none-eabi- --html_dir=elf_diff/plane $GITHUB_WORKSPACE/base_branch_bin/arduplane $GITHUB_WORKSPACE/pr_bin/arduplane
+             python3 -m elf_diff --bin_prefix=arm-none-eabi- --html_dir=elf_diff/copter $GITHUB_WORKSPACE/base_branch_bin/arducopter $GITHUB_WORKSPACE/pr_bin/arducopter
           fi
           zip -r elf_diff.zip elf_diff
 

.github/workflows/test_unit_tests.yml

@@ -23,7 +23,7 @@ jobs:
         ]
         config: [
             unit-tests,
-            python-cleanliness,
+            # python-cleanliness, # DISABLED for 4.3.x
             sitl
             # examples,
         ]

AntennaTracker/servos.cpp

@@ -74,7 +74,7 @@ void Tracker::update_pitch_position_servo()
     // PITCH2SRV_IMAX   4000.000000
 
     // calculate new servo position
-    float new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.update_error(nav_status.angle_error_pitch);
+    float new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.update_error(nav_status.angle_error_pitch, G_Dt);
 
     // position limit pitch servo
     if (new_servo_out <= pitch_min_cd) {
@@ -125,7 +125,7 @@ void Tracker::update_pitch_onoff_servo(float pitch) const
 */
 void Tracker::update_pitch_cr_servo(float pitch)
 {
-    const float pitch_out = constrain_float(g.pidPitch2Srv.update_error(nav_status.angle_error_pitch), -(-g.pitch_min+g.pitch_max) * 100/2, (-g.pitch_min+g.pitch_max) * 100/2);
+    const float pitch_out = constrain_float(g.pidPitch2Srv.update_error(nav_status.angle_error_pitch, G_Dt), -(-g.pitch_min+g.pitch_max) * 100/2, (-g.pitch_min+g.pitch_max) * 100/2);
     SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, pitch_out);
 }
 
@@ -187,7 +187,7 @@ void Tracker::update_yaw_position_servo()
       right direction
      */
 
-    float servo_change = g.pidYaw2Srv.update_error(nav_status.angle_error_yaw);
+    float servo_change = g.pidYaw2Srv.update_error(nav_status.angle_error_yaw, G_Dt);
     servo_change = constrain_float(servo_change, -18000, 18000);
     float new_servo_out = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_yaw) + servo_change, -18000, 18000);
 
@@ -238,6 +238,6 @@ void Tracker::update_yaw_onoff_servo(float yaw) const
  */
 void Tracker::update_yaw_cr_servo(float yaw)
 {
-    const float yaw_out = constrain_float(-g.pidYaw2Srv.update_error(nav_status.angle_error_yaw), -g.yaw_range * 100/2, g.yaw_range * 100/2);
+    const float yaw_out = constrain_float(-g.pidYaw2Srv.update_error(nav_status.angle_error_yaw, G_Dt), -g.yaw_range * 100/2, g.yaw_range * 100/2);
     SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, yaw_out);
 }

ArduCopter/AP_Arming.cpp

@@ -162,6 +162,12 @@ bool AP_Arming_Copter::board_voltage_checks(bool display_failure)
 // all data loaded
 bool AP_Arming_Copter::terrain_database_required() const
 {
+
+    if (copter.wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER) {
+        // primary terrain source is from rangefinder, allow arming without terrain database
+        return false;
+    }
+
     if (copter.wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE &&
         copter.mode_rtl.get_alt_type() == ModeRTL::RTLAltType::RTL_ALTTYPE_TERRAIN) {
         return true;
@@ -380,7 +386,7 @@ bool AP_Arming_Copter::gps_checks(bool display_failure)
     }
 
     // warn about hdop separately - to prevent user confusion with no gps lock
-    if (copter.gps.get_hdop() > copter.g.gps_hdop_good) {
+    if ((copter.gps.num_sensors() > 0) && (copter.gps.get_hdop() > copter.g.gps_hdop_good)) {
         check_failed(ARMING_CHECK_GPS, display_failure, "High GPS HDOP");
         AP_Notify::flags.pre_arm_gps_check = false;
         return false;

ArduCopter/Attitude.cpp

@@ -1,5 +1,23 @@
 #include "Copter.h"
 
+/*************************************************************
+ *  Attitude Rate controllers and timing
+ ****************************************************************/
+
+// update rate controllers and output to roll, pitch and yaw actuators
+//  called at 400hz by default
+void Copter::run_rate_controller()
+{
+    // set attitude and position controller loop time
+    const float last_loop_time_s = AP::scheduler().get_last_loop_time_s();
+    motors->set_dt(last_loop_time_s);
+    attitude_control->set_dt(last_loop_time_s);
+    pos_control->set_dt(last_loop_time_s);
+
+    // run low level rate controllers that only require IMU data
+    attitude_control->rate_controller_run(); 
+}
+
 /*************************************************************
  *  throttle control
  ****************************************************************/

ArduCopter/Copter.cpp

@@ -143,7 +143,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
 #endif
     FAST_TASK(Log_Video_Stabilisation),
 
-    SCHED_TASK(rc_loop,              100,    130,  3),
+    SCHED_TASK(rc_loop,              250,    130,  3),
     SCHED_TASK(throttle_loop,         50,     75,  6),
     SCHED_TASK_CLASS(AP_GPS,               &copter.gps,                 update,          50, 200,   9),
 #if AP_OPTICALFLOW_ENABLED
@@ -750,10 +750,15 @@ bool Copter::get_wp_crosstrack_error_m(float &xtrack_error) const
     return true;
 }
 
-// get the target body-frame angular velocities in rad/s (Z-axis component used by some gimbals)
-bool Copter::get_rate_bf_targets(Vector3f& rate_bf_targets) const
+// get the target earth-frame angular velocities in rad/s (Z-axis component used by some gimbals)
+bool Copter::get_rate_ef_targets(Vector3f& rate_ef_targets) const
 {
-    rate_bf_targets = attitude_control->rate_bf_targets();
+    // always returns zero vector if landed or disarmed
+    if (copter.ap.land_complete) {
+        rate_ef_targets.zero();
+    } else {
+        rate_ef_targets = attitude_control->get_rate_ef_targets();
+    }
     return true;
 }
 

ArduCopter/Copter.h

@@ -687,7 +687,7 @@ private:
     bool get_wp_distance_m(float &distance) const override;
     bool get_wp_bearing_deg(float &bearing) const override;
     bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
-    bool get_rate_bf_targets(Vector3f& rate_bf_targets) const override;
+    bool get_rate_ef_targets(Vector3f& rate_ef_targets) const override;
 
     // Attitude.cpp
     void update_throttle_hover();
@@ -696,7 +696,7 @@ private:
     void set_accel_throttle_I_from_pilot_throttle();
     void rotate_body_frame_to_NE(float &x, float &y);
     uint16_t get_pilot_speed_dn() const;
-    void run_rate_controller() { attitude_control->rate_controller_run(); }
+    void run_rate_controller();
 
 #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
     void run_custom_controller() { custom_control.update(); }

ArduCopter/GCS_Mavlink.cpp

@@ -1,6 +1,8 @@
 #include "Copter.h"
 
 #include "GCS_Mavlink.h"
+#include <AP_RPM/AP_RPM_config.h>
+#include <AP_EFI/AP_EFI_config.h>
 
 MAV_TYPE GCS_Copter::frame_type() const
 {
@@ -534,10 +536,15 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
     MSG_MAG_CAL_PROGRESS,
     MSG_EKF_STATUS_REPORT,
     MSG_VIBRATION,
+#if AP_RPM_ENABLED
     MSG_RPM,
+#endif
     MSG_ESC_TELEMETRY,
     MSG_GENERATOR_STATUS,
     MSG_WINCH_STATUS,
+#if HAL_EFI_ENABLED
+    MSG_EFI_STATUS,
+#endif
 };
 static const ap_message STREAM_PARAMS_msgs[] = {
     MSG_NEXT_PARAM

ArduCopter/ReleaseNotes.txt

@@ -1,5 +1,305 @@
 ArduPilot Copter Release Notes:
 ------------------------------------------------------------------
+Copter 4.3.8-beta1 12-Aug-2023
+Changes from 4.3.7
+1) Bug fixes
+    - DroneCAN GPS RTK injection fix
+    - INAxxx battery monitors allow for battery reset remaining
+    - Notch filter gyro glitch caused by race condition fixed
+    - Scripting restart memory corruption bug fixed
+------------------------------------------------------------------
+Copter 4.3.7 31-May-2023 / 4.3.7-beta1 24-May-2023
+Changes from 4.3.6
+1) Bug fixes
+    a) EKF3 accel bias calculations bug fix
+    b) EKF3 accel bias process noise adjusted for greater robustness
+    c) GSF yaw numerical stability fix caused by compassmot
+    d) INS batch sampler fix to avoid watchdog if INS_LOG_BAT_CNT changed without rebooting
+    e) Memory corruption bug in the STM32H757 (very rare)
+    f) RC input on IOMCU bug fix (RC might not be regained if lost)
+------------------------------------------------------------------
+Copter 4.3.6 05-Apr-2023 / 4.3.6-beta1, 4.3.6--beta2 27-Mar-2023
+Changes from 4.3.5
+1) Bi-directional DShot fix for possible motor stop approx 72min after startup 
+------------------------------------------------------------------
+Copter Copter 4.3.5 14-Mar-2023 / 4.3.5-rc1 01-Mar-2023
+Changes from 4.3.4
+1) Bug fixes
+    a) GPS unconfigured error fix for non-M10 uBlox GPS
+    b) Gremsy gimbal fix when attached to autopilot's serial3 (or higher)
+    c) Landing detector fix with large AHRS_TRIM values (>0.1)
+    d) MambaF405 2022 gets VTX power on support
+    e) MCU voltage enabled on H757 CPUs (including CubeOrangePlus)
+    f) PiccoloCAN fix for ESC voltage and current scaling 
+    g) Servo gimbal mount yaw handling fix (only affects 3-axis servo gimbals)
+------------------------------------------------------------------
+Copter 4.3.4 01-Mar-2023
+Changes from 4.3.4-rc1
+1) Lua script PWMSource feature disabled (will be back in 4.4.x)
+------------------------------------------------------------------
+Copter 4.3.4-rc1 14-Feb-2023
+Changes from 4.3.3
+1) AutoPilot specific enhancements
+    a) CubeOrangePlusBG support
+    b) Foxeer ReaperF745 supports external compass
+    c) MambaH743v4 supports VTX power
+2) Precision landing option to resume tracking target after pilot repositioning
+3) Bug fixes
+    a) Arming check fix for terrain following with rangefinder (failed if terrain db was disabled)
+    b) Arming check fix if BARO_FIELD_ELEV set
+    c) Compass calibration diagonals set to 1,1,1 if incorrectly set to 0,0,0
+    d) FFT notch tune feature disabled (will be re-released in 4.4)
+    e) Gimbal's yaw feed-forward set to zero when landed (affects Gremsy gimbals)
+    f) IOMCU double reset and safety disable fix
+    g) Logging fix for duplicate format messages
+    h) OpenDroneId sets emergency status on crash or chute deploy
+    i) Peripheral firmware updates using MAVCAN fixed
+    j) RC protocol cannot be changed once detected on boards with IOMCU
+    k) Surface tracking uses filtered and corrected rangefinder values
+------------------------------------------------------------------
+Copter 4.3.3 20-Jan-2023
+Changes from 4.3.3-rc1
+1) Bug fixes
+    a) MAVFTP fix to terminate session error (could cause FTP failures)
+    b) IMU fast fifo reset log message max frequency reduced
+------------------------------------------------------------------
+Copter 4.3.3-rc1 09-Jan-2023
+Changes from 4.3.2
+1) Autopilot related changes
+    a) AIRLink LTE module enable pin and HEAT_ params added
+    b) CUAV Nora/Nora+ bdshot firmware (allows Bi-directional DShot)
+    c) CubeOrange, CubeYellow gets fast reset of ICM20602
+    d) MambaH743v2 with dual ICM42688 supported
+    e) PixPilot-V6
+2) Attitude and Navigation controllers use real-time dt (better handles variable or slow main loop)
+3) MAVFTP speed improvement including faster param download
+4) Bug fixes
+    a) Analog rangefinder GPIO pin arming check fixed
+    b) Arming check of AHRS/EKF vs GPS location disabled if GPS disabled
+    c) CRSF gets RC_OPTIONS for ELRS baudrate to avoid RC failsafes
+    d) Null pointer checks avoid watchdog when out of memory
+    e) Position Controller limit handling improved to avoid overshooting and hard landings
+    f) Position Controller internal error after 70min of flight fixed
+    g) PSC_ANGLE_MAX param reduction causing WPNAV_ACCEL to be set too low fixed
+    h) Servo gimbal yaw jump to opposite side fixed
+    i) Siyi A8 gimbal driver's record video feature fixed
+    j) SToRM32 serial gimbal driver actual angle reporting fixed (pitch and yaw angle signs were reversed)
+    k) Takeoff in Auto, Guided fixed when target altitude is current altitude
+    l) Takeoff in Auto handles baro drift before takeoff
+    m) Takeoff twitch due to velocity integrator init bug fixed
+------------------------------------------------------------------
+Copter 4.3.2 23-Dec-2022
+Changes from 4.3.2-rc1
+1) Reverted arming check that main loop is running at configured speed (e.g. SCHED_LOOP_RATE)
+------------------------------------------------------------------
+Copter 4.3.2-rc1 10-Dec-2022
+Changes from 4.3.1
+1) Arming check that main loop is running at configured speed (e.g. SCHED_LOOP_RATE)
+2) uBlox M10 support
+3) Autopilot specific changes
+    a) CubeOrange defaults to using 2nd IMU as primary
+    b) SIRF and SBP GPS disabled on BeastF7v2, MatekF405-STD, MAtekF405-Wing, omnibusf4pro
+4) Bug fixes
+    a) AutoTune gains loaded correctly during pilot testing
+    b) Camera driver's CAM_MIN_INTERVAL fixed if pilot manually triggers extra picture
+    c) EKF3 fix when using EK3_RNG_USE_HGT/SPD params and rangefinder provides bad readings
+    d) Main loop slowdown after arming fixed (parameter logging was causing delays)
+    e) Main loop's fast tasks always run (caused twitches in Loiter on heavily loaded CPUs)
+    f) MAVLink commands received on private channels checked for valid target sysid
+    g) ModalAI cameras support fixed (ODOMETRY message frame was consumed incorrectly)
+    h) Param reset after firmware load fixed on these boards
+        - BeastF7v2
+        - CubeYellow-bdshot
+        - f405-MatekAirspeed
+        - FlywooF745Nano
+        - KakuteF4Mini
+        - KakuteF7-bdshot
+        - MatekF405-bdshot
+        - MatekF405-STD
+        - MatekF405-Wing-bdshot
+        - MatekF765-SE
+        - MatekF765-Wing-bdshot
+    i) Siyi A8 gimbal support fixed
+    j) Windows builds move to compiling only 64-bit executables
+------------------------------------------------------------------
+Copter 4.3.1 05-Dec-2022 / 4.3.1-rc1 17-Nov-2022
+Changes from 4.3.0
+1) Autopilot specific enhancements
+   a) ARKV6X support
+   b) MatekH743 supports 8 bi-directional dshot channels
+   c) Pixhawk boards support MS5607 baros
+   d) SpeedbyBee F405v3 support
+2) DroneCAN Airspeed sensor support including hygrometer (aka water vapour) readings
+3) EFI support (electronic fuel injection engines)
+4) Pre-arm warning if multiple UARTs with SERIALx_PROTOCOL = RCIN
+5) Siyi gimbal support
+6) Bug fixes
+    a) Arm check warning loses duplicate "AHRS" prefix
+    b) AtomRCF405NAVI bootloader file name fixed
+    c) BRD_SAFETY_MASK fixed on boards with both FMU safety switch and IOMCU
+    d) Compass calibration continues even if a single compass's cal fails
+    e) Gremsy gimbal driver sends autopilot info at lower rate to save bandwidth
+    f) Invensense 42605 and 42609 IMUs use anti-aliasing filter and notch filter
+    g) Mode change to AUTOTUNE message shortened
+    h) OSD stats screen fix
+    i) RC input on serial port uses first UART with SERIALx_PROTOCOL = 23 (was using last)
+    j) RunCam caching fix with enablement and setup on 3-pos switch
+    k) RTK CAN GPS fix when GPSs conneted to separate CAN ports on autopilot
+    l) SkyViper GPS fix
+    m) Turtle mode safety fixes (e.g. can only enter Tutle mode with at zero throttle)
+------------------------------------------------------------------
+Copter 4.3.0 31-Oct-2022 / 4.3.0-beta4 24-Oct-2022
+Changes from 4.3.0-beta3
+1) Scripting supports implementing AUX functions
+2) Bug fixes
+    a) BMI085 accel scaling fixed
+    b) Build with gcc 11.3 fixed (developer only)
+    c) EKF3 alt discrepancy if GPS or baro alt changed soon after startup fixed
+    d) Harmonic Notch and ESC telem fix when motor outputs are non-contiguous
+    e) NMEA GPS's KSXT message parsing fixed (affected position accuracy)
+    f) Scripting random number generator fix
+------------------------------------------------------------------
+Copter 4.3.0-beta3 14-Oct-2022
+Changes from 4.3.0-beta2
+1) Pixhawk1-1M, fmuv2, fmuv3 display warning if firmware mismatches board's flash size (1M and 2M)
+2) Scripting support for multi-byte i2c reads
+3) Bug fixes
+    a) Airspeed CAN sensor ordering fixed (ordering could change if using multiple airspeed sensors)
+    b) BRD_SAFETY_MASK fix for enabling outputs when safety is on
+    c) Defaults.parm file processing fixed when a line has >100 characters and/or no new line (developer only)
+    d) NMEA serial output precision fixed (was only accurate to 1m, now accurate to 1cm)
+------------------------------------------------------------------
+Copter 4.3.0-beta2 4-Oct-2022
+Changes from 4.3.0-beta1
+1) Autopilot specific fixes and enhancements
+    a) AIRLink autopilot supports UART2
+    b) CUAV V6X supports CAN battery monitor by default
+    c) MatekF405-CAN board uses less memory to fix compass calibration issues
+    d) Pixhawk1-1M only supports uBlox and NMEA GPSs to save flash space
+    e) SkystarsH7HD-bdshot (allows Bi-directional DShot)
+    f) SkystarsH7HD supports VTX power by default
+2) EFI support
+    a) Currawong ECU support (added as Electronic Fuel Injection driver)
+    b) Scripting support for EFI drivers (allows writing EFI drivers in Lua)
+    c) SkyPower and HFE CAN EFI drivers (via scripting)
+3) Safety features
+    a) Arming check that SPIN_MIN less than 0.3 and greater than SPIN_ARM
+4) Minor enhancements
+    a) Autopilot board names max length increased to 23 characters (was 13)
+    b) CAN actuators can report PWM equivalent values (eases debugging)
+    c) Log download speed improved for boards with "block" backends
+    d) Notch filter slew limit reduces chance of notch freq moving incorrectly 
+    e) SLCAN disabled when vehicle is armed to reduce CPU load
+5) Bug fixes
+    a) DO_JUMP mission command fixed if active command changed before changing to Auto mode
+    b) EKF3 altitude error fix when using dual GPSs and affinity enabled
+    c) FFT indexing bug fixed
+    d) Gimbal mount fix to default mode (see MNTx_DEFLT_MODE parameter)
+    e) MSP fix to report arm status to DJI FPV goggles
+    f) Notch fix for non-throttle notch (was being incorrectly disabled)
+    g) OSD fixes for params, font and resolution
+    h) RPM reporting from harmonic notch fixed
+    i) "Sending unknown message (50)" warning removed
+    j) SBF/GSOF/NOVA GPS auto detction of baud rate fixed
+    k) VideoTX fixes for buffer overruns and Tramp video transmitter support
+------------------------------------------------------------------
+Copter 4.3.0-beta1 14-Sep-2022
+Changes from 4.2.3
+1) New autopilot support
+    a) AtomRCF405
+    b) CubeOrange-SimOnHardWare
+    c) DevEBoxH7v2
+    d) KakuteH7Mini-Nand
+    e) KakuteH7v2
+    f) Mamba F405 Mk4
+    g) SkystarsH7HD
+    h) bi-directional dshot (aka "bdshot") versions for CubeOrange, CubeYellow, KakuteF7, KakuteH7, MatekF405-Wing, Matek F765, PH4-mini, Pixhawk-1M
+2) EKF enhancements and fixes
+    a) EK3_GPS_VACC_MAX threshold to control when GPS altitude is used as alt source
+    b) EKF ring buffer fix for very slow sensor updates (those that update once every few seconds)
+    c) EKF3 source set change captured in Replay logs
+3) Gimbal enhancements
+    a) Angle limit params renamed and scaled to degrees (e.g. MNT1_ROLL_MIN, MNT1_PITCH_MIN, etc)
+    b) BrushlessPWM driver (set MNT1_TYPE = 7) is unstabilized Servo driver
+    c) Dual mount support (see MNT1_, MNT2 params)
+    d) Gremsy driver added (set MNT1_TYPE = 6)
+    e) MAVLink gimbalv2 support including sending GIMBAL_DEVICE_STATUS_UPDATE (replaces MOUNT_STATUS message)
+    f) "Mount Lock" auxiliary switch supports follow and lock modes in RC targetting (aka earth-frame and body-frame)    
+    g) RC channels to control gimbal set using RCx_OPTION = 212 (Roll), 213 (Pitch) or 214 (Yaw)
+    h) RC targetting rotation rate in deg/sec (see MNT1_RC_RATE which replaces MNT_JSTICK_SPD)
+    i) Yaw can be disabled on 3-axis gimbals (set MNTx_YAW_MIN = MNTx_YAW_MAX)
+4) Navigation and Flight mode enhancements
+    a) Auto mode ATTITUDE_TIME command allows specifying lean angle for specified number of seconds (GPS not required)
+    b) Auto mode support of DO_GIMBAL_MANAGER_PITCHYAW command
+    c) Auto mode LOITER_TURNS command max radius increased to 2.5km
+    d) AutoTune allows higher ANGLE_P gains
+    e) Guided mode support DO_CHANGE_SPEED commands
+    f) Manual modes throttle mix reduced (improves landing)
+    g) Payload touchdown detection reliability improved    
+    h) Takeoff detection improved to reduce chance of flip before takeoff if GPS moves
+    i) TKOFF_SLEW_TIME allows slower takeoffs in Auto, Guided, etc
+5) Notch filter enhancements
+    a) Attitude and filter logging at main loop rate
+    b) Batch sampler logging both pre and post-filter
+    c) FFT frame averaging
+    d) In-flight throttle notch parameter learning using averaged FFTs
+    e) Triple harmonic notch 
+5) RemoteId and SecureBoot enhancements
+    a) Remote update of secure boot's public keys (also allows remote unlocking of bootloader)
+6) Safety enhancements
+    a) Arming checks of FRAME_CLASS/TYPE made mandatory (even if ARMING_CHECK=0)
+    b) crash_dump.bin file saved to SD Card on startup (includes details re cause of software failures)
+    c) Dead-reckoning for 30sec on loss of GPS (requires wind estimation be enabled)
+    d) Dead-reckoning Lua script (On loss of GPS flies towards home for specified number of seconds)
+    e) Disabling Fence clears any active breaches (e.g. FENCE_TYPE = 0 will clear breaches)
+    f) "GPS Glitch" message clarified to "GPS Glitch or Compass error"
+    g) Pre-arm check that configured AHRS is being used (e.g. checks AHRS_EKF_TYPE not changed since boot)
+    h) Pre-arm check that gimbals are healthy (currently only for Gremsy gimbals, others in future release)
+    i) Pre-arm check that all motors are setup
+    j) Pre-arm check that scripts are running
+    k) Pre-arm check that terrain data loaded if RTL_ALT_TYPE set to Terrain
+    l) Pre-arm messages are correctly prefixed with "PreArm:" (instead of "Arm:")
+    m) RC auxiliary switch option for Arm / Emergency Stop
+    n) RC failsafe made pre-arm check (previously only triggered at arming)
+    o) RC failsafe option (see FS_OPTIONS) to continue in Guided obeyed even if GCS failsafe disabled
+    p) TKOFF_RPM_MIN checks all motors spinning before takeoff
+    q) Vibration compensation disabled in manual modes
+7) Scripting enhancements
+    a) CAN2 port bindings to allow scripts to communicate on 2nd CAN bus
+    b) ESC RPM bindings to allow scripts to report engine RPM
+    c) Gimbal bingings to allow scripts to control gimbal
+    d) Pre-arm check bindings (allows scripts to check if pre-arm checks have passed)
+    e) Semicolon (:) and period (.) supported (e.g both Logger:write() and Logger.write will work)
+8) Sensor driver enhancements
+    a) Benewake H30 radar support
+    b) BMI270 IMU performance improvements
+    c) IRC Tramp VTX suppor
+    d) Logging pause-able with auxiliary switch.  see RCx_OPTION = 165 (Pause Stream Logging)
+    e) Proximity sensor support for up to 3 sensors
+    f) Precision Landing consumes LANDING_TARGET MAVLink message's PositionX,Y,Z fields
+    g) RichenPower generator maintenance-required messages can be suppressed using GEN_OPTIONS param
+    h) TeraRanger Neo rangefinder support
+    i) GPS support to provide ellipsoid altitude instead of AMSL (see GPS_DRV_OPTIONS)
+    j) W25N01GV 1Gb flash support
+9) Bug fixes
+    a) Accel calibration throws away queued commands from GCS (avoids commands being run long after they were sent)
+    b) Airmode throttle mix at zero throttle fix
+    c) Cygbot proximity sensor fix to support different orientations (see PRXx_ORIENT)
+    d) Loiter fix to avoid potential wobble on 2nd takeoff (was not clearing non-zero attitude target from previous landing)
+    e) Lutan EFI message flood reduced
+    f) Missions download to GCS corruption avoided by checking serial buffer has space
+    g) Payload place fix so vehicle flies to specified Lat,Lon (if provided).  Previously it could get stuck
+    h) Safety switch disabled if IOMCU is disabled (see BRD_IO_ENABLE=0)
+    i) Script restart memory leak fixed
+    j) Takeoff vertical velocity limits enforced correctly even if PILOT_TKOFF_ALT set to a significant height
+10) Developer items
+    a) Custom controller support
+    b) Fast loop task list available in real-time using @SYS/tasks.txt
+    c) Parameter defaults sent to GCS with param FTP and recorded in onboard logs
+    d) ROS+ArduPilot environment installation script
+    e) Sim on Hardware allows simulator to run on autopilot (good for exhibitions)
+    f) Timer info available in real-time using @SYS/timers.txt
+------------------------------------------------------------------
 Copter 4.2.3 30-Aug-2022
 Changes from 4.2.3-rc3
 1) OpenDroneId bug fix to consume open-drone-id-system-update message

ArduCopter/mode.cpp

@@ -292,7 +292,7 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
     }
 
     if (!new_flightmode->init(ignore_checks)) {
-        mode_change_failed(new_flightmode, "initialisation failed");
+        mode_change_failed(new_flightmode, "init failed");
         return false;
     }
 
@@ -667,6 +667,13 @@ void Mode::land_run_horizontal_control()
                     AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE);
                 }
                 copter.ap.land_repo_active = true;
+#if PRECISION_LANDING == ENABLED
+            } else {
+                // no override right now, check if we should allow precland
+                if (copter.precland.allow_precland_after_reposition()) {
+                    copter.ap.land_repo_active = false;
+                }
+#endif
             }
         }
 

ArduCopter/mode.h

@@ -221,7 +221,6 @@ protected:
     static float auto_takeoff_no_nav_alt_cm;
 
     // auto takeoff variables
-    static float auto_takeoff_start_alt_cm;     // start altitude expressed as cm above ekf origin
     static float auto_takeoff_complete_alt_cm;  // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt)
     static bool auto_takeoff_terrain_alt;       // true if altitudes are above terrain
     static bool auto_takeoff_complete;          // true when takeoff is complete
@@ -1641,6 +1640,9 @@ protected:
     const char *name4() const override { return "TRTL"; }
 
 private:
+    void arm_motors();
+    void disarm_motors();
+
     float motors_output;
     Vector2f motors_input;
 };

ArduCopter/mode_guided.cpp

@@ -778,7 +778,7 @@ void ModeGuided::accel_control_run()
             auto_yaw.set_rate(0.0f);
         }
         pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false);
-        pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false);
+        pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false);
     } else {
         // update position controller with new target
         pos_control->input_accel_xy(guided_accel_target_cmss);
@@ -865,7 +865,7 @@ void ModeGuided::velaccel_control_run()
         // set position errors to zero
         pos_control->stop_pos_xy_stabilisation();
     }
-    pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false);
+    pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false);
 
     // call velocity controller which includes z axis controller
     pos_control->update_xy_controller();
@@ -904,7 +904,7 @@ void ModeGuided::pause_control_run()
 
     // 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);
+    pos_control->input_vel_accel_z(vel_z, 0.0, false);
 
     // call velocity controller which includes z axis controller
     pos_control->update_xy_controller();

ArduCopter/mode_turtle.cpp

@@ -18,12 +18,29 @@ bool ModeTurtle::init(bool ignore_checks)
         return false;
     }
 
-    // do not enter the mode if sticks are not centered
+    // do not enter the mode if sticks are not centered or throttle is not at zero
     if (!is_zero(channel_pitch->norm_input_dz())
         || !is_zero(channel_roll->norm_input_dz())
-        || !is_zero(channel_yaw->norm_input_dz())) {
+        || !is_zero(channel_yaw->norm_input_dz())
+        || !is_zero(channel_throttle->norm_input_dz())) {
         return false;
     }
+
+    // turn on notify leds
+    AP_Notify::flags.esc_calibration = true;
+
+    return true;
+}
+
+void ModeTurtle::arm_motors()
+{
+    if (hal.util->get_soft_armed()) {
+        return;
+    }
+
+    // stop the spoolup block activating
+    motors->set_spoolup_block(false);
+
     // reverse the motors
     hal.rcout->disable_channel_mask_updates();
     change_motor_direction(true);
@@ -36,8 +53,6 @@ bool ModeTurtle::init(bool ignore_checks)
     // arm
     motors->armed(true);
     hal.util->set_soft_armed(true);
-
-    return true;
 }
 
 bool ModeTurtle::allows_arming(AP_Arming::Method method) const
@@ -47,6 +62,18 @@ bool ModeTurtle::allows_arming(AP_Arming::Method method) const
 
 void ModeTurtle::exit()
 {
+    disarm_motors();
+
+    // turn off notify leds
+    AP_Notify::flags.esc_calibration = false;
+}
+
+void ModeTurtle::disarm_motors()
+{
+    if (!hal.util->get_soft_armed()) {
+        return;
+    }
+
     // disarm
     motors->armed(false);
     hal.util->set_soft_armed(false);
@@ -142,6 +169,14 @@ void ModeTurtle::run()
 // actually write values to the motors
 void ModeTurtle::output_to_motors()
 {
+    // throttle needs to be raised
+    if (is_zero(channel_throttle->norm_input_dz())) {
+        disarm_motors();
+        return;
+    }
+
+    arm_motors();
+
     // check if motor are allowed to spin
     const bool allow_output = motors->armed() && motors->get_interlock();
 

ArduCopter/radio.cpp

@@ -41,7 +41,6 @@ void Copter::init_rc_in()
  // init_rc_out -- initialise motors
 void Copter::init_rc_out()
 {
-    motors->set_loop_rate(scheduler.get_loop_rate_hz());
     motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
 
     // enable aux servos to cope with multiple output channels per motor

ArduCopter/surface_tracking.cpp

@@ -17,7 +17,7 @@ void Copter::SurfaceTracking::update_surface_offset()
         // e.g. if vehicle is 10m above the EKF origin and rangefinder reports alt of 3m.  curr_surface_alt_above_origin_cm is 7m (or 700cm)
         RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
         const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
-        const float curr_surface_alt_above_origin_cm = copter.inertial_nav.get_position_z_up_cm() - dir * rf_state.alt_cm;
+        const float curr_surface_alt_above_origin_cm = rf_state.inertial_alt_cm - dir * rf_state.alt_cm_filt.get();
 
         // update position controller target offset to the surface's alt above the EKF origin
         copter.pos_control->set_pos_offset_target_z_cm(curr_surface_alt_above_origin_cm);

ArduCopter/system.cpp

@@ -448,15 +448,15 @@ void Copter::allocate_motors(void)
 #if FRAME_CONFIG != HELI_FRAME
     if ((AP_Motors::motor_frame_class)g2.frame_class.get() == AP_Motors::MOTOR_FRAME_6DOF_SCRIPTING) {
 #if AP_SCRIPTING_ENABLED
-        attitude_control = new AC_AttitudeControl_Multi_6DoF(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s());
+        attitude_control = new AC_AttitudeControl_Multi_6DoF(*ahrs_view, aparm, *motors);
         ac_var_info = AC_AttitudeControl_Multi_6DoF::var_info;
 #endif // AP_SCRIPTING_ENABLED
     } else {
-        attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s());
+        attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors);
         ac_var_info = AC_AttitudeControl_Multi::var_info;
     }
 #else
-    attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors, scheduler.get_loop_period_s());
+    attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors);
     ac_var_info = AC_AttitudeControl_Heli::var_info;
 #endif
     if (attitude_control == nullptr) {
@@ -464,7 +464,7 @@ void Copter::allocate_motors(void)
     }
     AP_Param::load_object_from_eeprom(attitude_control, ac_var_info);
         
-    pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control, scheduler.get_loop_period_s());
+    pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control);
     if (pos_control == nullptr) {
         AP_BoardConfig::allocation_error("PosControl");
     }

ArduCopter/takeoff.cpp

@@ -4,7 +4,6 @@ Mode::_TakeOff Mode::takeoff;
 
 bool Mode::auto_takeoff_no_nav_active = false;
 float Mode::auto_takeoff_no_nav_alt_cm = 0;
-float Mode::auto_takeoff_start_alt_cm = 0;
 float Mode::auto_takeoff_complete_alt_cm = 0;
 bool Mode::auto_takeoff_terrain_alt = false;
 bool Mode::auto_takeoff_complete = false;
@@ -125,6 +124,8 @@ void Mode::auto_takeoff_run()
     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());
+        // update auto_takeoff_no_nav_alt_cm
+        auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
         return;
     }
 
@@ -159,6 +160,8 @@ void Mode::auto_takeoff_run()
         attitude_control->reset_yaw_target_and_rate();
         attitude_control->reset_rate_controller_I_terms();
         attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0);
+        // update auto_takeoff_no_nav_alt_cm
+        auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
         return;
     }
     
@@ -220,9 +223,13 @@ void Mode::auto_takeoff_run()
         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_takeoff_start_alt_cm) >= ((auto_takeoff_complete_alt_cm + terr_offset - auto_takeoff_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;
+    // takeoff complete when we are less than 1% of the stopping distance from the target altitude
+    // and 10% our maximum climb rate
+    const float vel_threshold_fraction = 0.1;
+    // stopping distance from vel_threshold_fraction * max velocity
+    const float stop_distance = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_cms()) / copter.pos_control->get_max_accel_z_cmss();
+    bool reached_altitude = copter.pos_control->get_pos_target_z_cm() >= pos_z - stop_distance;
+    bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * vel_threshold_fraction;
     auto_takeoff_complete = reached_altitude && reached_climb_rate;
 
     // calculate completion for location in case it is needed for a smooth transition to wp_nav
@@ -234,13 +241,14 @@ void Mode::auto_takeoff_run()
 
 void Mode::auto_takeoff_start(float complete_alt_cm, bool terrain_alt)
 {
-    auto_takeoff_start_alt_cm = inertial_nav.get_position_z_up_cm();
+    // auto_takeoff_complete_alt_cm is a problem if equal to auto_takeoff_start_alt_cm
     auto_takeoff_complete_alt_cm = complete_alt_cm;
     auto_takeoff_terrain_alt = terrain_alt;
     auto_takeoff_complete = false;
+    // initialise auto_takeoff_no_nav_alt_cm
+    auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
     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 = auto_takeoff_start_alt_cm + g2.wp_navalt_min * 100;
         auto_takeoff_no_nav_active = true;
     } else {
         auto_takeoff_no_nav_active = false;

ArduCopter/version.h

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

ArduPlane/AP_Arming.cpp

@@ -306,6 +306,18 @@ bool AP_Arming_Plane::arm(const AP_Arming::Method method, const bool do_arming_c
         return false;
     }
 
+    if (plane.update_home()) {
+        // after update_home the home position could still be
+        // different from the current_loc if the EKF refused the
+        // resetHeightDatum call. If we are updating home then we want
+        // to force the home to be the current_loc so relative alt
+        // takeoffs work correctly
+        if (plane.ahrs.set_home(plane.current_loc)) {
+            // update current_loc
+            plane.update_current_loc();
+        }
+    }
+
     change_arm_state();
 
     // rising edge of delay_arming oneshot

ArduPlane/ArduPlane.cpp

@@ -23,6 +23,7 @@
 #include "Plane.h"
 
 #define SCHED_TASK(func, rate_hz, max_time_micros, priority) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros, priority)
+#define FAST_TASK(func) FAST_TASK_CLASS(Plane, &plane, func)
 
 
 /*
@@ -49,16 +50,18 @@ SCHED_TASK_CLASS arguments:
  - expected time (in MicroSeconds) that the method should take to run
  - priority (0 through 255, lower number meaning higher priority)
 
+FAST_TASK entries are run on every loop even if that means the loop
+overruns its allotted time
  */
 const AP_Scheduler::Task Plane::scheduler_tasks[] = {
                            // Units:   Hz      us
-    SCHED_TASK(ahrs_update,           400,    400,   3),
+    FAST_TASK(ahrs_update),
+    FAST_TASK(update_control_mode),
+    FAST_TASK(stabilize),
+    FAST_TASK(set_servos),
     SCHED_TASK(read_radio,             50,    100,   6),
     SCHED_TASK(check_short_failsafe,   50,    100,   9),
     SCHED_TASK(update_speed_height,    50,    200,  12),
-    SCHED_TASK(update_control_mode,   400,    100,  15),
-    SCHED_TASK(stabilize,             400,    100,  18),
-    SCHED_TASK(set_servos,            400,    100,  21),
     SCHED_TASK(update_throttle_hover, 100,     90,  24),
     SCHED_TASK(read_control_switch,     7,    100,  27),
     SCHED_TASK(update_GPS_50Hz,        50,    300,  30),
@@ -403,10 +406,7 @@ void Plane::update_GPS_50Hz(void)
 {
     gps.update();
 
-    // get position from AHRS
-    have_position = ahrs.get_location(current_loc);
-    ahrs.get_relative_position_D_home(relative_altitude);
-    relative_altitude *= -1.0f;
+    update_current_loc();
 }
 
 /*
@@ -547,16 +547,16 @@ void Plane::update_alt()
             distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
         }
 
-        float target_alt = relative_target_altitude_cm();
+        tecs_target_alt_cm = relative_target_altitude_cm();
 
         if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.g2.flight_options & FlightOptions::CLIMB_BEFORE_TURN))) {
             // ensure we do the initial climb in RTL. We add an extra
             // 10m in the demanded height to push TECS to climb
             // quickly
-            target_alt = MAX(target_alt, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100;
+            tecs_target_alt_cm = MAX(tecs_target_alt_cm, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100;
         }
 
-        TECS_controller.update_pitch_throttle(target_alt,
+        TECS_controller.update_pitch_throttle(tecs_target_alt_cm,
                                                  target_airspeed_cm,
                                                  flight_stage,
                                                  distance_beyond_land_wp,
@@ -796,26 +796,33 @@ bool Plane::set_velocity_match(const Vector2f &velocity)
 
 #endif // AP_SCRIPTING_ENABLED
 
-#if OSD_ENABLED
 // correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL
 void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const
 {
-   pitch = ahrs.pitch;
-   roll = ahrs.roll;
 #if HAL_QUADPLANE_ENABLED
     if (quadplane.show_vtol_view()) {
-       return;
-   }
-#endif
-   if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) {  // correct for TRIM_PITCH_CD
-      pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
-      return;
-   }
-#if HAL_QUADPLANE_ENABLED
         pitch = quadplane.ahrs_view->pitch;
         roll = quadplane.ahrs_view->roll;
+        return;
+    }
 #endif
+    pitch = ahrs.pitch;
+    roll = ahrs.roll;
+    if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) {  // correct for TRIM_PITCH_CD
+        pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
+    }
+}
+
+/*
+  update current_loc Location
+ */
+void Plane::update_current_loc(void)
+{
+    have_position = plane.ahrs.get_location(plane.current_loc);
+
+    // re-calculate relative altitude
+    ahrs.get_relative_position_D_home(plane.relative_altitude);
+    relative_altitude *= -1.0f;
 }
-#endif
 
 AP_HAL_MAIN_CALLBACKS(&plane);

ArduPlane/Attitude.cpp

@@ -607,6 +607,7 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler)
     int16_t rudder_in = rudder_input();
 
     int16_t commanded_rudder;
+    bool using_rate_controller = false;
 
     // Received an external msg that guides yaw in the last 3 seconds?
     if (control_mode->is_guided_mode() &&
@@ -617,7 +618,10 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler)
         // user is doing an AUTOTUNE with yaw rate control
         const float rudd_expo = rudder_in_expo(true);
         const float yaw_rate = (rudd_expo/SERVO_MAX) * g.acro_yaw_rate;
-        commanded_rudder = yawController.get_rate_out(yaw_rate,  speed_scaler, false);
+        // add in the corrdinated turn yaw rate to make it easier to fly while tuning the yaw rate controller
+        const float coordination_yaw_rate = degrees(GRAVITY_MSS * tanf(radians(nav_roll_cd*0.01f))/MAX(aparm.airspeed_min,smoothed_airspeed));
+        commanded_rudder = yawController.get_rate_out(yaw_rate+coordination_yaw_rate,  speed_scaler, false);
+        using_rate_controller = true;
     } else {
         if (control_mode == &mode_stabilize && rudder_in != 0) {
             disable_integrator = true;
@@ -631,6 +635,13 @@ void Plane::calc_nav_yaw_coordinated(float speed_scaler)
     }
 
     steering_control.rudder = constrain_int16(commanded_rudder, -4500, 4500);
+
+    if (!using_rate_controller) {
+        /*
+          When not running the yaw rate controller, we need to reset the rate
+        */
+        yawController.reset_rate_PID();
+    }
 }
 
 /*
@@ -749,8 +760,7 @@ void Plane::calc_nav_roll()
 
         float bank_limit = degrees(atanf(guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f;
 
-        g2.guidedHeading.update_error(error); // push error into AC_PID , possible improvement is to use update_all instead.?
-        g2.guidedHeading.set_dt(delta);
+        g2.guidedHeading.update_error(error, delta); // push error into AC_PID , possible improvement is to use update_all instead.?
 
         float i = g2.guidedHeading.get_i(); // get integrator TODO
         if (((is_negative(error) && !guided_state.target_heading_limit_low) || (is_positive(error) && !guided_state.target_heading_limit_high))) {

ArduPlane/GCS_Mavlink.cpp

@@ -1,6 +1,9 @@
 #include "GCS_Mavlink.h"
 
 #include "Plane.h"
+#include <AP_RPM/AP_RPM_config.h>
+#include <AP_Airspeed/AP_Airspeed_config.h>
+#include <AP_EFI/AP_EFI_config.h>
 
 MAV_TYPE GCS_Plane::frame_type() const
 {
@@ -416,12 +419,58 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
     case MSG_LANDING:
         plane.landing.send_landing_message(chan);
         break;
+
+    case MSG_HYGROMETER:
+#if AP_AIRSPEED_HYGROMETER_ENABLE
+        CHECK_PAYLOAD_SIZE(HYGROMETER_SENSOR);
+        send_hygrometer();
+#endif
+        break;
+
     default:
         return GCS_MAVLINK::try_send_message(id);
     }
     return true;
 }
 
+#if AP_AIRSPEED_HYGROMETER_ENABLE
+void GCS_MAVLINK_Plane::send_hygrometer()
+{
+    if (!HAVE_PAYLOAD_SPACE(chan, HYGROMETER_SENSOR)) {
+        return;
+    }
+
+    const auto *airspeed = AP::airspeed();
+    if (airspeed == nullptr) {
+        return;
+    } 
+    const uint32_t now = AP_HAL::millis();
+
+    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
+        uint8_t idx = (i+last_hygrometer_send_idx+1) % AIRSPEED_MAX_SENSORS;
+        float temperature, humidity;
+        uint32_t last_sample_ms;
+        if (!airspeed->get_hygrometer(idx, last_sample_ms, temperature, humidity)) {
+            continue;
+        }
+        if (now - last_sample_ms > 2000) {
+            // not updating, stop sending
+            continue;
+        }
+        if (!HAVE_PAYLOAD_SPACE(chan, HYGROMETER_SENSOR)) {
+            return;
+        }
+
+        mavlink_msg_hygrometer_sensor_send(
+            chan,
+            idx,
+            int16_t(temperature*100),
+            uint16_t(humidity*100));
+        last_hygrometer_send_idx = idx;
+    }
+}
+#endif // AP_AIRSPEED_HYGROMETER_ENABLE
+
 
 /*
   default stream rates to 1Hz
@@ -567,12 +616,19 @@ static const ap_message STREAM_EXTRA1_msgs[] = {
     MSG_ATTITUDE,
     MSG_SIMSTATE,
     MSG_AHRS2,
+#if AP_RPM_ENABLED
     MSG_RPM,
+#endif
     MSG_AOA_SSA,
     MSG_PID_TUNING,
     MSG_LANDING,
     MSG_ESC_TELEMETRY,
+#if HAL_EFI_ENABLED
     MSG_EFI_STATUS,
+#endif
+#if AP_AIRSPEED_HYGROMETER_ENABLE
+    MSG_HYGROMETER,
+#endif
 };
 static const ap_message STREAM_EXTRA2_msgs[] = {
     MSG_VFR_HUD

ArduPlane/GCS_Mavlink.h

@@ -2,6 +2,7 @@
 
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Logger/AP_Logger.h>
+#include <AP_Airspeed/AP_Airspeed_config.h>
 
 class GCS_MAVLINK_Plane : public GCS_MAVLINK
 {
@@ -71,6 +72,11 @@ private:
     uint8_t high_latency_wind_direction() const override;
 #endif // HAL_HIGH_LATENCY2_ENABLED
 
+#if AP_AIRSPEED_HYGROMETER_ENABLE
+    void send_hygrometer();
+    uint8_t last_hygrometer_send_idx;
+#endif
+
     MAV_VTOL_STATE vtol_state() const override;
     MAV_LANDED_STATE landed_state() const override;
 

ArduPlane/Log.cpp

@@ -147,7 +147,8 @@ struct PACKED log_Nav_Tuning {
     float   airspeed_error;
     int32_t target_lat;
     int32_t target_lng;
-    int32_t target_alt;
+    int32_t target_alt_wp;
+    int32_t target_alt_tecs;
     int32_t target_airspeed;
 };
 
@@ -166,7 +167,8 @@ void Plane::Log_Write_Nav_Tuning()
         airspeed_error      : airspeed_error,
         target_lat          : next_WP_loc.lat,
         target_lng          : next_WP_loc.lng,
-        target_alt          : next_WP_loc.alt,
+        target_alt_wp       : next_WP_loc.alt,
+        target_alt_tecs     : tecs_target_alt_cm,
         target_airspeed     : target_airspeed_cm,
     };
     logger.WriteBlock(&pkt, sizeof(pkt));
@@ -306,16 +308,17 @@ const struct LogStructure Plane::log_structure[] = {
 // @Field: Dist: distance to the current navigation waypoint
 // @Field: TBrg: bearing to the current navigation waypoint
 // @Field: NavBrg: the vehicle's desired heading
-// @Field: AltErr: difference between current vehicle height and target height
+// @Field: AltE: difference between current vehicle height and target height
 // @Field: XT: the vehicle's current distance from the current travel segment
 // @Field: XTi: integration of the vehicle's crosstrack error
-// @Field: AspdE: difference between vehicle's airspeed and desired airspeed
+// @Field: AsE: difference between vehicle's airspeed and desired airspeed
 // @Field: TLat: target latitude
 // @Field: TLng: target longitude
-// @Field: TAlt: target altitude
-// @Field: TAspd: target airspeed
+// @Field: TAW: target altitude WP
+// @Field: TAT: target altitude TECS
+// @Field: TAsp: target airspeed
     { LOG_NTUN_MSG, sizeof(log_Nav_Tuning),         
-      "NTUN", "QfcccfffLLii",  "TimeUS,Dist,TBrg,NavBrg,AltErr,XT,XTi,AspdE,TLat,TLng,TAlt,TAspd", "smddmmmnDUmn", "F0BBB0B0GGBB" , true },
+      "NTUN", "QfcccfffLLeee",  "TimeUS,Dist,TBrg,NavBrg,AltE,XT,XTi,AsE,TLat,TLng,TAW,TAT,TAsp", "smddmmmnDUmmn", "F0BBB0B0GG000" , true },
 
 // @LoggerMessage: ATRP
 // @Description: Plane AutoTune

ArduPlane/Plane.h

@@ -946,7 +946,13 @@ private:
 
     // commands.cpp
     void set_guided_WP(const Location &loc);
-    void update_home();
+
+    // update home position. Return true if update done
+    bool update_home();
+
+    // update current_loc
+    void update_current_loc(void);
+
     // set home location and store it persistently:
     bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;
 
@@ -984,9 +990,7 @@ private:
 
     // ArduPlane.cpp
     void disarm_if_autoland_complete();
-# if OSD_ENABLED
     void get_osd_roll_pitch_rad(float &roll, float &pitch) const override;
-#endif
     float tecs_hgt_afe(void);
     void efi_update(void);
     void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
@@ -1206,6 +1210,9 @@ private:
     // mode reason for entering previous mode
     ModeReason previous_mode_reason = ModeReason::UNKNOWN;
 
+    // last target alt we passed to tecs
+    int32_t tecs_target_alt_cm;
+
 public:
     void failsafe_check(void);
 #if AP_SCRIPTING_ENABLED

ArduPlane/RC_Channel.cpp

@@ -309,6 +309,7 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
         switch (ch_flag) {
         case AuxSwitchPos::HIGH:
             plane.quadplane.air_mode = AirMode::ON;
+            plane.quadplane.throttle_wait = false;
             break;
         case AuxSwitchPos::MIDDLE:
             break;
@@ -359,6 +360,7 @@ bool RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit
         RC_Channel::do_aux_function_armdisarm(ch_flag);
         if (plane.arming.is_armed()) {
             plane.quadplane.air_mode = AirMode::ON;
+            plane.quadplane.throttle_wait = false;
         }
         break;
 

ArduPlane/ReleaseNotes.txt

@@ -1,3 +1,383 @@
+Release 4.3.8-beta1 12th August 2023
+------------------------------------
+
+Changes since 4.3.7:
+
+ - fixed scripting restart bug which could cause a crash on a math error
+ - fixed RTK injection when first module is a moving baseline BASE
+ - fixed auto-enable of fence on forced arm
+ - fixed race condition that can cause gyro glitches with notch filtering
+ - fixed INAxxx battery monitors to allow for battery reset remaining
+
+Release 4.3.7 31st May 2023
+---------------------------
+
+This stable release is for the 4.3.x stable series and is being done
+because of a serious bug that has been found with RC input on boards
+that use an IOMCU for RC input (boards with a separate set of 8 "main"
+outputs from "aux" outputs).
+
+The bug was that when RC input is lost and the receiver is one that
+uses "no pulses" for loss of RC input then there is a chance that when
+the RC link is regained that ArduPilot will not regain RC control and
+will continue in RC failsafe.
+
+The bug is an old one, first introduced in the 4.0.6 release in
+September 2020. The bug does not occur often which is why it has been
+such a long time before it was noticed. We would like to thank CUAV
+for noticing and reporting the bug!
+
+This release also has some other changes, some of which are to sync
+with the Copter 4.3.6 release (which will go to 4.3.7 with this RC
+input bug fix) and some are other bugs found since the 4.3.5 plane
+release.
+
+This release skips the 4.3.6 number to sync with copter.
+
+The full list of changes is:
+
+ - fixed a fault in the INS batch sampler code if you change the INS_LOG_BAT_CNT parameter without rebooting
+ - fixed the RC input on IOMCU bug explained above
+ - fixed a bug in ICE engine control if you do a "delay engine start" mission command while flying
+ - added MCU voltage monitoring for the H757 microcontroller (eg. CubeOrangePlus)
+ - servo gimbal mount yaw handling fix (only affects 3-axis servo gimbals)
+ - PiccoloCAN fix for ESC voltage and current scaling
+ - Gremsy gimbal fix when attached to autopilot's serial3 (or higher)
+ - added CubeOrangePlus-bdshot build
+ - fixed a bug in handling bad UART data in the megasquirt serial EFI driver
+ - added -g option for configuring with debug symbols without full debug (helped with RCIN bug diagnosis)
+ - fixed airmode switch for QACRO and QSTABILIZE modes
+ - fixed a rare memory corruption bug in the STM32H757
+ - fixed an EKF3 bug in accel bias calculations
+ - adjust EKF3 accel bias process noise for greater robustness
+ - cope with compassmot impacting GSF yaw numerical stability
+
+
+Please test and report any issues!
+
+Release 4.3.5 26th March  2023
+------------------------------
+
+- fixed 32 bit microsecond wrap in BDShot code
+
+This release has a single bug fix for a critical bug for anyone using
+bi-directional DShot on their vehicle. If using bi-directional DShot
+and the vehicle is running its motors when 32 bit microsecond time
+wraps at 71 minutes (or multiples of 71 minutes) then the bug that is
+fixed in this release has an approximately 1 in 3 chance of causing
+the motor to stop.
+
+Note that the time is the time since boot, not the time since arming,
+so even vehicles flying for a short time could be vulnerable if they
+sit for a long time on the ground before takeoff.
+
+Release 4.3.4 1st March  2023
+-----------------------------
+
+- support CubeOrangePlus BG edition
+- enable VTX power on MambaH743v4
+- probe external compasses on Foxeer Reaper F745
+- fixed home update on bad GPS quality
+- fixed GPS unconfigured error for non-M10 uBlox GPS
+- don't allow RC protocol change on IOMCU once detected
+- fixed FBWA pitch limits when in VTOL qassist
+- fixed handling of zero compass diagonals
+- added an output buffer to MAVCAN
+- set emergency status in OpenDroneID on crash or chute deploy
+- avoid logging duplicate format messages
+- fixed bug in alt error arming check with BARO_FIELD_ELEV set
+- fixed handling of double IOMCU reset and safety disable
+- enable VTX power on MambaF405 2022
+- disable PWMSource feature in lua scripts (will be back in 4.4.x)
+- fixed throttle wait on rudder arming in quadplanes
+- fixed earth frame accel compensation for AHRS_TRIM
+
+Happy flying!
+
+Release 4.3.3 Jan 19th 2023
+---------------------------
+
+ - AIRLink LTE module enable pin added
+ - CUAV Nora/Nora+ bdshot firmware (allows Bi-directional DShot)
+ - CubeOrange, CubeYellow gets fast reset of ICM20602
+ - PixPilot-V6 support
+ - Attitude and Navigation controllers use real-time dt (better handles variable or slow main loop)
+ - Analog rangefinder GPIO pin arming check fixed
+ - Arming check of AHRS/EKF vs GPS location disabled if GPS disabled
+ - Position Controller limit handling improved to avoid overshooting and hard landings
+ - PSC_ANGLE_MAX param reduction causing WPNAV_ACCEL to be set too low fixed
+ - Servo gimbal yaw jump to opposite side fixed
+ - Siyi A8 gimbal driver's record video feature fixed
+ - SToRM32 serial gimbal driver actual angle reporting fixed (pitch and yaw angle signs were reversed)
+ - Takeoff in Auto, Guided fixed when target altitude is current altitude
+ - Takeoff in Auto handles baro drift before takeoff
+ - Takeoff twitch due to velocity integrator init bug fixed
+ - moved FTP MAVLink transfers to FTP thread and better control bandwidth
+ - switch to QRTL if indide RTL radius when using Q_RTL_MODE=3 (approach)
+ - check for 3 good frames for CRSF
+ - allow for ELRS at 420kbaud
+ - support MambaH743-v2
+ - support MambaF405-2022B
+ - fixed nullptr checks on new
+ - fixed a bug in terrain handling for ship landing lua script checks on new
+
+Happy flying!
+
+Release 4.3.2 Dec 23rd 2022
+---------------------------
+
+This is a minor release with bug fixes for the 4.3.x release series.
+
+Changes from 4.3.1:
+
+ - improved uBlox M10 support
+ - CubeOrange defaults to using 2nd IMU as primary
+ - SIRF and SBP GPS disabled on BeastF7v2, MatekF405-STD, MAtekF405-Wing, omnibusf4pro
+ - fixed loading of autotune gains during pilot testing
+ - Fixed CAM_MIN_INTERVAL to cope with mission and pilot triggering
+ - EKF3 fix when using EK3_RNG_USE_HGT/SPD params and rangefinder provides bad readings
+ - Main loop slowdown after arming fixed (parameter logging was causing delays)
+ - changed to 'fast task' scheme for critical loop updates
+ - MAVLink commands received on private channels checked for valid target sysid
+ - ModalAI camera support fixed (ODOMETRY message frame was consumed incorrectly)
+ - Param reset after firmware load fixed on several boards
+ - Siyi A8 gimbal support fixed
+ - Windows builds move to compiling only 64-bit executables
+ - ARKV6X support
+ - MatekH743 supports 8 bi-directional dshot channels
+ - Pixhawk1 boards support MS5607 baros
+ - SpeedbyBee F405v3 support
+ - DroneCAN Airspeed sensor support including hygrometer readings
+ - Pre-arm warning if multiple UARTs with SERIALx_PROTOCOL = RCIN
+ - Siyi gimbal support
+ - Arm check warning loses duplicate "AHRS" prefix
+ - AtomRCF405NAVI bootloader file name fixed
+ - BRD_SAFETY_MASK fixed on boards with both FMU safety switch and IOMCU
+ - Compass calibration continues even if a single compass's cal fails
+ - Gremsy gimbal driver sends autopilot info at lower rate to save bandwidth
+ - Invensense 42605 and 42609 IMUs use anti-aliasing filter and notch filter
+ - OSD stats screen fix
+ - RC input on serial port uses first UART with SERIALx_PROTOCOL = 23 (was using last)
+ - RunCam caching fix with enablement and setup on 3-pos switch
+ - RTK CAN GPS fix when GPSs conneted to separate CAN ports on autopilot
+ - fixed yaw rate for fixed wing autotune
+
+Release 4.3.1 24th Oct 2022
+---------------------------
+
+This is a minor release with some important fixes:
+
+ - fixed build with gcc 11.3
+ - fixed random number generator in lua core
+ - scale VTOL angle P with airspeed in quadplane back-transiton
+ - added support for implementing AUX functions in lua scripts
+ - fixed BMI085 accel scaling
+ - fixed KSXT NMEA parsing affecting position resolution
+ - fixed race condition in TECS control leading to 'nod' in forward transiton
+ - allow for expansion of notch filters to fix notch of fwd motors in quadplanes
+ - added logging of TECS target alt
+ - fixed EKF3 altitude discrepancy with GPS or baro alt change on startup
+ - allow auto mode sequencing to land in a fence breach
+
+Happy flying!
+
+Release 4.3.0 9th Oct 2022
+--------------------------
+
+The is the first 4.3.x stable release for plane. It is a major release
+with a lot of changes since 4.2.3.
+
+Changes since 4.3.0beta3:
+
+ - added 1M and 2M flash warning checks for for fmuv2, fmuv3 and Pixhawk1-1M firmwares
+ - added support for multi-byte i2c reads in scripting
+
+The change list from 4.2.3 stable is very long. Here are the main changes:
+
+ - fixed BRD_SAFETY_MASK for enabling outputs when safety on
+ - fixed persistence of mapping of CAN airspeed sensors to instances
+ - fixed precision of NMEA serial output function
+ - added report of "Engine Running" when using ICE
+ - fixed handling of defaults.parm files with lines over 100 chars
+ - fixed handling of defaults.parm files with no newline on last line
+ - fixed possible divide by zero when changing to GUIDED on quadplanes
+ - fixes for VideoTX, fixing buffer overrun and tramp handling
+ - fixed spurious error about sending RPM when RPM disabled
+ - fixed an EKF3 lane switch issue that can cause incorrect height with dual GPS
+ - fixed mission cmd to mission int initialisation error
+ - fixed mission jump tracking init on startup
+ - fixed OSD view roll/pitch error for tailsitters
+ - added SkystarsH7HD-bdshot
+ - fixed SkystarsH7HD VTX control
+ - reduced memory usage on MatekF405-CAN board
+ - disable SLCAN when armed to reduce CPU load
+ - enable CAN battery mon on CUAV V6X by default
+ - added arming check for Q_M_SPIN_MIN value too high
+ - fixed reporting of RPM from harmonic notch
+ - improved handling of airspeed errors and airspeed auto disable
+ - fixed SERVO_AUTO_TRIM for multiple outputs of same type
+ - fixed auto baud rate detection on SBF/GSOF/NOVA GPS
+ - increased max board name length for mavlink statustext to 23
+ - fixed incorrect disable of notches for non-throttle notch
+ - added notch filter slew limit to reduce notch errors
+ - added ARMING_OPTIONS to control display of pre-arm errors
+ - several OSD fixes for params, font and resolution
+ - support PWM type transmission for CAN PWM output
+ - support Currawong ECU as EFI backend
+ - support lua scripts for EFI backends
+ - implement SkyPower and HFE CAN EFI lua scripts
+ - improved speed of log download with dataflash block backends
+ - disabled all GPS drivers except uBlox and NMEA on Pixhawk1-1M to save flash
+ - disabled all GPS drivers except uBlox on MatekF405-bdshot and omnibusf4pro-bdshot
+ - fixed FFT indexing bug
+ - added USART2 for AIRLink
+ - allow reset to default airspeed using mission item DO_CHANGE_SPEED
+ - added new boards AtomRCF405, KakuteH7Mini-Nand, SkystarsH7HD
+ - added bi-directional dshot for several new boards
+ - EK3_GPS_VACC_MAX threshold to control when GPS altitude is used as alt source
+ - EKF ring buffer fix for slow sensor updates
+ - EKF3 source set change captured in replay logs
+ - numerous gimbal support improvements
+ - improved RemoteId support
+ - SecureBoot support with remote update of secure boot public keys
+ - crash_dump.bin file saved to SD Card on startup (includes details re cause of software failures)
+ - several new pre-arm checks (AHRS type, scripts, terrain)
+ - numerous scripting improvements
+ - fixed scripting restart leaking memory
+ - Benewake H30 radar support
+ - BMI270 IMU performance improvements
+ - Logging pause with auxiliary switch
+ - TeraRanger Neo rangefinder support
+ - support for both AMSL and ellipsoid height in most GPS drivers
+ - Custom controller support
+ - parameter defaults sent with param FTP and onboard logs
+ - Sim on Hardware allows simulator to run on autopilot
+ - added Q_LAND_ALTCHG parameter
+ - added climb before QRTL for safer QRTL from low altitudes
+ - added support for logging pre and post filtered FFT data
+ - support triple-notch harmonic notch filter
+ - support up to 32 actuators (with SERVO_32_ENABLE parameter)
+ - support EFI input over DroneCAN
+ - by default only run notch filter on first IMU
+ - added ESC_TLM_MAV_OFS parameter for mapping ESCs to MAVLink ESC telemetry
+ - added Q_NAVALT_MIN for quadplane takeoff
+ - added ICE redline governor
+ - added in-flight FFT notch tuning option
+ - added Sagetech ADSB support
+ - added INS_HNTCH_FM_RAT parameter for handling under-hover throttle
+ - improvements to filtering on ICM42xxx IMUs
+ - added option parameters to NAV_VTOL_LAND mission item for fixed wing approach
+
+Many thanks to the huge number of developers, testers and documenters
+who contributed to the 4.3.0 release!
+
+Special thanks to all the ArduPilot Parters who have worked closely
+with us on the 4.3.x development and testing, with many partners
+contributing suggestions or supporting particular features.
+
+Release 4.3.0beta3 7th Oct 2022
+-------------------------------
+
+This is the third beta of the 4.3.0 stable release. Changes since
+beta1 are:
+
+ - fixed BRD_SAFETY_MASK for enabling outputs when safety on
+ - fixed persistence of mapping of CAN airspeed sensors to instances
+ - fixed precision of NMEA serial output function
+ - added report of "Engine Running" when using ICE
+ - fixed handling of defaults.parm files with lines over 100 chars
+ - fixed handling of defaults.parm files with no newline on last line
+ - fixed possible divide by zero when changing to GUIDED on quadplanes
+
+Happy flying!
+
+Release 4.3.0beta2 3rd Oct 2022
+-------------------------------
+
+This is the second beta of the 4.3.0 stable release. Changes since
+beta1 are:
+
+ - fixes for VideoTX, fixing buffer overrun and tramp handling
+ - fixed spurious error about sending RPM when RPM disabled
+ - fixed an EKF3 lane switch issue that can cause incorrect height with dual GPS
+ - fixed mission cmd to mission int initialisation error
+ - fixed mission jump tracking init on startup
+ - fixed OSD view roll/pitch error for tailsitters
+ - added SkystarsH7HD-bdshot
+ - fixed SkystarsH7HD VTX control
+ - reduced memory usage on MatekF405-CAN board
+ - disable SLCAN when armed to reduce CPU load
+ - enable CAN battery mon on CUAV V6X by default
+ - added arming check for Q_M_SPIN_MIN value too high
+ - fixed reporting of RPM from harmonic notch
+ - improved handling of airspeed errors and airspeed auto disable
+ - fixed SERVO_AUTO_TRIM for multiple outputs of same type
+ - fixed auto baud rate detection on SBF/GSOF/NOVA GPS
+ - increased max board name length for mavlink statustext to 23
+ - fixed incorrect disable of notches for non-throttle notch
+ - added notch filter slew limit to reduce notch errors
+ - added ARMING_OPTIONS to control display of pre-arm errors
+ - several OSD fixes for params, font and resolution
+ - support PWM type transmission for CAN PWM output
+ - support Currawong ECU as EFI backend
+ - support lua scripts for EFI backends
+ - implement SkyPower and HFE CAN EFI lua scripts
+ - improved speed of log download with dataflash block backends
+ - disabled all GPS drivers except uBlox and NMEA on Pixhawk1-1M to save flash
+ - disabled all GPS drivers except uBlox on MatekF405-bdshot and omnibusf4pro-bdshot
+ - fixed FFT indexing bug
+ - added USART2 for AIRLink
+ - allow reset to default airspeed using mission item DO_CHANGE_SPEED
+
+Happy flying!
+
+
+Release 4.3.0beta1 13th Sep 2022
+--------------------------------
+
+This is the first beta of the 4.3.0 stable release. There are a lot of
+changes since the 4.2.3 stable release. Key changes are:
+
+- added new boards AtomRCF405, KakuteH7Mini-Nand, SkystarsH7HD
+- added bi-directional dshot for several new boards
+- EK3_GPS_VACC_MAX threshold to control when GPS altitude is used as alt source
+- EKF ring buffer fix for slow sensor updates
+- EKF3 source set change captured in replay logs
+- numerous gimbal support improvements
+- improved RemoteId support
+- SecureBoot support with remote update of secure boot public keys
+- crash_dump.bin file saved to SD Card on startup (includes details re cause of software failures)
+- several new pre-arm checks (AHRS type, scripts, terrain)
+- numerous scripting improvements
+- fixed scripting restart leaking memory
+- Benewake H30 radar support
+- BMI270 IMU performance improvements
+- Logging pause with auxiliary switch
+- TeraRanger Neo rangefinder support
+- support for both AMSL and ellipsoid height in most GPS drivers
+- Custom controller support
+- parameter defaults sent with param FTP and onboard logs
+- Sim on Hardware allows simulator to run on autopilot
+- added Q_LAND_ALTCHG parameter
+- added climb before QRTL for safer QRTL from low altitudes
+- added support for logging pre and post filtered FFT data
+- support triple-notch harmonic notch filter
+- support up to 32 actuators (with SERVO_32_ENABLE parameter)
+- support EFI input over DroneCAN
+- by default only run notch filter on first IMU
+- added ESC_TLM_MAV_OFS parameter for mapping ESCs to MAVLink ESC telemetry
+- added Q_NAVALT_MIN for quadplane takeoff
+- added ICE redline governor
+- added in-flight FFT notch tuning option
+- added Sagetech ADSB support
+- added INS_HNTCH_FM_RAT parameter for handling under-hover throttle
+- improvements to filtering on ICM42xxx IMUs
+- added option parameters to NAV_VTOL_LAND mission item for fixed wing approach
+
+Please report flight tests of the 4.3.0beta series on discuss.ardupilot.org
+
+Happy flying!
+
 Release 4.2.3 21st August 2022
 ------------------------------
 

ArduPlane/commands.cpp

@@ -105,11 +105,13 @@ void Plane::set_guided_WP(const Location &loc)
   update home location from GPS
   this is called as long as we have 3D lock and the arming switch is
   not pushed
+
+  returns true if home is changed
 */
-void Plane::update_home()
+bool Plane::update_home()
 {
     if (hal.util->was_watchdog_armed()) {
-        return;
+        return false;
     }
     if ((g2.home_reset_threshold == -1) ||
         ((g2.home_reset_threshold > 0) &&
@@ -118,24 +120,30 @@ void Plane::update_home()
         // significantly. This allows us to cope with slow baro drift
         // but not re-do home and the baro if we have changed height
         // significantly
-        return;
+        return false;
     }
-    if (ahrs.home_is_set() && !ahrs.home_is_locked()) {
+    bool ret = false;
+    if (ahrs.home_is_set() && !ahrs.home_is_locked() && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
         Location loc;
-        if(ahrs.get_location(loc) && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
+        if (ahrs.get_location(loc)) {
             // we take the altitude directly from the GPS as we are
             // about to reset the baro calibration. We can't use AHRS
             // altitude or we can end up perpetuating a bias in
             // altitude, as AHRS alt depends on home alt, which means
             // we would have a circular dependency
             loc.alt = gps.location().alt;
-            if (!AP::ahrs().set_home(loc)) {
-                // silently fail
-            }
+            ret = AP::ahrs().set_home(loc);
         }
     }
+
+    // even if home is not updated we do a baro reset to stop baro
+    // drift errors while disarmed
     barometer.update_calibration();
     ahrs.resetHeightDatum();
+
+    update_current_loc();
+
+    return ret;
 }
 
 bool Plane::set_home_persistently(const Location &loc)

ArduPlane/commands_logic.cpp

@@ -937,7 +937,11 @@ bool Plane::do_change_speed(const AP_Mission::Mission_Command& cmd)
     switch (cmd.content.speed.speed_type)
     {
     case 0:             // Airspeed
-        if ((cmd.content.speed.target_ms >= aparm.airspeed_min.get()) && (cmd.content.speed.target_ms <= aparm.airspeed_max.get()))  {
+        if (is_equal(cmd.content.speed.target_ms, -2.0f)) {
+            new_airspeed_cm = -1; // return to default airspeed
+            return true;
+        } else if ((cmd.content.speed.target_ms >= aparm.airspeed_min.get()) &&
+                   (cmd.content.speed.target_ms <= aparm.airspeed_max.get()))  {
             new_airspeed_cm = cmd.content.speed.target_ms * 100; //new airspeed target for AUTO or GUIDED modes
             gcs().send_text(MAV_SEVERITY_INFO, "Set airspeed %u m/s", (unsigned)cmd.content.speed.target_ms);
             return true;
@@ -1015,6 +1019,8 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
     const int8_t direction = is_negative(radius) ? -1 : 1;
     const float abs_radius = fabsf(radius);
 
+    loiter.direction = direction;
+
     switch (vtol_approach_s.approach_stage) {
         case RTL:
             {

ArduPlane/mode.cpp

@@ -50,6 +50,7 @@ bool Mode::enter()
     plane.guided_state.target_heading_type = GUIDED_HEADING_NONE;
     plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane.
     plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane.
+    plane.guided_state.target_alt_time_ms = 0;
     plane.guided_state.last_target_alt = 0;
 #endif
 

ArduPlane/mode.h

@@ -341,7 +341,7 @@ protected:
 private:
 
     // Switch to QRTL if enabled and within radius
-    bool switch_QRTL(bool check_loiter_target = true);
+    bool switch_QRTL();
 };
 
 class ModeStabilize : public Mode

ArduPlane/mode_qhover.cpp

@@ -8,7 +8,7 @@ bool ModeQHover::_enter()
     // set vertical speed and acceleration limits
     pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z);
     pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z);
-    quadplane.set_climb_rate_cms(0, false);
+    quadplane.set_climb_rate_cms(0);
 
     quadplane.init_throttle_wait();
     return true;

ArduPlane/mode_qloiter.cpp

@@ -103,13 +103,13 @@ void ModeQLoiter::run()
             quadplane.ahrs.set_touchdown_expected(true);
         }
 
-        quadplane.set_climb_rate_cms(-descent_rate_cms, descent_rate_cms>0);
+        pos_control->land_at_climb_rate_cm(-descent_rate_cms, descent_rate_cms>0);
         quadplane.check_land_complete();
     } else if (plane.control_mode == &plane.mode_guided && quadplane.guided_takeoff) {
-        quadplane.set_climb_rate_cms(0, false);
+        quadplane.set_climb_rate_cms(0);
     } else {
         // update altitude target and call position controller
-        quadplane.set_climb_rate_cms(quadplane.get_pilot_desired_climb_rate_cms(), false);
+        quadplane.set_climb_rate_cms(quadplane.get_pilot_desired_climb_rate_cms());
     }
     quadplane.run_z_controller();
 }

ArduPlane/mode_qrtl.cpp

@@ -88,7 +88,7 @@ void ModeQRTL::run()
                                                                           quadplane.get_weathervane_yaw_rate_cds());
 
             // climb at full WP nav speed
-            quadplane.set_climb_rate_cms(quadplane.wp_nav->get_default_speed_up(), false);
+            quadplane.set_climb_rate_cms(quadplane.wp_nav->get_default_speed_up());
             quadplane.run_z_controller();
 
             ftype alt_diff;

ArduPlane/mode_rtl.cpp

@@ -9,6 +9,8 @@ bool ModeRTL::_enter()
 #if HAL_QUADPLANE_ENABLED
     plane.vtol_approach_s.approach_stage = Plane::Landing_ApproachStage::RTL;
 
+    // Quadplane specific checks
+    if (plane.quadplane.available()) {
         // treat RTL as QLAND if we are in guided wait takeoff state, to cope
         // with failsafes during GUIDED->AUTO takeoff sequence
         if (plane.quadplane.guided_wait_takeoff_on_mode_enter) {
@@ -16,8 +18,27 @@ bool ModeRTL::_enter()
             return true;
         }
 
-    // do not check if we have reached the loiter target if switching from loiter this will trigger as the nav controller has not yet proceeded the new destination
-    switch_QRTL(false);
+        // if Q_RTL_MODE is QRTL always, immediately switch to QRTL mode
+        if (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::QRTL_ALWAYS) {
+            plane.set_mode(plane.mode_qrtl, ModeReason::QRTL_INSTEAD_OF_RTL);
+            return true;
+        }
+
+        // if VTOL landing is expected and quadplane motors are active and within QRTL radius and under QRTL altitude then switch to QRTL
+        const bool vtol_landing = (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::SWITCH_QRTL) || (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL);
+        if (vtol_landing && (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED)) {
+            uint16_t qrtl_radius = abs(plane.g.rtl_radius);
+            if (qrtl_radius == 0) {
+                qrtl_radius = abs(plane.aparm.loiter_radius);
+            }
+            int32_t alt_cm;
+            if ((plane.current_loc.get_distance(plane.next_WP_loc) < qrtl_radius) &&
+                plane.current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_cm) && (alt_cm < plane.quadplane.qrtl_alt*100)) {
+                plane.set_mode(plane.mode_qrtl, ModeReason::QRTL_INSTEAD_OF_RTL);
+                return true;
+            }
+        }
+    }
 #endif
 
     return true;
@@ -60,10 +81,10 @@ void ModeRTL::update()
 void ModeRTL::navigate()
 {
 #if HAL_QUADPLANE_ENABLED
-    if (plane.control_mode->mode_number() != QRTL) {
+    if ((plane.control_mode->mode_number() != QRTL) && plane.quadplane.available()) {
         // QRTL shares this navigate function with RTL
 
-        if (plane.quadplane.available() && (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL)) {
+        if (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::VTOL_APPROACH_QRTL) {
             // VTOL approach landing
             AP_Mission::Mission_Command cmd;
             cmd.content.location = plane.next_WP_loc;
@@ -118,24 +139,18 @@ void ModeRTL::navigate()
 
 #if HAL_QUADPLANE_ENABLED
 // Switch to QRTL if enabled and within radius
-bool ModeRTL::switch_QRTL(bool check_loiter_target)
+bool ModeRTL::switch_QRTL()
 {
-    if (!plane.quadplane.available() || ((plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::SWITCH_QRTL) && (plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::QRTL_ALWAYS))) {  
+    if (plane.quadplane.rtl_mode != QuadPlane::RTL_MODE::SWITCH_QRTL) {
         return false;
     }
 
-   // if Q_RTL_MODE is QRTL always, then immediately switch to QRTL mode
-   if (plane.quadplane.rtl_mode == QuadPlane::RTL_MODE::QRTL_ALWAYS) {
-       plane.set_mode(plane.mode_qrtl, ModeReason::QRTL_INSTEAD_OF_RTL);
-       return true;
-   }
-
     uint16_t qrtl_radius = abs(plane.g.rtl_radius);
     if (qrtl_radius == 0) {
         qrtl_radius = abs(plane.aparm.loiter_radius);
     }
 
-    if ( (check_loiter_target && plane.nav_controller->reached_loiter_target()) ||
+    if (plane.nav_controller->reached_loiter_target() ||
          plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc) ||
          plane.auto_state.wp_distance < MAX(qrtl_radius, plane.quadplane.stopping_distance())) {
         /*

ArduPlane/navigation.cpp

@@ -143,9 +143,9 @@ void Plane::calc_airspeed_errors()
     // Get the airspeed_estimate, update smoothed airspeed estimate
     // NOTE:  we use the airspeed estimate function not direct sensor
     //        as TECS may be using synthetic airspeed
-    float airspeed_measured = 0;
+    float airspeed_measured = 0.1;
     if (ahrs.airspeed_estimate(airspeed_measured)) {
-        smoothed_airspeed = smoothed_airspeed * 0.8f + airspeed_measured * 0.2f;
+        smoothed_airspeed = MAX(0.1, smoothed_airspeed * 0.8f + airspeed_measured * 0.2f);
     }
 
     // low pass filter speed scaler, with 1Hz cutoff, at 10Hz

ArduPlane/quadplane.cpp

@@ -258,7 +258,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: quadplane options
     // @Description: Level Transition:Keep wings within LEVEL_ROLL_LIMIT and only use forward motor(s) for climb during transition, Allow FW Takeoff: If bit is not set then NAV_TAKEOFF command on quadplanes will instead perform a NAV_VTOL takeoff, Allow FW Land:If bit is not set then NAV_LAND command on quadplanes will instead perform a NAV_VTOL_LAND, Vtol Takeoff Frame: command NAV_VTOL_TAKEOFF altitude is as set by the command's reference frame rather than a delta above current location, Always use FW spiral approach:Always use Use a fixed wing spiral approach for VTOL landings, USE QRTL:instead of QLAND for rc failsafe when in VTOL modes, Use Governor:Use ICE Idle Governor in MANUAL for forward motor, Force Qassist: on always,Mtrs_Only_Qassist: in tailsitters only, uses VTOL motors and not flying surfaces for QASSIST, Airmode_On_Arm:Airmode enabled when arming by aux switch, Disarmed Yaw Tilt:Enable motor tilt for yaw when disarmed, Delay Spoolup:Delay VTOL spoolup for 2 seconds after arming, ThrLandControl: enable throttle stick control of landing rate, DisableApproach: Disable use of approach and airbrake stages in VTOL landing, EnableLandResposition: enable pilot controlled repositioning in AUTO land. Descent will pause while repositioning. ARMVTOL: Arm only in VTOL or AUTO modes. CompleteTransition: to fixed wing if Q_TRANS_FAIL timer times out instead of QLAND. Force RTL mode: forces RTL mode on rc failsafe in VTOL modes overriding bit 5(USE_QRTL).
-    // @Bitmask: 0:Level Transition,1:Allow FW Takeoff,2:Allow FW Land,3:Vtol Takeoff Frame,4:Always use FW spiral approach,5:Use QRTL,6:Use Governor,7:Force Qassist,8:Mtrs_Only_Qassist,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed,13:Disable Ground Effect Compensation,14:Ignore forward flight angle limits in Qmodes,15:ThrLandControl,16:DisableApproach,17:EnableLandReposition,18:ARMVtol, 19: CompleteTransition if Q_TRANS_FAIL, 20: Force RTL mode on VTOL failsafes overriding bit 5(USE QRTL)
+    // @Bitmask: 0:Level Transition,1:Allow FW Takeoff,2:Allow FW Land,3:Vtol Takeoff Frame,4:Always use FW spiral approach,5:Use QRTL,6:Use Governor,7:Force Qassist,8:Mtrs_Only_Qassist,10:Disarmed Yaw Tilt,11:Delay Spoolup,12:disable Qassist based on synthetic airspeed,13:Disable Ground Effect Compensation,14:Ignore forward flight angle limits in Qmodes,15:ThrLandControl,16:DisableApproach,17:EnableLandReposition,18:ARMVtol, 19: CompleteTransition if Q_TRANS_FAIL, 20: Force RTL mode on VTOL failsafes overriding bit 5(USE QRTL), 21:Tilt rotor tilt motors up when disarmed in FW modes (except manual) to prevent ground strikes
     AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0),
 
     AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2),
@@ -607,7 +607,6 @@ bool QuadPlane::setup(void)
     if (!enable || hal.util->get_soft_armed()) {
         return false;
     }
-    float loop_delta_t = 1.0 / plane.scheduler.get_loop_rate_hz();
 
     /*
       cope with upgrade from old AP_Motors values for frame_class
@@ -683,12 +682,12 @@ bool QuadPlane::setup(void)
 
     switch ((AP_Motors::motor_frame_class)frame_class) {
     case AP_Motors::MOTOR_FRAME_TRI:
-        motors = new AP_MotorsTri(plane.scheduler.get_loop_rate_hz(), rc_speed);
+        motors = new AP_MotorsTri(rc_speed);
         motors_var_info = AP_MotorsTri::var_info;
         break;
     case AP_Motors::MOTOR_FRAME_TAILSITTER:
         // this is a duo-motor tailsitter
-        tailsitter.tailsitter_motors = new AP_MotorsTailsitter(plane.scheduler.get_loop_rate_hz(), rc_speed);
+        tailsitter.tailsitter_motors = new AP_MotorsTailsitter(rc_speed);
         motors = tailsitter.tailsitter_motors;
         motors_var_info = AP_MotorsTailsitter::var_info;
         break;
@@ -699,7 +698,7 @@ bool QuadPlane::setup(void)
 #endif // AP_SCRIPTING_ENABLED
             break;
     default:
-        motors = new AP_MotorsMatrix(plane.scheduler.get_loop_rate_hz(), rc_speed);
+        motors = new AP_MotorsMatrix(rc_speed);
         motors_var_info = AP_MotorsMatrix::var_info;
         break;
     }
@@ -716,13 +715,13 @@ bool QuadPlane::setup(void)
         AP_BoardConfig::allocation_error("ahrs_view");
     }
 
-    attitude_control = new AC_AttitudeControl_TS(*ahrs_view, aparm, *motors, loop_delta_t);
+    attitude_control = new AC_AttitudeControl_TS(*ahrs_view, aparm, *motors);
     if (!attitude_control) {
         AP_BoardConfig::allocation_error("attitude_control");
     }
 
     AP_Param::load_object_from_eeprom(attitude_control, attitude_control->var_info);
-    pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control, loop_delta_t);
+    pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control);
     if (!pos_control) {
         AP_BoardConfig::allocation_error("pos_control");
     }
@@ -963,7 +962,7 @@ void QuadPlane::run_z_controller(void)
         // never run Z controller in tailsitter transtion
         return;
     }
-    if ((now - last_pidz_active_ms) > 20) {
+    if ((now - last_pidz_active_ms) > 20 || !pos_control->is_active_z()) {
         // 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);
 
@@ -1004,9 +1003,9 @@ void QuadPlane::check_yaw_reset(void)
     }
 }
 
-void QuadPlane::set_climb_rate_cms(float target_climb_rate_cms, bool force_descend)
+void QuadPlane::set_climb_rate_cms(float target_climb_rate_cms)
 {
-    pos_control->input_vel_accel_z(target_climb_rate_cms, 0, force_descend);
+    pos_control->input_vel_accel_z(target_climb_rate_cms, 0, false);
 }
 
 /*
@@ -1024,7 +1023,7 @@ void QuadPlane::hold_hover(float target_climb_rate_cms)
     multicopter_attitude_rate_update(get_desired_yaw_rate_cds(false));
 
     // call position controller
-    set_climb_rate_cms(target_climb_rate_cms, false);
+    set_climb_rate_cms(target_climb_rate_cms);
 
     run_z_controller();
 }
@@ -1325,6 +1324,12 @@ void QuadPlane::set_armed(bool armed)
     if (plane.control_mode == &plane.mode_guided) {
         guided_wait_takeoff = armed;
     }
+
+    // re-init throttle wait on arm and disarm, to prevent rudder
+    // arming on 2nd flight causing yaw
+    if (!air_mode_active()) {
+        init_throttle_wait();
+    }
 }
 
 
@@ -1530,17 +1535,6 @@ void SLT_Transition::update()
         transition_low_airspeed_ms = 0;
     }
 
-    if (transition_state < TRANSITION_TIMER) {
-        // set a single loop pitch limit in TECS
-        if (plane.ahrs.groundspeed() < 3) {
-            // until we have some ground speed limit to zero pitch
-            plane.TECS_controller.set_pitch_max_limit(0);
-        } else {
-            plane.TECS_controller.set_pitch_max_limit(quadplane.transition_pitch_max);
-        }
-    } else if (transition_state < TRANSITION_DONE) {
-        plane.TECS_controller.set_pitch_max_limit((quadplane.transition_pitch_max+1)*2);
-    }
     if (transition_state < TRANSITION_DONE) {
         // during transition we ask TECS to use a synthetic
         // airspeed. Otherwise the pitch limits will throw off the
@@ -1997,6 +1991,11 @@ void QuadPlane::motors_output(bool run_rate_controller)
             // relax if have been inactive
             relax_attitude_control();
         }
+        // run low level rate controllers that only require IMU data and set loop time
+        const float last_loop_time_s = AP::scheduler().get_last_loop_time_s();
+        motors->set_dt(last_loop_time_s);
+        attitude_control->set_dt(last_loop_time_s);
+        pos_control->set_dt(last_loop_time_s);
         attitude_control->rate_controller_run();
         last_att_control_ms = now;
     }
@@ -2501,7 +2500,11 @@ void QuadPlane::vtol_position_controller(void)
         const float distance = diff_wp.length();
         const Vector2f rel_groundspeed_vector = landing_closing_velocity();
         const float rel_groundspeed_sq = rel_groundspeed_vector.length_squared();
-        const float closing_groundspeed = rel_groundspeed_vector * diff_wp.normalized();
+        float closing_groundspeed = 0;
+
+        if (distance > 0.1) {
+            closing_groundspeed = rel_groundspeed_vector * diff_wp.normalized();
+        }
 
         // calculate speed we should be at to reach the position2
         // target speed at the position2 distance threshold, assuming
@@ -2622,6 +2625,10 @@ void QuadPlane::vtol_position_controller(void)
 
         // call attitude controller
         disable_yaw_rate_time_constant();
+
+        // setup scaling of roll and pitch angle P gains to match fixed wing gains
+        setup_rp_fw_angle_gains();
+
         if (have_target_yaw) {
             attitude_control->input_euler_angle_roll_pitch_yaw(plane.nav_roll_cd,
                                                                plane.nav_pitch_cd,
@@ -2789,7 +2796,7 @@ void QuadPlane::vtol_position_controller(void)
             float zero = 0;
             pos_control->input_pos_vel_accel_z(target_z, zero, 0);
         } else {
-            set_climb_rate_cms(0, false);
+            set_climb_rate_cms(0);
         }
         break;
     }
@@ -2803,7 +2810,7 @@ void QuadPlane::vtol_position_controller(void)
             }
         }
         const float descent_rate_cms = landing_descent_rate_cms(height_above_ground);
-        set_climb_rate_cms(-descent_rate_cms, descent_rate_cms>0);
+        pos_control->land_at_climb_rate_cm(-descent_rate_cms, descent_rate_cms>0);
         break;
     }
 
@@ -2951,10 +2958,10 @@ void QuadPlane::takeoff_controller(void)
             vel_z = 0;
             pos_control->input_pos_vel_accel_z(pos_z, vel_z, 0);
         } else {
-            set_climb_rate_cms(vel_z, false);
+            set_climb_rate_cms(vel_z);
         }
     } else {
-        set_climb_rate_cms(vel_z, false);
+        set_climb_rate_cms(vel_z);
     }
 
     run_z_controller();
@@ -2999,7 +3006,7 @@ void QuadPlane::waypoint_controller(void)
                                                        true);
 
     // climb based on altitude error
-    set_climb_rate_cms(assist_climb_rate_cms(), false);
+    set_climb_rate_cms(assist_climb_rate_cms());
     run_z_controller();
 }
 
@@ -4232,6 +4239,43 @@ MAV_VTOL_STATE SLT_Transition::get_mav_vtol_state() const
     return MAV_VTOL_STATE_UNDEFINED;
 }
 
+// Set FW roll and pitch limits and keep TECS informed
+void SLT_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing)
+{
+    if (quadplane.in_vtol_mode() || quadplane.in_vtol_airbrake()) {
+        // not in FW flight
+        return;
+    }
+
+    if (transition_state == TRANSITION_DONE) {
+        // transition complete, nothing to do
+        return;
+    }
+
+    if (!plane.control_mode->does_auto_throttle()) {
+        // don't limit pitch when in manually controlled modes like FBWA, ACRO
+        return;
+    }
+
+    float max_pitch;
+    if (transition_state < TRANSITION_TIMER) {
+        if (plane.ahrs.groundspeed() < 3.0) {
+            // until we have some ground speed limit to zero pitch
+            max_pitch = 0.0;
+        } else {
+            max_pitch = quadplane.transition_pitch_max;
+        }
+    } else {
+        max_pitch = (quadplane.transition_pitch_max+1.0)*2.0;
+    }
+
+    // set a single loop pitch limit in TECS
+    plane.TECS_controller.set_pitch_max_limit(max_pitch);
+
+    // ensure pitch is constrained to limit
+    nav_pitch_cd = constrain_int32(nav_pitch_cd, -max_pitch*100.0, max_pitch*100.0);
+}
+
 /*
   see if we are in a VTOL takeoff
  */
@@ -4305,4 +4349,45 @@ bool QuadPlane::landing_with_fixed_wing_spiral_approach(void) const
              cmd.p1 == NAV_VTOL_LAND_OPTIONS_FW_SPIRAL_APPROACH));
 }
 
+/*
+  setup scaling of roll and pitch angle P gains to match fixed wing gains
+
+  we setup the angle P gain to match fixed wing at high speed (above
+  ARSPD_FBW_MIN) where fixed wing surfaces are presumed to
+  dominate. At lower speeds we use the multicopter angle P gains.
+*/
+void QuadPlane::setup_rp_fw_angle_gains(void)
+{
+    const float mc_angR = attitude_control->get_angle_roll_p().kP();
+    const float mc_angP = attitude_control->get_angle_pitch_p().kP();
+    const float fw_angR = 1.0/plane.rollController.tau();
+    const float fw_angP = 1.0/plane.pitchController.tau();
+
+    if (!is_positive(mc_angR) || !is_positive(mc_angP)) {
+        // bad configuration, don't scale
+        return;
+    }
+
+    float aspeed;
+    if (!ahrs.airspeed_estimate(aspeed)) {
+        // can't get airspeed, no scaling of VTOL angle gains
+        return;
+    }
+
+    const float low_airspeed = 3.0;
+    if (aspeed <= low_airspeed || plane.aparm.airspeed_min <= low_airspeed) {
+        // no scaling
+        return;
+    }
+
+    const float angR_scale = linear_interpolate(mc_angR, fw_angR,
+                                                aspeed,
+                                                low_airspeed, plane.aparm.airspeed_min) / mc_angR;
+    const float angP_scale = linear_interpolate(mc_angP, fw_angP,
+                                                aspeed,
+                                                low_airspeed, plane.aparm.airspeed_min) / mc_angP;
+    const Vector3f gain_scale{angR_scale, angP_scale, 1.0};
+    attitude_control->set_angle_P_scale(gain_scale);
+}
+
 #endif  // HAL_QUADPLANE_ENABLED

ArduPlane/quadplane.h

@@ -225,7 +225,7 @@ private:
     void hold_stabilize(float throttle_in);
 
     // set climb rate in position controller
-    void set_climb_rate_cms(float target_climb_rate_cms, bool force_descend);
+    void set_climb_rate_cms(float target_climb_rate_cms);
 
     // get pilot desired yaw rate in cd/s
     float get_pilot_input_yaw_rate_cds(void) const;
@@ -557,6 +557,7 @@ private:
         ONLY_ARM_IN_QMODE_OR_AUTO=(1<<18),
         TRANS_FAIL_TO_FW=(1<<19),
         FS_RTL=(1<<20),
+        DISARMED_TILT_UP=(1<<21),
     };
     bool option_is_set(OPTION option) const {
         return (options.get() & int32_t(option)) != 0;
@@ -665,6 +666,11 @@ private:
      */
     float get_scaled_wp_speed(float target_bearing_deg) const;
 
+    /*
+      setup scaling of roll and pitch angle P gains to match fixed wing gains
+     */
+    void setup_rp_fw_angle_gains(void);
+
 public:
     void motor_test_output();
     MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,

ArduPlane/system.cpp

@@ -197,6 +197,26 @@ void Plane::startup_ground(void)
 }
 
 
+#if AP_FENCE_ENABLED
+/*
+  return true if a mode reason is an automatic mode change due to
+  landing sequencing.
+ */
+static bool mode_reason_is_landing_sequence(const ModeReason reason)
+{
+    switch (reason) {
+    case ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND:
+    case ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL:
+    case ModeReason::QRTL_INSTEAD_OF_RTL:
+    case ModeReason::QLAND_INSTEAD_OF_RTL:
+        return true;
+    default:
+        break;
+    }
+    return false;
+}
+#endif // AP_FENCE_ENABLED
+
 bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
 {
 
@@ -246,8 +266,12 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
 
 #if AP_FENCE_ENABLED
     // may not be allowed to change mode if recovering from fence breach
-    if (hal.util->get_soft_armed() && fence.enabled() && fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) &&
-                                            fence.get_breaches() && in_fence_recovery()) {
+    if (hal.util->get_soft_armed() &&
+        fence.enabled() &&
+        fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) &&
+        fence.get_breaches() &&
+        in_fence_recovery() &&
+        !mode_reason_is_landing_sequence(reason)) {
         gcs().send_text(MAV_SEVERITY_NOTICE,"Mode change to %s denied, in fence recovery", new_mode.name());
         AP_Notify::events.user_mode_change_failed = 1;
         return false;

ArduPlane/tiltrotor.cpp

@@ -219,7 +219,10 @@ void Tiltrotor::continuous_update(void)
     if (!quadplane.in_vtol_mode() && (!hal.util->get_soft_armed() || !quadplane.assisted_flight)) {
         // we are in pure fixed wing mode. Move the tiltable motors all the way forward and run them as
         // a forward motor
-        slew(get_forward_flight_tilt());
+
+        // option set then if disarmed move to VTOL position to prevent ground strikes, allow tilt forward in manual mode for testing
+        const bool disarmed_tilt_up = !hal.util->get_soft_armed() && (plane.control_mode != &plane.mode_manual) && quadplane.option_is_set(QuadPlane::OPTION::DISARMED_TILT_UP);
+        slew(disarmed_tilt_up ? 0.0 : get_forward_flight_tilt());
 
         max_change = tilt_max_change(false);
 

ArduPlane/transition.h

@@ -87,6 +87,8 @@ public:
 
     bool show_vtol_view() const override;
 
+    void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) override;
+
     bool set_FW_roll_limit(int32_t& roll_limit_cd) override;
 
     bool allow_update_throttle_mix() const override;

ArduPlane/version.h

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

ArduSub/ArduSub.cpp

@@ -132,9 +132,14 @@ constexpr int8_t Sub::_failsafe_priorities[5];
 
 void Sub::run_rate_controller()
 {
+    const float last_loop_time_s = AP::scheduler().get_last_loop_time_s();
+    motors.set_dt(last_loop_time_s);
+    attitude_control.set_dt(last_loop_time_s);
+    pos_control.set_dt(last_loop_time_s);
+
     //don't run rate controller in manual or motordetection modes
     if (control_mode != MANUAL && control_mode != MOTOR_DETECT) {
-        // run low level rate controllers that only require IMU data
+        // run low level rate controllers that only require IMU data and set loop time
         attitude_control.rate_controller_run();
     }
 }

ArduSub/GCS_Mavlink.cpp

@@ -1,6 +1,7 @@
 #include "Sub.h"
 
 #include "GCS_Mavlink.h"
+#include <AP_RPM/AP_RPM_config.h>
 
 MAV_TYPE GCS_Sub::frame_type() const
 {

ArduSub/Sub.cpp

@@ -32,8 +32,8 @@ Sub::Sub()
           auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),
           inertial_nav(ahrs),
           ahrs_view(ahrs, ROTATION_NONE),
-          attitude_control(ahrs_view, aparm, motors, scheduler.get_loop_period_s()),
-          pos_control(ahrs_view, inertial_nav, motors, attitude_control, scheduler.get_loop_period_s()),
+          attitude_control(ahrs_view, aparm, motors),
+          pos_control(ahrs_view, inertial_nav, motors, attitude_control),
           wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control),
           loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control),
           circle_nav(inertial_nav, ahrs_view, pos_control),

ArduSub/radio.cpp

@@ -50,7 +50,6 @@ void Sub::init_rc_in()
 void Sub::init_rc_out()
 {
     motors.set_update_rate(g.rc_speed);
-    motors.set_loop_rate(scheduler.get_loop_rate_hz());
     motors.init((AP_Motors::motor_frame_class)g.frame_configuration.get(), AP_Motors::motor_frame_type::MOTOR_FRAME_TYPE_PLUS);
     motors.convert_pwm_min_max_param(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
     motors.update_throttle_range();

Blimp/Blimp.h

@@ -224,13 +224,13 @@ private:
     AP_InertialNav inertial_nav;
 
     // Vel & pos PIDs
-    AC_PID_2D pid_vel_xy{3, 0.2, 0, 0, 0.2, 3, 3, 0.02}; //These are the defaults - P I D FF IMAX FiltHz FiltDHz DT
-    AC_PID_Basic pid_vel_z{7, 1.5, 0, 0, 1, 3, 3, 0.02};
-    AC_PID_Basic pid_vel_yaw{3, 0.4, 0, 0, 0.2, 3, 3, 0.02};
+    AC_PID_2D pid_vel_xy{3, 0.2, 0, 0, 0.2, 3, 3}; //These are the defaults - P I D FF IMAX FiltHz FiltDHz DT
+    AC_PID_Basic pid_vel_z{7, 1.5, 0, 0, 1, 3, 3};
+    AC_PID_Basic pid_vel_yaw{3, 0.4, 0, 0, 0.2, 3, 3};
 
-    AC_PID_2D pid_pos_xy{1, 0.05, 0, 0, 0.1, 3, 3, 0.02};
-    AC_PID_Basic pid_pos_z{0.7, 0, 0, 0, 0, 3, 3, 0.02};
-    AC_PID pid_pos_yaw{1.2, 0.5, 0, 0, 2, 3, 3, 3, 0.02}; //p, i, d, ff, imax, filt_t, filt_e, filt_d, dt, opt srmax, opt srtau
+    AC_PID_2D pid_pos_xy{1, 0.05, 0, 0, 0.1, 3, 3};
+    AC_PID_Basic pid_pos_z{0.7, 0, 0, 0, 0, 3, 3};
+    AC_PID pid_pos_yaw{1.2, 0.5, 0, 0, 2, 3, 3, 3}; //p, i, d, ff, imax, filt_t, filt_e, filt_d, dt, opt srmax, opt srtau
 
     // System Timers
     // --------------

Blimp/GCS_Mavlink.cpp

@@ -1,6 +1,7 @@
 #include "Blimp.h"
 
 #include "GCS_Mavlink.h"
+#include <AP_RPM/AP_RPM_config.h>
 
 MAV_TYPE GCS_Blimp::frame_type() const
 {
@@ -358,7 +359,9 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
     MSG_MAG_CAL_PROGRESS,
     MSG_EKF_STATUS_REPORT,
     MSG_VIBRATION,
+#if AP_RPM_ENABLED
     MSG_RPM,
+#endif
     MSG_ESC_TELEMETRY,
     MSG_GENERATOR_STATUS,
 };

Blimp/mode_loiter.cpp

@@ -14,11 +14,13 @@
 //Runs the main loiter controller
 void ModeLoiter::run()
 {
+    const float dt = blimp.scheduler.get_last_loop_time_s();
+    
     Vector3f pilot;
-    pilot.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s();
-    pilot.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s();
-    pilot.z = channel_down->get_control_in()  / float(RC_SCALE) * g.max_pos_z * blimp.scheduler.get_loop_period_s();
-    float pilot_yaw = channel_yaw->get_control_in()  / float(RC_SCALE) * g.max_pos_yaw * blimp.scheduler.get_loop_period_s();
+    pilot.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_pos_xy * dt;
+    pilot.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * dt;
+    pilot.z = channel_down->get_control_in()  / float(RC_SCALE) * g.max_pos_z * dt;
+    float pilot_yaw = channel_yaw->get_control_in()  / float(RC_SCALE) * g.max_pos_yaw * dt;
 
     if (g.simple_mode == 0){
         //If simple mode is disabled, input is in body-frame, thus needs to be rotated.
@@ -30,10 +32,10 @@ void ModeLoiter::run()
     target_yaw = wrap_PI(target_yaw + pilot_yaw);
 
     //Pos controller's output becomes target for velocity controller
-    Vector3f target_vel_ef{blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, {0,0,0}), 0};
-    target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z);
+    Vector3f target_vel_ef{blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, dt, {0,0,0}), 0};
+    target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt);
     float yaw_ef = blimp.ahrs.get_yaw();
-    float target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef));
+    float target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef), dt);
     blimp.pid_pos_yaw.set_target_rate(target_yaw);
     blimp.pid_pos_yaw.set_actual_rate(yaw_ef);
 
@@ -42,10 +44,10 @@ void ModeLoiter::run()
                               constrain_float(target_vel_ef.z, -g.max_vel_z, g.max_vel_z)};
     float target_vel_yaw_c = constrain_float(target_vel_yaw, -g.max_vel_yaw, g.max_vel_yaw);
 
-    Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c, blimp.vel_ned_filtd, {0,0,0});
-    float act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z, blimp.vel_ned_filtd.z);
+    Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c, blimp.vel_ned_filtd, dt, {0,0,0});
+    float act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z, blimp.vel_ned_filtd.z, dt);
     blimp.rotate_NE_to_BF(actuator);
-    float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c, blimp.vel_yaw_filtd);
+    float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c, blimp.vel_yaw_filtd, dt);
 
     if(!(blimp.g.dis_mask & (1<<(2-1)))){
         motors->front_out = actuator.x;

Blimp/mode_velocity.cpp

@@ -6,6 +6,8 @@
 // Runs the main velocity controller
 void ModeVelocity::run()
 {
+    const float dt = blimp.scheduler.get_last_loop_time_s();
+    
     Vector3f target_vel;
     target_vel.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_vel_xy;
     target_vel.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_vel_xy;
@@ -13,10 +15,10 @@ void ModeVelocity::run()
     target_vel.z = channel_down->get_control_in()  / float(RC_SCALE) * g.max_vel_z;
     float target_vel_yaw = channel_yaw->get_control_in() / float(RC_SCALE) * g.max_vel_yaw;
 
-    Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel, blimp.vel_ned_filtd, {0,0,0});
+    Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel, blimp.vel_ned_filtd, dt, {0,0,0});
     blimp.rotate_NE_to_BF(actuator);
-    float act_down = blimp.pid_vel_z.update_all(target_vel.z, blimp.vel_ned_filtd.z);
-    float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw, blimp.vel_yaw_filtd);
+    float act_down = blimp.pid_vel_z.update_all(target_vel.z, blimp.vel_ned_filtd.z, dt);
+    float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw, blimp.vel_yaw_filtd, dt);
 
     if(!(blimp.g.dis_mask & (1<<(2-1)))){
         motors->front_out = actuator.x;

Rover/GCS_Mavlink.cpp

@@ -2,7 +2,9 @@
 
 #include "GCS_Mavlink.h"
 
+#include <AP_RPM/AP_RPM_config.h>
 #include <AP_RangeFinder/AP_RangeFinder_Backend.h>
+#include <AP_EFI/AP_EFI_config.h>
 
 MAV_TYPE GCS_Rover::frame_type() const
 {
@@ -565,9 +567,14 @@ static const ap_message STREAM_EXTRA3_msgs[] = {
     MSG_MAG_CAL_PROGRESS,
     MSG_EKF_STATUS_REPORT,
     MSG_VIBRATION,
+#if AP_RPM_ENABLED
     MSG_RPM,
+#endif
     MSG_WHEEL_DISTANCE,
     MSG_ESC_TELEMETRY,
+#if HAL_EFI_ENABLED
+    MSG_EFI_STATUS,
+#endif
 };
 static const ap_message STREAM_PARAMS_msgs[] = {
     MSG_NEXT_PARAM

Rover/Parameters.cpp

@@ -500,10 +500,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @DisplayName: BalanceBot Maximum Pitch
     // @Description: Pitch angle in degrees at 100% throttle
     // @Units: deg
-    // @Range: 0 5
+    // @Range: 0 15
     // @Increment: 0.1
     // @User: Standard
-    AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 2),
+    AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 10),
 
     // @Param: CRASH_ANGLE
     // @DisplayName: Crash Angle

Rover/Rover.cpp

@@ -405,6 +405,9 @@ void Rover::update_logging2(void)
 
     if (should_log(MASK_LOG_IMU)) {
         AP::ins().Write_Vibration();
+#if HAL_GYROFFT_ENABLED
+        gyro_fft.write_log_messages();
+#endif
     }
 }
 

Rover/balance_bot.cpp

@@ -7,15 +7,8 @@ void Rover::balancebot_pitch_control(float &throttle)
     // calculate desired pitch angle
     const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max) + radians(g2.bal_pitch_trim);
 
-    // calculate speed from wheel encoders
-    float veh_speed_pct = 0.0f;
-    if (g2.wheel_encoder.enabled(0) && g2.wheel_encoder.enabled(1) && is_positive(g2.wheel_rate_control.get_rate_max_rads())) {
-        veh_speed_pct = (g2.wheel_encoder.get_rate(0) + g2.wheel_encoder.get_rate(1)) / (2.0f * g2.wheel_rate_control.get_rate_max_rads()) * 100.0f;
-        veh_speed_pct = constrain_float(veh_speed_pct, -100.0f, 100.0f);
-    }
-
     // calculate required throttle using PID controller
-    throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, veh_speed_pct, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, G_Dt) * 100.0f;
+    throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, radians(g2.bal_pitch_max), g2.motors.limit.throttle_lower || g2.motors.limit.throttle_upper, G_Dt) * 100.0f;
 }
 
 // returns true if vehicle is a balance bot

Rover/mode.cpp

@@ -317,7 +317,9 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled)
             bool stopped;
             throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt, stopped);
         } else {
-            throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt);
+            bool motor_lim_low = g2.motors.limit.throttle_lower || attitude_control.pitch_limited();
+            bool motor_lim_high = g2.motors.limit.throttle_upper || attitude_control.pitch_limited();
+            throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, motor_lim_low, motor_lim_high, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt);
         }
 
         // if vehicle is balance bot, calculate actual throttle required for balancing

Rover/release-notes.txt

@@ -1,5 +1,267 @@
 Rover Release Notes:
 ------------------------------------------------------------------
+Rover 4.3.0-beta14 12-Aug-2023
+Changes from 4.3.0-beta13
+1) Bug fixes
+    - DroneCAN GPS RTK injection fix
+    - INAxxx battery monitors allow for battery reset remaining
+    - Notch filter gyro glitch caused by race condition fixed
+    - Scripting restart memory corruption bug fixed
+------------------------------------------------------------------
+Rover 4.3.0-beta13 27-Mar-2023
+Changes from 4.3.0-beta12
+1) Bug fixes
+    a) EKF3 accel bias calculations bug fix
+    b) EKF3 accel bias process noise adjusted for greater robustness
+    c) GSF yaw numerical stability fix caused by compassmot
+    d) INS batch sampler fix to avoid watchdog if INS_LOG_BAT_CNT changed without rebooting
+    e) Memory corruption bug in the STM32H757 (very rare)
+    f) RC input on IOMCU bug fix (RC might not be regained if lost)
+------------------------------------------------------------------
+Rover 4.3.0-beta11/beta12 27-Mar-2023
+Changes from 4.3.0-beta10
+1) Bi-directional DShot fix for possible motor stop approx 72min after startup 
+------------------------------------------------------------------
+Rover 4.3.0-beta10 01-Mar-2023
+Changes from 4.3.0-beta9
+1) Bug fixes
+    a) GPS unconfigured error fix for non-M10 uBlox GPS
+    b) Gremsy gimbal fix when attached to autopilot's serial3 (or higher)
+    c) MambaF405 2022 gets VTX power on support
+    d) MCU voltage enabled on H757 CPUs (including CubeOrangePlus)
+    e) PiccoloCAN fix for ESC voltage and current scaling 
+    f) Servo gimbal mount yaw handling fix (only affects 3-axis servo gimbals)
+------------------------------------------------------------------
+Rover 4.3.0-beta9 14-Feb-2023
+Changes from 4.3.0-beta8
+1) AutoPilot specific enhancements
+    a) CubeOrangePlusBG support
+    b) Foxeer ReaperF745 supports external compass
+    c) MambaH743v4 supports VTX power
+2) Bug fixes
+    a) Arming check fix if BARO_FIELD_ELEV set
+    b) Compass calibration diagonals set to 1,1,1 if incorrectly set to 0,0,0
+    c) Gimbal's yaw feed-forward set to zero when landed (affects Gremsy gimbals)
+    d) IOMCU double reset and safety disable fix
+    e) Logging fix for duplicate format messages
+    f) OpenDroneId sets emergency status on crash or chute deploy
+    g) Peripheral firmware updates using MAVCAN fixed
+    h) RC protocol cannot be changed once detected on boards with IOMCU
+------------------------------------------------------------------
+Rover 4.3.0-beta8 20-Jan-2023
+Changes from 4.3.0-beta7
+1) Bug fixes
+    a) MAVFTP fix to terminate session error (could cause FTP failures)
+    b) IMU fast fifo reset log message max frequency reduced
+------------------------------------------------------------------
+Rover 4.3.0-beta7 09-Jan-2023
+Changes from 4.3.0-beta6
+1) Autopilot related changes
+    a) AIRLink LTE module enable pin and HEAT_ params added
+    b) CUAV Nora/Nora+ bdshot firmware (allows Bi-directional DShot)
+    c) CubeOrange, CubeYellow gets fast reset of ICM20602
+    d) MambaH743v2 with dual ICM42688 supported
+    e) PixPilot-V6
+2) MAVFTP speed improvement including faster param download
+3) Bug fixes
+    a) Analog rangefinder GPIO pin arming check fixed
+    b) Arming check of AHRS/EKF vs GPS location disabled if GPS disabled
+    c) CRSF gets RC_OPTIONS for ELRS baudrate to avoid RC failsafes
+    d) Null pointer checks avoid watchdog when out of memory
+    e) Servo gimbal yaw jump to opposite side fixed
+    f) Siyi A8 gimbal driver's record video feature fixed
+    g) SToRM32 serial gimbal driver actual angle reporting fixed (pitch and yaw angle signs were reversed)
+------------------------------------------------------------------
+Rover 4.3.0-beta6 10-Dec-2022
+Changes from 4.3.0-beta5
+1) Arming check that main loop is running at configured speed (e.g. SCHED_LOOP_RATE)
+2) uBlox M10 support
+3) Autopilot specific changes
+    a) CubeOrange defaults to using 2nd IMU as primary
+    b) SIRF and SBP GPS disabled on BeastF7v2, MatekF405-STD, MAtekF405-Wing, omnibusf4pro
+4) Bug fixes
+    a) Camera driver's CAM_MIN_INTERVAL fixed if pilot manually triggers extra picture
+    b) Main loop slowdown after arming fixed (parameter logging was causing delays)
+    c) Main loop's fast tasks always run (caused twitches in Loiter on heavily loaded CPUs)
+    d) MAVLink commands received on private channels checked for valid target sysid
+    e) ModalAI cameras support fixed (ODOMETRY message frame was consumed incorrectly)
+    f) Param reset after firmware load fixed on these boards
+        - BeastF7v2
+        - CubeYellow-bdshot
+        - f405-MatekAirspeed
+        - FlywooF745Nano
+        - KakuteF4Mini
+        - KakuteF7-bdshot
+        - MatekF405-bdshot
+        - MatekF405-STD
+        - MatekF405-Wing-bdshot
+        - MatekF765-SE
+        - MatekF765-Wing-bdshot
+    g) Siyi A8 gimbal support fixed
+    h) Windows builds move to compiling only 64-bit executables
+------------------------------------------------------------------
+Rover 4.3.0-beta5 17-Nov-2022
+Changes from 4.3.0-beta4
+1) Autopilot specific enhancements
+   a) ARKV6X support
+   b) MatekH743 supports 8 bi-directional dshot channels
+   c) Pixhawk boards support MS5607 baros
+   d) SpeedbyBee F405v3 support
+2) Balancebot pitch control improvements and pitch limiting
+3) DroneCAN Airspeed sensor support including hygrometer (aka water vapour) readings
+4) EFI support (electronic fuel injection engines)
+5) Harmonic Notch support (Rover only)
+6) Pre-arm warning if multiple UARTs with SERIALx_PROTOCOL = RCIN
+7) Siyi gimbal support
+8) Bug fixes
+    a) AtomRCF405NAVI bootloader file name fixed
+    b) BRD_SAFETY_MASK fixed on boards with both FMU safety switch and IOMCU
+    c) Compass calibration continues even if a single compass's cal fails
+    d) Gremsy gimbal driver sends autopilot info at lower rate to save bandwidth
+    e) Invensense 42605 and 42609 IMUs use anti-aliasing filter and notch filter
+    f) OSD stats screen fix
+    g) RC input on serial port uses first UART with SERIALx_PROTOCOL = 23 (was using last)
+    h) RunCam caching fix with enablement and setup on 3-pos switch
+    i) RTK CAN GPS fix when GPSs conneted to separate CAN ports on autopilot
+------------------------------------------------------------------
+Rover 4.3.0-beta4 24-Oct-2022
+Changes from 4.3.0-beta3
+1) Scripting supports implementing AUX functions
+2) Bug fixes
+    a) BMI085 accel scaling fixed
+    b) Build with gcc 11.3 fixed (developer only)
+    c) EKF3 alt discrepancy if GPS or baro alt changed soon after startup fixed
+    d) Harmonic Notch and ESC telem fix when motor outputs are non-contiguous
+    e) NMEA GPS's KSXT message parsing fixed (affected position accuracy)
+    f) Scripting random number generator fix
+------------------------------------------------------------------
+Rover 4.3.0-beta3 14-Oct-2022
+Changes from 4.3.0-beta2
+1) Pixhawk1-1M, fmuv2, fmuv3 display warning if firmware mismatches board's flash size (1M and 2M)
+2) Scripting support for multi-byte i2c reads
+3) Bug fixes
+    a) Airspeed CAN sensor ordering fixed (ordering could change if using multiple airspeed sensors)
+    b) BRD_SAFETY_MASK fix for enabling outputs when safety is on
+    c) Defaults.parm file processing fixed when a line has >100 characters and/or no new line (developer only)
+    d) NMEA serial output precision fixed (was only accurate to 1m, now accurate to 1cm)
+------------------------------------------------------------------
+Rover 4.3.0-beta2 04-Oct-2022
+Changes from 4.3.0-beta1
+1) Autopilot specific fixes and enhancements
+    a) AIRLink autopilot supports UART2
+    b) CUAV V6X supports CAN battery monitor by default
+    c) MatekF405-CAN board uses less memory to fix compass calibration issues
+    d) Pixhawk1-1M only supports uBlox and NMEA GPSs to save flash space
+    e) SkystarsH7HD-bdshot (allows Bi-directional DShot)
+    f) SkystarsH7HD supports VTX power by default
+2) EFI support
+    a) Currawong ECU support (added as Electronic Fuel Injection driver)
+    b) Scripting support for EFI drivers (allows writing EFI drivers in Lua)
+    c) SkyPower and HFE CAN EFI drivers (via scripting)
+3) Safety features
+    a) Arming check that SPIN_MIN less than 0.3 and greater than SPIN_ARM
+    b) Arming option to disable itermittant display of pre-arm warnings (see ARMING_OPTIONS)
+4) Minor enhancements
+    a) Autopilot board names max length increased to 23 characters (was 13)
+    b) CAN actuators can report PWM equivalent values (eases debugging)
+    c) Log download speed improved for boards with "block" backends
+    d) Notch filter slew limit reduces chance of notch freq moving incorrectly 
+    e) SLCAN disabled when vehicle is armed to reduce CPU load
+5) Bug fixes
+    a) DO_JUMP mission command fixed if active command changed before changing to Auto mode
+    b) EKF3 altitude error fix when using dual GPSs and affinity enabled
+    c) FFT indexing bug fixed
+    d) Gimbal mount fix to default mode (see MNTx_DEFLT_MODE parameter)
+    e) MSP fix to report arm status to DJI FPV goggles
+    f) Notch fix for non-throttle notch (was being incorrectly disabled)
+    g) OSD fixes for params, font and resolution
+    h) RPM reporting from harmonic notch fixed
+    i) "Sending unknown message (50)" warning removed
+    j) SBF/GSOF/NOVA GPS auto detction of baud rate fixed
+    k) VideoTX fixes for buffer overruns and Tramp video transmitter support
+------------------------------------------------------------------
+Rover 4.3.0-beta1 14-Sep-2022
+Changes from 4.2.3
+1) Rover specific enhancements
+    a) Aux switch for SaveWP displays, "Mission Cleared" if vehicle not armed
+    b) Dock mode using modified precision landing library
+    c) Manual mode steering scaling with speed can be disabled using MANUAL_OPTIONS parameter
+    d) S-Curves for Auto, Guided, RTL
+2) Rover specific bug fixes
+    a) Wheel encoder timestamp fix (WRC_xx params may need to be changed)
+    b) Auto mode stick mixing fixed (see STICK_MIXING parameter)
+    c) Arming check removed to support mixed Ackerman and skid-stering vehicles
+3) New autopilot support
+    a) AtomRCF405
+    b) CubeOrange-SimOnHardWare
+    c) DevEBoxH7v2
+    d) KakuteH7Mini-Nand
+    e) KakuteH7v2
+    f) Mamba F405 Mk4
+    g) SkystarsH7HD
+    h) bi-directional dshot (aka "bdshot") versions for CubeOrange, CubeYellow, KakuteF7, KakuteH7, MatekF405-Wing, Matek F765, PH4-mini, Pixhawk-1M
+4) EKF enhancements and fixes
+    a) EK3_GPS_VACC_MAX threshold to control when GPS altitude is used as alt source
+    b) EKF ring buffer fix for very slow sensor updates (those that update once every few seconds)
+    c) EKF3 source set change captured in Replay logs
+5) Gimbal enhancements
+    a) Angle limit params renamed and scaled to degrees (e.g. MNT1_ROLL_MIN, MNT1_PITCH_MIN, etc)
+    b) BrushlessPWM driver (set MNT1_TYPE = 7) is unstabilized Servo driver
+    c) Dual mount support (see MNT1_, MNT2 params)
+    d) Gremsy driver added (set MNT1_TYPE = 6)
+    e) MAVLink gimbalv2 support including sending GIMBAL_DEVICE_STATUS_UPDATE (replaces MOUNT_STATUS message)
+    f) "Mount Lock" auxiliary switch supports follow and lock modes in RC targetting (aka earth-frame and body-frame)    
+    g) RC channels to control gimbal set using RCx_OPTION = 212 (Roll), 213 (Pitch) or 214 (Yaw)
+    h) RC targetting rotation rate in deg/sec (see MNT1_RC_RATE which replaces MNT_JSTICK_SPD)
+    i) Yaw can be disabled on 3-axis gimbals (set MNTx_YAW_MIN = MNTx_YAW_MAX)
+6) Notch filter enhancements
+    a) Attitude and filter logging at main loop rate
+    b) Batch sampler logging both pre and post-filter
+    c) FFT frame averaging
+    d) In-flight throttle notch parameter learning using averaged FFTs
+    e) Triple harmonic notch 
+7) RemoteId and SecureBoot enhancements
+    a) Remote update of secure boot's public keys (also allows remote unlocking of bootloader)
+8) Safety enhancements
+    a) crash_dump.bin file saved to SD Card on startup (includes details re cause of software failures)
+    b) Disabling Fence clears any active breaches (e.g. FENCE_TYPE = 0 will clear breaches)
+    c) "GPS Glitch" message clarified to "GPS Glitch or Compass error"
+    d) Pre-arm check that configured AHRS is being used (e.g. checks AHRS_EKF_TYPE not changed since boot)
+    e) Pre-arm check that gimbals are healthy (currently only for Gremsy gimbals, others in future release)
+    f) Pre-arm check that scripts are running
+    g) Pre-arm messages are correctly prefixed with "PreArm:" (instead of "Arm:")
+    h) RC auxiliary switch option for Arm / Emergency Stop
+9) Scripting enhancements
+    a) CAN2 port bindings to allow scripts to communicate on 2nd CAN bus
+    b) ESC RPM bindings to allow scripts to report engine RPM
+    c) Gimbal bingings to allow scripts to control gimbal
+    d) Pre-arm check bindings (allows scripts to check if pre-arm checks have passed)
+    e) Semicolon (:) and period (.) supported (e.g both Logger:write() and Logger.write will work)
+10) Sensor driver enhancements
+    a) Benewake H30 radar support
+    b) BMI270 IMU performance improvements
+    c) IRC Tramp VTX suppor
+    d) Logging pause-able with auxiliary switch.  see RCx_OPTION = 165 (Pause Stream Logging)
+    e) Proximity sensor support for up to 3 sensors
+    f) Precision Landing consumes LANDING_TARGET MAVLink message's PositionX,Y,Z fields
+    g) RichenPower generator maintenance-required messages can be suppressed using GEN_OPTIONS param
+    h) TeraRanger Neo rangefinder support
+    i) GPS support to provide ellipsoid altitude instead of AMSL (see GPS_DRV_OPTIONS)
+    j) W25N01GV 1Gb flash support
+11) Bug fixes
+    a) Accel calibration throws away queued commands from GCS (avoids commands being run long after they were sent)
+    b) Cygbot proximity sensor fix to support different orientations (see PRXx_ORIENT)
+    c) Lutan EFI message flood reduced
+    d) Missions download to GCS corruption avoided by checking serial buffer has space
+    e) Safety switch disabled if IOMCU is disabled (see BRD_IO_ENABLE=0)
+    f) Script restart memory leak fixed
+12) Developer items
+    a) Fast loop task list available in real-time using @SYS/tasks.txt
+    b) Parameter defaults sent to GCS with param FTP and recorded in onboard logs
+    c) ROS+ArduPilot environment installation script
+    d) Sim on Hardware allows simulator to run on autopilot (good for exhibitions)
+    e) Timer info available in real-time using @SYS/timers.txt
+------------------------------------------------------------------
 Rover 4.2.3 30-Aug-2022
 Changes from 4.2.3-rc3
 1) OpenDroneId bug fix to consume open-drone-id-system-update message

Rover/version.h

@@ -6,14 +6,14 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ArduRover V4.3.0-dev"
+#define THISFIRMWARE "ArduRover V4.3.0-beta14"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,3,0,FIRMWARE_VERSION_TYPE_DEV
+#define FIRMWARE_VERSION 4,3,0,FIRMWARE_VERSION_TYPE_BETA+14
 
 #define FW_MAJOR 4
 #define FW_MINOR 3
 #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

@@ -43,6 +43,7 @@ TARGET_HW_ARK_CAN_GPS                  81
 TARGET_HW_ARK_CAN_RTK_GPS              82
 TARGET_HW_FF_RTK_CAN_GPS               85
 TARGET_HW_ATL_MANTIS_EDU               97
+TARGET_HW_ARK_FMU_V6X                  57
 
 
 Reserved  PX4 [BL] FMU v5X.x           51
@@ -178,6 +179,11 @@ AP_HW_SKYSTARSH7HD                   1075
 AP_HW_PixSurveyA1                    1076
 AP_HW_AEROFOX_AIRSPEED               1077
 AP_HW_ATOMRCF405                     1078
+AP_HW_CUBEPILOT_CANMOD               1079
+AP_HW_AEROFOX_PMU                    1080
+AP_HW_JHEMCUGF16F405                 1081
+AP_HW_SPEEDYBEEF4V3                  1082
+AP_HW_PixPilot-V6                    1083
 
 AP_HW_CUBEORANGE_PERIPH              1400
 AP_HW_CUBEBLACK_PERIPH               1401

Tools/AP_Bootloader/mcu_f4.h

@@ -29,12 +29,12 @@ const mcu_des_t mcu_descriptions[] = {
 };
 
 const mcu_rev_t silicon_revs[] = {
-    {MCU_REV_STM32F4_REV_3, '3', false}, /* Revision 3 */
+    {MCU_REV_STM32F4_REV_3, '3'}, /* Revision 3 */
 
-    {MCU_REV_STM32F4_REV_A, 'A', true}, /* Revision A */
-    {MCU_REV_STM32F4_REV_Z, 'Z', true}, /* Revision Z */
-    {MCU_REV_STM32F4_REV_Y, 'Y', true}, /* Revision Y */
-    {MCU_REV_STM32F4_REV_1, '1', true}, /* Revision 1 */
+    {MCU_REV_STM32F4_REV_A, 'A'}, /* Revision A */
+    {MCU_REV_STM32F4_REV_Z, 'Z'}, /* Revision Z */
+    {MCU_REV_STM32F4_REV_Y, 'Y'}, /* Revision Y */
+    {MCU_REV_STM32F4_REV_1, '1'}, /* Revision 1 */
 };
 
 #endif // STM32F4

Tools/AP_Bootloader/mcu_h7.h

@@ -13,10 +13,10 @@ mcu_des_t mcu_descriptions[] = {
 };
 
 const mcu_rev_t silicon_revs[] = {
-    {0x1001, 'Z', false},
-    {0x1003, 'Y', false},
-    {0x2001, 'X', false},
-    {0x2003, 'V', false},
+    {0x1001, 'Z'},
+    {0x1003, 'Y'},
+    {0x2001, 'X'},
+    {0x2003, 'V'},
 };
 
 #endif // STM32H7

Tools/AP_Bootloader/support.cpp

@@ -274,24 +274,6 @@ uint32_t get_mcu_desc(uint32_t max, uint8_t *revstr)
     return  strp - revstr;
 }
 
-/*
-  see if we should limit flash to 1M on devices with older revisions
- */
-bool check_limit_flash_1M(void)
-{
-#ifdef STM32F427xx
-    uint32_t idcode = (*(uint32_t *)DBGMCU_BASE);
-    uint16_t revid = ((idcode & REVID_MASK) >> 16);
-
-    for (int i = 0; i < ARRAY_SIZE(silicon_revs); i++) {
-        if (silicon_revs[i].revid == revid) {
-            return silicon_revs[i].limit_flash_size_1M;
-        }
-    }
-#endif
-    return false;
-}
-
 void led_on(unsigned led)
 {
 #ifdef HAL_GPIO_PIN_LED_BOOTLOADER

Tools/AP_Bootloader/support.h

@@ -36,7 +36,6 @@ bool flash_write_buffer(uint32_t address, const uint32_t *v, uint8_t nwords);
 
 uint32_t get_mcu_id(void);
 uint32_t get_mcu_desc(uint32_t len, uint8_t *buf);
-bool check_limit_flash_1M(void);
 
 uint32_t board_get_rtc_signature(void);
 void board_set_rtc_signature(uint32_t sig);
@@ -62,5 +61,4 @@ typedef struct mcu_des_t {
 typedef struct mcu_rev_t {
     uint16_t revid;
     char  rev;
-    bool limit_flash_size_1M;
 } mcu_rev_t;

Tools/AP_Periph/AP_Periph.h

@@ -219,7 +219,8 @@ public:
     void rcout_init();
     void rcout_init_1Hz();
     void rcout_esc(int16_t *rc, uint8_t num_channels);
-    void rcout_srv(const uint8_t actuator_id, const float command_value);
+    void rcout_srv_unitless(const uint8_t actuator_id, const float command_value);
+    void rcout_srv_PWM(const uint8_t actuator_id, const float command_value);
     void rcout_update();
     void rcout_handle_safety_state(uint8_t safety_state);
 #endif

Tools/AP_Periph/can.cpp

@@ -796,11 +796,14 @@ static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer)
     }
 
     for (uint8_t i=0; i < data_count; i++) {
-        if (data[i].command_type != UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_UNITLESS) {
-            // this is the only type we support
-            continue;
+        switch (data[i].command_type) {
+        case UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_UNITLESS:
+            periph.rcout_srv_unitless(data[i].actuator_id, data[i].command_value);
+            break;
+        case UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_PWM:
+            periph.rcout_srv_PWM(data[i].actuator_id, data[i].command_value);
+            break;
         }
-        periph.rcout_srv(data[i].actuator_id, data[i].command_value);
     }
 }
 #endif // HAL_PERIPH_ENABLE_RC_OUT

Tools/AP_Periph/rc_out.cpp

@@ -99,7 +99,7 @@ void AP_Periph_FW::rcout_esc(int16_t *rc, uint8_t num_channels)
     rcout_has_new_data_to_update = true;
 }
 
-void AP_Periph_FW::rcout_srv(uint8_t actuator_id, const float command_value)
+void AP_Periph_FW::rcout_srv_unitless(uint8_t actuator_id, const float command_value)
 {
 #if HAL_PWM_COUNT > 0
     const SRV_Channel::Aux_servo_function_t function = SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + actuator_id - 1);
@@ -109,6 +109,16 @@ void AP_Periph_FW::rcout_srv(uint8_t actuator_id, const float command_value)
 #endif
 }
 
+void AP_Periph_FW::rcout_srv_PWM(uint8_t actuator_id, const float command_value)
+{
+#if HAL_PWM_COUNT > 0
+    const SRV_Channel::Aux_servo_function_t function = SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + actuator_id - 1);
+    SRV_Channels::set_output_pwm(function, uint16_t(command_value+0.5));
+
+    rcout_has_new_data_to_update = true;
+#endif
+}
+
 void AP_Periph_FW::rcout_handle_safety_state(uint8_t safety_state)
 {
     if (safety_state == 255) {

Tools/Frame_params/ArduRoller-balancebot.param

@@ -10,7 +10,6 @@ ATC_BAL_FLTE,0
 ATC_BAL_I,7
 ATC_BAL_IMAX,1
 ATC_BAL_P,5
-ATC_BAL_SPD_FF,1.0
 ATC_BRAKE,1
 ATC_STR_ACC_MAX,120
 BAL_PITCH_MAX,10

Tools/Frame_params/SkyViper-2450GPS/defaults.parm

@@ -25,6 +25,9 @@ TMODE_TMIN    0.9
 RELAY_PIN 54
 RELAY_PIN2 55
 RELAY_DEFAULT 1
+# Set to GPIO so they are used as RELAY
+SERVO5_FUNCTION  -1
+SERVO6_FUNCTION  -1
 
 AHRS_ORIENTATION 12
 COMPASS_EXTERNAL 1

Tools/ardupilotwaf/boards.py

@@ -114,6 +114,14 @@ class Board:
         else:
             cfg.msg("Enabled firmware ID checking", 'no', color='YELLOW')
 
+        if cfg.options.enable_gps_logging:
+            env.DEFINES.update(
+                AP_GPS_DEBUG_LOGGING_ENABLED=1,
+            )
+            cfg.msg("GPS Debug Logging", 'yes')
+        else:
+            cfg.msg("GPS Debug Logging", 'no', color='YELLOW')
+
         # allow enable of custom controller for any board
         # enabled on sitl by default
         if (cfg.options.enable_custom_controller or self.get_name() == "sitl") and not cfg.options.no_gcs:
@@ -241,6 +249,9 @@ class Board:
                 env.CXXFLAGS += [
                     '-Werror=implicit-fallthrough',
                 ]
+            env.CXXFLAGS += [
+                '-fcheck-new',
+            ]
 
         if cfg.env.DEBUG:
             env.CFLAGS += [
@@ -250,6 +261,10 @@ class Board:
             env.DEFINES.update(
                 HAL_DEBUG_BUILD = 1,
             )
+        elif cfg.options.g:
+            env.CFLAGS += [
+                '-g',
+            ]
         if cfg.env.COVERAGE:
             env.CFLAGS += [
                 '-fprofile-arcs',

Tools/ardupilotwaf/chibios.py

@@ -235,7 +235,7 @@ def sign_firmware(image, private_keyfile):
     try:
         import monocypher
     except ImportError:
-        Logs.error("Please install monocypher with: python3 -m pip install pymonocypher")
+        Logs.error("Please install monocypher with: python3 -m pip install pymonocypher==3.1.3.2")
         return None
     try:
         key = open(private_keyfile, 'r').read()
@@ -704,6 +704,7 @@ def build(bld):
                 'fiprintf','printf',
                 'fopen', 'fflush', 'fwrite', 'fread', 'fputs', 'fgets',
                 'clearerr', 'fseek', 'ferror', 'fclose', 'tmpfile', 'getc', 'ungetc', 'feof',
-                'ftell', 'freopen', 'remove', 'vfprintf', 'fscanf' ]
+                'ftell', 'freopen', 'remove', 'vfprintf', 'fscanf',
+                '_gettimeofday', '_times', '_times_r', '_gettimeofday_r', 'time', 'clock' ]
     for w in wraplist:
         bld.env.LINKFLAGS += ['-Wl,--wrap,%s' % w]

Tools/autotest/arducopter.py

@@ -264,27 +264,17 @@ class AutoTestCopter(AutoTest):
 
         self.do_RTL()
 
-    def watch_altitude_maintained(self, min_alt, max_alt, timeout=10):
-        '''watch alt, relative alt must remain between min_alt and max_alt'''
-        tstart = self.get_sim_time_cached()
-        while True:
-            if self.get_sim_time_cached() - tstart > timeout:
-                return
-            m = self.mav.recv_match(type='VFR_HUD', blocking=True)
-            if m.alt <= min_alt:
-                raise NotAchievedException("Altitude not maintained: want >%f got=%f" % (min_alt, m.alt))
-
     def ModeAltHold(self):
         '''Test AltHold Mode'''
         self.takeoff(10, mode="ALT_HOLD")
-        self.watch_altitude_maintained(9, 11, timeout=5)
+        self.watch_altitude_maintained(altitude_min=9, altitude_max=11)
         # feed in full elevator and aileron input and make sure we
         # retain altitude:
         self.set_rc_from_map({
             1: 1000,
             2: 1000,
         })
-        self.watch_altitude_maintained(9, 11, timeout=5)
+        self.watch_altitude_maintained(altitude_min=9, altitude_max=11)
         self.set_rc_from_map({
             1: 1500,
             2: 1500,
@@ -1119,7 +1109,7 @@ class AutoTestCopter(AutoTest):
         self.change_mode("LAND")
 
         # check vehicle descends to 2m or less within 40 seconds
-        self.wait_altitude(-5, 2, timeout=40, relative=True)
+        self.wait_altitude(-5, 2, timeout=50, relative=True)
 
         # force disarm of vehicle (it will likely not automatically disarm)
         self.disarm_vehicle(force=True)
@@ -2612,6 +2602,54 @@ class AutoTestCopter(AutoTest):
         self.wait_disarmed()
         self.progress("MOTORS DISARMED OK")
 
+    def GuidedEKFLaneChange(self):
+        '''test lane change with GPS diff on startup'''
+        self.set_parameters({
+            "EK3_SRC1_POSZ": 3,
+            "EK3_AFFINITY" : 1,
+            "GPS_TYPE2" : 1,
+            "SIM_GPS2_DISABLE" : 0,
+            "SIM_GPS2_GLTCH_Z" : -30
+            })
+        self.reboot_sitl()
+
+        self.change_mode("GUIDED")
+        self.wait_ready_to_arm()
+
+        self.progress("waiting for both EKF lanes to init")
+        self.delay_sim_time(10)
+
+        self.set_parameters({
+            "SIM_GPS2_GLTCH_Z" : 0
+            })
+
+        self.progress("waiting for EKF to do a position Z reset")
+        self.delay_sim_time(20)
+
+        self.arm_vehicle()
+        self.user_takeoff(alt_min=20)
+        m = self.mav.recv_match(type='GPS_RAW_INT', blocking=True)
+        gps_alt = m.alt*0.001
+        self.progress("Initial guided alt=%.1fm" % gps_alt)
+
+        self.context_collect('STATUSTEXT')
+        self.progress("force a lane change")
+        self.set_parameters({
+            "INS_ACCOFFS_X" : 5
+            })
+        self.wait_statustext("EKF3 lane switch 1", timeout=10, check_context=True)
+
+        self.delay_sim_time(10)
+        m = self.mav.recv_match(type='GPS_RAW_INT', blocking=True)
+        gps_alt2 = m.alt*0.001
+        alt_change = gps_alt - gps_alt2
+        self.progress("GPS Alt change by %.1fm" % abs(alt_change))
+
+        if abs(alt_change) > 2:
+            raise NotAchievedException("Altitude changed on lane switch %.1fm" % alt_change)
+        self.disarm_vehicle(force=True)
+        self.reboot_sitl()
+
     def MotorFail(self, fail_servo=0, fail_mul=0.0, holdtime=30):
         """Test flight with reduced motor efficiency"""
 
@@ -4572,7 +4610,7 @@ class AutoTestCopter(AutoTest):
         self.change_mode("STABILIZE")
         self.change_mode("GUIDED")
         self.set_rc(3, 1700)
-        self.watch_altitude_maintained(-1, 0.2) # should not take off in guided
+        self.watch_altitude_maintained(altitude_min=-1, altitude_max=0.2) # should not take off in guided
         self.run_cmd_do_set_mode(
             "ACRO",
             want_result=mavutil.mavlink.MAV_RESULT_FAILED)
@@ -9037,6 +9075,7 @@ class AutoTestCopter(AutoTest):
             self.DefaultIntervalsFromFiles,
             self.GPSTypes,
             self.MultipleGPS,
+            self.GuidedEKFLaneChange,
         ])
         return ret
 

Tools/autotest/arduplane.py

@@ -1723,6 +1723,42 @@ class AutoTestPlane(AutoTest):
         self.run_subtest("Mission test",
                          lambda: self.fly_mission("ap1.txt", strict=False))
 
+    def PitotBlockage(self):
+        '''Test detection and isolation of a blocked pitot tube'''
+        self.set_parameters({
+            "ARSPD_OPTIONS": 15,
+            "ARSPD_USE": 1,
+            "SIM_WIND_SPD": 7,
+            "SIM_WIND_DIR": 0,
+            "ARSPD_WIND_MAX": 15,
+        })
+        self.change_mode("TAKEOFF")
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        # simulate the effect of a blocked pitot tube
+        self.set_parameter("ARSPD_RATIO", 0.1)
+        self.delay_sim_time(10)
+        if (self.get_parameter("ARSPD_USE") == 0):
+            self.progress("Faulty Sensor Disabled")
+        else:
+            raise NotAchievedException("Airspeed Sensor Not Disabled")
+        self.delay_sim_time(20)
+        # simulate the effect of blockage partially clearing
+        self.set_parameter("ARSPD_RATIO", 1.0)
+        self.delay_sim_time(60)
+        if (self.get_parameter("ARSPD_USE") == 0):
+            self.progress("Faulty Sensor Remains Disabled")
+        else:
+            raise NotAchievedException("Fault Sensor Re-Enabled")
+        # simulate the effect of blockage fully clearing
+        self.set_parameter("ARSPD_RATIO", 2.0)
+        self.delay_sim_time(60)
+        if (self.get_parameter("ARSPD_USE") == 1):
+            self.progress("Sensor Re-Enabled")
+        else:
+            raise NotAchievedException("Airspeed Sensor Not Re-Enabled")
+        self.disarm_vehicle(force=True)
+
     def AIRSPEED_AUTOCAL(self):
         '''Test AIRSPEED_AUTOCAL'''
         self.progress("Ensure no AIRSPEED_AUTOCAL on ground")
@@ -3393,22 +3429,6 @@ class AutoTestPlane(AutoTest):
         attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=6)
         attempt_fence_breached_disable(start_mode="FBWA", end_mode="FBWA", expected_mode="GUIDED", action=7)
 
-    def run_auxfunc(self,
-                    function,
-                    level,
-                    want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
-        self.run_cmd(
-            mavutil.mavlink.MAV_CMD_DO_AUX_FUNCTION,
-            function,  # p1
-            level,  # p2
-            0,  # p3
-            0,  # p4
-            0,  # p5
-            0,  # p6
-            0,  # p7
-            want_result=want_result
-        )
-
     def MAV_DO_AUX_FUNCTION(self):
         '''Test triggering Auxiliary Functions via mavlink'''
         self.context_collect('STATUSTEXT')
@@ -3925,6 +3945,38 @@ class AutoTestPlane(AutoTest):
         self.disarm_vehicle(force=True)
         self.reboot_sitl()
 
+    def AltResetBadGPS(self):
+        '''Tests the handling of poor GPS lock pre-arm alt resets'''
+        self.set_parameters({
+            "SIM_GPS_GLITCH_Z": 0,
+            "SIM_GPS_ACC": 0.3,
+        })
+        self.wait_ready_to_arm()
+
+        m = self.assert_receive_message('GLOBAL_POSITION_INT')
+        relalt = m.relative_alt*0.001
+        if abs(relalt) > 3:
+            raise NotAchievedException("Bad relative alt %.1f" % relalt)
+
+        self.progress("Setting low accuracy, glitching GPS")
+        self.set_parameter("SIM_GPS_ACC", 40)
+        self.set_parameter("SIM_GPS_GLITCH_Z", -47)
+
+        self.progress("Waiting 10s for height update")
+        self.delay_sim_time(10)
+
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+
+        m = self.assert_receive_message('GLOBAL_POSITION_INT')
+        relalt = m.relative_alt*0.001
+        if abs(relalt) > 3:
+            raise NotAchievedException("Bad glitching relative alt %.1f" % relalt)
+
+        self.disarm_vehicle()
+        # reboot to clear potentially bad state
+        self.reboot_sitl()
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestPlane, self).tests()
@@ -3943,6 +3995,7 @@ class AutoTestPlane(AutoTest):
             self.TestGripperMission,
             self.Parachute,
             self.ParachuteSinkRate,
+            self.PitotBlockage,
             self.AIRSPEED_AUTOCAL,
             self.RangeFinder,
             self.FenceStatic,
@@ -4000,6 +4053,7 @@ class AutoTestPlane(AutoTest):
             self.GlideSlopeThresh,
             self.HIGH_LATENCY2,
             self.MidAirDisarmDisallowed,
+            self.AltResetBadGPS,
         ])
         return ret
 

Tools/autotest/ardusub.py

@@ -112,10 +112,10 @@ class AutoTestSub(AutoTest):
         msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5)
         if msg is None:
             raise NotAchievedException("Did not get GLOBAL_POSITION_INT")
-        pwm = 1000
-        if msg.relative_alt/1000.0 < -5.5:
+        pwm = 1300
+        if msg.relative_alt/1000.0 < -6.0:
             # need to g`o up, not down!
-            pwm = 2000
+            pwm = 1700
         self.set_rc(Joystick.Throttle, pwm)
         self.wait_altitude(altitude_min=-6, altitude_max=-5)
         self.set_rc(Joystick.Throttle, 1500)

Tools/autotest/autotest.py

@@ -10,6 +10,11 @@ from __future__ import print_function
 import atexit
 import fnmatch
 import copy
+try:
+    import distutils.dir_util
+except ImportError:
+    # we copy with this with try/except in copy_tree, below
+    pass
 import glob
 import optparse
 import os
@@ -20,7 +25,6 @@ import subprocess
 import sys
 import time
 import traceback
-from distutils.dir_util import copy_tree
 
 import rover
 import arducopter
@@ -661,6 +665,13 @@ class TestResults(object):
             f.write(badge)
 
 
+def copy_tree(f, t, dirs_exist_ok=False):
+    try:
+        distutils.dir_util.copy_tree(f, t)
+    except Exception:
+        shutil.copytree(f, t, dirs_exist_ok=dirs_exist_ok)
+
+
 def write_webresults(results_to_write):
     """Write webpage results."""
     t = mavtemplate.MAVTemplate()
@@ -671,7 +682,7 @@ def write_webresults(results_to_write):
         f.close()
     for f in glob.glob(util.reltopdir('Tools/autotest/web/*.png')):
         shutil.copy(f, buildlogs_path(os.path.basename(f)))
-    copy_tree(util.reltopdir("Tools/autotest/web/css"), buildlogs_path("css"))
+    copy_tree(util.reltopdir("Tools/autotest/web/css"), buildlogs_path("css"), dirs_exist_ok=True)
     results_to_write.generate_badge()
 
 

Tools/autotest/balancebot.py

@@ -60,7 +60,6 @@ class AutoTestBalanceBot(AutoTestRover):
         '''make sure wheel encoders are generally working'''
         ex = None
         try:
-            self.set_parameter("ATC_BAL_SPD_FF", 0)
             self.set_parameter("WENC_TYPE", 10)
             self.set_parameter("AHRS_EKF_TYPE", 10)
             self.reboot_sitl()

Tools/autotest/common.py

@@ -3398,6 +3398,22 @@ class AutoTest(ABC):
     def get_mission_count(self):
         return self.get_parameter("MIS_TOTAL")
 
+    def run_auxfunc(self,
+                    function,
+                    level,
+                    want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED):
+        self.run_cmd(
+            mavutil.mavlink.MAV_CMD_DO_AUX_FUNCTION,
+            function,  # p1
+            level,  # p2
+            0,  # p3
+            0,  # p4
+            0,  # p5
+            0,  # p6
+            0,  # p7
+            want_result=want_result
+        )
+
     def assert_mission_count(self, expected):
         count = self.get_mission_count()
         if count != expected:
@@ -5837,6 +5853,14 @@ class AutoTest(ABC):
             **kwargs
         )
 
+    def watch_altitude_maintained(self, altitude_min, altitude_max, minimum_duration=5, relative=True):
+        """Watch altitude is maintained or not between altitude_min and altitude_max during minimum_duration"""
+        return self.wait_altitude(altitude_min=altitude_min,
+                                  altitude_max=altitude_max,
+                                  relative=relative,
+                                  minimum_duration=minimum_duration,
+                                  timeout=minimum_duration + 1)
+
     def wait_climbrate(self, speed_min, speed_max, timeout=30, **kwargs):
         """Wait for a given vertical rate."""
         assert speed_min <= speed_max, "Minimum speed should be less than maximum speed."
@@ -7635,6 +7659,11 @@ Also, ignores heartbeats not from our target system'''
                                     condition='MISSION_ITEM_INT.mission_type==%u' % mission_type)
             if m is None:
                 raise NotAchievedException("Did not receive MISSION_ITEM_INT")
+            if m.target_system != self.mav.source_system:
+                raise NotAchievedException("Wrong target system (want=%u got=%u)" %
+                                           (self.mav.source_system, m.target_system))
+            if m.target_component != self.mav.source_component:
+                raise NotAchievedException("Wrong target component")
             self.progress("Got (%s)" % str(m))
             if m.mission_type != mission_type:
                 raise NotAchievedException("Received waypoint of wrong type")

Tools/autotest/default_params/balancebot.parm

@@ -1,7 +1,6 @@
-ATC_BAL_P 18.0
-ATC_BAL_I 80.0
-ATC_BAL_D 0.005
-ATC_BAL_SPD_FF 0.000000
+ATC_BAL_P 3.5
+ATC_BAL_I 5.0
+ATC_BAL_D 0.06
 ATC_SPEED_P 0.9
 ATC_SPEED_I 0.5
 AHRS_EKF_TYPE 10

Tools/autotest/param_metadata/param.py

@@ -113,6 +113,7 @@ known_units = {
              'octal'   : 'octal'                 ,
              'RPM'     : 'Revolutions Per Minute',
              'kg/m/m'  : 'kilograms per square meter', # metre is the SI unit name, meter is the american spelling of it
+             'kg/m/m/m': 'kilograms per cubic meter',
              }
 
 required_param_fields = [

Tools/autotest/pysim/util.py

@@ -21,7 +21,7 @@ import pexpect
 
 from pymavlink.rotmat import Vector3, Matrix3
 
-if (sys.version_info[0] >= 3):
+if sys.version_info[0] >= 3:
     ENCODING = 'ascii'
 else:
     ENCODING = None
@@ -53,11 +53,11 @@ def mps2kt(x):
 def topdir():
     """Return top of git tree where autotest is running from."""
     d = os.path.dirname(os.path.realpath(__file__))
-    assert(os.path.basename(d) == 'pysim')
+    assert os.path.basename(d) == 'pysim'
     d = os.path.dirname(d)
-    assert(os.path.basename(d) == 'autotest')
+    assert os.path.basename(d) == 'autotest'
     d = os.path.dirname(d)
-    assert(os.path.basename(d) == 'Tools')
+    assert os.path.basename(d) == 'Tools'
     d = os.path.dirname(d)
     return d
 
@@ -92,7 +92,7 @@ def rmfile(path):
     """Remove a file if it exists."""
     try:
         os.unlink(path)
-    except Exception:
+    except (OSError, FileNotFoundError):
         pass
 
 
@@ -368,10 +368,10 @@ def kill_mac_terminal():
 
 
 class FakeMacOSXSpawn(object):
-    '''something that looks like a pspawn child so we can ignore attempts
+    """something that looks like a pspawn child so we can ignore attempts
     to pause (and otherwise kill(1) SITL.  MacOSX using osascript to
     start/stop sitl
-    '''
+    """
     def __init__(self):
         pass
 
@@ -435,8 +435,8 @@ def start_SITL(binary,
             # attach gdb to the gdbserver:
             f = open("/tmp/x.gdb", "w")
             f.write("target extended-remote localhost:3333\nc\n")
-            for breakpoint in breakpoints:
-                f.write("b %s\n" % (breakpoint,))
+            for breakingpoint in breakpoints:
+                f.write("b %s\n" % (breakingpoint,))
             if disable_breakpoints:
                 f.write("disable\n")
             f.close()
@@ -445,8 +445,8 @@ def start_SITL(binary,
     elif gdb:
         f = open("/tmp/x.gdb", "w")
         f.write("set pagination off\n")
-        for breakpoint in breakpoints:
-            f.write("b %s\n" % (breakpoint,))
+        for breakingpoint in breakpoints:
+            f.write("b %s\n" % (breakingpoint,))
         if disable_breakpoints:
             f.write("disable\n")
         if not gdb_no_tui:
@@ -466,8 +466,8 @@ def start_SITL(binary,
                         'gdb', '-x', '/tmp/x.gdb', binary, '--args'])
     elif lldb:
         f = open("/tmp/x.lldb", "w")
-        for breakpoint in breakpoints:
-            f.write("b %s\n" % (breakpoint,))
+        for breakingpoint in breakpoints:
+            f.write("b %s\n" % (breakingpoint,))
         if disable_breakpoints:
             f.write("disable\n")
         f.write("settings set target.process.stop-on-exec false\n")
@@ -569,19 +569,19 @@ def start_SITL(binary,
 
 
 def mavproxy_cmd():
-    '''return path to which mavproxy to use'''
+    """return path to which mavproxy to use"""
     return os.getenv('MAVPROXY_CMD', 'mavproxy.py')
 
 
 def MAVProxy_version():
-    '''return the current version of mavproxy as a tuple e.g. (1,8,8)'''
+    """return the current version of mavproxy as a tuple e.g. (1,8,8)"""
     command = "%s --version" % mavproxy_cmd()
     output = subprocess.Popen(command, shell=True, stdout=subprocess.PIPE).communicate()[0]
     output = output.decode('ascii')
     match = re.search("MAVProxy Version: ([0-9]+)[.]([0-9]+)[.]([0-9]+)", output)
     if match is None:
         raise ValueError("Unable to determine MAVProxy version from (%s)" % output)
-    return (int(match.group(1)), int(match.group(2)), int(match.group(3)))
+    return int(match.group(1)), int(match.group(2)), int(match.group(3))
 
 
 def start_MAVProxy_SITL(atype,
@@ -670,7 +670,7 @@ def lock_file(fname):
     f = open(fname, mode='w')
     try:
         fcntl.lockf(f, fcntl.LOCK_EX | fcntl.LOCK_NB)
-    except Exception:
+    except OSError:
         return None
     return f
 
@@ -680,13 +680,13 @@ def check_parent(parent_pid=None):
     if parent_pid is None:
         try:
             parent_pid = os.getppid()
-        except Exception:
+        except OSError:
             pass
     if parent_pid is None:
         return
     try:
         os.kill(parent_pid, 0)
-    except Exception:
+    except OSError:
         print("Parent had finished - exiting")
         sys.exit(1)
 
@@ -751,7 +751,7 @@ def gps_newpos(lat, lon, bearing, distance):
                 cos(lat1) * sin(dr) * cos(brng))
     lon2 = lon1 + atan2(sin(brng) * sin(dr) * cos(lat1),
                         cos(dr) - sin(lat1) * sin(lat2))
-    return (degrees(lat2), degrees(lon2))
+    return degrees(lat2), degrees(lon2)
 
 
 def gps_distance(lat1, lon1, lat2, lon2):
@@ -823,7 +823,7 @@ class Wind(object):
         w_delta -= (self.turbulance_mul - 1.0) * (deltat / self.turbulance_time_constant)
         self.turbulance_mul += w_delta
         speed = self.speed * math.fabs(self.turbulance_mul)
-        return (speed, self.direction)
+        return speed, self.direction
 
     # Calculate drag.
     def drag(self, velocity, deltat=None):
@@ -877,7 +877,7 @@ def apparent_wind(wind_sp, obj_speed, alpha):
     else:
         beta = acos((delta + obj_speed) / rel_speed)
 
-    return (rel_speed, beta)
+    return rel_speed, beta
 
 
 def drag_force(wind, sp):
@@ -922,7 +922,7 @@ def constrain(value, minv, maxv):
 
 
 def load_local_module(fname):
-    '''load a python module from within the ardupilot tree'''
+    """load a python module from within the ardupilot tree"""
     fname = os.path.join(topdir(), fname)
     if sys.version_info.major >= 3:
         import importlib.util

Tools/autotest/quadplane.py

@@ -258,7 +258,7 @@ class AutoTestQuadPlane(AutoTest):
     def TestMotorMask(self):
         """Check operation of output_motor_mask"""
         """copter tailsitters will add condition: or (int(self.get_parameter('Q_TAILSIT_MOTMX')) & 1)"""
-        if not(int(self.get_parameter('Q_TILT_MASK')) & 1):
+        if not (int(self.get_parameter('Q_TILT_MASK')) & 1):
             self.progress("output_motor_mask not in use")
             return
         self.progress("Testing output_motor_mask")
@@ -266,7 +266,7 @@ class AutoTestQuadPlane(AutoTest):
 
         """Default channel for Motor1 is 5"""
         self.progress('Assert that SERVO5 is Motor1')
-        assert(33 == self.get_parameter('SERVO5_FUNCTION'))
+        assert 33 == self.get_parameter('SERVO5_FUNCTION')
 
         modes = ('MANUAL', 'FBWA', 'QHOVER')
         for mode in modes: