AP_NavEKF2: zero H_MAG to prevent compiler error

this avoids leaving the debug pins in a state where they may be
vulnerable to ESD issues

.gitmodules

@@ -1,21 +1,21 @@
 [submodule "modules/uavcan"]
 	path = modules/uavcan
-	url = git://github.com/ArduPilot/uavcan.git
+	url = https://github.com/ArduPilot/uavcan.git
 [submodule "modules/waf"]
 	path = modules/waf
-	url = git://github.com/ArduPilot/waf.git
+	url = https://github.com/ArduPilot/waf.git
 [submodule "modules/gbenchmark"]
 	path = modules/gbenchmark
-	url = git://github.com/google/benchmark.git
+	url = https://github.com/google/benchmark.git
 [submodule "modules/mavlink"]
 	path = modules/mavlink
-	url = git://github.com/ArduPilot/mavlink
+	url = https://github.com/ArduPilot/mavlink
 [submodule "gtest"]
 	path = modules/gtest
-	url = git://github.com/ArduPilot/googletest
+	url = https://github.com/ArduPilot/googletest
 [submodule "modules/ChibiOS"]
 	path = modules/ChibiOS
-	url = git://github.com/ArduPilot/ChibiOS.git
+	url = https://github.com/ArduPilot/ChibiOS.git
 [submodule "modules/libcanard"]
 	path = modules/libcanard
-	url = git://github.com/ArduPilot/libcanard.git
+	url = https://github.com/ArduPilot/libcanard.git

.travis.yml

@@ -18,8 +18,6 @@ addons:
       - python-pip
       - python-dev
       - zlib1g-dev
-      - gcc-4.9
-      - g++-4.9
       - gdb
       - cmake
       - cmake-data
@@ -62,22 +60,19 @@ matrix:
   include:
     - if: type != cron
       compiler: "gcc"
-      env: CI_BUILD_TARGET="revo-bootloader periph-build CubeOrange-bootloader iofirmware stm32f7 stm32h7 fmuv2-plane"
+      env: CI_BUILD_TARGET="stm32f7 stm32h7 fmuv2-plane"
     - if: type != cron
       compiler: "gcc"
       env: CI_BUILD_TARGET="sitltest-copter"
     - if: type != cron
       compiler: "gcc"
       env: CI_BUILD_TARGET="sitltest-quadplane sitltest-plane"
-    - if: type != cron
-      compiler: "clang-7"
-      env: CI_BUILD_TARGET="sitltest-rover sitltest-sub sitltest-balancebot"
     - if: type != cron
       compiler: "gcc"
       env: CI_BUILD_TARGET="unit-tests"
     - if: type != cron
       compiler: "clang-7"
-      env: CI_BUILD_TARGET="sitl""
+      env: CI_BUILD_TARGET="sitl"
     - language: python
       python: 3.7
       addons:  # speedup: This test does not need addons

APMrover2/GCS_Mavlink.cpp

@@ -58,7 +58,7 @@ uint32_t GCS_Rover::custom_mode() const
     return rover.control_mode->mode_number();
 }
 
-MAV_STATE GCS_MAVLINK_Rover::system_status() const
+MAV_STATE GCS_MAVLINK_Rover::vehicle_system_status() const
 {
     if ((rover.failsafe.triggered != 0) || rover.failsafe.ekf) {
         return MAV_STATE_CRITICAL;

APMrover2/GCS_Mavlink.h

@@ -43,7 +43,7 @@ private:
     void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override;
 
     MAV_MODE base_mode() const override;
-    MAV_STATE system_status() const override;
+    MAV_STATE vehicle_system_status() const override;
 
     int16_t vfr_hud_throttle() const override;
 

APMrover2/GCS_Rover.cpp

@@ -34,7 +34,7 @@ void GCS_Rover::update_vehicle_sensor_status_flags(void)
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
     }
     const AP_Proximity *proximity = AP_Proximity::get_singleton();
-    if (proximity && proximity->get_status() > AP_Proximity::Proximity_NotConnected) {
+    if (proximity && proximity->get_status() > AP_Proximity::Status::NotConnected) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
     }
@@ -73,7 +73,7 @@ void GCS_Rover::update_vehicle_sensor_status_flags(void)
             control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         }
     }
-    if (proximity && proximity->get_status() != AP_Proximity::Proximity_NoData) {
+    if (proximity && proximity->get_status() != AP_Proximity::Status::NoData) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
     }
 }

APMrover2/Parameters.cpp

@@ -671,7 +671,7 @@ ParametersG2::ParametersG2(void)
     wheel_rate_control(wheel_encoder),
     attitude_control(rover.ahrs),
     smart_rtl(),
-    proximity(rover.serial_manager),
+    proximity(),
     avoid(),
     follow(),
     windvane(),
@@ -755,15 +755,7 @@ void Rover::load_parameters(void)
         g2.crash_angle.set_default(30);
     }
 
-    const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old,  Parameters::k_param_rc_2_old,
-                                      Parameters::k_param_rc_3_old,  Parameters::k_param_rc_4_old,
-                                      Parameters::k_param_rc_5_old,  Parameters::k_param_rc_6_old,
-                                      Parameters::k_param_rc_7_old,  Parameters::k_param_rc_8_old,
-                                      Parameters::k_param_rc_9_old,  Parameters::k_param_rc_10_old,
-                                      Parameters::k_param_rc_11_old, Parameters::k_param_rc_12_old,
-                                      Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old };
-    const uint16_t old_aux_chan_mask = 0x3FFA;
-    SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, &rcmap);
+    SRV_Channels::upgrade_parameters();
     hal.console->printf("load_all took %uus\n", unsigned(micros() - before));
 
     // set a more reasonable default NAVL1_PERIOD for rovers

APMrover2/RC_Channel.h

@@ -37,7 +37,7 @@ public:
     RC_Channel_Rover obj_channels[NUM_RC_CHANNELS];
 
     RC_Channel_Rover *channel(const uint8_t chan) override {
-        if (chan > NUM_RC_CHANNELS) {
+        if (chan >= NUM_RC_CHANNELS) {
             return nullptr;
         }
         return &obj_channels[chan];

APMrover2/sensors.cpp

@@ -123,7 +123,6 @@ void Rover::read_rangefinders(void)
 void Rover::init_proximity(void)
 {
     g2.proximity.init();
-    g2.proximity.set_rangefinder(&rangefinder);
 }
 
 /*

APMrover2/system.cpp

@@ -199,9 +199,7 @@ void Rover::startup_ground(void)
 #endif
 
 #ifdef ENABLE_SCRIPTING
-    if (!g2.scripting.init()) {
-        gcs().send_text(MAV_SEVERITY_ERROR, "Scripting failed to start");
-    }
+    g2.scripting.init();
 #endif // ENABLE_SCRIPTING
 
     // we don't want writes to the serial port to cause us to pause

APMrover2/wscript

@@ -15,7 +15,6 @@ def build(bld):
             'AP_Navigation',
             'AR_WPNav',
             'AP_RCMapper',
-            'AC_PID',
             'AP_Stats',
             'AP_Beacon',
             'AP_AdvancedFailsafe',

AntennaTracker/GCS_Mavlink.cpp

@@ -67,7 +67,7 @@ uint32_t GCS_Tracker::custom_mode() const
     return tracker.control_mode;
 }
 
-MAV_STATE GCS_MAVLINK_Tracker::system_status() const
+MAV_STATE GCS_MAVLINK_Tracker::vehicle_system_status() const
 {
     if (tracker.control_mode == INITIALISING) {
         return MAV_STATE_CALIBRATING;
@@ -627,15 +627,3 @@ void GCS_MAVLINK_Tracker::send_global_position_int()
         0,                        // Z speed cm/s (+ve Down)
         tracker.ahrs.yaw_sensor); // compass heading in 1/100 degree
 }
-
-
-/* dummy methods to avoid having to link against AP_Camera */
-void AP_Camera::control_msg(const mavlink_message_t &) {}
-void AP_Camera::configure(float, float, float, float, float, float, float) {}
-void AP_Camera::control(float, float, float, float, float, float) {}
-void AP_Camera::send_feedback(mavlink_channel_t chan) {}
-/* end dummy methods to avoid having to link against AP_Camera */
-
-// dummy method to avoid linking AFS
-bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) {return false;}
-AP_AdvancedFailsafe *AP::advancedfailsafe() { return nullptr; }

AntennaTracker/GCS_Mavlink.h

@@ -42,7 +42,7 @@ private:
     void send_global_position_int() override;
 
     MAV_MODE base_mode() const override;
-    MAV_STATE system_status() const override;
+    MAV_STATE vehicle_system_status() const override;
 
     bool waypoint_receiving;
 };

AntennaTracker/RC_Channel.h

@@ -19,7 +19,7 @@ public:
 
     RC_Channel_Tracker obj_channels[NUM_RC_CHANNELS];
     RC_Channel_Tracker *channel(const uint8_t chan) override {
-        if (chan > NUM_RC_CHANNELS) {
+        if (chan >= NUM_RC_CHANNELS) {
             return nullptr;
         }
         return &obj_channels[chan];

AntennaTracker/camera.cpp

@@ -1,13 +0,0 @@
-/* dummy methods to avoid having to link against AP_Camera */
-
-#include <AP_Camera/AP_Camera.h>
-
-namespace AP {
-    AP_Camera *camera() {
-        return nullptr;
-    }
-};
-
-void AP_Camera::take_picture()
-{
-}

AntennaTracker/system.cpp

@@ -60,9 +60,7 @@ void Tracker::init_tracker()
 #endif
 
 #ifdef ENABLE_SCRIPTING
-    if (!scripting.init()) {
-        gcs().send_text(MAV_SEVERITY_ERROR, "Scripting failed to start");
-    }
+    scripting.init();
 #endif // ENABLE_SCRIPTING
 
     // initialise compass
@@ -267,3 +265,28 @@ bool Tracker::should_log(uint32_t mask)
     }
     return true;
 }
+
+
+#include <AP_Camera/AP_Camera.h>
+#include <AP_AdvancedFailsafe/AP_AdvancedFailsafe.h>
+
+/* dummy methods to avoid having to link against AP_Camera */
+void AP_Camera::control_msg(const mavlink_message_t &) {}
+void AP_Camera::configure(float, float, float, float, float, float, float) {}
+void AP_Camera::control(float, float, float, float, float, float) {}
+void AP_Camera::send_feedback(mavlink_channel_t chan) {}
+void AP_Camera::take_picture() {}
+void AP_Camera::cam_mode_toggle() {}
+void AP_Camera::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) {}
+AP_Camera *AP_Camera::_singleton;
+namespace AP {
+    AP_Camera *camera() {
+        return nullptr;
+    }
+};
+
+/* end dummy methods to avoid having to link against AP_Camera */
+
+// dummy method to avoid linking AFS
+bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) {return false;}
+AP_AdvancedFailsafe *AP::advancedfailsafe() { return nullptr; }

AntennaTracker/wscript

@@ -7,7 +7,6 @@ def build(bld):
         name=vehicle + '_libs',
         ap_vehicle=vehicle,
         ap_libraries=bld.ap_common_vehicle_libraries() + [
-            'AC_PID',
             'AP_Beacon',
             'AP_Arming',
             'AP_Stats',

ArduCopter/AP_Arming.cpp

@@ -47,16 +47,17 @@ bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure)
         check_failed(display_failure, "Motor Interlock Enabled");
     }
 
-    // succeed if pre arm checks are disabled
+    // if pre arm checks are disabled run only the mandatory checks
     if (checks_to_perform == 0) {
-        return true;
+        return mandatory_checks(display_failure);
     }
 
     return fence_checks(display_failure)
         & parameter_checks(display_failure)
         & motor_checks(display_failure)
         & pilot_throttle_checks(display_failure)
-        & oa_checks(display_failure) &
+        & oa_checks(display_failure)
+        & gcs_failsafe_check(display_failure) &
         AP_Arming::pre_arm_checks(display_failure);
 }
 
@@ -91,8 +92,9 @@ bool AP_Arming_Copter::compass_checks(bool display_failure)
     if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_COMPASS)) {
         // check compass offsets have been set.  AP_Arming only checks
         // this if learning is off; Copter *always* checks.
-        if (!AP::compass().configured()) {
-            check_failed(ARMING_CHECK_COMPASS, display_failure, "Compass not calibrated");
+        char failure_msg[50] = {};
+        if (!AP::compass().configured(failure_msg, ARRAY_SIZE(failure_msg))) {
+            check_failed(ARMING_CHECK_COMPASS, display_failure, "%s", failure_msg);
             ret = false;
         }
     }
@@ -143,6 +145,14 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
     // check various parameter values
     if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) {
 
+	    // checks MOT_PWM_MIN/MAX for acceptable values
+#if (FRAME_CONFIG != HELI_FRAME)
+        if (copter.motors->check_mot_pwm_params()) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check MOT_PWM_MAX/MIN");
+            return false;
+        } 
+#endif
+
         // ensure all rc channels have different functions
         if (rc().duplicate_options_exist()) {
             check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Duplicate Aux Switch Options");
@@ -191,6 +201,14 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
             check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Heli motors checks failed");
             return false;
         }
+
+        char fail_msg[50];
+        // check input mangager parameters
+        if (!copter.input_manager.parameter_check(fail_msg, ARRAY_SIZE(fail_msg))) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "%s", fail_msg);
+            return false;
+        }
+
         // Inverted flight feature disabled for Heli Single and Dual frames
         if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD &&
             rc().find_channel_for_option(RC_Channel::aux_func_t::INVERTED) != nullptr) {
@@ -249,6 +267,14 @@ bool AP_Arming_Copter::motor_checks(bool display_failure)
         return false;
     }
 
+	    //servo_test check
+#if FRAME_CONFIG == HELI_FRAME
+    if(copter.motors->servo_test_running()) {
+        check_failed(display_failure, "Servo Test is still running");
+        return false;
+    }
+#endif
+
     // if this is a multicopter using ToshibaCAN ESCs ensure MOT_PMW_MIN = 1000, MOT_PWM_MAX = 2000
 #if HAL_WITH_UAVCAN && (FRAME_CONFIG != HELI_FRAME)
     bool tcan_active = false;
@@ -329,17 +355,9 @@ bool AP_Arming_Copter::rc_calibration_checks(bool display_failure)
 // performs pre_arm gps related checks and returns true if passed
 bool AP_Arming_Copter::gps_checks(bool display_failure)
 {
-    AP_Notify::flags.pre_arm_gps_check = false;
-
-    const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
-
-    // always check if inertial nav has started and is ready
-    if (!ahrs.prearm_healthy()) {
-        const char *reason = ahrs.prearm_failure_reason();
-        if (reason == nullptr) {
-            reason = "AHRS not healthy";
-        }
-        check_failed(display_failure, "%s", reason);
+    // run mandatory gps checks first
+    if (!mandatory_gps_checks(display_failure)) {
+        AP_Notify::flags.pre_arm_gps_check = false;
         return false;
     }
 
@@ -359,46 +377,6 @@ bool AP_Arming_Copter::gps_checks(bool display_failure)
         return true;
     }
 
-    // ensure GPS is ok
-    if (!copter.position_ok()) {
-        const char *reason = ahrs.prearm_failure_reason();
-        if (reason == nullptr) {
-            if (!mode_requires_gps && fence_requires_gps) {
-                // clarify to user why they need GPS in non-GPS flight mode
-                reason = "Fence enabled, need 3D Fix";
-            } else {
-                reason = "Need 3D Fix";
-            }
-        }
-        check_failed(display_failure, "%s", reason);
-        return false;
-    }
-
-    // check for GPS glitch (as reported by EKF)
-    nav_filter_status filt_status;
-    if (ahrs.get_filter_status(filt_status)) {
-        if (filt_status.flags.gps_glitching) {
-            check_failed(display_failure, "GPS glitching");
-            return false;
-        }
-    }
-
-    // check EKF compass variance is below failsafe threshold
-    float vel_variance, pos_variance, hgt_variance, tas_variance;
-    Vector3f mag_variance;
-    Vector2f offset;
-    ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset);
-    if (copter.g.fs_ekf_thresh > 0 && mag_variance.length() >= copter.g.fs_ekf_thresh) {
-        check_failed(display_failure, "EKF compass variance");
-        return false;
-    }
-
-    // check home and EKF origin are not too far
-    if (copter.far_from_EKF_origin(ahrs.get_home())) {
-        check_failed(display_failure, "EKF-home variance");
-        return false;
-    }
-
     // return true immediately if gps check is disabled
     if (!(checks_to_perform == ARMING_CHECK_ALL || checks_to_perform & ARMING_CHECK_GPS)) {
         AP_Notify::flags.pre_arm_gps_check = true;
@@ -408,11 +386,13 @@ 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) {
         check_failed(ARMING_CHECK_GPS, display_failure, "High GPS HDOP");
+        AP_Notify::flags.pre_arm_gps_check = false;
         return false;
     }
 
     // call parent gps checks
     if (!AP_Arming::gps_checks(display_failure)) {
+        AP_Notify::flags.pre_arm_gps_check = false;
         return false;
     }
 
@@ -490,6 +470,89 @@ bool AP_Arming_Copter::proximity_checks(bool display_failure) const
     return true;
 }
 
+// performs mandatory gps checks.  returns true if passed
+bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure)
+{
+    // always check if inertial nav has started and is ready
+    const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
+    if (!ahrs.prearm_healthy()) {
+        const char *reason = ahrs.prearm_failure_reason();
+        if (reason == nullptr) {
+            reason = "AHRS not healthy";
+        }
+        check_failed(display_failure, "%s", reason);
+        return false;
+    }
+
+    // check if flight mode requires GPS
+    bool mode_requires_gps = copter.flightmode->requires_GPS();
+
+    // check if fence requires GPS
+    bool fence_requires_gps = false;
+    #if AC_FENCE == ENABLED
+    // if circular or polygon fence is enabled we need GPS
+    fence_requires_gps = (copter.fence.get_enabled_fences() & (AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON)) > 0;
+    #endif
+
+    // return true if GPS is not required
+    if (!mode_requires_gps && !fence_requires_gps) {
+        return true;
+    }
+
+    // ensure GPS is ok
+    if (!copter.position_ok()) {
+        const char *reason = ahrs.prearm_failure_reason();
+        if (reason == nullptr) {
+            if (!mode_requires_gps && fence_requires_gps) {
+                // clarify to user why they need GPS in non-GPS flight mode
+                reason = "Fence enabled, need 3D Fix";
+            } else {
+                reason = "Need 3D Fix";
+            }
+        }
+        check_failed(display_failure, "%s", reason);
+        return false;
+    }
+
+    // check for GPS glitch (as reported by EKF)
+    nav_filter_status filt_status;
+    if (ahrs.get_filter_status(filt_status)) {
+        if (filt_status.flags.gps_glitching) {
+            check_failed(display_failure, "GPS glitching");
+            return false;
+        }
+    }
+
+    // check EKF compass variance is below failsafe threshold
+    float vel_variance, pos_variance, hgt_variance, tas_variance;
+    Vector3f mag_variance;
+    Vector2f offset;
+    ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset);
+    if (copter.g.fs_ekf_thresh > 0 && mag_variance.length() >= copter.g.fs_ekf_thresh) {
+        check_failed(display_failure, "EKF compass variance");
+        return false;
+    }
+
+    // check home and EKF origin are not too far
+    if (copter.far_from_EKF_origin(ahrs.get_home())) {
+        check_failed(display_failure, "EKF-home variance");
+        return false;
+    }
+
+    // if we got here all must be ok
+    return true;
+}
+
+// Check GCS failsafe
+bool AP_Arming_Copter::gcs_failsafe_check(bool display_failure)
+{
+    if (copter.failsafe.gcs) {
+        check_failed(display_failure, "GCS failsafe on");
+        return false;
+    }
+    return true;
+}
+
 // arm_checks - perform final checks before arming
 //  always called just before arming.  Return true if ok to arm
 //  has side-effect that logging is started
@@ -610,6 +673,15 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
     return AP_Arming::arm_checks(method);
 }
 
+// mandatory checks that will be run if ARMING_CHECK is zero or arming forced
+bool AP_Arming_Copter::mandatory_checks(bool display_failure)
+{
+    // call mandatory gps checks and update notify status because regular gps checks will not run
+    const bool result = mandatory_gps_checks(display_failure);
+    AP_Notify::flags.pre_arm_gps_check = result;
+    return result;
+}
+
 void AP_Arming_Copter::set_pre_arm_check(bool b)
 {
     copter.ap.pre_arm_check = b;

ArduCopter/AP_Arming.h

@@ -34,6 +34,9 @@ protected:
     bool proximity_checks(bool display_failure) const override;
     bool arm_checks(AP_Arming::Method method) override;
 
+    // mandatory checks that cannot be bypassed.  This function will only be called if ARMING_CHECK is zero or arming forced
+    bool mandatory_checks(bool display_failure) override;
+
     // NOTE! the following check functions *DO* call into AP_Arming:
     bool ins_checks(bool display_failure) override;
     bool compass_checks(bool display_failure) override;
@@ -46,6 +49,8 @@ protected:
     bool motor_checks(bool display_failure);
     bool pilot_throttle_checks(bool display_failure);
     bool oa_checks(bool display_failure);
+    bool mandatory_gps_checks(bool display_failure);
+    bool gcs_failsafe_check(bool display_failure);
 
     void set_pre_arm_check(bool b);
 

ArduCopter/AP_State.cpp

@@ -74,6 +74,9 @@ void Copter::set_failsafe_radio(bool b)
 void Copter::set_failsafe_gcs(bool b)
 {
     failsafe.gcs = b;
+
+    // update AP_Notify
+        AP_Notify::flags.failsafe_gcs = b;
 }
 
 // ---------------------------------------------

ArduCopter/Attitude.cpp

@@ -10,18 +10,16 @@ float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
     }
     float yaw_request;
 
+    // range check expo
+    g2.acro_y_expo = constrain_float(g2.acro_y_expo, 0.0f, 1.0f);
+
     // calculate yaw rate request
-    if (g2.acro_y_expo <= 0) {
+    if (is_zero(g2.acro_y_expo)) {
         yaw_request = stick_angle * g.acro_yaw_p;
     } else {
         // expo variables
         float y_in, y_in3, y_out;
 
-        // range check expo
-        if (g2.acro_y_expo > 1.0f || g2.acro_y_expo < 0.5f) {
-            g2.acro_y_expo = 1.0f;
-        }
-
         // yaw expo
         y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX;
         y_in3 = y_in*y_in*y_in;
@@ -40,7 +38,6 @@ float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
 //  called at 100hz
 void Copter::update_throttle_hover()
 {
-#if FRAME_CONFIG != HELI_FRAME
     // if not armed or landed exit
     if (!motors->armed() || ap.land_complete) {
         return;
@@ -59,13 +56,12 @@ void Copter::update_throttle_hover()
     // get throttle output
     float throttle = motors->get_throttle();
 
-    // calc average throttle if we are in a level hover
+    // calc average throttle if we are in a level hover.  accounts for heli hover roll trim
     if (throttle > 0.0f && fabsf(inertial_nav.get_velocity_z()) < 60 &&
-        labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
+        labs(ahrs.roll_sensor-attitude_control->get_roll_trim_cd()) < 500 && labs(ahrs.pitch_sensor) < 500) {
         // Can we set the time constant automatically
         motors->update_throttle_hover(0.01f);
     }
-#endif
 }
 
 // set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared

ArduCopter/Copter.cpp

@@ -107,7 +107,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
     SCHED_TASK(read_rangefinder,      20,    100),
 #endif
 #if PROXIMITY_ENABLED == ENABLED
-    SCHED_TASK_CLASS(AP_Proximity,         &copter.g2.proximity,        update,         100,  50),
+    SCHED_TASK_CLASS(AP_Proximity,         &copter.g2.proximity,        update,         200,  50),
 #endif
 #if BEACON_ENABLED == ENABLED
     SCHED_TASK_CLASS(AP_Beacon,            &copter.g2.beacon,           update,         400,  50),
@@ -216,9 +216,6 @@ void Copter::setup()
     // Load the default values of variables listed in var_info[]s
     AP_Param::setup_sketch_defaults();
 
-    // setup storage layout for copter
-    StorageManager::set_layout_copter();
-
     init_ardupilot();
 
     // initialise the main loop scheduler
@@ -250,6 +247,9 @@ void Copter::fast_loop()
 
 #if FRAME_CONFIG == HELI_FRAME
     update_heli_control_dynamics();
+    #if MODE_AUTOROTATE_ENABLED == ENABLED
+        heli_update_autorotation();
+    #endif
 #endif //HELI_FRAME
 
     // Inertial Nav
@@ -294,7 +294,7 @@ void Copter::rc_loop()
 void Copter::throttle_loop()
 {
     // update throttle_low_comp value (controls priority of throttle vs attitude control)
-    update_throttle_thr_mix();
+    update_throttle_mix();
 
     // check auto_armed status
     update_auto_armed();
@@ -403,6 +403,13 @@ void Copter::twentyfive_hz_logging()
     // log output
     Log_Write_Precland();
 #endif
+
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    if (should_log(MASK_LOG_ATTITUDE_MED) || should_log(MASK_LOG_ATTITUDE_FAST)) {
+        //update autorotation log
+        g2.arot.Log_Write_Autorotation();
+    }
+#endif
 }
 
 // three_hz_loop - 3.3hz loop

ArduCopter/Copter.h

@@ -84,6 +84,10 @@
  #define MOTOR_CLASS AP_MotorsMulticopter
 #endif
 
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+ #include <AC_Autorotation/AC_Autorotation.h> // Autorotation controllers
+#endif
+
 #include "RC_Channel.h"         // RC Channel Library
 
 #include "GCS_Mavlink.h"
@@ -95,6 +99,7 @@
 #if BEACON_ENABLED == ENABLED
  #include <AP_Beacon/AP_Beacon.h>
 #endif
+
 #if AC_AVOID_ENABLED == ENABLED
  #include <AC_Avoidance/AC_Avoid.h>
 #endif
@@ -230,6 +235,7 @@ public:
     friend class ModeSystemId;
     friend class ModeThrow;
     friend class ModeZigZag;
+    friend class ModeAutorotate;
 
     Copter(void);
 
@@ -286,20 +292,21 @@ private:
         void set_target_alt_cm(float target_alt_cm);
 
         // get target and actual distances (in m) for logging purposes
-        bool get_dist_for_logging(float &target_dist, float &actual_dist) const;
+        bool get_target_dist_for_logging(float &target_dist) const;
+        float get_dist_for_logging() const;
         void invalidate_for_logging() { valid_for_logging = false; }
 
-        // surface tracking states
-        enum class SurfaceTrackingState {
-            SURFACE_TRACKING_DISABLED = 0,
-            SURFACE_TRACKING_GROUND = 1,
-            SURFACE_TRACKING_CEILING = 2
+        // surface tracking surface
+        enum class Surface {
+            NONE = 0,
+            GROUND = 1,
+            CEILING = 2
         };
-        // set direction
-        void set_state(SurfaceTrackingState state);
+        // set surface to track
+        void set_surface(Surface new_surface);
 
     private:
-        SurfaceTrackingState tracking_state = SurfaceTrackingState::SURFACE_TRACKING_GROUND;
+        Surface surface = Surface::GROUND;
         float target_dist_cm;       // desired distance in cm from ground or ceiling
         uint32_t last_update_ms;    // system time of last update to target_alt_cm
         uint32_t last_glitch_cleared_ms;    // system time of last handle glitch recovery
@@ -477,6 +484,7 @@ private:
     AC_PosControl *pos_control;
     AC_WPNav *wp_nav;
     AC_Loiter *loiter_nav;
+
 #if MODE_CIRCLE_ENABLED == ENABLED
     AC_Circle *circle_nav;
 #endif
@@ -575,6 +583,7 @@ private:
     typedef struct {
         uint8_t dynamic_flight          : 1;    // 0   // true if we are moving at a significant speed (used to turn on/off leaky I terms)
         uint8_t inverted_flight         : 1;    // 1   // true for inverted flight mode
+        uint8_t in_autorotation         : 1;    // 2   // true when heli is in autorotation
     } heli_flags_t;
     heli_flags_t heli_flags;
 
@@ -591,9 +600,6 @@ private:
 
     bool standby_active;
 
-    // set when we are upgrading parameters from 3.4
-    bool upgrading_frame_params;
-
     static const AP_Scheduler::Task scheduler_tasks[];
     static const AP_Param::Info var_info[];
     static const struct LogStructure log_structure[];
@@ -616,6 +622,14 @@ private:
         Failsafe_Action_Terminate      = 5
     };
 
+    enum class FailsafeOption {
+        RC_CONTINUE_IF_AUTO             = (1<<0),   // 1
+        GCS_CONTINUE_IF_AUTO            = (1<<1),   // 2
+        RC_CONTINUE_IF_GUIDED           = (1<<2),   // 4
+        CONTINUE_IF_LANDING             = (1<<3),   // 8
+        GCS_CONTINUE_IF_PILOT_CONTROL   = (1<<4),   // 16
+    };
+
     static constexpr int8_t _failsafe_priorities[] = {
                                                       Failsafe_Action_Terminate,
                                                       Failsafe_Action_Land,
@@ -710,10 +724,12 @@ private:
     void esc_calibration_setup();
 
     // events.cpp
+    bool failsafe_option(FailsafeOption opt) const;
     void failsafe_radio_on_event();
     void failsafe_radio_off_event();
     void handle_battery_failsafe(const char* type_str, const int8_t action);
     void failsafe_gcs_check();
+    void failsafe_gcs_on_event(void);
     void failsafe_gcs_off_event(void);
     void failsafe_terrain_check();
     void failsafe_terrain_set_status(bool data_ok);
@@ -723,6 +739,7 @@ private:
     void set_mode_SmartRTL_or_RTL(ModeReason reason);
     void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);
     bool should_disarm_on_failsafe();
+    void do_failsafe_action(Failsafe_Action action, ModeReason reason);
 
     // failsafe.cpp
     void failsafe_enable();
@@ -739,8 +756,12 @@ private:
     void check_dynamic_flight(void);
     void update_heli_control_dynamics(void);
     void heli_update_landing_swash();
+    float get_pilot_desired_rotor_speed() const;
     void heli_update_rotor_speed_targets();
-
+    void heli_update_autorotation();
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    void heli_set_autorotation(bool autotrotation);
+#endif
     // inertia.cpp
     void read_inertia();
 
@@ -749,7 +770,7 @@ private:
     void update_land_detector();
     void set_land_complete(bool b);
     void set_land_complete_maybe(bool b);
-    void update_throttle_thr_mix();
+    void update_throttle_mix();
 
     // landing_gear.cpp
     void landinggear_update();
@@ -813,6 +834,7 @@ private:
     void convert_pid_parameters(void);
     void convert_lgr_parameters(void);
     void convert_tradheli_parameters(void);
+    void convert_fs_options_params(void);
 
     // precision_landing.cpp
     void init_precland();
@@ -968,6 +990,9 @@ private:
 #if MODE_ZIGZAG_ENABLED == ENABLED
     ModeZigZag mode_zigzag;
 #endif
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    ModeAutorotate mode_autorotate;
+#endif
 
     // mode.cpp
     Mode *mode_from_mode_num(const Mode::Number mode);

ArduCopter/GCS_Copter.cpp

@@ -93,7 +93,9 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void)
     case Mode::Number::THROW:
     case Mode::Number::SMART_RTL:
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
         break;
     case Mode::Number::ALT_HOLD:
     case Mode::Number::GUIDED_NOGPS:
@@ -101,6 +103,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void)
     case Mode::Number::AUTOTUNE:
     case Mode::Number::FLOWHOLD:
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
         break;
     default:
         // stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual)

ArduCopter/GCS_Mavlink.cpp

@@ -72,7 +72,7 @@ uint32_t GCS_Copter::custom_mode() const
     return (uint32_t)copter.control_mode;
 }
 
-MAV_STATE GCS_MAVLINK_Copter::system_status() const
+MAV_STATE GCS_MAVLINK_Copter::vehicle_system_status() const
 {
     // set system as critical if any failsafe have triggered
     if (copter.any_failsafe_triggered())  {

ArduCopter/GCS_Mavlink.h

@@ -57,7 +57,7 @@ private:
                         const mavlink_message_t &msg) override;
 
     MAV_MODE base_mode() const override;
-    MAV_STATE system_status() const override;
+    MAV_STATE vehicle_system_status() const override;
 
     int16_t vfr_hud_throttle() const override;
     float vfr_hud_alt() const override;

ArduCopter/Log.cpp

@@ -41,10 +41,9 @@ void Copter::Log_Write_Control_Tuning()
     }
 
     // get surface tracking alts
-    float desired_rangefinder_alt, rangefinder_alt;
-    if (!surface_tracking.get_dist_for_logging(desired_rangefinder_alt, rangefinder_alt)) {
+    float desired_rangefinder_alt;
+    if (!surface_tracking.get_target_dist_for_logging(desired_rangefinder_alt)) {
         desired_rangefinder_alt = AP::logger().quiet_nan();
-        rangefinder_alt = AP::logger().quiet_nan();;
     }
 
     struct log_Control_Tuning pkt = {
@@ -58,7 +57,7 @@ void Copter::Log_Write_Control_Tuning()
         inav_alt            : inertial_nav.get_altitude() / 100.0f,
         baro_alt            : baro_alt,
         desired_rangefinder_alt : desired_rangefinder_alt,
-        rangefinder_alt     : rangefinder_alt,
+        rangefinder_alt     : surface_tracking.get_dist_for_logging(),
         terr_alt            : terr_alt,
         target_climb_rate   : target_climb_rate_cms,
         climb_rate          : int16_t(inertial_nav.get_velocity_z()), // float -> int16_t
@@ -463,7 +462,7 @@ const struct LogStructure Copter::log_structure[] = {
     { LOG_PARAMTUNE_MSG, sizeof(log_ParameterTuning),
       "PTUN", "QBfff",         "TimeUS,Param,TunVal,TunMin,TunMax", "s----", "F----" },
     { LOG_CONTROL_TUNING_MSG, sizeof(log_Control_Tuning),
-      "CTUN", "Qffffffefffhhf", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt,N", "s----mmmmmmnnz", "F----00B0BBBB-" },
+      "CTUN", "Qffffffefffhhf", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt,N", "s----mmmmmmnnz", "F----00B000BB-" },
     { LOG_MOTBATT_MSG, sizeof(log_MotBatt),
       "MOTB", "Qffff",  "TimeUS,LiftMax,BatVolt,BatRes,ThLimit", "s-vw-", "F-00-" },
     { LOG_DATA_INT16_MSG, sizeof(log_Data_Int16t),         
@@ -482,7 +481,7 @@ const struct LogStructure Copter::log_structure[] = {
 #endif
 #if PRECISION_LANDING == ENABLED
     { LOG_PRECLAND_MSG, sizeof(log_Precland),
-      "PL",    "QBBfffffffIIB",    "TimeUS,Heal,TAcq,pX,pY,vX,vY,mX,mY,mZ,LastMeasUS,EKFOutl,Est", "s--ddmmddms--","F--00BB00BC--" },
+      "PL",    "QBBfffffffIIB",    "TimeUS,Heal,TAcq,pX,pY,vX,vY,mX,mY,mZ,LastMeasMS,EKFOutl,Est", "s--mmnnmmms--","F--BBBBBBBC--" },
 #endif
     { LOG_SYSIDD_MSG, sizeof(log_SysIdD),
       "SIDD", "Qfffffffff",  "TimeUS,Time,Targ,F,Gx,Gy,Gz,Ax,Ay,Az", "ss-zkkkooo", "F---------" },

ArduCopter/Parameters.cpp

@@ -53,7 +53,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @Param: SYSID_MYGCS
     // @DisplayName: My ground station number
     // @Description: Allows restricting radio overrides to only come from my ground station
-    // @Values: 255:Mission Planner and DroidPlanner, 252: AP Planner 2
+    // @Range: 1 255
     // @User: Advanced
     GSCALAR(sysid_my_gcs,   "SYSID_MYGCS",     255),
 
@@ -172,10 +172,10 @@ const AP_Param::Info Copter::var_info[] = {
 
     // @Param: FS_GCS_ENABLE
     // @DisplayName: Ground Station Failsafe Enable
-    // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. NB. The GCS Failsafe is only active when RC_OVERRIDE is being used to control the vehicle.
-    // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode,3:Enabled always SmartRTL or RTL,4:Enabled always SmartRTL or Land
+    // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. See FS_OPTIONS param for additional actions, or for cases allowing Mission continuation, when GCS failsafe is enabled.
+    // @Values: 0:Disabled/NoAction,1:RTL,2:RTL or Continue with Mission in Auto Mode (Deprecated in 4.0+-see FS_OPTIONS),3:SmartRTL or RTL,4:SmartRTL or Land,5:Land (4.0+ Only)
     // @User: Standard
-    GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL),
+    GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED),
 
     // @Param: GPS_HDOP_GOOD
     // @DisplayName: GPS Hdop Good
@@ -237,14 +237,14 @@ const AP_Param::Info Copter::var_info[] = {
     // @Param: FS_THR_ENABLE
     // @DisplayName: Throttle Failsafe Enable
     // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel
-    // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode,3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land
+    // @Values:  0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Deprecated in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land
     // @User: Standard
     GSCALAR(failsafe_throttle,  "FS_THR_ENABLE",   FS_THR_ENABLED_ALWAYS_RTL),
 
     // @Param: FS_THR_VALUE
     // @DisplayName: Throttle Failsafe Value
     // @Description: The PWM level in microseconds on channel 3 below which throttle failsafe triggers
-    // @Range: 925 1100
+    // @Range: 910 1100
     // @Units: PWM
     // @Increment: 1
     // @User: Standard
@@ -262,42 +262,42 @@ const AP_Param::Info Copter::var_info[] = {
     // @Param: FLTMODE1
     // @DisplayName: Flight Mode 1
     // @Description: Flight mode when Channel 5 pwm is <= 1230
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
     // @User: Standard
     GSCALAR(flight_mode1, "FLTMODE1",               (uint8_t)FLIGHT_MODE_1),
 
     // @Param: FLTMODE2
     // @DisplayName: Flight Mode 2
     // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
     // @User: Standard
     GSCALAR(flight_mode2, "FLTMODE2",               (uint8_t)FLIGHT_MODE_2),
 
     // @Param: FLTMODE3
     // @DisplayName: Flight Mode 3
     // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
     // @User: Standard
     GSCALAR(flight_mode3, "FLTMODE3",               (uint8_t)FLIGHT_MODE_3),
 
     // @Param: FLTMODE4
     // @DisplayName: Flight Mode 4
     // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
     // @User: Standard
     GSCALAR(flight_mode4, "FLTMODE4",               (uint8_t)FLIGHT_MODE_4),
 
     // @Param: FLTMODE5
     // @DisplayName: Flight Mode 5
     // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
     // @User: Standard
     GSCALAR(flight_mode5, "FLTMODE5",               (uint8_t)FLIGHT_MODE_5),
 
     // @Param: FLTMODE6
     // @DisplayName: Flight Mode 6
     // @Description: Flight mode when Channel 5 pwm is >=1750
-    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID
+    // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,9:Land,11:Drift,13:Sport,14:Flip,15:AutoTune,16:PosHold,17:Brake,18:Throw,19:Avoid_ADSB,20:Guided_NoGPS,21:Smart_RTL,22:FlowHold,23:Follow,24:ZigZag,25:SystemID,26:Heli_Autorotate
     // @User: Standard
     GSCALAR(flight_mode6, "FLTMODE6",               (uint8_t)FLIGHT_MODE_6),
 
@@ -333,13 +333,13 @@ const AP_Param::Info Copter::var_info[] = {
     // @DisplayName: Channel 6 Tuning
     // @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob
     // @User: Standard
-    // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,57:Winch
+    // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,57:Winch,58:SysID Magnitude
     GSCALAR(radio_tuning, "TUNE",                   0),
 
     // @Param: FRAME_TYPE
     // @DisplayName: Frame Type (+, X, V, etc)
     // @Description: Controls motor mixing for multicopters.  Not used for Tri or Traditional Helicopters.
-    // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F, 12:BetaFlightX, 13:DJIX, 14:ClockwiseX, 15: I
+    // @Values: 0:Plus, 1:X, 2:V, 3:H, 4:V-Tail, 5:A-Tail, 10:Y6B, 11:Y6F, 12:BetaFlightX, 13:DJIX, 14:ClockwiseX, 15: I, 18: BetaFlightXReversed
     // @User: Standard
     // @RebootRequired: True
     GSCALAR(frame_type, "FRAME_TYPE", HAL_FRAME_TYPE_DEFAULT),
@@ -712,7 +712,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @Path: ../libraries/AP_OSD/AP_OSD.cpp
     GOBJECT(osd, "OSD", AP_OSD),
 #endif
-    
+
     // @Group:
     // @Path: Parameters.cpp
     GOBJECT(g2, "",  ParametersG2),
@@ -939,7 +939,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: mode_systemid.cpp
     AP_SUBGROUPPTR(mode_systemid_ptr, "SID", 34, ParametersG2, ModeSystemId),
 #endif
-    
+
     // @Param: FS_VIBE_ENABLE
     // @DisplayName: Vibration Failsafe enable
     // @Description: This enables the vibration failsafe which will use modified altitude estimation and control during high vibrations
@@ -947,6 +947,22 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("FS_VIBE_ENABLE", 35, ParametersG2, fs_vibe_enabled, 1),
 
+    // @Param: FS_OPTIONS
+    // @DisplayName: Failsafe options bitmask
+    // @Description: Bitmask of additional options for battery, radio, & GCS failsafes. 0 (default) disables all options.
+    // @Values: 0:Disabled, 1:Continue if in Auto on RC failsafe only, 2:Continue if in Auto on GCS failsafe only, 3:Continue if in Auto on RC and/or GCS failsafe, 4:Continue if in Guided on RC failsafe only, 8:Continue if landing on any failsafe, 16:Continue if in pilot controlled modes on GCS failsafe, 19:Continue if in Auto on RC and/or GCS failsafe and continue if in pilot controlled modes on GCS failsafe
+    // @Bitmask: 0:Continue if in Auto on RC failsafe, 1:Continue if in Auto on GCS failsafe, 2:Continue if in Guided on RC failsafe, 3:Continue if landing on any failsafe, 4:Continue if in pilot controlled modes on GCS failsafe
+    // @User: Advanced
+    AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, 0),
+
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    // @Group: AROT_
+    // @Path: ../libraries/AC_Autorotation/AC_Autorotation.cpp
+    AP_SUBGROUPINFO(arot, "AROT_", 37, ParametersG2, AC_Autorotation),
+#endif
+
+
+
     AP_GROUPEND
 };
 
@@ -1010,7 +1026,7 @@ ParametersG2::ParametersG2(void)
     , beacon(copter.serial_manager)
 #endif
 #if PROXIMITY_ENABLED == ENABLED
-    , proximity(copter.serial_manager)
+    , proximity()
 #endif
 #if ADVANCED_FAILSAFE == ENABLED
     ,afs(copter.mode_auto.mission)
@@ -1033,6 +1049,9 @@ ParametersG2::ParametersG2(void)
 #if MODE_SYSTEMID_ENABLED == ENABLED
     ,mode_systemid_ptr(&copter.mode_systemid)
 #endif
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    ,arot(copter.inertial_nav)
+#endif
 {
     AP_Param::setup_object_defaults(this, var_info);
 }
@@ -1103,6 +1122,9 @@ void Copter::load_parameters(void)
     // convert landing gear parameters
     convert_lgr_parameters();
 
+    // convert fs_options parameters
+    convert_fs_options_params();
+
     hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
 
     // setup AP_Param frame type flags
@@ -1110,7 +1132,7 @@ void Copter::load_parameters(void)
 
 }
 
-// handle conversion of PID gains from Copter-3.3 to Copter-3.4
+// handle conversion of PID gains
 void Copter::convert_pid_parameters(void)
 {
     // conversion info
@@ -1140,6 +1162,7 @@ void Copter::convert_pid_parameters(void)
         { Parameters::k_param_pid_rate_yaw,   7, AP_PARAM_FLOAT, "ATC_RAT_YAW_ILMI" },
 #endif
     };
+    // conversion from Copter-3.3 to Copter-3.4
     const AP_Param::ConversionInfo angle_and_filt_conversion_info[] = {
         { Parameters::k_param_p_stabilize_roll, 0, AP_PARAM_FLOAT, "ATC_ANG_RLL_P" },
         { Parameters::k_param_p_stabilize_pitch, 0, AP_PARAM_FLOAT, "ATC_ANG_PIT_P" },
@@ -1156,7 +1179,7 @@ void Copter::convert_pid_parameters(void)
         { Parameters::k_param_pid_accel_z, 1, AP_PARAM_FLOAT, "PSC_ACCZ_I" },
         { Parameters::k_param_pid_accel_z, 2, AP_PARAM_FLOAT, "PSC_ACCZ_D" },
         { Parameters::k_param_pid_accel_z, 5, AP_PARAM_FLOAT, "PSC_ACCZ_IMAX" },
-        { Parameters::k_param_pid_accel_z, 6, AP_PARAM_FLOAT, "PSC_ACCZ_FILT" },
+        { Parameters::k_param_pid_accel_z, 6, AP_PARAM_FLOAT, "PSC_ACCZ_FLTE" },
         { Parameters::k_param_p_alt_hold, 0, AP_PARAM_FLOAT, "PSC_POSZ_P" },
         { Parameters::k_param_p_pos_xy, 0, AP_PARAM_FLOAT, "PSC_POSXY_P" },
     };
@@ -1233,7 +1256,7 @@ void Copter::convert_pid_parameters(void)
     AP_Param::set_defaults_from_table(heli_defaults_table, ARRAY_SIZE(heli_defaults_table));
 #endif
 
-    // attitude control filter parameter changes (from _FILT to FLTD, FLTE, FLTT) for Copter-4.0
+    // attitude and position control filter parameter changes (from _FILT to FLTD, FLTE, FLTT) for Copter-4.0
     // magic numbers shown below are discovered by setting AP_PARAM_KEY_DUMP = 1
     const AP_Param::ConversionInfo ff_and_filt_conversion_info[] = {
 #if FRAME_CONFIG == HELI_FRAME
@@ -1252,6 +1275,7 @@ void Copter::convert_pid_parameters(void)
         { Parameters::k_param_attitude_control, 450, AP_PARAM_FLOAT, "ATC_RAT_PIT_FF" },
         { Parameters::k_param_attitude_control, 451, AP_PARAM_FLOAT, "ATC_RAT_YAW_FF" },
 #endif
+        { Parameters::k_param_pos_control, 388, AP_PARAM_FLOAT, "PSC_ACCZ_FLTE" },
     };
     uint8_t filt_table_size = ARRAY_SIZE(ff_and_filt_conversion_info);
     for (uint8_t i=0; i<filt_table_size; i++) {
@@ -1268,19 +1292,8 @@ void Copter::convert_pid_parameters(void)
         AP_Param::convert_old_parameters(&notchfilt_conversion_info[i], 1.0f);
     }
 
-    const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old,  Parameters::k_param_rc_2_old,
-                                      Parameters::k_param_rc_3_old,  Parameters::k_param_rc_4_old,
-                                      Parameters::k_param_rc_5_old,  Parameters::k_param_rc_6_old,
-                                      Parameters::k_param_rc_7_old,  Parameters::k_param_rc_8_old,
-                                      Parameters::k_param_rc_9_old,  Parameters::k_param_rc_10_old,
-                                      Parameters::k_param_rc_11_old, Parameters::k_param_rc_12_old,
-                                      Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old };
-    const uint16_t old_aux_chan_mask = 0x3FF0;
-    // note that we don't pass in rcmap as we don't want output channel functions changed based on rcmap
-    if (SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, nullptr)) {
-        // the rest needs to be done after motors allocation
-        upgrading_frame_params = true;
-    }
+    // make any SRV_Channel upgrades needed
+    SRV_Channels::upgrade_parameters();
 }
 
 /*
@@ -1524,5 +1537,49 @@ void Copter::convert_tradheli_parameters(void)
         AP_Param::set_and_save_by_name("H_TAIL_SPEED", tailspeed_pct );
     }
 
+    // table of stabilize collective parameters to be converted with scaling
+    const AP_Param::ConversionInfo collhelipct_conversion_info[] = {
+        { Parameters::k_param_input_manager, 1, AP_PARAM_INT16,  "IM_STB_COL_1" },
+        { Parameters::k_param_input_manager, 2, AP_PARAM_INT16,  "IM_STB_COL_2" },
+        { Parameters::k_param_input_manager, 3, AP_PARAM_INT16,  "IM_STB_COL_3" },
+        { Parameters::k_param_input_manager, 4, AP_PARAM_INT16,  "IM_STB_COL_4" },
+    };
+
+    // convert stabilize collective parameters with scaling
+    table_size = ARRAY_SIZE(collhelipct_conversion_info);
+    for (uint8_t i=0; i<table_size; i++) {
+        AP_Param::convert_old_parameter(&collhelipct_conversion_info[i], 0.1f);
+    }
+
 }
 #endif
+
+void Copter::convert_fs_options_params(void)
+{
+    // If FS_OPTIONS has already been configured and we don't change it.
+    enum ap_var_type ptype;
+    AP_Int32 *fs_opt = (AP_Int32 *)AP_Param::find("FS_OPTIONS", &ptype);
+
+    if (fs_opt == nullptr || fs_opt->configured_in_storage() || ptype != AP_PARAM_INT32) {
+        return;
+    }
+
+    // Variable to capture the new FS_OPTIONS setting
+    int32_t fs_options_converted = 0;
+
+    // If FS_THR_ENABLED is 2 (continue mission), change to RTL and add continue mission to the new FS_OPTIONS parameter
+    if (g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
+        fs_options_converted |= int32_t(FailsafeOption::RC_CONTINUE_IF_AUTO);
+        AP_Param::set_and_save_by_name("FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL);
+    }
+
+    // If FS_GCS_ENABLED is 2 (continue mission), change to RTL and add continue mission to the new FS_OPTIONS parameter
+    if (g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
+        fs_options_converted |= int32_t(FailsafeOption::GCS_CONTINUE_IF_AUTO);
+        AP_Param::set_and_save_by_name("FS_GCS_ENABLE", FS_GCS_ENABLED_ALWAYS_RTL);
+    }
+
+    // Write the new value to FS_OPTIONS
+    // AP_Param::set_and_save_by_name("FS_OPTIONS", fs_options_converted);
+    fs_opt->set_and_save(fs_options_converted);
+}

ArduCopter/Parameters.h

@@ -612,6 +612,14 @@ public:
 
     // vibration failsafe enable/disable
     AP_Int8 fs_vibe_enabled;
+
+    // Failsafe options bitmask #36
+    AP_Int32 fs_options;
+
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    // Autonmous autorotation
+    AC_Autorotation arot;
+#endif
 };
 
 extern const AP_Param::Info        var_info[];

ArduCopter/RC_Channel.cpp

@@ -349,9 +349,15 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
             break;
 
         case AUX_FUNC::MOTOR_INTERLOCK:
-            // Turn on when above LOW, because channel will also be used for speed
-            // control signal in tradheli
+#if FRAME_CONFIG == HELI_FRAME
+            // The interlock logic for ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH is handled 
+            // in heli_update_rotor_speed_targets.  Otherwise turn on when above low.
+            if (copter.motors->get_rsc_mode() != ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH) {
+                copter.ap.motor_interlock_switch = (ch_flag == HIGH || ch_flag == MIDDLE);
+            }
+#else
             copter.ap.motor_interlock_switch = (ch_flag == HIGH || ch_flag == MIDDLE);
+#endif
             break;
 
         case AUX_FUNC::BRAKE:
@@ -562,13 +568,13 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
         case AUX_FUNC::SURFACE_TRACKING:
             switch (ch_flag) {
             case LOW:
-                copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_GROUND);
+                copter.surface_tracking.set_surface(Copter::SurfaceTracking::Surface::GROUND);
                 break;
             case MIDDLE:
-                copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_DISABLED);
+                copter.surface_tracking.set_surface(Copter::SurfaceTracking::Surface::NONE);
                 break;
             case HIGH:
-                copter.surface_tracking.set_state(Copter::SurfaceTracking::SurfaceTrackingState::SURFACE_TRACKING_CEILING);
+                copter.surface_tracking.set_surface(Copter::SurfaceTracking::Surface::CEILING);
                 break;
             }
             break;

ArduCopter/RC_Channel.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <RC_Channel/RC_Channel.h>
+#include <AP_Motors/AP_Motors.h>
 #include "mode.h"
 
 class RC_Channel_Copter : public RC_Channel
@@ -31,7 +32,7 @@ public:
 
     RC_Channel_Copter obj_channels[NUM_RC_CHANNELS];
     RC_Channel_Copter *channel(const uint8_t chan) override {
-        if (chan > NUM_RC_CHANNELS) {
+        if (chan >= NUM_RC_CHANNELS) {
             return nullptr;
         }
         return &obj_channels[chan];

ArduCopter/ReleaseNotes.txt

@@ -1,5 +1,381 @@
 ArduPilot Copter Release Notes:
 ------------------------------------------------------------------
+Copter 4.0.8 12-Oct-2021 (TradHeli only release)
+Changes from 4.0.7
+1) TradHeli landing detector fix
+2) Proximity sensor distance validity checked before pushing to object database
+------------------------------------------------------------------
+Copter 4.0.7 22-Feb-2021
+Changes from 4.0.7-rc1
+1) fixed build on Durandal board
+2) multiple fixes for mRo boards: ControlZero*, PixracerPro
+------------------------------------------------------------------
+Copter 4.0.7rc1 6-Feb-2021
+Changes from 4.0.6
+1) added automatic backup/restore of parameters in case of FRAM corruption for F7/H7 boards with 32k FRAM parameter storage
+2) fixed a bug in EKF2/EKF3 that could cause memory corruption if external naviagtion sources (such as vision based position and velocity data) is supplied before the EKF has initialised
+3) fixed a problem with low accuracy data from UAVCAN GPS modules when GPS blending is enabled
+4) fixed an arming check failure with u-blox M9 based GPS modules
+5) fixed a race condition in SmartRTL which could cause a LAND mode to be triggered
+------------------------------------------------------------------
+Copter 4.0.6 25-Jan-2021 / 4.0.6-rc2 16-Jan-2021
+Changes from 4.0.6-rc1
+1) Add support for keeping a backup of storage for last 100 boots
+2) Bug fixes:
+    a) Fix support for BLHeli_S passthru
+------------------------------------------------------------------
+Copter 4.0.6-rc1 23-Dec-2020
+Changes from 4.0.5
+1) Bug fixes:
+    a) Fix vertical flyaways when rangefinder stops providing data and the user has configured EK*_ALT_SOURCE=1
+    b) Correct units on raw accel data
+    c) Fport RSSI value fix
+    d) Correct compilation when Advanced Failsafe compile time option is enabled
+    e) Correct time wrap in RAW_IMU mavlink message
+    f) PixracerPro - Fix analog volt pin assignments
+    g) fix landing detector for tradheli in acro mode
+2) Small enhancements:
+    a) Parameter documentation enhancements and corrections
+    b) Improve harmonic notch filter parameter documentation
+    c) Report prearm check status in the MAV_SYS_STATUS_PREARM_CHECK flag of the SYS_STATUS mavlink message
+    d) Smooth I term reset over 0.5 seconds
+3) TradHeli enhancements:
+    a) Differential Collective Pitch (DCP) trim support for Dual Heli
+    b) Incorporate hover collective learning
+    c) Option for pitch, roll and yaw I term to be based on takeoff/landing
+------------------------------------------------------------------
+Copter 4.0.5 27-Oct-2020 / 4.0.5-rc2 08-Oct-2020
+Changes from 4.0.5-rc1
+1) Bug fixes:
+    a) Serial/UART DMA race condition that could cause watdog reset fixed (Critical fix)
+    b) SBUS output when no RC input fixed (Critical fix)
+------------------------------------------------------------------
+Copter 4.0.5-rc1 02-Oct-2020
+Changes from 4.0.4
+1) Bug fixes:
+    a) Acro expo calculation fix (fixes sluggish behaviour)
+    b) F32Lightening board IMU fast sampling fix
+    c) GPS minimum accuracy added to protect EKF from unrealistic values
+    d) KakuteF7/mini DShot glitch fix
+    e) RC input gets additional protection against out-of-range inputs (<RCx_MIN or >RCx_MAX)
+    f) RC_OPTION = 4 fix on boards with IOMCU (Pixhawk, etc). This allows ignoring SBUS failsafes 
+2) Small enhancements:
+    a) Linux boards accept up to 16 RC inputs when using UDP
+    b) Protect against two many interrupts from RPM sensor, etc
+    c) RM3100 compass support for probing all four I2C addresses
+    d) Durandal telem3 port enabled
+------------------------------------------------------------------
+Copter 4.0.4 16-Sep-2020
+Changes from 4.0.4-rc4
+1) Matek H743, 765-Wing and F405-Wing get DPS310 Baro support 
+------------------------------------------------------------------
+Copter 4.0.4-rc4 28-Aug-2020
+Changes from 4.0.4-rc3
+1) Bug fixes:
+    a) Compass startup reordering based on compass priorities fix
+    b) TradHeli servo test fix
+    c) Precision landing logging fix
+    d) Mavlink commands ignored after reboot request
+------------------------------------------------------------------
+Copter 4.0.4-rc3 30-Jul-2020
+Changes from 4.0.4-rc2
+1) Bug Fixes and minor enhancements:
+    a) Compass ids from missing compasses reset after compass cal
+    b) LIS3MDL compass enabled on all boards
+    c) Lightware I2C lidar fix when out-of-range
+    d) Parameter erase fix for revo-mini and other boards that store to flash  
+------------------------------------------------------------------
+Copter 4.0.4-rc2 16-Jun-2020
+Changes from 4.0.4-rc1
+1) Bug Fixes:
+    a) Watchdog monitor memory increased (may have caused watchdog reset)
+    b) Compass ordering fix when COMPASS_PRIOx_ID is 0
+    c) Hex CubeOrange 2nd current sensor pin correction
+    d) Hott telemetry fix
+    e) Lightware I2C driver fix when out-of-range
+    f) MatekF765-Wing voltage and scaling fixed
+    g) MatekH743 baro on I2C2 bus
+    h) Proximity (360 lidar) ignore zone fix
+2) Flight controller support:
+    a) Bitcraze Crazyflie 2.1
+    b) CUAV Nora V1.2
+    c) Holybro Pix32v5
+    d) mRo Pixracer Pro
+3) Minor enhancements:
+    a) Benewake RangeFinder parameter descriptions clarified
+    b) Pre-arm check of attitude ignores DCM if multiple EKF cores present
+------------------------------------------------------------------
+Copter 4.0.4-rc1 26-May-2020
+Changes from 4.0.3
+1) Bug Fixes:
+    a) I/O CPU fix so safety remains off after inflight reboot (Critical fix)
+    b) Acro mode yaw expo supports values under 0.5 (see ACRO_Y_EXPO param)
+    c) Auto mode Loiter-Turn commands points towards center
+    d) Change-Speed commands applied smoothly
+    e) Compass scaling factor threshhold increased
+    f) EKF compass variance reporting to GCS made consistent with onboard logs
+    g) Gimbal control using RC input ignores RCx_TRIM param
+    h) Holybro Durandal buzzer fix
+    i) Parameter reset fix caused by Eeprom race condition
+    j) Read-only parameter write failure msg only sent once to GCS
+    k) Compass declination can be overriden with COMPASS_DEC param (and EKF stops using world magnetic tables)
+    l) Terrain database (SRTM) file fix (will cause all terrain to be reloaded after upgrade)
+2) Bootloader update to reduce chance of param resets during firmware load
+3) Compass ordering and prioritisation improvements
+4) Flight controller support:
+    a) CUAV-Nora
+    b) CUAV-X7
+    c) MatekSys H743
+    e) mRo Nexus
+    d) R9Pilot
+5) GPS moving baseline (aka Yaw from GPS) for UBlox F9 GPSs
+6) Graupner Hott telemetry support
+7) Landing detector filter improvement improves detection on hard surfaces
+8) Object Avoidances Fixes and improvements:
+    a) BendyRuler runs more slowly to reduce CPU load and reduce timeouts
+    b) Dijkstra's avoidance works with more fence points
+    c) Proximity drivers (i.e. 360deg lidar) simplified to reduce CPU load
+9) ProfiLED LEDs support
+10) Smart Battery improvements:
+    a) Cycle count added
+    b) NeoDesign battery support
+    c) SUI battery support 
+11) Other enhancements:
+    a) Betaflight X frame type support
+    b) Landing gear auto deploy/retract configurable using LGR_OPTIONS param
+    c) MOT_PWM_MIN/MAX pre-arm check (checks fail if only one has been set)
+    d) Solo gimbal and camera control improvements
+    e) USB IDs updated to new ArduPilot specific IDs
+------------------------------------------------------------------
+Copter 4.0.3 28-Feb-2020 / 4.0.3-rc1 20-Feb-2020
+Changes from 4.0.2
+1) Bug Fixes:
+    a) "RCInput: decoding" message watchdog reset when using MAVLink signing (Critical Fix)
+    b) HeliQuad yaw control fix
+    c) Do-Set-Servo commands can affect sprayer, gripper outputs
+    d) BLHeli passthrough fix for H7 boards (CubeOrange, Holybro Durandal)
+2) USB IDs updated for "composite" devices (fixes GCS<->autopilot connection issues for boards which present 2 USB ports)
+3) RCOut banner helps confirm correct setup for pwm, oneshot, dshot 
+4) ZigZag mode supports arming, takeoff and landing
+------------------------------------------------------------------
+Copter 4.0.2 11-Feb-2020
+Changes from 4.0.2-rc4
+1) MAVFTP stack size increased to 3k (fixes reboot when using MAVFTP)
+------------------------------------------------------------------
+Copter 4.0.2 11-Feb-2020 / 4.0.2-rc4 05-Feb-2020
+Changes from 4.0.2-rc3
+1) Bug Fixes:
+    a) Spektrum receivers decoding fix for Pixracer
+    b) Current Alt frame always relative to home (RTL could return at wrong alt)
+    c) Circle mode pitch control direction fixed
+    d) EKF only uses world magnetic tables if COMPASS_SCALE is set
+    e) Logging reliability improvements especially for FRAM logs
+    f) RangeFinders using PWM interface use RNGFNDx_OFFSET param (attempt2)
+    g) SpeedyBeeF5 probes all I2C ports for external baro
+2) Rangefinder fallback support (both must have same _ORIENT) 
+------------------------------------------------------------------
+Copter 4.0.2-rc3 01-Feb-2020
+Changes from 4.0.2-rc2
+1) Bug Fixes:
+    a) AutoTune fix to restore original gains when AutoTune completes
+------------------------------------------------------------------
+Copter 4.0.2-rc2 31-Jan-2020
+Changes from 4.0.1
+1) Bug Fixes:
+    b) IO CPU timing fix which reduces ESC sync issues
+    c) PX4Flow driver probes all I2C ports on Hex Cubes
+    d) RangeFinders using PWM interface (like Garmin LidarLite) use RNGFNDx_OFFSET param
+    e) RC override fix when RC_OVERRIDE_TIME=-1 (allows disabling timeout when using joystick) 
+    f) TradHeli attitude control parameter description fixes (does not affect flight)
+    g) cygwin compiler fix (affects developers only)
+2) Minor enhancements:
+    a) GCS failsafe warning lights and tones
+    b) Circle mode pitch control direction swapped
+------------------------------------------------------------------
+Copter 4.0.1 25-Jan-2020 / 4.0.1-rc3 19-Jan-2020
+Changes from 4.0.1-rc2
+1) Bug Fixes:
+    a) Semaphore fixes for Logging, Filesystem and UAVCAN Dyanmic Node Allocation
+    b) RangeFinder parameter fix (4th rangefinder was using 1st rangefinder's params)
+    c) TradHeli STB_COL_x parameter description fixed
+2) Minor Enhancements:
+    a) Autorotate flight mode renamed to Heli_Autorotate
+    b) Solo default parameters updated
+    c) "Prepared log system" initialisation message removed
+------------------------------------------------------------------
+Copter 4.0.1-rc2 10-Jan-2020
+Changes from 4.0.1-rc1
+1) FrSky telemetry status text handling fix (Critical Fix)
+------------------------------------------------------------------
+Copter 4.0.1-rc1 10-Jan-2020
+Changes from 4.0.0
+1) Circle mode allows pilot control of radius and rotation speed
+2) CAN servo feedback logged
+3) Magnetic declination tables updated
+4) Serial0 protocol forced to MAVLink (avoids accidental disabling of USB port)
+5) Bug Fixes:
+   a) TradHeli RSC RC passthrough fixed
+   b) CubeOrange and Durandal I2C timing fixed (was running slow)
+   c) Compass calibration auto orientation skips "pitch 7" which could cause cal to fail
+   d) Durandal's fourth I2C port fixed
+   e) Linux boards with CAN support fixed
+   f) Neopixel added to SERVOx_FUNCTION param description
+   g) NMEA Output fixed (was sending an extra CR)
+   h) Optflow messages sent even if EKF has no height estimate
+   i) SkyViper build fixed
+   j) Spektrum/DSM 22ms RC input fixed
+   k) "UC Node Down" message removed (was unnecessarily scary)
+------------------------------------------------------------------
+Copter 4.0.0 29-Dec-2019 / 4.0.0-rc6 28-Dec-2019
+Changes from 4.0.0-rc5
+1) Compiler updated to gcc 6.3.1 (2nd attempt)
+------------------------------------------------------------------
+Copter 4.0.0-rc5 23-Dec-2019
+Changes from 4.0.0-rc4
+1) RM3100 compass enabled on all boards
+2) GCS failsafe disabled by default (see FS_GCS_ENABLE parameter)
+3) Bug Fixes
+   a) Bootloader fix for H7 boards (could brick CubeOrange, CUAV V5 Nano, etc)
+   b) OmnibusF4pro GPS fix
+   c) MatekF405-Wing missing serial ports restored
+   d) MatekF765-Wing RTSCTS parameter defaults set correctly
+   e) CUAV V5 Nano battery monitor param defaults improved
+------------------------------------------------------------------
+Copter 4.0.0-rc4 20-Dec-2019
+Changes from 4.0.0-rc3
+1) Compiler updated to gcc 6.3.1
+2) Solo default parameters updated
+3) Bug Fix to RCMAP channel number sanity check
+------------------------------------------------------------------
+Copter 4.0.0-rc3 16-Dec-2019
+Changes from 4.0.0-rc2 
+1) Flight mode and control improvements:
+   a) Auto mode Takeoff getting stuck fixed (very rare)
+   b) AutoTune protection against ESC sync loss at beginning of a twitch
+   c) SystemID mode parameters hidden by default (set SID_AXIS to non-zero to see all)
+   d) Takeoff from slanted surfaces improved by reducing I-term build-up in attitude controllers 
+2) Lua Script related enhancements:
+   a) MAV FTP support to ease uploading and downloading Lua scripts
+   b) NeoPixel/WS2812 LED control from Lua scripts
+   c) Pre-arm check that Lua scripting feature has enough memory
+3) TradHeli enhancements:
+   a) Autonomous autorotation (compile time option, not available in stable firmware)
+   b) CCW Direct Drive Fixed Pitch tail support (see H_TAIL_TYPE parameter)
+   c) Parameter description improvements
+   d) STAB_COL_1/2/3 param range changed to 0 to 100 (was 0 to 1000)
+4) Lightware SF40c driver for latest sensors with "streaming" protocol
+5) Board/Frame specfic fixes:
+   a) Hex CubeOrange IMU heater control gain tuning improvement
+   b) Holybro Durandal IMUs ordering fix so EKx_IMU_MASK bits are intuitive
+   c) Holybro Pixhawk4 B/E LED fix (was flickering)
+   d) MatekF765 PWM outputs 5 and 6 now functional
+   e) MatekF765, MatekF405 time wrap CRITICAL fix for vehicles flying more than 72min
+   f) MatekF765 LED fixes
+   g) mRobotics ControlZeroF7 I2C bus numbering fix
+   h) Solo default params updated for 4.0.0
+   i) Bootloaders for boards using STM32H7 CPUs
+   j) Bootloader protection against line noise that could cause the board to get stuck during bootup
+   k) LED timing fix for FMUv5 boards with LEDs on one of first four auxiliary PWM outputs
+6) Minor Enhancements and Bug Fixes
+   a) I2C storm High level protection
+   b) Internal errors (like I2C storms) reported to GCS by setting Heartbeat status to Critical
+   c) Dataflash Logging CTUN.TAlt and SAlt scaling fixed
+   d) DO_DIGICAM_CONTROL messages are sent using Mavlink1/2 as specified by SERIALx_PROTOCOL
+   e) DO_SET_SPEED sanity check fixed to protect against negative speeds
+   f) FrSky telemetry scheduling improvement (fixes issue in which GPS data would not be sent)
+   g) GPS auto configuration fix for non-Ublox-F9 (F9 config was interfering with other Ublox GPSs)
+   h) Landing gear DEPLOY message fix (could be displayed multiple times or when gear not configured)
+   i) Pre-arm check of position estimate when arming in Loiter even if arming is "forced"
+   j) Pre-arm check that Terrain database has enough memory
+   k) RangeFinder parameter conversion fix (some parameters were not upgraded from 3.6.x to 4.0.0)
+   l) RangeFinder TYPE parameter descriptions clarified for LidarLite and Benewake Lidars
+   m) Serial parameters hidden if they do not exist on the particular board
+   n) UAVCAN GPS fix for GPSs that don't provide "Aux" message (like the Here2)
+   o) NMEA Output bug fix (output was stopping after 90 seconds)
+------------------------------------------------------------------
+Copter 4.0.0-rc2 04-Nov-2019
+Changes from 4.0.0-rc1
+1) Failsafe changes:
+   a) GCS failsafe triggers when telemetry connection is lost (previously only triggered when using joystick)
+   b) FS_OPTION parameter allows continue-in-auto and continue-in-pilot-modes for RC/Radio and GCS failsafe
+2) Dynamic Notch Filter supports filter range based on BLHeli ESC feedback
+3) Improved protection against high roll/pitch gains affecting altitude control
+4) Bug Fixes:
+   a) Altitude control param conversion fix (PSC_ACCZ_FILT converted to PSC_ACCZ_FLTE)
+   b) BLHeli fix to RPM calcs and telemetry (aka 0x8000 error)
+   c) ChibiOS SPI timeout fix (non-critical)
+   d) Fence upload is less strict about altitude types (fences don't have altitudes)
+   e) Motor loss detection bug fix (would sometimes think the wrong motor had failed)
+   f) Pre-arm message fix to reports AHRS/EKF issue (was blank)
+   g) Sparky2 autopilot firmware available
+   h) Startup message "AK09916: Unable to get bus semaphore" removed (was not an error)
+   i) Yaw control fix for fast descent and after large attitude disturbances
+------------------------------------------------------------------
+Copter 4.0.0-rc1 25-Oct-2019
+Changes from 3.6.11
+1) Path Planning for Object Avoidance (aka Bendy Ruler and Dijkstra's) replaces "Dodge" avoidance
+2) Complex Fence support (aka stay-out zones)
+3) Lua scripting support on the flight controller
+4) New flight controllers:
+    a) Hex Cube Orange
+    b) Holybro Durandal
+5) Attitude Control changes:
+    a) Attitude Control filters on target, Error and D-term (see ATC_RAT_x_FLTT/FLTE/FLTD)
+    b) Harmonic notch filter
+6) Flight mode changes:
+    a) ZigZag mode (designed for crop spraying) 
+    b) SystemId for "chirping" attitude controllers to determine vehicle response
+    c) StandBy mode for vehicles with multiple flight controllers
+    d) SmartRTL provides warnings when buffer is nearly full
+    e) Follow mode offsets reset to zero when vehicle leaves follow mode
+    f) Upward facing surface tracking using lidar
+    g) Circle mode points more accurately towards center
+7) Traditional Heli:
+    a) Removed the parameter H_LAND_COL_MIN and functionality now uses H_COL_MID. CAUTION: ensure H_COL_MID is set to collective blade pitch that produces zero thrust
+    b) Incorporated a rotor speed governor in rotor speed control (RSC)
+    c) Moved all RSC parameters to the RSC library
+    d) Converted throttle curve parameters to percent
+    e) Converted RSC_CRITICAL, RSC_IDLE, and RSC_SETPOINT to percent
+    f) Created swashplate library that has presaved swashplate types for use with Heli_Single and Heli_Dual frames
+    g) Motor interlock with passthrough settable through RC option feature 
+    h) Removed collective too high pre-arm check
+    i) Added virtual flybar for Acro flight mode
+    j) Fixed H_SV_MAN minimum and maximum settings for Heli_Dual
+8) Frames:
+    a) BiCopter support
+    b) OctoV mixing improvements 
+9) RC input/output changes:
+    a) Serial protocols supported on any serial port
+    b) IBUS R/C input support
+    c) DO_SET_SERVO and manual passthrough can operate on the same channel
+10) Battery improvements:
+    a) Up to 10 batteries can be monitored
+    b) "Sum" type consolidates monitoring across batteries
+    c) Fuel flow battery (for use with gas tanks)
+11) Sensor/Accessory changes:
+    a) Robotis servo support
+    b) KDECAN ESCs
+    c) ToshibaCAN ESCs
+    d) BenewakeTF03 lidar
+    e) SD Card reliability improvements (if card removed, logging restarts)
+    f) Yaw from some GPS (including uBlox RTK GPS with moving baseline)
+    g) WS2812 LEDs (aka NeoPixel LEDs)
+    h) NTF_BUZZ_VOLUME allows controlling buzzer volume
+12) Landing Gear:
+    a) Retracts automatically after Takeoff in Auto completes
+    b) Deployed automatically using SRTM database or Lidar
+13) UAVCAN improvements:
+    a) dynamic node allocation
+    b) SLCAN pass-through
+    c) support for UAVCAN rangefinders, buzzers, safety switch, safety LED
+14) Serial and Telemetry:
+    a) MAVLink Message-Interval allows reducing telemetry bandwidth requirements
+    b) SERIALn_OPTIONS for inversion, half-duplex and swap 
+15) Safety Improvements:
+    a) Vibration failsafe (switches to vibration resistant estimation and control)
+    b) Independent WatchDog gets improved logging
+    c) EKF failsafe triggers slightly more quickly  
+------------------------------------------------------------------
 Copter 3.6.11 01-Oct-2019 / 3.6.11-rc1 16-Sep-2019
 Changes from 3.6.10
 1) EKF and IMU improvements:

ArduCopter/compassmot.cpp

@@ -221,10 +221,10 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
             mavlink_msg_compassmot_status_send(gcs_chan.get_chan(),
                                                channel_throttle->get_control_in(),
                                                current,
-                                               interference_pct[compass.get_primary()],
-                                               motor_compensation[compass.get_primary()].x,
-                                               motor_compensation[compass.get_primary()].y,
-                                               motor_compensation[compass.get_primary()].z);
+                                               interference_pct[0],
+                                               motor_compensation[0].x,
+                                               motor_compensation[0].y,
+                                               motor_compensation[0].z);
         }
     }
 

ArduCopter/config.h

@@ -372,6 +372,22 @@
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////////////////////////
+// Autorotate - autonomous auto-rotation - helicopters only
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    #if FRAME_CONFIG == HELI_FRAME
+        #ifndef MODE_AUTOROTATE_ENABLED
+        # define MODE_AUTOROTATE_ENABLED !HAL_MINIMIZE_FEATURES
+        #endif
+    #else
+        # define MODE_AUTOROTATE_ENABLED DISABLED
+    #endif
+#else
+    # define MODE_AUTOROTATE_ENABLED DISABLED
+#endif
+//////////////////////////////////////////////////////////////////////////////
+
 // Beacon support - support for local positioning systems
 #ifndef BEACON_ENABLED
 # define BEACON_ENABLED !HAL_MINIMIZE_FEATURES

ArduCopter/crash_check.cpp

@@ -35,6 +35,14 @@ void Copter::crash_check()
         return;
     }
 
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    //return immediately if in autorotation mode
+    if (control_mode == Mode::Number::AUTOROTATE) {
+        crash_counter = 0;
+        return;
+    }
+#endif
+
     // vehicle not crashed if 1hz filtered acceleration is more than 3m/s (1G on Z-axis has been subtracted)
     if (land_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
         crash_counter = 0;

ArduCopter/defines.h

@@ -172,6 +172,14 @@ enum LoggingParameters {
      LOG_SYSIDS_MSG,
 };
 
+// Harmonic notch update mode
+enum HarmonicNotchDynamicMode {
+    HarmonicNotch_Fixed,
+    HarmonicNotch_UpdateThrottle,
+    HarmonicNotch_UpdateRPM,
+    HarmonicNotch_UpdateBLHeli,
+};
+
 #define MASK_LOG_ATTITUDE_FAST          (1<<0)
 #define MASK_LOG_ATTITUDE_MED           (1<<1)
 #define MASK_LOG_GPS                    (1<<2)
@@ -196,7 +204,7 @@ enum LoggingParameters {
 // Radio failsafe definitions (FS_THR parameter)
 #define FS_THR_DISABLED                            0
 #define FS_THR_ENABLED_ALWAYS_RTL                  1
-#define FS_THR_ENABLED_CONTINUE_MISSION            2
+#define FS_THR_ENABLED_CONTINUE_MISSION            2    // Deprecated in 4.0+, now use fs_options
 #define FS_THR_ENABLED_ALWAYS_LAND                 3
 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL      4
 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND     5
@@ -204,9 +212,10 @@ enum LoggingParameters {
 // GCS failsafe definitions (FS_GCS_ENABLE parameter)
 #define FS_GCS_DISABLED                        0
 #define FS_GCS_ENABLED_ALWAYS_RTL              1
-#define FS_GCS_ENABLED_CONTINUE_MISSION        2
+#define FS_GCS_ENABLED_CONTINUE_MISSION        2    // Deprecated in 4.0+, now use fs_options
 #define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL  3
 #define FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND 4
+#define FS_GCS_ENABLED_ALWAYS_LAND             5
 
 // EKF failsafe definitions (FS_EKF_ACTION parameter)
 #define FS_EKF_ACTION_LAND                  1       // switch to LAND mode on EKF failsafe

ArduCopter/ekf_check.cpp

@@ -105,9 +105,11 @@ bool Copter::ekf_over_threshold()
     Vector2f offset;
     ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset);
 
+    const float mag_max = fmaxf(fmaxf(mag_variance.x,mag_variance.y),mag_variance.z);
+
     // return true if two of compass, velocity and position variances are over the threshold OR velocity variance is twice the threshold
     uint8_t over_thresh_count = 0;
-    if (mag_variance.length() >= g.fs_ekf_thresh) {
+    if (mag_max >= g.fs_ekf_thresh) {
         over_thresh_count++;
     }
     if (!optflow.healthy() && (vel_variance >= (2.0f * g.fs_ekf_thresh))) {

ArduCopter/events.cpp

@@ -4,104 +4,115 @@
  *       This event will be called when the failsafe changes
  *       boolean failsafe reflects the current state
  */
+
+bool Copter::failsafe_option(FailsafeOption opt) const
+{
+    return (g2.fs_options & (uint32_t)opt);
+}
+
 void Copter::failsafe_radio_on_event()
 {
-    // if motors are not armed there is nothing to do
-    if( !motors->armed() ) {
-        return;
-    }
-
-    if (should_disarm_on_failsafe()) {
-        arming.disarm();
-    } else {
-        if (control_mode == Mode::Number::AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
-            // continue mission
-        } else if (flightmode->is_landing() &&
-                   battery.has_failsafed() &&
-                   battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY) {
-            // continue landing or other high priority failsafes
-        } else {
-            if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
-                set_mode_RTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
-            } else if (g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
-                set_mode_RTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
-            } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
-                set_mode_SmartRTL_or_RTL(ModeReason::RADIO_FAILSAFE);
-            } else if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
-                set_mode_SmartRTL_or_land_with_pause(ModeReason::RADIO_FAILSAFE);
-            } else { // g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND
-                set_mode_land_with_pause(ModeReason::RADIO_FAILSAFE);
-            }
-        }
-    }
-
     AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED);
 
+    // set desired action based on FS_THR_ENABLE parameter
+    Failsafe_Action desired_action;
+    switch (g.failsafe_throttle) {
+        case FS_THR_DISABLED:
+            desired_action = Failsafe_Action_None;
+            break;
+        case FS_THR_ENABLED_ALWAYS_RTL:
+        case FS_THR_ENABLED_CONTINUE_MISSION:
+            desired_action = Failsafe_Action_RTL;
+            break;
+        case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
+            desired_action = Failsafe_Action_SmartRTL;
+            break;
+        case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
+            desired_action = Failsafe_Action_SmartRTL_Land;
+            break;
+        case FS_THR_ENABLED_ALWAYS_LAND:
+            desired_action = Failsafe_Action_Land;
+            break;
+        default:
+            desired_action = Failsafe_Action_Land;
+    }
+
+    // Conditions to deviate from FS_THR_ENABLE selection and send specific GCS warning
+    if (should_disarm_on_failsafe()) {
+        // should immediately disarm when we're on the ground
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Disarming");
+        arming.disarm();
+        desired_action = Failsafe_Action_None;
+
+    } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
+        // Allow landing to continue when battery failsafe requires it (not a user option)
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio + Battery Failsafe - Continuing Landing");
+        desired_action = Failsafe_Action_Land;
+
+    } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) {
+        // Allow landing to continue when FS_OPTIONS is set to continue landing
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Landing");
+        desired_action = Failsafe_Action_Land;
+
+    } else if (control_mode == Mode::Number::AUTO && failsafe_option(FailsafeOption::RC_CONTINUE_IF_AUTO)) {
+        // Allow mission to continue when FS_OPTIONS is set to continue mission
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Auto Mode");       
+        desired_action = Failsafe_Action_None;
+
+    } else if ((flightmode->in_guided_mode()) &&
+      (failsafe_option(FailsafeOption::RC_CONTINUE_IF_GUIDED)) && (g.failsafe_gcs != FS_GCS_DISABLED)) {
+        // Allow guided mode to continue when FS_OPTIONS is set to continue in guided mode.  Only if the GCS failsafe is enabled.
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Guided Mode");
+        desired_action = Failsafe_Action_None;
+
+    } else {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe");
+    }
+
+    // Call the failsafe action handler
+    do_failsafe_action(desired_action, ModeReason::RADIO_FAILSAFE);
 }
 
 // failsafe_off_event - respond to radio contact being regained
-// we must be in AUTO, LAND or RTL modes
-// or Stabilize or ACRO mode but with motors disarmed
 void Copter::failsafe_radio_off_event()
 {
     // no need to do anything except log the error as resolved
     // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
     AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED);
+    gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared");
 }
 
 void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
 {
     AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED);
 
-    // failsafe check
+    Failsafe_Action desired_action = (Failsafe_Action)action;
+
+    // Conditions to deviate from BATT_FS_XXX_ACT parameter setting
     if (should_disarm_on_failsafe()) {
+        // should immediately disarm when we're on the ground
         arming.disarm();
+        desired_action = Failsafe_Action_None;
+        gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Disarming");
+
+    } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING) && desired_action != Failsafe_Action_None) {
+        // Allow landing to continue when FS_OPTIONS is set to continue when landing
+        desired_action = Failsafe_Action_Land;
+        gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Continuing Landing");
     } else {
-        switch ((Failsafe_Action)action) {
-            case Failsafe_Action_None:
-                return;
-            case Failsafe_Action_Land:
-                set_mode_land_with_pause(ModeReason::BATTERY_FAILSAFE);
-                break;
-            case Failsafe_Action_RTL:
-                set_mode_RTL_or_land_with_pause(ModeReason::BATTERY_FAILSAFE);
-                break;
-            case Failsafe_Action_SmartRTL:
-                set_mode_SmartRTL_or_RTL(ModeReason::BATTERY_FAILSAFE);
-                break;
-            case Failsafe_Action_SmartRTL_Land:
-                set_mode_SmartRTL_or_land_with_pause(ModeReason::BATTERY_FAILSAFE);
-                break;
-            case Failsafe_Action_Terminate:
-#if ADVANCED_FAILSAFE == ENABLED
-                char battery_type_str[17];
-                snprintf(battery_type_str, 17, "%s battery", type_str);
-                g2.afs.gcs_terminate(true, battery_type_str);
-#else
-                arming.disarm();
-#endif
-        }
+        gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe");
     }
+
+    // Battery FS options already use the Failsafe_Options enum. So use them directly.
+    do_failsafe_action(desired_action, ModeReason::BATTERY_FAILSAFE);
+
 }
 
 // failsafe_gcs_check - check for ground station failsafe
 void Copter::failsafe_gcs_check()
 {
-    if (failsafe.gcs) {
-        // we must run the failsafe checks if we are in failsafe -
-        // otherwise we will never leave failsafe
-    } else if (g.failsafe_gcs == FS_GCS_DISABLED) {
-        // simply disabled
-        return;
-    } else if (failsafe.last_heartbeat_ms == 0) {
-        // GCS has never connected
-        return;
-    } else if (RC_Channels::has_active_overrides()) {
-        // GCS is currently telling us what to do!
-    } else if (control_mode == Mode::Number::GUIDED ||
-               control_mode == Mode::Number::GUIDED_NOGPS) {
-        // GCS is currently telling us what to do!
-    } else {
+    // Bypass GCS failsafe checks if disabled or GCS never connected
+    if (g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0) {
         return;
     }
 
@@ -109,47 +120,96 @@ void Copter::failsafe_gcs_check()
     // note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
     const uint32_t last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
 
-    // check if all is well
-    if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS) {
-        // check for recovery from gcs failsafe
-        if (failsafe.gcs) {
-            failsafe_gcs_off_event();
-            set_failsafe_gcs(false);
-        }
-        return;
-    }
+    // Determine which event to trigger
+    if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && failsafe.gcs) {
+        // Recovery from a GCS failsafe
+        set_failsafe_gcs(false);
+        failsafe_gcs_off_event();
 
-    // do nothing if gcs failsafe already triggered or motors disarmed
-    if (failsafe.gcs || !motors->armed()) {
-        return;
-    }
+    } else if (last_gcs_update_ms < FS_GCS_TIMEOUT_MS && !failsafe.gcs) {
+        // No problem, do nothing
 
-    // GCS failsafe event has occurred
-    set_failsafe_gcs(true);
+    } else if (last_gcs_update_ms > FS_GCS_TIMEOUT_MS && failsafe.gcs) {
+        // Already in failsafe, do nothing
+
+    } else if (last_gcs_update_ms > FS_GCS_TIMEOUT_MS && !failsafe.gcs) {
+        // New GCS failsafe event, trigger events
+        set_failsafe_gcs(true);
+        failsafe_gcs_on_event();
+    }
+}
+
+// failsafe_gcs_on_event - actions to take when GCS contact is lost
+void Copter::failsafe_gcs_on_event(void)
+{
     AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED);
-
-    // clear overrides so that RC control can be regained with radio.
     RC_Channels::clear_overrides();
 
-    if (should_disarm_on_failsafe()) {
-        arming.disarm();
-    } else {
-        if (control_mode == Mode::Number::AUTO &&
-            g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
-            // continue mission
-        } else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL) {
-            set_mode_SmartRTL_or_RTL(ModeReason::GCS_FAILSAFE);
-        } else if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND) {
-            set_mode_SmartRTL_or_land_with_pause(ModeReason::GCS_FAILSAFE);
-        } else { // g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL
-            set_mode_RTL_or_land_with_pause(ModeReason::GCS_FAILSAFE);
-        }
+    // convert the desired failsafe response to the Failsafe_Action enum
+    Failsafe_Action desired_action;
+    switch (g.failsafe_gcs) {
+        case FS_GCS_DISABLED:
+            desired_action = Failsafe_Action_None;
+            break;
+        case FS_GCS_ENABLED_ALWAYS_RTL:
+        case FS_GCS_ENABLED_CONTINUE_MISSION:
+            desired_action = Failsafe_Action_RTL;
+            break;
+        case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
+            desired_action = Failsafe_Action_SmartRTL;
+            break;
+        case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
+            desired_action = Failsafe_Action_SmartRTL_Land;
+            break;
+        case FS_GCS_ENABLED_ALWAYS_LAND:
+            desired_action = Failsafe_Action_Land;
+            break;
+        default: // if an invalid parameter value is set, the fallback is RTL
+            desired_action = Failsafe_Action_RTL;
     }
+
+    // Conditions to deviate from FS_GCS_ENABLE parameter setting
+    if (!motors->armed()) {
+        desired_action = Failsafe_Action_None;
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe");
+
+    } else if (should_disarm_on_failsafe()) {
+        // should immediately disarm when we're on the ground
+        arming.disarm();
+        desired_action = Failsafe_Action_None;
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Disarming");
+
+    } else if (flightmode->is_landing() && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
+        // Allow landing to continue when battery failsafe requires it (not a user option)
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS + Battery Failsafe - Continuing Landing");
+        desired_action = Failsafe_Action_Land;
+
+    } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING)) {
+        // Allow landing to continue when FS_OPTIONS is set to continue landing
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Landing");
+        desired_action = Failsafe_Action_Land;
+
+    } else if (control_mode == Mode::Number::AUTO && failsafe_option(FailsafeOption::GCS_CONTINUE_IF_AUTO)) {
+        // Allow mission to continue when FS_OPTIONS is set to continue mission
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Auto Mode");
+        desired_action = Failsafe_Action_None;
+
+    } else if (failsafe_option(FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL) && !flightmode->is_autopilot()) {
+        // should continue when in a pilot controlled mode because FS_OPTIONS is set to continue in pilot controlled modes
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Pilot Control");
+        desired_action = Failsafe_Action_None;
+    } else {
+        gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe");
+    }
+
+    // Call the failsafe action handler
+    do_failsafe_action(desired_action, ModeReason::GCS_FAILSAFE);
 }
 
 // failsafe_gcs_off_event - actions to take when GCS contact is restored
 void Copter::failsafe_gcs_off_event(void)
 {
+    gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Cleared");
     AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);
 }
 
@@ -294,3 +354,34 @@ bool Copter::should_disarm_on_failsafe() {
             return ap.land_complete;
     }
 }
+
+
+void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){
+
+    // Execute the specified desired_action
+    switch (action) {
+        case Failsafe_Action_None:
+            return;
+        case Failsafe_Action_Land:
+            set_mode_land_with_pause(reason);
+            break;
+        case Failsafe_Action_RTL:
+            set_mode_RTL_or_land_with_pause(reason);
+            break;
+        case Failsafe_Action_SmartRTL:
+            set_mode_SmartRTL_or_RTL(reason);
+            break;
+        case Failsafe_Action_SmartRTL_Land:
+            set_mode_SmartRTL_or_land_with_pause(reason);
+            break;
+        case Failsafe_Action_Terminate: {
+#if ADVANCED_FAILSAFE == ENABLED
+            g2.afs.gcs_terminate(true, "Failsafe");
+#else
+            arming.disarm();
+#endif
+        }
+        break;
+    }
+}
+

ArduCopter/heli.cpp

@@ -5,7 +5,7 @@
 #if FRAME_CONFIG == HELI_FRAME
 
 #ifndef HELI_DYNAMIC_FLIGHT_SPEED_MIN
- #define HELI_DYNAMIC_FLIGHT_SPEED_MIN      500     // we are in "dynamic flight" when the speed is over 5m/s for 2 seconds
+ #define HELI_DYNAMIC_FLIGHT_SPEED_MIN      250     // we are in "dynamic flight" when the speed is over 2.5m/s for 2 seconds
 #endif
 
 // counter to control dynamic flight profile
@@ -73,8 +73,20 @@ void Copter::check_dynamic_flight(void)
 // should be run between the rate controller and the servo updates.
 void Copter::update_heli_control_dynamics(void)
 {
-    // Use Leaky_I if we are not moving fast
-    attitude_control->use_leaky_i(!heli_flags.dynamic_flight);
+
+    if (!motors->using_leaky_integrator()) {
+        //turn off leaky_I
+        attitude_control->use_leaky_i(false);
+        if (ap.land_complete || ap.land_complete_maybe) {
+            motors->set_land_complete(true);
+        } else {
+            motors->set_land_complete(false);
+        }
+    } else {
+        // Use Leaky_I if we are not moving fast
+        attitude_control->use_leaky_i(!heli_flags.dynamic_flight);
+        motors->set_land_complete(false);
+    }
 
     if (ap.land_complete || (is_zero(motors->get_desired_rotor_speed()))){
         // if we are landed or there is no rotor power demanded, decrement slew scalar
@@ -131,6 +143,17 @@ void Copter::heli_update_landing_swash()
     }
 }
 
+// convert motor interlock switch's position to desired rotor speed expressed as a value from 0 to 1
+// returns zero if motor interlock auxiliary switch hasn't been defined
+float Copter::get_pilot_desired_rotor_speed() const
+{
+    RC_Channel *rc_ptr = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK);
+    if (rc_ptr != nullptr) {
+        return (float)rc_ptr->get_control_in() * 0.001f;
+    }
+    return 0.0f;
+}
+
 // heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
 void Copter::heli_update_rotor_speed_targets()
 {
@@ -139,17 +162,15 @@ void Copter::heli_update_rotor_speed_targets()
 
     // get rotor control method
     uint8_t rsc_control_mode = motors->get_rsc_mode();
-    float rsc_control_deglitched = 0.0f;
-    RC_Channel *rc_ptr = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK);
-    if (rc_ptr != nullptr) {
-        rsc_control_deglitched = rotor_speed_deglitch_filter.apply((float)rc_ptr->get_control_in()) * 0.001f;
-    }
+
     switch (rsc_control_mode) {
         case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH:
             // pass through pilot desired rotor speed from the RC
-            if (motors->get_interlock()) {
-                motors->set_desired_rotor_speed(rsc_control_deglitched);
+            if (get_pilot_desired_rotor_speed() > 0.01) {
+                ap.motor_interlock_switch = true;
+                motors->set_desired_rotor_speed(get_pilot_desired_rotor_speed());
             } else {
+                ap.motor_interlock_switch = false;
                 motors->set_desired_rotor_speed(0.0f);
             }
             break;
@@ -180,4 +201,38 @@ void Copter::heli_update_rotor_speed_targets()
     rotor_runup_complete_last = motors->rotor_runup_complete();
 }
 
+
+// heli_update_autorotation - determines if aircraft is in autorotation and sets motors flag and switches
+// to autorotation flight mode if manual collective is not being used.
+void Copter::heli_update_autorotation()
+{
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+    //set autonomous autorotation flight mode
+    if (!ap.land_complete && !motors->get_interlock() && !flightmode->has_manual_throttle() && g2.arot.is_enable()) {
+        heli_flags.in_autorotation = true;
+        set_mode(Mode::Number::AUTOROTATE, ModeReason::AUTOROTATION_START);
+    } else {
+        heli_flags.in_autorotation = false;
+    }
+
+    // sets autorotation flags through out libraries
+    heli_set_autorotation(heli_flags.in_autorotation);
+    if (!ap.land_complete && g2.arot.is_enable()) {
+        motors->set_enable_bailout(true);
+    } else {
+        motors->set_enable_bailout(false);
+    }
+#else
+    heli_flags.in_autorotation = false;
+    motors->set_enable_bailout(false);
+#endif
+}
+
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+// heli_set_autorotation - set the autorotation f`lag throughout libraries
+void Copter::heli_set_autorotation(bool autorotation)
+{
+    motors->set_in_autorotation(autorotation);
+}
+#endif
 #endif  // FRAME_CONFIG == HELI_FRAME

ArduCopter/inertia.cpp

@@ -17,12 +17,11 @@ void Copter::read_inertia()
         return;
     }
 
-    if (ahrs.home_is_set()) {
-        current_loc.set_alt_cm(inertial_nav.get_altitude(),
-                               Location::AltFrame::ABOVE_HOME);
-    } else {
-        // without home use alt above the EKF origin
-        current_loc.set_alt_cm(inertial_nav.get_altitude(),
-                               Location::AltFrame::ABOVE_ORIGIN);
+    // current_loc.alt is alt-above-home, converted from inertial nav's alt-above-ekf-origin
+    const int32_t alt_above_origin_cm = inertial_nav.get_altitude();
+    current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_ORIGIN);
+    if (!ahrs.home_is_set() || !current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
+        // if home has not been set yet we treat alt-above-origin as alt-above-home
+        current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_HOME);
     }
 }

ArduCopter/land_detector.cpp

@@ -47,7 +47,7 @@ void Copter::update_land_detector()
     } else if (ap.land_complete) {
 #if FRAME_CONFIG == HELI_FRAME
         // if rotor speed and collective pitch are high then clear landing flag
-        if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
+        if (motors->get_takeoff_collective() && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
 #else
         // if throttle output is high then clear landing flag
         if (motors->get_throttle() > get_non_takeoff_throttle()) {
@@ -60,8 +60,11 @@ void Copter::update_land_detector()
     } else {
 
 #if FRAME_CONFIG == HELI_FRAME
-        // check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN)
-        bool motor_at_lower_limit = motors->limit.throttle_lower;
+        // check for both manual collective modes and modes that use altitude hold. For manual collective (called throttle 
+        // because multi's use throttle), check that collective pitch is below mid collective (zero thrust) position.  For modes 
+        // that use altitude hold, check that the pilot is commanding a descent and collective is at min allowed for altitude hold modes.
+        bool motor_at_lower_limit = ((flightmode->has_manual_throttle() && motors->get_below_mid_collective() && fabsf(ahrs.get_roll()) < M_PI/2.0f) 
+                                    || (motors->limit.throttle_lower && pos_control->get_desired_velocity().z < 0.0f));
 #else
         // check that the average throttle output is near minimum (less than 12.5% hover throttle)
         bool motor_at_lower_limit = motors->limit.throttle_lower && attitude_control->is_throttle_mix_min();
@@ -137,14 +140,14 @@ void Copter::set_land_complete_maybe(bool b)
     ap.land_complete_maybe = b;
 }
 
-// update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state
+// sets motors throttle_low_comp value depending upon vehicle state
 //  low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
 //  has no effect when throttle is above hover throttle
-void Copter::update_throttle_thr_mix()
+void Copter::update_throttle_mix()
 {
 #if FRAME_CONFIG != HELI_FRAME
     // if disarmed or landed prioritise throttle
-    if(!motors->armed() || ap.land_complete) {
+    if (!motors->armed() || ap.land_complete) {
         attitude_control->set_throttle_mix_min();
         return;
     }
@@ -168,15 +171,13 @@ void Copter::update_throttle_thr_mix()
         bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);
 
         // check for large acceleration - falling or high turbulence
-        Vector3f accel_ef = ahrs.get_accel_ef_blended();
-        accel_ef.z += GRAVITY_MSS;
-        bool accel_moving = (accel_ef.length() > LAND_CHECK_ACCEL_MOVING);
+        const bool accel_moving = (land_accel_ef_filter.get().length() > LAND_CHECK_ACCEL_MOVING);
 
         // check for requested decent
         bool descent_not_demanded = pos_control->get_desired_velocity().z >= 0.0f;
 
-        if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
-            attitude_control->set_throttle_mix_max();
+        if (large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
+            attitude_control->set_throttle_mix_max(pos_control->get_vel_z_control_ratio());
         } else {
             attitude_control->set_throttle_mix_min();
         }

ArduCopter/landing_gear.cpp

@@ -12,12 +12,6 @@ void Copter::landinggear_update()
     // last status (deployed or retracted) used to check for changes, initialised to startup state of landing gear
     static bool last_deploy_status = landinggear.deployed();
 
-    // if we are doing an automatic landing procedure, force the landing gear to deploy.
-    // To-Do: should we pause the auto-land procedure to give time for gear to come down?
-    if (flightmode->landing_gear_should_be_deployed()) {
-        landinggear.set_position(AP_LandingGear::LandingGear_Deploy);
-    }
-
     // send event message to datalog if status has changed
     if (landinggear.deployed() != last_deploy_status) {
         if (landinggear.deployed()) {

ArduCopter/mode.cpp

@@ -165,6 +165,12 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode)
             break;
 #endif
 
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+        case Mode::Number::AUTOROTATE:
+            ret = &mode_autorotate;
+            break;
+#endif
+
         default:
             break;
     }
@@ -198,11 +204,29 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
 #if FRAME_CONFIG == HELI_FRAME
     // do not allow helis to enter a non-manual throttle mode if the
     // rotor runup is not complete
-    if (!ignore_checks && !new_flightmode->has_manual_throttle() && (motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) {
-        gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
-        AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
-        return false;
+    if (!ignore_checks && 
+    !new_flightmode->has_manual_throttle() && 
+    (motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) {
+        #if MODE_AUTOROTATE_ENABLED == ENABLED
+            //if the mode being exited is the autorotation mode allow mode change despite rotor not being at
+            //full speed.  This will reduce altitude loss on bail-outs back to non-manual throttle modes
+            bool in_autorotation_check = (flightmode != &mode_autorotate || new_flightmode != &mode_autorotate);
+        #else
+            bool in_autorotation_check = false;
+        #endif
+
+        if (!in_autorotation_check) {
+            gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
+            AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
+            return false;
+        }
     }
+
+    #if MODE_AUTOROTATE_ENABLED == ENABLED
+        // If changing to autorotate flight mode from a non-manual throttle mode, store the previous flight mode
+        // to exit back to it when interlock is re-engaged
+        prev_control_mode = control_mode;
+    #endif
 #endif
 
 #if FRAME_CONFIG != HELI_FRAME
@@ -461,7 +485,7 @@ void Mode::make_safe_spool_down()
     case AP_Motors::SpoolState::SHUT_DOWN:
     case AP_Motors::SpoolState::GROUND_IDLE:
         // relax controllers during idle states
-        attitude_control->reset_rate_controller_I_terms();
+        attitude_control->reset_rate_controller_I_terms_smoothly();
         attitude_control->set_yaw_target_to_current_heading();
         break;
 

ArduCopter/mode.h

@@ -1,7 +1,6 @@
 #pragma once
 
 #include "Copter.h"
-
 class Parameters;
 class ParametersG2;
 
@@ -36,6 +35,7 @@ public:
         FOLLOW    =    23,  // follow attempts to follow another vehicle or ground station
         ZIGZAG    =    24,  // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
         SYSTEMID  =    25,  // System ID mode produces automated system identification signals in the controllers
+        AUTOROTATE =   26,  // Autonomous autorotation
     };
 
     // constructor
@@ -66,7 +66,6 @@ public:
     virtual bool is_taking_off() const;
     static void takeoff_stop() { takeoff.stop(); }
 
-    virtual bool landing_gear_should_be_deployed() const { return false; }
     virtual bool is_landing() const { return false; }
 
     // functions for reporting to GCS
@@ -354,7 +353,6 @@ public:
     void nav_guided_start();
 
     bool is_landing() const override;
-    bool landing_gear_should_be_deployed() const override;
 
     bool is_taking_off() const override;
 
@@ -596,6 +594,7 @@ private:
 
     // Circle
     bool pilot_yaw_override = false; // true if pilot is overriding yaw
+    bool speed_changing = false;     // true when the roll stick is being held to facilitate stopping at 0 rate
 };
 
 
@@ -855,7 +854,6 @@ public:
     bool is_autopilot() const override { return true; }
 
     bool is_landing() const override { return true; };
-    bool landing_gear_should_be_deployed() const override { return true; };
 
     void do_not_use_GPS();
 
@@ -1017,7 +1015,6 @@ public:
     bool state_complete() { return _state_complete; }
 
     bool is_landing() const override;
-    bool landing_gear_should_be_deployed() const override;
 
     void restart_without_terrain();
 
@@ -1104,6 +1101,10 @@ private:
     void land();
     SmartRTLState smart_rtl_state = SmartRTL_PathFollow;
 
+    // keep track of how long we have failed to get another return
+    // point while following our path home.  If we take too long we
+    // may choose to land the vehicle.
+    uint32_t path_follow_last_pop_fail_ms;
 };
 
 
@@ -1217,7 +1218,7 @@ private:
         MIX_THROTTLE = 13,  // mixer throttle axis is being excited
     };
 
-    AP_Int8 axis;               // Controls which axis are being excited
+    AP_Int8 axis;               // Controls which axis are being excited. Set to non-zero to display other parameters
     AP_Float waveform_magnitude;// Magnitude of chirp waveform
     AP_Float frequency_start;   // Frequency at the start of the chirp
     AP_Float frequency_stop;    // Frequency at the end of the chirp
@@ -1348,7 +1349,7 @@ class ModeZigZag : public Mode {
 
 public:
 
-    // inherit constructor
+    // Inherit constructor
     using Mode::Mode;
 
     bool init(bool ignore_checks) override;
@@ -1356,7 +1357,7 @@ public:
 
     bool requires_GPS() const override { return true; }
     bool has_manual_throttle() const override { return false; }
-    bool allows_arming(bool from_gcs) const override { return false; }
+    bool allows_arming(bool from_gcs) const override { return true; }
     bool is_autopilot() const override { return true; }
 
     // save current position as A (dest_num = 0) or B (dest_num = 1).  If both A and B have been saved move to the one specified
@@ -1388,3 +1389,83 @@ private:
 
     uint32_t reach_wp_time_ms = 0;  // time since vehicle reached destination (or zero if not yet reached)
 };
+
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+class ModeAutorotate : public Mode {
+
+public:
+
+    // inherit constructor
+    using Mode::Mode;
+
+    bool init(bool ignore_checks) override;
+    void run() override;
+
+    bool is_autopilot() const override { return true; }
+    bool requires_GPS() const override { return false; }
+    bool has_manual_throttle() const override { return false; }
+    bool allows_arming(bool from_gcs) const override { return false; };
+
+    static const struct AP_Param::GroupInfo  var_info[];
+
+protected:
+
+    const char *name() const override { return "AUTOROTATE"; }
+    const char *name4() const override { return "AROT"; }
+
+private:
+
+    // --- Internal variables ---
+    float _initial_rpm;             // Head speed recorded at initiation of flight mode (RPM)
+    float _target_head_speed;       // The terget head main rotor head speed.  Normalised by main rotor set point
+    float _fwd_speed_target;        // Target forward speed (cm/s)
+    float _desired_v_z;             // Desired vertical
+    int32_t _pitch_target;          // Target pitch attitude to pass to attitude controller
+    float _collective_aggression;   // The 'aggresiveness' of collective appliction
+    float _z_touch_down_start;      // The height in cm that the touch down phase began
+    float _t_touch_down_initiate;   // The time in ms that the touch down phase began
+    float now;                      // Current time in millis
+    float _entry_time_start;        // Time remaining until entry phase moves on to glide phase
+    float _hs_decay;                // The head accerleration during the entry phase
+    float _bail_time;               // Timer for exiting the bail out phase (s)
+    float _bail_time_start;         // Time at start of bail out
+    float _des_z;                   // Desired vertical position
+    float _target_climb_rate_adjust;// Target vertical acceleration used during bail out phase
+    float _target_pitch_adjust;     // Target pitch rate used during bail out phase
+    uint16_t log_counter;           // Used to reduce the data flash logging rate
+
+    enum class Autorotation_Phase {
+        ENTRY,
+        SS_GLIDE,
+        FLARE,
+        TOUCH_DOWN,
+        BAIL_OUT } phase_switch;
+        
+    enum class Navigation_Decision {
+        USER_CONTROL_STABILISED,
+        STRAIGHT_AHEAD,
+        INTO_WIND,
+        NEAREST_RALLY} nav_pos_switch;
+
+    // --- Internal flags ---
+    struct controller_flags {
+            bool entry_initial             : 1;
+            bool ss_glide_initial          : 1;
+            bool flare_initial             : 1;
+            bool touch_down_initial        : 1;
+            bool straight_ahead_initial    : 1;
+            bool level_initial             : 1;
+            bool break_initial             : 1;
+            bool bail_out_initial          : 1;
+            bool bad_rpm                   : 1;
+    } _flags;
+
+    struct message_flags {
+            bool bad_rpm                   : 1;
+    } _msg_flags;
+
+    //--- Internal functions ---
+    void warning_message(uint8_t message_n);    //Handles output messages to the terminal
+
+};
+#endif

ArduCopter/mode_acro.cpp

@@ -30,17 +30,20 @@ void ModeAcro::run()
         attitude_control->set_attitude_target_to_current_attitude();
         attitude_control->reset_rate_controller_I_terms();
         break;
+
     case AP_Motors::SpoolState::GROUND_IDLE:
         // Landed
         attitude_control->set_attitude_target_to_current_attitude();
-        attitude_control->reset_rate_controller_I_terms();
+        attitude_control->reset_rate_controller_I_terms_smoothly();
         break;
+
     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
         // clear landing flag above zero throttle
         if (!motors->limit.throttle_lower) {
             set_land_complete(false);
         }
         break;
+
     case AP_Motors::SpoolState::SPOOLING_UP:
     case AP_Motors::SpoolState::SPOOLING_DOWN:
         // do nothing
@@ -79,20 +82,18 @@ void ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in,
         roll_in *= ratio;
         pitch_in *= ratio;
     }
+
+    // range check expo
+    g.acro_rp_expo = constrain_float(g.acro_rp_expo, 0.0f, 1.0f);
     
     // calculate roll, pitch rate requests
-    if (g.acro_rp_expo <= 0) {
+    if (is_zero(g.acro_rp_expo)) {
         rate_bf_request.x = roll_in * g.acro_rp_p;
         rate_bf_request.y = pitch_in * g.acro_rp_p;
     } else {
         // expo variables
         float rp_in, rp_in3, rp_out;
 
-        // range check expo
-        if (g.acro_rp_expo > 1.0f) {
-            g.acro_rp_expo = 1.0f;
-        }
-
         // roll expo
         rp_in = float(roll_in)/ROLL_PITCH_YAW_INPUT_MAX;
         rp_in3 = rp_in*rp_in*rp_in;

ArduCopter/mode_acro_heli.cpp

@@ -49,16 +49,16 @@ void ModeAcro_Heli::run()
         attitude_control->reset_rate_controller_I_terms();
         break;
     case AP_Motors::SpoolState::GROUND_IDLE:
-        // Landed
-        if (motors->init_targets_on_arming()) {
+        // If aircraft is landed, set target heading to current and reset the integrator
+        // Otherwise motors could be at ground idle for practice autorotation
+        if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
             attitude_control->set_attitude_target_to_current_attitude();
-            attitude_control->reset_rate_controller_I_terms();
+            attitude_control->reset_rate_controller_I_terms_smoothly();
         }
         break;
     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
-        // clear landing flag above zero throttle
-        if (!motors->limit.throttle_lower) {
-            set_land_complete(false);
+        if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
+            attitude_control->reset_rate_controller_I_terms_smoothly();
         }
         break;
     case AP_Motors::SpoolState::SPOOLING_UP:

ArduCopter/mode_althold.cpp

@@ -48,25 +48,21 @@ void ModeAltHold::run()
     switch (althold_state) {
 
     case AltHold_MotorStopped:
-
         attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
         break;
 
     case AltHold_Landed_Ground_Idle:
-
-        attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
         // FALLTHROUGH
 
     case AltHold_Landed_Pre_Takeoff:
-
+        attitude_control->reset_rate_controller_I_terms_smoothly();
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
         break;
 
     case AltHold_Takeoff:
-
         // initiate take-off
         if (!takeoff.running()) {
             takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));

ArduCopter/mode_auto.cpp

@@ -249,6 +249,9 @@ void ModeAuto::land_start(const Vector3f& destination)
 
     // initialise yaw
     auto_yaw.set_mode(AUTO_YAW_HOLD);
+
+    // optionally deploy landing gear
+    copter.landinggear.deploy_for_landing();
 }
 
 // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
@@ -317,6 +320,10 @@ void ModeAuto::circle_start()
 
     // initialise circle controller
     copter.circle_nav->init(copter.circle_nav->get_center());
+
+    if (auto_yaw.mode() != AUTO_YAW_ROI) {
+        auto_yaw.set_mode(AUTO_YAW_HOLD);
+    }
 }
 
 // auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
@@ -370,20 +377,7 @@ bool ModeAuto::is_landing() const
 
 bool ModeAuto::is_taking_off() const
 {
-    return _mode == Auto_TakeOff;
-}
-
-bool ModeAuto::landing_gear_should_be_deployed() const
-{
-    switch(_mode) {
-    case Auto_Land:
-        return true;
-    case Auto_RTL:
-        return copter.mode_rtl.landing_gear_should_be_deployed();
-    default:
-        return false;
-    }
-    return false;
+    return ((_mode == Auto_TakeOff) && !wp_nav->reached_wp_destination());
 }
 
 // auto_payload_place_start - initialises controller to implement a placing
@@ -741,10 +735,6 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
 void ModeAuto::takeoff_run()
 {
     auto_takeoff_run();
-    if (wp_nav->reached_wp_destination()) {
-        const Vector3f target = wp_nav->get_wp_destination();
-        wp_start(target, wp_nav->origin_and_destination_are_terrain_alt());
-    }
 }
 
 // auto_wp_run - runs the auto waypoint controller
@@ -1512,7 +1502,7 @@ bool ModeAuto::verify_takeoff()
 
     // retract the landing gear
     if (reached_wp_dest) {
-        copter.landinggear.set_position(AP_LandingGear::LandingGear_Retract);
+        copter.landinggear.retract_after_takeoff();
     }
 
     return reached_wp_dest;
@@ -1528,7 +1518,7 @@ bool ModeAuto::verify_land()
             // check if we've reached the location
             if (copter.wp_nav->reached_wp_destination()) {
                 // get destination so we can use it for loiter target
-                Vector3f dest = copter.wp_nav->get_wp_destination();
+                const Vector3f& dest = copter.wp_nav->get_wp_destination();
 
                 // initialise landing controller
                 land_start(dest);

ArduCopter/mode_autorotate.cpp

@@ -0,0 +1,325 @@
+/* -----------------------------------------------------------------------------------------
+    This is currently a SITL only function until the project is complete.
+    To trial this in SITL you will need to use Real Flight 8.
+    Instructions for how to set this up in SITL can be found here:
+    https://discuss.ardupilot.org/t/autonomous-autorotation-gsoc-project-blog/42139
+ -----------------------------------------------------------------------------------------*/
+
+#include "Copter.h"
+#include <AC_Autorotation/AC_Autorotation.h>
+#include "mode.h"
+
+#include <utility>
+
+#if MODE_AUTOROTATE_ENABLED == ENABLED
+
+#define AUTOROTATE_ENTRY_TIME          2.0f    // (s) number of seconds that the entry phase operates for
+#define BAILOUT_MOTOR_RAMP_TIME        1.0f    // (s) time set on bailout ramp up timer for motors - See AC_MotorsHeli_Single
+#define HEAD_SPEED_TARGET_RATIO        1.0f    // Normalised target main rotor head speed (unit: -)
+
+bool ModeAutorotate::init(bool ignore_checks)
+{
+#if FRAME_CONFIG != HELI_FRAME
+    // Only allow trad heli to use autorotation mode
+    return false;
+#endif
+
+    // Check that mode is enabled
+    if (!g2.arot.is_enable()) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Autorot Mode Not Enabled");
+        return false;
+    }
+
+    // Check that interlock is disengaged
+    if (motors->get_interlock()) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Autorot Mode Change Fail: Interlock Engaged");
+        return false;
+    }
+
+    // Initialise controllers
+    // This must be done before RPM value is fetched
+    g2.arot.init_hs_controller();
+    g2.arot.init_fwd_spd_controller();
+
+    // Retrive rpm and start rpm sensor health checks
+    _initial_rpm = g2.arot.get_rpm(true);
+
+    // Display message 
+    gcs().send_text(MAV_SEVERITY_INFO, "Autorotation initiated");
+
+     // Set all inial flags to on
+    _flags.entry_initial = 1;
+    _flags.ss_glide_initial = 1;
+    _flags.flare_initial = 1;
+    _flags.touch_down_initial = 1;
+    _flags.level_initial = 1;
+    _flags.break_initial = 1;
+    _flags.straight_ahead_initial = 1;
+    _flags.bail_out_initial = 1;
+    _msg_flags.bad_rpm = true;
+
+    // Setting default starting switches
+    phase_switch = Autorotation_Phase::ENTRY;
+
+    // Set entry timer
+    _entry_time_start = millis();
+
+    // The decay rate to reduce the head speed from the current to the target
+    _hs_decay = ((_initial_rpm/g2.arot.get_hs_set_point()) - HEAD_SPEED_TARGET_RATIO) / AUTOROTATE_ENTRY_TIME;
+
+    return true;
+}
+
+
+
+void ModeAutorotate::run()
+{
+    // Check if interlock becomes engaged
+    if (motors->get_interlock() && !copter.ap.land_complete) {
+        phase_switch = Autorotation_Phase::BAIL_OUT;
+    } else if (motors->get_interlock() && copter.ap.land_complete) {
+        // Aircraft is landed and no need to bail out
+        set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT);
+    }
+
+    // Current time
+    now = millis(); //milliseconds
+
+    // Initialise internal variables
+    float curr_vel_z = inertial_nav.get_velocity().z;   // Current vertical descent
+
+    //----------------------------------------------------------------
+    //                  State machine logic
+    //----------------------------------------------------------------
+
+     // Setting default phase switch positions
+     nav_pos_switch = Navigation_Decision::USER_CONTROL_STABILISED;
+
+    // Timer from entry phase to progress to glide phase
+    if (phase_switch == Autorotation_Phase::ENTRY){
+
+        if ((now - _entry_time_start)/1000.0f > AUTOROTATE_ENTRY_TIME) {
+            // Flight phase can be progressed to steady state glide
+            phase_switch = Autorotation_Phase::SS_GLIDE;
+        }
+
+    }
+
+
+    //----------------------------------------------------------------
+    //                  State machine actions
+    //----------------------------------------------------------------
+    switch (phase_switch) {
+
+        case Autorotation_Phase::ENTRY:
+        {
+            // Entry phase functions to be run only once
+            if (_flags.entry_initial == 1) {
+
+                #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+                    gcs().send_text(MAV_SEVERITY_INFO, "Entry Phase");
+                #endif
+
+                // Set following trim low pass cut off frequency
+                g2.arot.set_col_cutoff_freq(g2.arot.get_col_entry_freq());
+
+                // Target head speed is set to rpm at initiation to prevent unwanted changes in attitude
+                _target_head_speed = _initial_rpm/g2.arot.get_hs_set_point();
+
+                // Set desired forward speed target
+                g2.arot.set_desired_fwd_speed();
+
+                // Prevent running the initial entry functions again
+                _flags.entry_initial = 0;
+
+            }
+
+            // Slowly change the target head speed until the target head speed matches the parameter defined value
+            if (g2.arot.get_rpm() > HEAD_SPEED_TARGET_RATIO*1.005f  ||  g2.arot.get_rpm() < HEAD_SPEED_TARGET_RATIO*0.995f) {
+                _target_head_speed -= _hs_decay*G_Dt;
+            } else {
+                _target_head_speed = HEAD_SPEED_TARGET_RATIO;
+            }
+
+            // Set target head speed in head speed controller
+            g2.arot.set_target_head_speed(_target_head_speed);
+
+            // Run airspeed/attitude controller
+            g2.arot.set_dt(G_Dt);
+            g2.arot.update_forward_speed_controller();
+
+            // Retrieve pitch target
+            _pitch_target = g2.arot.get_pitch();
+
+            // Update controllers
+            _flags.bad_rpm = g2.arot.update_hs_glide_controller(G_Dt); //run head speed/ collective controller
+
+            break;
+        }
+
+        case Autorotation_Phase::SS_GLIDE:
+        {
+            // Steady state glide functions to be run only once
+            if (_flags.ss_glide_initial == 1) {
+
+                #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+                    gcs().send_text(MAV_SEVERITY_INFO, "SS Glide Phase");
+                #endif
+
+                // Set following trim low pass cut off frequency
+                g2.arot.set_col_cutoff_freq(g2.arot.get_col_glide_freq());
+
+                // Set desired forward speed target
+                g2.arot.set_desired_fwd_speed();
+
+                // Set target head speed in head speed controller
+                _target_head_speed = HEAD_SPEED_TARGET_RATIO;  //Ensure target hs is set to glide incase hs hasent reached target for glide
+                g2.arot.set_target_head_speed(_target_head_speed);
+
+                // Prevent running the initial glide functions again
+                _flags.ss_glide_initial = 0;
+            }
+
+            // Run airspeed/attitude controller
+            g2.arot.set_dt(G_Dt);
+            g2.arot.update_forward_speed_controller();
+
+            // Retrieve pitch target 
+            _pitch_target = g2.arot.get_pitch();
+
+            // Update head speed/ collective controller
+            _flags.bad_rpm = g2.arot.update_hs_glide_controller(G_Dt); 
+            // Attitude controller is updated in navigation switch-case statements
+
+            break;
+        }
+
+        case Autorotation_Phase::FLARE:
+        case Autorotation_Phase::TOUCH_DOWN:
+        {
+            break;
+        }
+
+        case Autorotation_Phase::BAIL_OUT:
+        {
+        if (_flags.bail_out_initial == 1) {
+                // Functions and settings to be done once are done here.
+
+                #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+                    gcs().send_text(MAV_SEVERITY_INFO, "Bailing Out of Autorotation");
+                #endif
+
+                // Set bail out timer remaining equal to the paramter value, bailout time 
+                // cannot be less than the motor spool-up time: BAILOUT_MOTOR_RAMP_TIME.
+                _bail_time = MAX(g2.arot.get_bail_time(),BAILOUT_MOTOR_RAMP_TIME+0.1f);
+
+                // Set bail out start time
+                _bail_time_start = now;
+
+                // Set initial target vertical speed
+                _desired_v_z = curr_vel_z;
+
+                // Initialise position and desired velocity
+                if (!pos_control->is_active_z()) {
+                    pos_control->relax_alt_hold_controllers(g2.arot.get_last_collective());
+                }
+
+                // Get pilot parameter limits
+                const float pilot_spd_dn = -get_pilot_speed_dn();
+                const float pilot_spd_up = g.pilot_speed_up;
+
+                // Set speed limit
+                pos_control->set_max_speed_z(curr_vel_z, pilot_spd_up);
+
+                float pilot_des_v_z = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
+                pilot_des_v_z = constrain_float(pilot_des_v_z, pilot_spd_dn, pilot_spd_up);
+
+                // Calculate target climb rate adjustment to transition from bail out decent speed to requested climb rate on stick.
+                _target_climb_rate_adjust = (curr_vel_z - pilot_des_v_z)/(_bail_time - BAILOUT_MOTOR_RAMP_TIME); //accounting for 0.5s motor spool time
+
+                // Calculate pitch target adjustment rate to return to level
+                _target_pitch_adjust = _pitch_target/_bail_time;
+
+                // Set acceleration limit
+                pos_control->set_max_accel_z(abs(_target_climb_rate_adjust));
+
+                motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+
+                _flags.bail_out_initial = 0;
+            }
+
+        if ((now - _bail_time_start)/1000.0f >= BAILOUT_MOTOR_RAMP_TIME) {
+            // Update desired vertical speed and pitch target after the bailout motor ramp timer has completed
+            _desired_v_z -= _target_climb_rate_adjust*G_Dt;
+            _pitch_target -= _target_pitch_adjust*G_Dt;
+        }
+        // Set position controller
+        pos_control->set_alt_target_from_climb_rate(_desired_v_z, G_Dt, false);
+
+        // Update controllers
+        pos_control->update_z_controller();
+
+        if ((now - _bail_time_start)/1000.0f >= _bail_time) {
+            // Bail out timer complete.  Change flight mode. Do not revert back to auto. Prevent aircraft
+            // from continuing mission and potentially flying further away after a power failure.
+            if (copter.prev_control_mode == Mode::Number::AUTO) {
+                set_mode(Mode::Number::ALT_HOLD, ModeReason::AUTOROTATION_BAILOUT);
+            } else {
+                set_mode(copter.prev_control_mode, ModeReason::AUTOROTATION_BAILOUT);
+            }
+        }
+
+        break;
+        }
+    }
+
+
+    switch (nav_pos_switch) {
+
+        case Navigation_Decision::USER_CONTROL_STABILISED:
+        {
+            // Operator is in control of roll and yaw.  Controls act as if in stabilise flight mode.  Pitch 
+            // is controlled by speed-height controller.
+            float pilot_roll, pilot_pitch;
+            get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
+
+            // Get pilot's desired yaw rate
+            float pilot_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+
+            // Pitch target is calculated in autorotation phase switch above
+            attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pilot_roll, _pitch_target, pilot_yaw_rate);
+            break;
+        }
+
+        case Navigation_Decision::STRAIGHT_AHEAD:
+        case Navigation_Decision::INTO_WIND:
+        case Navigation_Decision::NEAREST_RALLY:
+        {
+            break;
+        }
+    }
+
+    // Output warning messaged if rpm signal is bad
+    if (_flags.bad_rpm) {
+        warning_message(1);
+    }
+
+} // End function run()
+
+void ModeAutorotate::warning_message(uint8_t message_n)
+{
+    switch (message_n) {
+        case 1:
+        {
+            if (_msg_flags.bad_rpm) {
+                // Bad rpm sensor health.
+                gcs().send_text(MAV_SEVERITY_INFO, "Warning: Poor RPM Sensor Health");
+                gcs().send_text(MAV_SEVERITY_INFO, "Action: Minimum Collective Applied");
+                _msg_flags.bad_rpm = false;
+            }
+            break;
+        }
+    }
+}
+
+#endif

ArduCopter/mode_autotune.cpp

@@ -60,7 +60,7 @@ void AutoTune::run()
         } else {
             copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
         }
-        copter.attitude_control->reset_rate_controller_I_terms();
+        copter.attitude_control->reset_rate_controller_I_terms_smoothly();
         copter.attitude_control->set_yaw_target_to_current_heading();
 
         float target_roll, target_pitch, target_yaw_rate;

ArduCopter/mode_circle.cpp

@@ -10,6 +10,7 @@
 bool ModeCircle::init(bool ignore_checks)
 {
     pilot_yaw_override = false;
+    speed_changing = false;
 
     // initialize speeds and accelerations
     pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy());
@@ -39,6 +40,52 @@ void ModeCircle::run()
         pilot_yaw_override = true;
     }
 
+    // pilot changes to circle rate and radius
+    // skip if in radio failsafe
+    if (!copter.failsafe.radio && copter.circle_nav->pilot_control_enabled()) {
+        // update the circle controller's radius target based on pilot pitch stick inputs
+        const float radius_current = copter.circle_nav->get_radius();           // circle controller's radius target, which begins as the circle_radius parameter
+        const float pitch_stick = channel_pitch->norm_input_dz();               // pitch stick normalized -1 to 1
+        const float nav_speed = copter.wp_nav->get_default_speed_xy();          // copter WP_NAV parameter speed
+        const float radius_pilot_change = (pitch_stick * nav_speed) * G_Dt;     // rate of change (pitch stick up reduces the radius, as in moving forward)
+        const float radius_new = MAX(radius_current + radius_pilot_change,0);   // new radius target
+
+        if (!is_equal(radius_current, radius_new)) {
+            copter.circle_nav->set_radius(radius_new);
+        }
+
+        // update the orbicular rate target based on pilot roll stick inputs
+        // skip if using CH6 tuning knob for circle rate
+        if (g.radio_tuning != TUNING_CIRCLE_RATE) {
+            const float roll_stick = channel_roll->norm_input_dz();         // roll stick normalized -1 to 1
+
+            if (is_zero(roll_stick)) {
+                // no speed change, so reset speed changing flag
+                speed_changing = false;
+            } else {
+                const float rate = copter.circle_nav->get_rate();           // circle controller's rate target, which begins as the circle_rate parameter
+                const float rate_current = copter.circle_nav->get_rate_current(); // current adjusted rate target, which is probably different from _rate
+                const float rate_pilot_change = (roll_stick * G_Dt);        // rate of change from 0 to 1 degrees per second
+                float rate_new = rate_current;                              // new rate target
+                if (is_positive(rate)) {
+                    // currently moving clockwise, constrain 0 to 90
+                    rate_new = constrain_float(rate_current + rate_pilot_change, 0, 90);
+
+                } else if (is_negative(rate)) {
+                    // currently moving counterclockwise, constrain -90 to 0
+                    rate_new = constrain_float(rate_current + rate_pilot_change, -90, 0);
+
+                } else if (is_zero(rate) && !speed_changing) {
+                    // Stopped, pilot has released the roll stick, and pilot now wants to begin moving with the roll stick
+                    rate_new = rate_pilot_change;
+                }
+
+                speed_changing = true;
+                copter.circle_nav->set_rate(rate_new);
+            }
+        }
+    }
+
     // get pilot desired climb rate (or zero if in radio failsafe)
     float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
     // adjust climb rate using rangefinder

ArduCopter/mode_drift.cpp

@@ -94,17 +94,20 @@ void ModeDrift::run()
         attitude_control->set_yaw_target_to_current_heading();
         attitude_control->reset_rate_controller_I_terms();
         break;
+
     case AP_Motors::SpoolState::GROUND_IDLE:
         // Landed
         attitude_control->set_yaw_target_to_current_heading();
-        attitude_control->reset_rate_controller_I_terms();
+        attitude_control->reset_rate_controller_I_terms_smoothly();
         break;
+
     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
         // clear landing flag above zero throttle
         if (!motors->limit.throttle_lower) {
             set_land_complete(false);
         }
         break;
+
     case AP_Motors::SpoolState::SPOOLING_UP:
     case AP_Motors::SpoolState::SPOOLING_DOWN:
         // do nothing

ArduCopter/mode_flip.cpp

@@ -200,6 +200,7 @@ void ModeFlip::run()
             Log_Write_Event(DATA_FLIP_END);
         }
         break;
+
     }
     case FlipState::Abandon:
         // restore original flight mode

ArduCopter/mode_flowhold.cpp

@@ -254,7 +254,6 @@ void ModeFlowHold::run()
     switch (flowhold_state) {
 
     case AltHold_MotorStopped:
-
         copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
         copter.attitude_control->reset_rate_controller_I_terms();
         copter.attitude_control->set_yaw_target_to_current_heading();
@@ -283,12 +282,11 @@ void ModeFlowHold::run()
         break;
 
     case AltHold_Landed_Ground_Idle:
-        attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
         // FALLTHROUGH
 
     case AltHold_Landed_Pre_Takeoff:
-
+        attitude_control->reset_rate_controller_I_terms_smoothly();
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
         break;
 

ArduCopter/mode_guided.cpp

@@ -370,7 +370,11 @@ void ModeGuided::takeoff_run()
 {
     auto_takeoff_run();
     if (wp_nav->reached_wp_destination()) {
-        const Vector3f target = wp_nav->get_wp_destination();
+        // optionally retract landing gear
+        copter.landinggear.retract_after_takeoff();
+
+        // switch to position control mode but maintain current target
+        const Vector3f& target = wp_nav->get_wp_destination();
         set_destination(target);
     }
 }

ArduCopter/mode_land.cpp

@@ -37,6 +37,9 @@ bool ModeLand::init(bool ignore_checks)
     // initialise yaw
     auto_yaw.set_mode(AUTO_YAW_HOLD);
 
+    // optionally deploy landing gear
+    copter.landinggear.deploy_for_landing();
+
     return true;
 }
 

ArduCopter/mode_loiter.cpp

@@ -117,7 +117,6 @@ void ModeLoiter::run()
     switch (loiter_state) {
 
     case AltHold_MotorStopped:
-
         attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
@@ -127,7 +126,6 @@ void ModeLoiter::run()
         break;
 
     case AltHold_Takeoff:
-
         // initiate take-off
         if (!takeoff.running()) {
             takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
@@ -152,13 +150,11 @@ void ModeLoiter::run()
         break;
 
     case AltHold_Landed_Ground_Idle:
-
-        attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
         // FALLTHROUGH
 
     case AltHold_Landed_Pre_Takeoff:
-
+        attitude_control->reset_rate_controller_I_terms_smoothly();
         loiter_nav->init_target();
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
@@ -166,7 +162,6 @@ void ModeLoiter::run()
         break;
 
     case AltHold_Flying:
-
         // set motors to full range
         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 

ArduCopter/mode_poshold.cpp

@@ -107,7 +107,6 @@ void ModePosHold::run()
     switch (poshold_state) {
 
     case AltHold_MotorStopped:
-
         attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
@@ -121,7 +120,6 @@ void ModePosHold::run()
         break;
 
     case AltHold_Takeoff:
-
         // initiate take-off
         if (!takeoff.running()) {
             takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
@@ -148,16 +146,14 @@ void ModePosHold::run()
         break;
 
     case AltHold_Landed_Ground_Idle:
-
         loiter_nav->clear_pilot_desired_acceleration();
         loiter_nav->init_target();
         loiter_nav->update();
-        attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
         // FALLTHROUGH
 
     case AltHold_Landed_Pre_Takeoff:
-
+        attitude_control->reset_rate_controller_I_terms_smoothly();
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
 
         // set poshold state to pilot override
@@ -166,7 +162,6 @@ void ModePosHold::run()
         break;
 
     case AltHold_Flying:
-
         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
 #if AC_AVOID_ENABLED == ENABLED

ArduCopter/mode_rtl.cpp

@@ -266,6 +266,9 @@ void ModeRTL::descent_start()
 
     // initialise yaw
     auto_yaw.set_mode(AUTO_YAW_HOLD);
+
+    // optionally deploy landing gear
+    copter.landinggear.deploy_for_landing();
 }
 
 // rtl_descent_run - implements the final descent to the RTL_ALT
@@ -349,6 +352,9 @@ void ModeRTL::land_start()
 
     // initialise yaw
     auto_yaw.set_mode(AUTO_YAW_HOLD);
+
+    // optionally deploy landing gear
+    copter.landinggear.deploy_for_landing();
 }
 
 bool ModeRTL::is_landing() const
@@ -356,19 +362,6 @@ bool ModeRTL::is_landing() const
     return _state == RTL_Land;
 }
 
-bool ModeRTL::landing_gear_should_be_deployed() const
-{
-    switch(_state) {
-    case RTL_LoiterAtHome:
-    case RTL_Land:
-    case RTL_FinalDescent:
-        return true;
-    default:
-        return false;
-    }
-    return false;
-}
-
 // rtl_returnhome_run - return home
 //      called by rtl_run at 100hz or more
 void ModeRTL::land_run(bool disarm_on_land)

ArduCopter/mode_smart_rtl.cpp

@@ -69,6 +69,7 @@ void ModeSmartRTL::wait_cleanup_run()
 
     // check if return path is computed and if yes, begin journey home
     if (g2.smart_rtl.request_thorough_cleanup()) {
+        path_follow_last_pop_fail_ms = 0;
         smart_rtl_state = SmartRTL_PathFollow;
     }
 }
@@ -87,7 +88,10 @@ void ModeSmartRTL::path_follow_run()
     // if we are close to current target point, switch the next point to be our target.
     if (wp_nav->reached_wp_destination()) {
         Vector3f next_point;
+        // this pop_point can fail if the IO task currently has the
+        // path semaphore.
         if (g2.smart_rtl.pop_point(next_point)) {
+            path_follow_last_pop_fail_ms = 0;
             bool fast_waypoint = true;
             if (g2.smart_rtl.get_num_points() == 0) {
                 // this is the very last point, add 2m to the target alt and move to pre-land state
@@ -98,8 +102,18 @@ void ModeSmartRTL::path_follow_run()
             // send target to waypoint controller
             wp_nav->set_wp_destination_NED(next_point);
             wp_nav->set_fast_waypoint(fast_waypoint);
-        } else {
-            // this can only happen if we fail to get the semaphore which should never happen but just in case, land
+        } else if (g2.smart_rtl.get_num_points() == 0) {
+            // We should never get here; should always have at least
+            // two points and the "zero points left" is handled above.
+            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+            smart_rtl_state = SmartRTL_PreLandPosition;
+        } else if (path_follow_last_pop_fail_ms == 0) {
+            // first time we've failed to pop off (ever, or after a success)
+            path_follow_last_pop_fail_ms = AP_HAL::millis();
+        } else if (AP_HAL::millis() - path_follow_last_pop_fail_ms > 10000) {
+            // we failed to pop a point off for 10 seconds.  This is
+            // almost certainly a bug.
+            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
             smart_rtl_state = SmartRTL_PreLandPosition;
         }
     }

ArduCopter/mode_sport.cpp

@@ -79,14 +79,12 @@ void ModeSport::run()
     switch (sport_state) {
 
     case AltHold_MotorStopped:
-
         attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
         break;
 
     case AltHold_Takeoff:
-
         // initiate take-off
         if (!takeoff.running()) {
             takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
@@ -104,12 +102,11 @@ void ModeSport::run()
         break;
 
     case AltHold_Landed_Ground_Idle:
-        attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
         // FALLTHROUGH
 
     case AltHold_Landed_Pre_Takeoff:
-
+        attitude_control->reset_rate_controller_I_terms_smoothly();
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
         break;
 

ArduCopter/mode_stabilize.cpp

@@ -34,17 +34,20 @@ void ModeStabilize::run()
         attitude_control->set_yaw_target_to_current_heading();
         attitude_control->reset_rate_controller_I_terms();
         break;
+
     case AP_Motors::SpoolState::GROUND_IDLE:
         // Landed
         attitude_control->set_yaw_target_to_current_heading();
-        attitude_control->reset_rate_controller_I_terms();
+        attitude_control->reset_rate_controller_I_terms_smoothly();
         break;
+
     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
         // clear landing flag above zero throttle
         if (!motors->limit.throttle_lower) {
             set_land_complete(false);
         }
         break;
+
     case AP_Motors::SpoolState::SPOOLING_UP:
     case AP_Motors::SpoolState::SPOOLING_DOWN:
         // do nothing

ArduCopter/mode_stabilize_heli.cpp

@@ -54,16 +54,16 @@ void ModeStabilize_Heli::run()
         attitude_control->reset_rate_controller_I_terms();
         break;
     case AP_Motors::SpoolState::GROUND_IDLE:
-        // Landed
-        if (motors->init_targets_on_arming()) {
+        // If aircraft is landed, set target heading to current and reset the integrator
+        // Otherwise motors could be at ground idle for practice autorotation
+        if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
             attitude_control->set_yaw_target_to_current_heading();
-            attitude_control->reset_rate_controller_I_terms();
+            attitude_control->reset_rate_controller_I_terms_smoothly();
         }
         break;
     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
-        // clear landing flag above zero throttle
-        if (!motors->limit.throttle_lower) {
-            set_land_complete(false);
+        if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
+            attitude_control->reset_rate_controller_I_terms_smoothly();
         }
     case AP_Motors::SpoolState::SPOOLING_UP:
     case AP_Motors::SpoolState::SPOOLING_DOWN:

ArduCopter/mode_systemid.cpp

@@ -10,10 +10,10 @@ const AP_Param::GroupInfo ModeSystemId::var_info[] = {
 
     // @Param: _AXIS
     // @DisplayName: System identification axis
-    // @Description: Controls which axis are being excited
+    // @Description: Controls which axis are being excited.  Set to non-zero to see more parameters
     // @User: Standard
     // @Values: 0:None, 1:Input Roll Angle, 2:Input Pitch Angle, 3:Input Yaw Angle, 4:Recovery Roll Angle, 5:Recovery Pitch Angle, 6:Recovery Yaw Angle, 7:Rate Roll, 8:Rate Pitch, 9:Rate Yaw, 10:Mixer Roll, 11:Mixer Pitch, 12:Mixer Yaw, 13:Mixer Thrust
-    AP_GROUPINFO("_AXIS", 1, ModeSystemId, axis, 0),
+    AP_GROUPINFO_FLAGS("_AXIS", 1, ModeSystemId, axis, 0, AP_PARAM_FLAG_ENABLE),
 
     // @Param: _MAGNITUDE
     // @DisplayName: System identification Chirp Magnitude
@@ -74,6 +74,12 @@ ModeSystemId::ModeSystemId(void) : Mode()
 // systemId_init - initialise systemId controller
 bool ModeSystemId::init(bool ignore_checks)
 {
+    // check if enabled
+    if (axis == 0) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "No axis selected, SID_AXIS = 0");
+        return false;
+    }
+
     // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
     if (motors->armed() && copter.ap.land_complete && !copter.flightmode->has_manual_throttle()) {
         return false;
@@ -128,21 +134,24 @@ void ModeSystemId::run()
         attitude_control->set_yaw_target_to_current_heading();
         attitude_control->reset_rate_controller_I_terms();
         break;
+
     case AP_Motors::SpoolState::GROUND_IDLE:
         // Landed
         // Tradheli initializes targets when going from disarmed to armed state. 
         // init_targets_on_arming is always set true for multicopter.
         if (motors->init_targets_on_arming()) {
             attitude_control->set_yaw_target_to_current_heading();
-            attitude_control->reset_rate_controller_I_terms();
+            attitude_control->reset_rate_controller_I_terms_smoothly();
         }
         break;
+
     case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
         // clear landing flag above zero throttle
         if (!motors->limit.throttle_lower) {
             set_land_complete(false);
         }
         break;
+
     case AP_Motors::SpoolState::SPOOLING_UP:
     case AP_Motors::SpoolState::SPOOLING_DOWN:
         // do nothing

ArduCopter/mode_zigzag.cpp

@@ -11,8 +11,20 @@
 // initialise zigzag controller
 bool ModeZigZag::init(bool ignore_checks)
 {
-    // initialize's loiter position and velocity on xy-axes from current pos and velocity
-    loiter_nav->clear_pilot_desired_acceleration();
+    if (!copter.failsafe.radio) {
+        // apply simple mode transform to pilot inputs
+        update_simple_mode();
+
+        // convert pilot input to lean angles
+        float target_roll, target_pitch;
+        get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
+
+        // process pilot's roll and pitch input
+        loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
+    } else {
+        // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
+        loiter_nav->clear_pilot_desired_acceleration();
+    }
     loiter_nav->init_target();
 
     // initialise position_z and desired velocity_z
@@ -37,16 +49,13 @@ void ModeZigZag::run()
     pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
     pos_control->set_max_accel_z(g.pilot_accel_z);
 
-    // if not auto armed or motors not enabled set throttle to zero and exit immediately
-    if (is_disarmed_or_landed() || !motors->get_interlock() ) {
-        zero_throttle_and_relax_ac(copter.is_tradheli() && motors->get_interlock());
-        return;
-    }
-
     // auto control
     if (stage == AUTO) {
-        // if vehicle has reached destination switch to manual control
-        if (reached_destination()) {
+        if (is_disarmed_or_landed() || !motors->get_interlock()) {
+            // vehicle should be under manual control when disarmed or landed
+            return_to_manual_control(false);
+        } else if (reached_destination()) {
+            // if vehicle has reached destination switch to manual control
             AP_Notify::events.waypoint_complete = 1;
             return_to_manual_control(true);
         } else {
@@ -123,7 +132,7 @@ void ModeZigZag::return_to_manual_control(bool maintain_target)
         stage = MANUAL_REGAIN;
         loiter_nav->clear_pilot_desired_acceleration();
         if (maintain_target) {
-            const Vector3f wp_dest = wp_nav->get_wp_destination();
+            const Vector3f& wp_dest = wp_nav->get_wp_destination();
             loiter_nav->init_target(wp_dest);
             if (wp_nav->origin_and_destination_are_terrain_alt()) {
                 copter.surface_tracking.set_target_alt_cm(wp_dest.z);
@@ -169,6 +178,7 @@ void ModeZigZag::manual_control()
 {
     float target_yaw_rate = 0.0f;
     float target_climb_rate = 0.0f;
+    float takeoff_climb_rate = 0.0f;
 
     // process pilot inputs unless we are in radio failsafe
     if (!copter.failsafe.radio) {
@@ -194,26 +204,82 @@ void ModeZigZag::manual_control()
         loiter_nav->clear_pilot_desired_acceleration();
     }
 
-    // set motors to full range
-    motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+    // relax loiter target if we might be landed
+    if (copter.ap.land_complete_maybe) {
+        loiter_nav->soften_for_landing();
+    }
 
-    // run loiter controller
-    loiter_nav->update();
+    // Loiter State Machine Determination
+    AltHoldModeState althold_state = get_alt_hold_state(target_climb_rate);
 
-    // call attitude controller
-    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
+    // althold state machine
+    switch (althold_state) {
 
-    // adjust climb rate using rangefinder
-    target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
+    case AltHold_MotorStopped:
+        attitude_control->reset_rate_controller_I_terms();
+        attitude_control->set_yaw_target_to_current_heading();
+        pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
+        loiter_nav->init_target();
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
+        pos_control->update_z_controller();
+        break;
 
-    // get avoidance adjusted climb rate
-    target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
+    case AltHold_Takeoff:
+        // initiate take-off
+        if (!takeoff.running()) {
+            takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
+        }
 
-    // update altitude target and call position controller
-    pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
+        // get takeoff adjusted pilot and takeoff climb rates
+        takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
 
-    // adjusts target up or down using a climb rate
-    pos_control->update_z_controller();
+        // get avoidance adjusted climb rate
+        target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
+
+        // run loiter controller
+        loiter_nav->update();
+
+        // call attitude controller
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
+
+        // update altitude target and call position controller
+        pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
+        pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
+        pos_control->update_z_controller();
+        break;
+
+    case AltHold_Landed_Ground_Idle:
+        attitude_control->set_yaw_target_to_current_heading();
+        FALLTHROUGH;
+
+    case AltHold_Landed_Pre_Takeoff:
+        attitude_control->reset_rate_controller_I_terms_smoothly();
+        loiter_nav->init_target();
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
+        pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
+        pos_control->update_z_controller();
+        break;
+
+    case AltHold_Flying:
+        // set motors to full range
+        motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+
+        // run loiter controller
+        loiter_nav->update();
+
+        // call attitude controller
+        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
+
+        // adjust climb rate using rangefinder
+        target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
+
+        // get avoidance adjusted climb rate
+        target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
+
+        pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
+        pos_control->update_z_controller();
+        break;
+    }
 }
 
 // return true if vehicle is within a small area around the destination

ArduCopter/sensors.cpp

@@ -33,7 +33,7 @@ void Copter::read_rangefinder(void)
 #if RANGEFINDER_TILT_CORRECTION == ENABLED
     const float tilt_correction = MAX(0.707f, ahrs.get_rotation_body_to_ned().c.z);
 #else
-    const float tile_correction = 1.0f;
+    const float tilt_correction = 1.0f;
 #endif
 
     // iterate through downward and upward facing lidar
@@ -199,7 +199,6 @@ void Copter::init_proximity(void)
 {
 #if PROXIMITY_ENABLED == ENABLED
     g2.proximity.init();
-    g2.proximity.set_rangefinder(&rangefinder);
 #endif
 }
 

ArduCopter/surface_tracking.cpp

@@ -7,9 +7,9 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
 {
 #if RANGEFINDER_ENABLED == ENABLED
     // check tracking state and that range finders are healthy
-    if ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_DISABLED) ||
-        ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) && (!copter.rangefinder_alt_ok() || (copter.rangefinder_state.glitch_count != 0))) ||
-        ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) && !copter.rangefinder_up_ok()) || (copter.rangefinder_up_state.glitch_count != 0)) {
+    if ((surface == Surface::NONE) ||
+        ((surface == Surface::GROUND) && (!copter.rangefinder_alt_ok() || (copter.rangefinder_state.glitch_count != 0))) ||
+        ((surface == Surface::CEILING) && !copter.rangefinder_up_ok()) || (copter.rangefinder_up_state.glitch_count != 0)) {
         return target_rate;
     }
 
@@ -17,8 +17,8 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
     const float current_alt_error = copter.pos_control->get_alt_target() - copter.inertial_nav.get_altitude();
 
     // init based on tracking direction/state
-    RangeFinderState &rf_state = (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
-    const float dir = (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? 1.0f : -1.0f;
+    RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
+    const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
 
     // reset target altitude if this controller has just been engaged
     // target has been changed between upwards vs downwards
@@ -39,11 +39,13 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
     }
     valid_for_logging = true;
 
+#if AC_AVOID_ENABLED == ENABLED
     // upward facing terrain following never gets closer than avoidance margin
-    if (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) {
+    if (surface == Surface::CEILING) {
         const float margin_cm = copter.avoid.enabled() ? copter.avoid.get_margin() * 100.0f : 0.0f;
         target_dist_cm = MAX(target_dist_cm, margin_cm);
     }
+#endif
 
     // calc desired velocity correction from target rangefinder alt vs actual rangefinder alt (remove the error already passed to Altitude controller to avoid oscillations)
     const float distance_error = (target_dist_cm - rf_state.alt_cm) - (dir * current_alt_error);
@@ -62,7 +64,7 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate)
 bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
 {
     // fail if we are not tracking downwards
-    if (tracking_state != SurfaceTrackingState::SURFACE_TRACKING_GROUND) {
+    if (surface != Surface::GROUND) {
         return false;
     }
     // check target has been updated recently
@@ -77,40 +79,45 @@ bool Copter::SurfaceTracking::get_target_alt_cm(float &_target_alt_cm) const
 void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
 {
     // fail if we are not tracking downwards
-    if (tracking_state != SurfaceTrackingState::SURFACE_TRACKING_GROUND) {
+    if (surface != Surface::GROUND) {
         return;
     }
     target_dist_cm = _target_alt_cm;
     last_update_ms = AP_HAL::millis();
 }
 
-bool Copter::SurfaceTracking::get_dist_for_logging(float &target_dist, float &actual_dist) const
+bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) const
 {
-    if (!valid_for_logging || (tracking_state == SurfaceTrackingState::SURFACE_TRACKING_DISABLED)) {
+    if (!valid_for_logging || (surface == Surface::NONE)) {
         return false;
     }
+
     target_dist = target_dist_cm * 0.01f;
-    actual_dist = ((tracking_state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) ? copter.rangefinder_state.alt_cm : copter.rangefinder_up_state.alt_cm) * 0.01f;
     return true;
 }
 
-// set direction
-void Copter::SurfaceTracking::set_state(SurfaceTrackingState state)
+float Copter::SurfaceTracking::get_dist_for_logging() const
 {
-    if (tracking_state == state) {
+    return ((surface == Surface::CEILING) ? copter.rangefinder_up_state.alt_cm : copter.rangefinder_state.alt_cm) * 0.01f;
+}
+
+// set direction
+void Copter::SurfaceTracking::set_surface(Surface new_surface)
+{
+    if (surface == new_surface) {
         return;
     }
     // check we have a range finder in the correct direction
-    if ((state == SurfaceTrackingState::SURFACE_TRACKING_GROUND) && !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
+    if ((new_surface == Surface::GROUND) && !copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
         copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no downward rangefinder");
         AP_Notify::events.user_mode_change_failed = 1;
         return;
     }
-    if ((state == SurfaceTrackingState::SURFACE_TRACKING_CEILING) && !copter.rangefinder.has_orientation(ROTATION_PITCH_90)) {
+    if ((new_surface == Surface::CEILING) && !copter.rangefinder.has_orientation(ROTATION_PITCH_90)) {
         copter.gcs().send_text(MAV_SEVERITY_WARNING, "SurfaceTracking: no upward rangefinder");
         AP_Notify::events.user_mode_change_failed = 1;
         return;
     }
-    tracking_state = state;
+    surface = new_surface;
     reset_target = true;
 }

ArduCopter/system.cpp

@@ -1,4 +1,5 @@
 #include "Copter.h"
+#include <AP_BLHeli/AP_BLHeli.h>
 
 /*****************************************************************************
 *   The init_ardupilot function processes everything we need for an in - air restart
@@ -68,8 +69,10 @@ void Copter::init_ardupilot()
     g2.gripper.init();
 #endif
 
+#if AC_FENCE == ENABLED
     fence.init();
-
+#endif
+    
     // init winch and wheel encoder
     winch_init();
 
@@ -229,9 +232,7 @@ void Copter::init_ardupilot()
     startup_INS_ground();
 
 #ifdef ENABLE_SCRIPTING
-    if (!g2.scripting.init()) {
-        gcs().send_text(MAV_SEVERITY_ERROR, "Scripting failed to start");
-    }
+    g2.scripting.init();
 #endif // ENABLE_SCRIPTING
 
     // set landed flags
@@ -256,6 +257,8 @@ void Copter::init_ardupilot()
     // disable safety if requested
     BoardConfig.init_safety();
 
+    vehicle_setup();
+
     hal.console->printf("\nReady to FLY ");
 
     // flag that initialisation has completed
@@ -294,18 +297,31 @@ void Copter::update_dynamic_notch()
         return;
     }
 
-    if (is_tradheli()) {
+    switch (ins.get_gyro_harmonic_notch_tracking_mode()) {
+        case HarmonicNotch_UpdateThrottle: // throttle based tracking
+            // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
+            ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref)));
+            break;
+
 #if RPM_ENABLED == ENABLED
-        if (rpm_sensor.healthy(0)) {
-            // set the harmonic notch filter frequency from the main rotor rpm
-            ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm_sensor.get_rpm(0) * ref / 60.0f));
-        } else {
-            ins.update_harmonic_notch_freq_hz(ref_freq);
-        }
+        case HarmonicNotch_UpdateRPM: // rpm sensor based tracking
+            if (rpm_sensor.healthy(0)) {
+                // set the harmonic notch filter frequency from the main rotor rpm
+                ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm_sensor.get_rpm(0) * ref / 60.0f));
+            } else {
+                ins.update_harmonic_notch_freq_hz(ref_freq);
+            }
+            break;
 #endif
-    } else {
-        // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
-        ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref)));
+#ifdef HAVE_AP_BLHELI_SUPPORT
+        case HarmonicNotch_UpdateBLHeli: // BLHeli based tracking
+            ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP_BLHeli::get_singleton()->get_average_motor_frequency_hz() * ref));
+            break;
+#endif
+        case HarmonicNotch_Fixed: // static
+        default:
+            ins.update_harmonic_notch_freq_hz(ref_freq);
+            break;
     }
 }
 
@@ -630,31 +646,6 @@ void Copter::allocate_motors(void)
         g.rc_speed.set_default(16000);
     }
     
-    if (upgrading_frame_params) {
-        // do frame specific upgrade. This is only done the first time we run the new firmware
-#if FRAME_CONFIG == HELI_FRAME
-        SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 12, CH_1);
-        SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 13, CH_2);
-        SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 14, CH_3);
-        SRV_Channels::upgrade_motors_servo(Parameters::k_param_motors, 15, CH_4);
-#else
-        if (g2.frame_class == AP_Motors::MOTOR_FRAME_TRI) {
-            const AP_Param::ConversionInfo tri_conversion_info[] = {
-                { Parameters::k_param_motors, 32, AP_PARAM_INT16, "SERVO7_TRIM" },
-                { Parameters::k_param_motors, 33, AP_PARAM_INT16, "SERVO7_MIN" },
-                { Parameters::k_param_motors, 34, AP_PARAM_INT16, "SERVO7_MAX" },
-                { Parameters::k_param_motors, 35, AP_PARAM_FLOAT, "MOT_YAW_SV_ANGLE" },
-            };
-            // we need to use CONVERT_FLAG_FORCE as the SERVO7_* parameters will already be set from RC7_*
-            AP_Param::convert_old_parameters(tri_conversion_info, ARRAY_SIZE(tri_conversion_info), AP_Param::CONVERT_FLAG_FORCE);
-            const AP_Param::ConversionInfo tri_conversion_info_rev { Parameters::k_param_motors, 31, AP_PARAM_INT8,  "SERVO7_REVERSED" };
-            AP_Param::convert_old_parameter(&tri_conversion_info_rev, 1, AP_Param::CONVERT_FLAG_REVERSE | AP_Param::CONVERT_FLAG_FORCE);
-            // AP_MotorsTri was converted from having nested var_info to one level
-            AP_Param::convert_parent_class(Parameters::k_param_motors, motors, motors->var_info);
-        }
-#endif
-    }
-
     // upgrade parameters. This must be done after allocating the objects
     convert_pid_parameters();
 #if FRAME_CONFIG == HELI_FRAME

ArduCopter/toy_mode.cpp

@@ -205,7 +205,7 @@ void ToyMode::update()
     
     if (!done_first_update) {
         done_first_update = true;
-        copter.set_mode(Mode::Number(primary_mode[0].get()), MODE_REASON_TMODE);
+        copter.set_mode(Mode::Number(primary_mode[0].get()), ModeReason::TOY_MODE);
         copter.motors->set_thrust_compensation_callback(FUNCTOR_BIND_MEMBER(&ToyMode::thrust_limiting, void, float *, uint8_t));
     }
 
@@ -428,7 +428,7 @@ void ToyMode::update()
                 /*
                   support auto-switching to ALT_HOLD, then upgrade to LOITER once GPS available
                  */
-                if (set_and_remember_mode(Mode::Number::ALT_HOLD, MODE_REASON_TMODE)) {
+                if (set_and_remember_mode(Mode::Number::ALT_HOLD, ModeReason::TOY_MODE)) {
                     gcs().send_text(MAV_SEVERITY_INFO, "Tmode: ALT_HOLD update arm");
 #if AC_FENCE == ENABLED
                     copter.fence.enable(false);
@@ -436,7 +436,7 @@ void ToyMode::update()
                     if (!copter.arming.arm(AP_Arming::Method::MAVLINK)) {
                         // go back to LOITER
                         gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: ALT_HOLD arm failed");
-                        set_and_remember_mode(Mode::Number::LOITER, MODE_REASON_TMODE);
+                        set_and_remember_mode(Mode::Number::LOITER, ModeReason::TOY_MODE);
                     } else {
                         upgrade_to_loiter = true;
 #if 0
@@ -458,7 +458,7 @@ void ToyMode::update()
 #if 0
             AP_Notify::flags.hybrid_loiter = false;
 #endif
-        } else if (copter.position_ok() && set_and_remember_mode(Mode::Number::LOITER, MODE_REASON_TMODE)) {
+        } else if (copter.position_ok() && set_and_remember_mode(Mode::Number::LOITER, ModeReason::TOY_MODE)) {
 #if AC_FENCE == ENABLED
             copter.fence.enable(true);
 #endif
@@ -468,7 +468,7 @@ void ToyMode::update()
 
     if (copter.control_mode == Mode::Number::RTL && (flags & FLAG_RTL_CANCEL) && throttle_near_max) {
         gcs().send_text(MAV_SEVERITY_INFO, "Tmode: RTL cancel");        
-        set_and_remember_mode(Mode::Number::LOITER, MODE_REASON_TMODE);
+        set_and_remember_mode(Mode::Number::LOITER, ModeReason::TOY_MODE);
     }
     
     enum Mode::Number old_mode = copter.control_mode;
@@ -619,9 +619,9 @@ void ToyMode::update()
             load_test.running = false;
             gcs().send_text(MAV_SEVERITY_INFO, "Tmode: load_test off");
             copter.init_disarm_motors();
-            copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_TMODE);
+            copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::TOY_MODE);
         } else {
-            copter.set_mode(Mode::Number::ALT_HOLD, MODE_REASON_TMODE);
+            copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::TOY_MODE);
 #if AC_FENCE == ENABLED
             copter.fence.enable(false);
 #endif
@@ -646,7 +646,7 @@ void ToyMode::update()
 #if AC_FENCE == ENABLED
         copter.fence.enable(false);
 #endif
-        if (set_and_remember_mode(new_mode, MODE_REASON_TX_COMMAND)) {
+        if (set_and_remember_mode(new_mode, ModeReason::TOY_MODE)) {
             gcs().send_text(MAV_SEVERITY_INFO, "Tmode: mode %s", copter.flightmode->name4());
             // force fence on in all GPS flight modes
 #if AC_FENCE == ENABLED
@@ -659,7 +659,7 @@ void ToyMode::update()
             if (new_mode == Mode::Number::RTL) {
                 // if we can't RTL then land
                 gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: LANDING");
-                set_and_remember_mode(Mode::Number::LAND, MODE_REASON_TMODE);
+                set_and_remember_mode(Mode::Number::LAND, ModeReason::TOY_MODE);
 #if AC_FENCE == ENABLED
                 if (copter.landing_with_GPS()) {
                     copter.fence.enable(true);
@@ -674,7 +674,7 @@ void ToyMode::update()
 /*
   set a mode, remembering what mode we set, and the previous mode we were in
  */
-bool ToyMode::set_and_remember_mode(Mode::Number mode, mode_reason_t reason)
+bool ToyMode::set_and_remember_mode(Mode::Number mode, ModeReason reason)
 {
     if (copter.control_mode == mode) {
         return true;

ArduCopter/toy_mode.h

@@ -38,7 +38,7 @@ private:
     void action_arm(void);
     void blink_update(void);
     void send_named_int(const char *name, int32_t value);
-    bool set_and_remember_mode(Mode::Number mode, mode_reason_t reason);
+    bool set_and_remember_mode(Mode::Number mode, ModeReason reason);
 
     void thrust_limiting(float *thrust, uint8_t num_motors);
     void arm_check_compass(void);

ArduCopter/version.h

@@ -6,12 +6,13 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ArduCopter V4.0.0-dev"
+#define THISFIRMWARE "ArduCopter V4.0.8"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,0,0,FIRMWARE_VERSION_TYPE_DEV
+#define FIRMWARE_VERSION 4,0,8,FIRMWARE_VERSION_TYPE_OFFICIAL+8
 
 #define FW_MAJOR 4
 #define FW_MINOR 0
-#define FW_PATCH 0
-#define FW_TYPE FIRMWARE_VERSION_TYPE_DEV
+#define FW_PATCH 8
+#define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL
+

ArduCopter/wscript

@@ -9,9 +9,9 @@ def build(bld):
         ap_libraries=bld.ap_common_vehicle_libraries() + [
             'AC_AttitudeControl',
             'AC_InputManager',
-            'AC_PID',
             'AC_PrecLand',
             'AC_Sprayer',
+            'AC_Autorotation',
             'AC_WPNav',
             'AP_Camera',
             'AP_IRLock',

ArduPlane/AP_Arming.cpp

@@ -58,7 +58,7 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
     }
 
     if (plane.channel_throttle->get_reverse() && 
-        plane.g.throttle_fs_enabled &&
+        Plane::ThrFailsafe(plane.g.throttle_fs_enabled.get()) != Plane::ThrFailsafe::Disabled &&
         plane.g.throttle_fs_value < 
         plane.channel_throttle->get_radio_max()) {
         check_failed(display_failure, "Invalid THR_FS_VALUE for rev throttle");
@@ -88,6 +88,21 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure)
         }
     }
 
+#if AP_TERRAIN_AVAILABLE
+    if (plane.g.terrain_follow || plane.mission.contains_terrain_relative()) {
+        // check terrain data is loaded and healthy
+        uint16_t terr_pending=0, terr_loaded=0;
+        plane.terrain.get_statistics(terr_pending, terr_loaded);
+        if (plane.terrain.status() != AP_Terrain::TerrainStatusOK) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "terrain data unhealthy");
+            ret = false;
+        } else if (terr_pending != 0) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "waiting for terrain data");
+            ret = false;
+        }
+    }
+#endif
+    
     if (plane.control_mode == &plane.mode_auto && plane.mission.num_commands() <= 1) {
         check_failed(display_failure, "No mission loaded");
         ret = false;

ArduPlane/ArduPlane.cpp

@@ -52,7 +52,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(afs_fs_check,           10,    100),
 #endif
     SCHED_TASK_CLASS(GCS,            (GCS*)&plane._gcs,       update_receive,   300,  500),
-    SCHED_TASK_CLASS(GCS,            (GCS*)&plane._gcs,       update_send,      300,  500),
+    SCHED_TASK_CLASS(GCS,            (GCS*)&plane._gcs,       update_send,      300,  750),
     SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events,          50,  150),
     SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300),
     SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150),
@@ -183,7 +183,7 @@ void Plane::update_speed_height(void)
 
     if (quadplane.in_vtol_mode() ||
         quadplane.in_assisted_flight()) {
-        quadplane.update_throttle_thr_mix();
+        quadplane.update_throttle_mix();
     }
 }
 
@@ -462,6 +462,7 @@ void Plane::update_navigation()
             // we've reached the RTL point, see if we have a landing sequence
             if (mission.jump_to_landing_sequence()) {
                 // switch from RTL -> AUTO
+                mission.set_force_resume(true);
                 set_mode(mode_auto, ModeReason::UNKNOWN);
             }
 
@@ -474,6 +475,7 @@ void Plane::update_navigation()
             // Go directly to the landing sequence
             if (mission.jump_to_landing_sequence()) {
                 // switch from RTL -> AUTO
+                mission.set_force_resume(true);
                 set_mode(mode_auto, ModeReason::UNKNOWN);
             }
 
@@ -544,6 +546,10 @@ void Plane::update_alt()
 {
     barometer.update();
 
+    if (quadplane.available()) {
+        quadplane.motors->set_air_density_ratio(barometer.get_air_density_ratio());
+    }
+
     // calculate the sink rate.
     float sink_rate;
     Vector3f vel;
@@ -578,7 +584,16 @@ void Plane::update_alt()
         }
 #endif
         
-        SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
+        float target_alt = relative_target_altitude_cm();
+
+        if (control_mode == &mode_rtl && !rtl.done_climb && g2.rtl_climb_min > 0) {
+            // 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 + (g2.rtl_climb_min+10)*100);
+        }
+
+        SpdHgt_Controller->update_pitch_throttle(target_alt,
                                                  target_airspeed_cm,
                                                  flight_stage,
                                                  distance_beyond_land_wp,

ArduPlane/GCS_Mavlink.cpp

@@ -93,7 +93,7 @@ uint32_t GCS_Plane::custom_mode() const
     return plane.control_mode->mode_number();
 }
 
-MAV_STATE GCS_MAVLINK_Plane::system_status() const
+MAV_STATE GCS_MAVLINK_Plane::vehicle_system_status() const
 {
     if (plane.control_mode == &plane.mode_initializing) {
         return MAV_STATE_CALIBRATING;

ArduPlane/GCS_Mavlink.h

@@ -55,7 +55,7 @@ private:
     void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override;
 
     MAV_MODE base_mode() const override;
-    MAV_STATE system_status() const override;
+    MAV_STATE vehicle_system_status() const override;
 
     uint8_t radio_in_rssi() const;
 

ArduPlane/Parameters.cpp

@@ -460,10 +460,10 @@ const AP_Param::Info Plane::var_info[] = {
 
     // @Param: THR_FAILSAFE
     // @DisplayName: Throttle and RC Failsafe Enable
-    // @Description: This enables failsafe on loss of RC input. How this is detected depends on the type of RC receiver being used. For older radios an input below the THR_FS_VALUE is used to trigger failsafe. For newer radios the failsafe trigger is part of the protocol between the autopilot and receiver.
-    // @Values: 0:Disabled,1:Enabled
+    // @Description: This enables failsafe on loss of RC input. How this is detected depends on the type of RC receiver being used. For older radios an input below the THR_FS_VALUE is used to trigger failsafe. For newer radios the failsafe trigger is part of the protocol between the autopilot and receiver. A value of 2 means that the RC input won't be used when throttle goes below the THR_FS_VALUE, but it won't trigger a failsafe
+    // @Values: 0:Disabled,1:Enabled,2:EnabledNoFailsafe
     // @User: Standard
-    GSCALAR(throttle_fs_enabled,    "THR_FAILSAFE",   1),
+    GSCALAR(throttle_fs_enabled,    "THR_FAILSAFE",   int(ThrFailsafe::Enabled)),
 
 
     // @Param: THR_FS_VALUE
@@ -1239,6 +1239,38 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("DSPOILER_AILMTCH", 21, ParametersG2, crow_flap_aileron_matching, 100),
 
+    // @Param: FWD_BAT_VOLT_MAX
+    // @DisplayName: Forward throttle battery voltage compensation maximum voltage
+    // @Description: Forward throttle battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust).  Recommend 4.4 * cell count, 0 = Disabled
+    // @Range: 6 35
+    // @Units: V
+    // @User: Advanced
+    AP_GROUPINFO("FWD_BAT_VOLT_MAX", 23, ParametersG2, fwd_thr_batt_voltage_max, 0.0f),
+
+    // @Param: FWD_BAT_VOLT_MIN
+    // @DisplayName: Forward throttle battery voltage compensation minimum voltage
+    // @Description: Forward throttle battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust).  Recommend 3.5 * cell count, 0 = Disabled
+    // @Range: 6 35
+    // @Units: V
+    // @User: Advanced
+    AP_GROUPINFO("FWD_BAT_VOLT_MIN", 24, ParametersG2, fwd_thr_batt_voltage_min, 0.0f),
+
+    // @Param: FWD_BAT_IDX
+    // @DisplayName: Forward throttle battery compensation index
+    // @Description: Which battery monitor should be used for doing compensation for the forward throttle
+    // @Values: 0:First battery, 1:Second battery
+    // @User: Advanced
+    AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_thr_batt_idx, 0),
+
+    // @Param: RTL_CLIMB_MIN
+    // @DisplayName: RTL minimum climb
+    // @Description: The vehicle will climb this many m during the initial climb portion of the RTL. During this time the roll will be limited to LEVEL_ROLL_LIMIT degrees.
+    // @Units: m
+    // @Range: 0 30
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("RTL_CLIMB_MIN", 27, ParametersG2, rtl_climb_min, 0),
+
     AP_GROUPEND
 };
 
@@ -1340,15 +1372,7 @@ void Plane::load_parameters(void)
     g2.servo_channels.set_default_function(CH_3, SRV_Channel::k_throttle);
     g2.servo_channels.set_default_function(CH_4, SRV_Channel::k_rudder);
         
-    const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old,  Parameters::k_param_rc_2_old,
-                                      Parameters::k_param_rc_3_old,  Parameters::k_param_rc_4_old,
-                                      Parameters::k_param_rc_5_old,  Parameters::k_param_rc_6_old,
-                                      Parameters::k_param_rc_7_old,  Parameters::k_param_rc_8_old,
-                                      Parameters::k_param_rc_9_old,  Parameters::k_param_rc_10_old,
-                                      Parameters::k_param_rc_11_old, Parameters::k_param_rc_12_old,
-                                      Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old };
-    const uint16_t old_aux_chan_mask = 0x3FF0;
-    SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, &rcmap);
+    SRV_Channels::upgrade_parameters();
 
     // possibly convert elevon and vtail mixers
     convert_mixers();

ArduPlane/Parameters.h

@@ -564,6 +564,14 @@ public:
     AP_Int8 crow_flap_weight_inner;
     AP_Int8 crow_flap_options;
     AP_Int8 crow_flap_aileron_matching;
+
+    // Forward throttle battery voltage compenstaion
+    AP_Float fwd_thr_batt_voltage_max;
+    AP_Float fwd_thr_batt_voltage_min;
+    AP_Int8  fwd_thr_batt_idx;
+
+    // min initial climb in RTL
+    AP_Int16        rtl_climb_min;
 };
 
 extern const AP_Param::Info var_info[];

ArduPlane/Plane.h

@@ -537,8 +537,7 @@ private:
 #if LANDING_GEAR_ENABLED == ENABLED
     // landing gear state
     struct {
-        int8_t last_auto_cmd;
-        int8_t last_cmd;
+        AP_Vehicle::FixedWing::FlightStage last_flight_stage;
     } gear;
 #endif
     
@@ -747,6 +746,10 @@ private:
         uint32_t last_trim_save;
     } auto_trim;
 
+    struct {
+        bool done_climb;
+    } rtl;
+
     // last time home was updated while disarmed
     uint32_t last_home_update_ms;
 
@@ -946,13 +949,13 @@ private:
     void set_servos_controlled(void);
     void set_servos_old_elevons(void);
     void set_servos_flaps(void);
-    void change_landing_gear(AP_LandingGear::LandingGearCommand cmd);
     void set_landing_gear(void);
     void dspoiler_update(void);
     void servo_output_mixers(void);
     void servos_output(void);
     void servos_auto_trim(void);
     void servos_twin_engine_mix();
+    void throttle_voltage_comp();
     void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle);
     void update_is_flying_5Hz(void);
     void crash_detection_update(void);
@@ -1053,6 +1056,12 @@ private:
     static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
                   "_failsafe_priorities is missing the sentinel");
 
+    enum class ThrFailsafe {
+        Disabled    = 0,
+        Enabled     = 1,
+        EnabledNoFS = 2
+    };
+
 public:
     void mavlink_delay_cb();
     void failsafe_check(void);

ArduPlane/RC_Channel.cpp

@@ -18,7 +18,7 @@ int8_t RC_Channels_Plane::flight_mode_channel_number() const
 
 bool RC_Channels_Plane::has_valid_input() const
 {
-    if (plane.failsafe.rc_failsafe) {
+    if (plane.rc_failsafe_active() || plane.failsafe.rc_failsafe) {
         return false;
     }
     if (plane.failsafe.throttle_counter != 0) {

ArduPlane/RC_Channel.h

@@ -25,7 +25,7 @@ public:
 
     RC_Channel_Plane obj_channels[NUM_RC_CHANNELS];
     RC_Channel_Plane *channel(const uint8_t chan) override {
-        if (chan > NUM_RC_CHANNELS) {
+        if (chan >= NUM_RC_CHANNELS) {
             return nullptr;
         }
         return &obj_channels[chan];

ArduPlane/events.cpp

@@ -171,7 +171,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
             }
             FALLTHROUGH;
         case Failsafe_Action_RTL:
-            if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland ) {
+            if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND && control_mode != &mode_qland && !quadplane.in_vtol_land_sequence()) {
                 // never stop a landing if we were already committed
                 set_mode(mode_rtl, ModeReason::BATTERY_FAILSAFE);
                 aparm.throttle_cruise.load();

ArduPlane/geofence.cpp

@@ -160,6 +160,11 @@ bool Plane::geofence_present(void)
  */
 void Plane::geofence_update_pwm_enabled_state() 
 {
+    if (rc_failsafe_active()) {
+        // do nothing based on the radio channel value as it may be at bind value
+        return;
+    }
+
     bool is_pwm_enabled;
     if (g.fence_channel == 0) {
         is_pwm_enabled = false;
@@ -494,7 +499,8 @@ void Plane::geofence_send_status(mavlink_channel_t chan)
                                       (int8_t)geofence_state->fence_triggered,
                                       geofence_state->breach_count,
                                       geofence_state->breach_type,
-                                      geofence_state->breach_time);
+                                      geofence_state->breach_time,
+                                      0);
     }
 }
 

ArduPlane/mode_rtl.cpp

@@ -8,6 +8,7 @@ bool ModeRTL::_enter()
     plane.auto_navigation_mode = true;
     plane.prev_WP_loc = plane.current_loc;
     plane.do_RTL(plane.get_RTL_altitude());
+    plane.rtl.done_climb = false;
 
     return true;
 }
@@ -17,5 +18,19 @@ void ModeRTL::update()
     plane.calc_nav_roll();
     plane.calc_nav_pitch();
     plane.calc_throttle();
+
+    if (plane.g2.rtl_climb_min > 0) {
+        /*
+          when RTL first starts limit bank angle to LEVEL_ROLL_LIMIT
+          until we have climbed by RTL_CLIMB_MIN meters
+         */
+        if (!plane.rtl.done_climb && (plane.current_loc.alt - plane.prev_WP_loc.alt)*0.01 > plane.g2.rtl_climb_min) {
+            plane.rtl.done_climb = true;
+        }
+        if (!plane.rtl.done_climb) {
+            plane.roll_limit_cd = MIN(plane.roll_limit_cd, plane.g.level_roll_limit*100);
+            plane.nav_roll_cd = constrain_int32(plane.nav_roll_cd, -plane.roll_limit_cd, plane.roll_limit_cd);
+        }
+    }
 }
 

ArduPlane/mode_takeoff.cpp

@@ -65,7 +65,7 @@ void ModeTakeoff::update()
 {
     if (!takeoff_started) {
         // see if we will skip takeoff as already flying
-        if (plane.is_flying() && plane.ahrs.groundspeed() > 3) {
+        if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && plane.ahrs.groundspeed() > 3) {
             gcs().send_text(MAV_SEVERITY_INFO, "Takeoff skipped - circling");
             plane.prev_WP_loc = plane.current_loc;
             plane.next_WP_loc = plane.current_loc;

ArduPlane/navigation.cpp

@@ -36,8 +36,9 @@ void Plane::loiter_angle_update(void)
 
     const int32_t target_bearing_cd = nav_controller->target_bearing_cd();
     int32_t loiter_delta_cd;
+    const bool reached_target = reached_loiter_target();
 
-    if (loiter.sum_cd == 0 && !reached_loiter_target()) {
+    if (loiter.sum_cd == 0 && !reached_target) {
         // we don't start summing until we are doing the real loiter
         loiter_delta_cd = 0;
     } else if (loiter.sum_cd == 0) {
@@ -53,7 +54,35 @@ void Plane::loiter_angle_update(void)
     loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
     loiter.sum_cd += loiter_delta_cd * loiter.direction;
 
-    if (labs(current_loc.alt - next_WP_loc.alt) < 500) {
+    bool reached_target_alt = false;
+
+    if (reached_target) {
+        // once we reach the position target we start checking the
+        // altitude target
+        bool terrain_status_ok = false;
+#if AP_TERRAIN_AVAILABLE
+        /*
+          if doing terrain following then we check against terrain
+          target, fetch the terrain information
+        */
+        float altitude_agl = 0;
+        if (target_altitude.terrain_following) {
+            if (terrain.status() == AP_Terrain::TerrainStatusOK &&
+                terrain.height_above_terrain(altitude_agl, true)) {
+                terrain_status_ok = true;
+            }
+        }
+        if (terrain_status_ok &&
+            fabsf(altitude_agl - target_altitude.terrain_alt_cm*0.01) < 5) {
+            reached_target_alt = true;
+        } else
+#endif
+        if (!terrain_status_ok && labs(current_loc.alt - target_altitude.amsl_cm) < 500) {
+            reached_target_alt = true;
+        }
+    }
+
+    if (reached_target_alt) {
         loiter.reached_target_alt = true;
         loiter.unable_to_acheive_target_alt = false;
         loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd;

ArduPlane/quadplane.cpp

@@ -461,6 +461,15 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @User: Advanced
     AP_GROUPINFO("TKOFF_ARSP_LIM", 15, QuadPlane, maximum_takeoff_airspeed, 0),
 
+    // @Param: ASSIST_ALT
+    // @DisplayName: Quadplane assistance altitude
+    // @Description: This is the altitude below which quadplane assistance will be triggered. This acts the same way as Q_ASSIST_ANGLE and Q_ASSIST_SPEED, but triggers if the aircraft drops below the given altitude while the VTOL motors are not running. A value of zero disables this feature. The altutude is calculated as being above ground level. The height above ground is given from a Lidar used if available and RNGFND_LANDING=1. Otherwise it comes from terrain data if TERRAIN_FOLLOW=1 and comes from height above home otherwise.
+    // @Units: m
+    // @Range: 0 120
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("ASSIST_ALT", 16, QuadPlane, assist_alt, 0),
+
     AP_GROUPEND
 };
 
@@ -1081,6 +1090,9 @@ void QuadPlane::init_qland(void)
     poscontrol.state = QPOS_LAND_DESCEND;
     landing_detect.lower_limit_start_ms = 0;
     landing_detect.land_start_ms = 0;
+#if LANDING_GEAR_ENABLED == ENABLED
+    plane.g2.landing_gear.deploy_for_landing();
+#endif
 }
 
 
@@ -1377,6 +1389,7 @@ bool QuadPlane::assistance_needed(float aspeed)
         angle_error_start_ms = 0;
         return false;
     }
+
     if (aspeed < assist_speed) {
         // assistance due to Q_ASSIST_SPEED
         in_angle_assist = false;
@@ -1384,6 +1397,31 @@ bool QuadPlane::assistance_needed(float aspeed)
         return true;
     }
 
+    const uint32_t now = AP_HAL::millis();
+
+    /*
+      optional assistance when altitude is too close to the ground
+     */
+    if (assist_alt > 0) {
+        float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
+        if (height_above_ground < assist_alt) {
+            if (alt_error_start_ms == 0) {
+                alt_error_start_ms = now;
+            }
+            if (now - alt_error_start_ms > 500) {
+                // we've been below assistant alt for 0.5s
+                if (!in_alt_assist) {
+                    in_alt_assist = true;
+                    gcs().send_text(MAV_SEVERITY_INFO, "Alt assist %.1fm", height_above_ground);
+                }
+                return true;
+            }
+        } else {
+            in_alt_assist = false;
+            alt_error_start_ms = 0;
+        }
+    }
+
     if (assist_angle <= 0) {
         in_angle_assist = false;
         angle_error_start_ms = 0;
@@ -1412,7 +1450,7 @@ bool QuadPlane::assistance_needed(float aspeed)
         in_angle_assist = false;
         return false;
     }
-    const uint32_t now = AP_HAL::millis();
+
     if (angle_error_start_ms == 0) {
         angle_error_start_ms = now;
     }
@@ -1567,7 +1605,7 @@ void QuadPlane::update_transition(void)
         plane.rollController.reset_I();
 
         // give full authority to attitude control
-        attitude_control->set_throttle_mix_max();
+        attitude_control->set_throttle_mix_max(1.0f);
         break;
     }
         
@@ -2329,6 +2367,10 @@ void QuadPlane::setup_target_position(void)
     // setup vertical speed and acceleration
     pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
     pos_control->set_max_accel_z(pilot_accel_z);
+
+    // setup horizontal speed and acceleration
+    pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy());
+    pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());
 }
 
 /*
@@ -2594,6 +2636,13 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
     set_alt_target_current();
 
     plane.complete_auto_takeoff();
+
+    if (plane.control_mode == &plane.mode_auto) {
+        // we reset TECS so that the target height filter is not
+        // constrained by the climb and sink rates from the initial
+        // takeoff height.
+        plane.SpdHgt_Controller->reset();
+    }
     
     return true;
 }
@@ -2681,6 +2730,9 @@ bool QuadPlane::verify_vtol_land(void)
     if (poscontrol.state == QPOS_POSITION2 &&
         plane.auto_state.wp_distance < 2) {
         poscontrol.state = QPOS_LAND_DESCEND;
+#if LANDING_GEAR_ENABLED == ENABLED
+        plane.g2.landing_gear.deploy_for_landing();
+#endif
         gcs().send_text(MAV_SEVERITY_INFO,"Land descend started");
         if (plane.control_mode == &plane.mode_auto) {
             // set height to mission height, so we can use the mission
@@ -2936,6 +2988,11 @@ bool QuadPlane::guided_mode_enabled(void)
     if (plane.control_mode != &plane.mode_guided && plane.control_mode != &plane.mode_auto) {
         return false;
     }
+    if (plane.control_mode == &plane.mode_auto &&
+        plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_LOITER_TURNS) {
+        // loiter turns is a fixed wing only operation
+        return false;
+    }
     return guided_mode != 0;
 }
 
@@ -3059,7 +3116,7 @@ float QuadPlane::stopping_distance(void)
 #define LAND_CHECK_LARGE_ANGLE_CD   1500.0f     // maximum angle target to be considered landing
 #define LAND_CHECK_ACCEL_MOVING     3.0f        // maximum acceleration after subtracting gravity
 
-void QuadPlane::update_throttle_thr_mix(void)
+void QuadPlane::update_throttle_mix(void)
 {
     // transition will directly manage the mix
     if (in_transition()) {
@@ -3067,7 +3124,7 @@ void QuadPlane::update_throttle_thr_mix(void)
     }
 
     // if disarmed or landed prioritise throttle
-    if(!motors->armed()) {
+    if (!motors->armed()) {
         attitude_control->set_throttle_mix_min();
         return;
     }
@@ -3098,8 +3155,8 @@ void QuadPlane::update_throttle_thr_mix(void)
         // check for requested decent
         bool descent_not_demanded = pos_control->get_desired_velocity().z >= 0.0f;
 
-        if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
-            attitude_control->set_throttle_mix_max();
+        if (large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
+            attitude_control->set_throttle_mix_max(1.0);
         } else {
             attitude_control->set_throttle_mix_min();
         }
@@ -3137,3 +3194,11 @@ bool QuadPlane::in_vtol_land_final(void) const
 {
     return in_vtol_land_descent() && poscontrol.state == QPOS_LAND_FINAL;
 }
+
+/*
+  see if we are in any of the phases of a VTOL landing
+ */
+bool QuadPlane::in_vtol_land_sequence(void) const
+{
+    return in_vtol_land_approach() || in_vtol_land_descent() || in_vtol_land_final();
+}

ArduPlane/quadplane.h

@@ -53,7 +53,7 @@ public:
     void takeoff_controller(void);
     void waypoint_controller(void);
 
-    void update_throttle_thr_mix(void);
+    void update_throttle_mix(void);
     
     // update transition handling
     void update(void);
@@ -279,7 +279,12 @@ private:
     // angular error at which quad assistance is given
     AP_Int8 assist_angle;
     uint32_t angle_error_start_ms;
-    
+
+    // altitude to trigger assistance
+    AP_Int16 assist_alt;
+    uint32_t alt_error_start_ms;
+    bool in_alt_assist;
+
     // maximum yaw rate in degrees/second
     AP_Float yaw_rate_max;
 
@@ -548,6 +553,11 @@ private:
       are we in the final landing phase of a VTOL landing?
      */
     bool in_vtol_land_final(void) const;
+
+    /*
+      are we in any of the phases of a VTOL landing?
+     */
+    bool in_vtol_land_sequence(void) const;
     
 public:
     void motor_test_output();

ArduPlane/radio.cpp

@@ -179,6 +179,8 @@ void Plane::read_radio()
 {
     if (!rc().read_input()) {
         control_failsafe();
+        airspeed_nudge_cm = 0;
+        throttle_nudge = 0;
         return;
     }
 
@@ -280,7 +282,7 @@ void Plane::control_failsafe()
         }
     }
 
-    if(g.throttle_fs_enabled == 0) {
+    if (ThrFailsafe(g.throttle_fs_enabled.get()) != ThrFailsafe::Enabled) {
         return;
     }
 
@@ -373,7 +375,7 @@ bool Plane::trim_radio()
  */
 bool Plane::rc_throttle_value_ok(void) const
 {
-    if (!g.throttle_fs_enabled) {
+    if (ThrFailsafe(g.throttle_fs_enabled.get()) == ThrFailsafe::Disabled) {
         return true;
     }
     if (channel_throttle->get_reverse()) {

ArduPlane/release-notes.txt

@@ -1,3 +1,265 @@
+Release 4.0.6beta1, 23rd May 2020
+---------------------------------
+
+This is a major release with a significant number of new features and
+bug fixes.
+
+ - scripting generator bug fix
+ - changed LED scripting API to allow more than 32 LEDs on a pin
+ - added support for ProfiLED LEDs
+ - added u-blox GPS moving baseline u-blox auto-configuration
+ - fixed handling of GPS antenna positions on EKF GPS switch
+ - changed default USB IDs to new ArduPilot specific IDs
+ - fixed bug in handling trim for RC control of camera mounts
+ - added LGR_OPTIONS bits to control landing gear behaviour on takeoff/landing
+ - improved mavlink streaming output control to better allocate time to each channel
+ - fixed send of mavlink PARAM_VALUE on set of a readonly parameter
+ - fixed mag variance reporting in EKF_STATUS_REPORT mavlink message
+ - fixed time wrap bug in BMP085 barometer driver
+ - fixed buffer overflow in ST24 RC input driver
+ - fixed EKF usage of WMM tables when user has specified a specific
+   declination with COMPASS_DEC and COMPASS_AUTODEC
+ - fixed bug in AP_Terrain on-disk format
+ - added script for offline generation of terrain data
+ - severel improvements to smbus battery drivers
+ - fixed a race condition in parameter storage on ChibiOS
+ - fixed use of zero GNSS timestamp in UAVCAN GPS driver
+ - improved GCS messages during bootloader flash
+ - fixed CS pin in bootloader that could corrupt FRAM on some boards
+ - added GPS yaw to MAVLink GPS_RAW_INT message
+ - added Hott telemetry support
+ - added FRSky FPort support
+ - fixed bug in CAN clock and queue handling on H7 based boards
+ - added support for BRD_ALT_CONFIG for alternative hardware configs on several boards
+ - added new boards CUAV-Nora, CUAV-X7, MatekH743, R9Pilot, mRoNexus
+ - improved reporting of internal errors to GCS
+ - fixed recursion bug in tonealarm player
+ - fixed flaperon SERVO_AUTO_TRIM behaviour
+ - added option to compensate forward throttle for battery voltage
+ - added compensation in VTOL gains for pressure altitude
+ - switched to new more flexible compass ordering system
+ - fixed forcing of safety off on IOMCU reset
+ - increased maximum compass scale factor to 1.4
+
+
+Release 4.0.5, 4th March 2020
+-----------------------------
+
+This release includes a one important bug fix and some minor
+enhancements. The changes are:
+
+ - fixed bug handling change of RC input source while MAVLink signing
+   is active. This could cause a stack overflow and a crash
+
+ - added display of RC output types to aid in debugging DShot outputs
+
+ - modified clocking on H7 boards to produce more accurate clocks for
+   DShot
+
+ - switched to new USB VIDs for dual-CDC boards
+
+ - fixed a bug in handling LOITER_TURNS for quadplanes when
+   Q_GUIDED_MODE is enabled
+
+ - added a TECS reset at the end of a VTOL takeoff to handle aircraft
+   with TECS climb rates below VTOL climb rates
+
+Happy flying!
+
+Release 4.0.4, 16th February 2020
+---------------------------------
+
+This release includes a significant number of changes from 4.0.3. Key
+changes are:
+
+ - re-sync the 4.0 release with Copter-4.0, bringing them in line so
+   as to maximise cross-vehicle testing
+
+ - fixed a timing issue in IOMCU that could lead to loss of RC input
+   frames and increased latency of output to ESCs on the IOMCU
+
+ - fixed a bug in restoring gains in QAUTOTUNE that could cause the
+   aircraft to be unstable on leaving QAUTOTUNE mode
+
+ - fixed stack size of ftp thread
+
+The Copter-4.0 re-sync brings in quite a few structural changes. The
+main user visible changes are:
+
+ - UAVCAN DNA server no longer needs a microSD card to operate
+
+ - added logging of UAVCAN servos and ESC feedback messages
+
+ - reduced QTUN logging rate to 25Hz
+
+ - reduced memory usage in EKF with multiple lanes
+
+ - Minor OSD improvements
+
+ - added a lot more scripting bindings
+
+ - fixed UAVCAN GPS status when not connected
+
+ - added EK3_MAG_EF_LIM limit for earth frame mag errors
+
+ - added MAVLink FTP support
+
+ - added support for WS2812 LEDs
+
+Due to the significant number of changes with the re-sync I would
+particularly appreciate as much flight testing as we can get on this
+release.
+
+Happy flying!
+
+Release 4.0.3, 21st January 2020
+--------------------------------
+
+This is a minor release with a few bug fixes and enhancements. The
+changes since beta1 are:
+
+ - fixed 3 missing semaphore waits
+ - fixed checking for bouncebuffer allocation on microSD card IO
+ - fixed incorrect param count
+ - prevent failsafe action from overriding a VTOL land
+ - fixed compass calibration failures with auto-rotation detection
+ - fixed errors on STM32H7 I2C (affects CubeOrange and Durandal)
+ - fixed a race condition in FrSky passthrough telemetry
+ - fixed DSM/Spektrum parsing for 22ms protocols
+ - added fixed yaw compass calibration method
+ - re-generated magnetic field tables
+ - ensure SERIAL0_PROTOCOL is mavlink on boot
+
+The most important fix is for FrSky pass-through telemetry. Pass
+through telemetry support had a race condition which could lead to the
+flight controller generating a fault and rebooting. Until you are
+running a firmware with the fix you should disable FrSky pass-through
+telemetry by changing SERIALn_PROTOCOL from 10 to 0 on the where you
+have SPort enabled.
+
+Happy flying!
+
+Release 4.0.2, 30th December 2019
+---------------------------------
+
+This is a minor release with a few bug fixes and enhancements. Changes
+are:
+
+ - fixed voltage scaling on CUAVv5Nano
+ - fixed 10Hz NMEA output
+ - fixed range check on RC channel selection
+ - scale UART RX size with baudrate
+ - default fast sampling enabled on first IMU for all capable boards
+ - fixed bootloader flash alignment bug
+ - fixed PWM 5 and 6 for MatekF765-Wing
+ - support RM3100 compass on I2C
+ - fixed error on AHRS level trim in preflight calibration
+ - fixed handling of SB without BUSY on I2Cv1 devices
+ - update bootloaders to be more robust to unexpected data
+ - added new COMPASS_SCALE parameters and estimator to fix issue with
+   compass in Here2 GPS
+ - fixed issue with millis wrap on boards with 16 bit system timer
+   (MatekF405, MatekF765, speedybeef4 and KakuteF4)
+ - fixed i2c internal masks for several boards
+ - fixed scaling of Blheli32 RPM conversion
+ - changed to WFQ FrSky telemetry scheduler
+ - enable LED pin on MatekF765
+ - added params for Durandal battery monitor pins to param docs
+ - updated bootloaders to reduce change of line noise stopping boot
+ - fixed DMA error causing memory corruption in ChibiOS I2C driver
+ - fixed param conversion from plane 3.9.x to plane 4.0.x for rangefinders
+ - cope with UAVCAN GPS that doesn't provide AUX messages (Here2 GPS)
+ - send temperatures for IMUs on mavlink
+ - fixed I2C clock speed error on STM32H7
+ - fixed CR/LF output error for NMEA output
+
+Happy flying!
+
+Release 4.0.1, 22nd November 2019
+---------------------------------
+
+This is a minor release with a few bug fixes and enhancements. Changes
+are:
+
+ - Added Q_ASSIST_ALT parameter which offers a way for VTOL assistance
+   at low altitudes
+
+ - fixed channels 5 and 6 on the MatekF765-Wing
+
+ - fixed a bug with sending data on a full UART in mavlink parameter
+   download
+
+ - fixed use of UAVCAN primary GPS with UART secondary GPS
+
+ - fixed failover between rangefinders of same orientation
+
+ - added RC option for TAKEOFF mode
+
+ - fixed logging of current on secondary battery monitors
+
+ - fixed register checking on DPS280 for mRoControlZeroF7
+
+ - added clock panel to OSD
+
+ - fixed B/E led brightness on Pixhawk4
+
+ - support RTCM injection to UAVCAN GPS for RTK support
+
+ - fixed an RC failsafe bug that could cause the geofence to disable
+
+ - fixed a bug in the SDP33 airspeed driver
+
+Happy flying!
+
+
+Release 4.0.1beta1, 17th November 2019
+--------------------------------------
+
+This is a minor release with a few bug fixes and enhancements for the
+4.0 stable version.
+
+Changes are:
+
+ - Added Q_ASSIST_ALT parameter which offers a way for VTOL assistance
+   at low altitudes
+
+ - fixed channels 5 and 6 on the MatekF765-Wing
+
+ - fixed a bug with sending data on a full UART in mavlink parameter
+   download
+
+ - added TECS_LAND_PMIN for constraining pitch minimum during landing
+
+ - fixed use of UAVCAN primary GPS with UART secondary GPS
+
+ - fixed failover between rangefinders of same orientation
+
+ - added RC option for TAKEOFF mode
+
+ - fixed logging of current on secondary battery monitors
+
+ - fixed register checking on DPS280 for mRoControlZeroF7
+
+ - added clock panel to OSD
+
+ - fixed B/E led brightness on Pixhawk4
+
+ - support RTCM injection to UAVCAN GPS for RTK support
+
+Happy flying!
+
+Release 4.0.0, 28th October 2019
+--------------------------------
+
+The final release of stable 4.0.0 has just one change from beta4,
+which is to fix a bug in the new TAKEOFF flight mode.
+
+Many thanks to everyone who has been testing the 4.0 release, and
+special thanks to Henry for his fantastic work in bringing the wiki up
+to date for this release!
+
+Happy flying!
+
 Release 4.0.0beta4, 19th October 2019
 ------------------------------------
 

ArduPlane/servos.cpp

@@ -330,6 +330,33 @@ void Plane::set_servos_manual_passthrough(void)
     SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));
 }
 
+/*
+  Scale the throttle to conpensate for battery voltage drop
+ */
+void Plane::throttle_voltage_comp()
+{
+    // return if not enabled, or setup incorrectly
+    if (g2.fwd_thr_batt_voltage_min >= g2.fwd_thr_batt_voltage_max || !is_positive(g2.fwd_thr_batt_voltage_max)) {
+        return;
+    }
+
+    float batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(g2.fwd_thr_batt_idx);
+    // Return for a very low battery
+    if (batt_voltage_resting_estimate < 0.25f * g2.fwd_thr_batt_voltage_min) {
+        return;
+    }
+
+    // constrain read voltage to min and max params
+    batt_voltage_resting_estimate = constrain_float(batt_voltage_resting_estimate,g2.fwd_thr_batt_voltage_min,g2.fwd_thr_batt_voltage_max);
+
+    // Scale the throttle up to compensate for voltage drop
+    // Ratio = 1 when voltage = voltage max, ratio increases as voltage drops
+    const float ratio = g2.fwd_thr_batt_voltage_max / batt_voltage_resting_estimate;
+
+    SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
+                                        constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * ratio, -100, 100));
+}
+
 /*
   calculate any throttle limits based on the watt limiter
  */
@@ -408,10 +435,13 @@ void Plane::set_servos_controlled(void)
     } else if (landing.is_flaring()) {
         min_throttle = 0;
     }
-    
+
+    // conpensate for battery voltage drop
+    throttle_voltage_comp();
+
     // apply watt limiter
     throttle_watt_limiter(min_throttle, max_throttle);
-    
+
     SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
                                     constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle));
     
@@ -544,47 +574,24 @@ void Plane::set_servos_flaps(void)
 }
 
 #if LANDING_GEAR_ENABLED == ENABLED
-/*
-  change and report landing gear
- */
-void Plane::change_landing_gear(AP_LandingGear::LandingGearCommand cmd)
-{
-    if (cmd != (AP_LandingGear::LandingGearCommand)gear.last_cmd) {
-        if (SRV_Channels::function_assigned(SRV_Channel::k_landing_gear_control)) {
-            g2.landing_gear.set_position(cmd);
-            gcs().send_text(MAV_SEVERITY_INFO, "LandingGear: %s", cmd==AP_LandingGear::LandingGear_Retract?"RETRACT":"DEPLOY");
-        }
-        gear.last_cmd = (int8_t)cmd;
-    }
-}
-
 /*
   setup landing gear state
  */
 void Plane::set_landing_gear(void)
 {
-    if (control_mode == &mode_auto && hal.util->get_soft_armed() && is_flying()) {
-        AP_LandingGear::LandingGearCommand cmd = (AP_LandingGear::LandingGearCommand)gear.last_auto_cmd;
+    if (control_mode == &mode_auto && hal.util->get_soft_armed() && is_flying() && gear.last_flight_stage != flight_stage) {
         switch (flight_stage) {
         case AP_Vehicle::FixedWing::FLIGHT_LAND:
-            cmd = AP_LandingGear::LandingGear_Deploy;
+            g2.landing_gear.deploy_for_landing();
             break;
         case AP_Vehicle::FixedWing::FLIGHT_NORMAL:
-            cmd = AP_LandingGear::LandingGear_Retract;
-            break;
-        case AP_Vehicle::FixedWing::FLIGHT_VTOL:
-            if (quadplane.is_vtol_land(mission.get_current_nav_cmd().id)) {
-                cmd = AP_LandingGear::LandingGear_Deploy;
-            }
+            g2.landing_gear.retract_after_takeoff();
             break;
         default:
             break;
         }
-        if (cmd != gear.last_auto_cmd) {
-            gear.last_auto_cmd = (int8_t)cmd;
-            change_landing_gear(cmd);
-        }
     }
+    gear.last_flight_stage = flight_stage;
 }
 #endif // LANDING_GEAR_ENABLED
 
@@ -895,7 +902,7 @@ void Plane::servos_auto_trim(void)
     g2.servo_channels.adjust_trim(SRV_Channel::k_vtail_right, pitch_I);
 
     g2.servo_channels.adjust_trim(SRV_Channel::k_flaperon_left,  roll_I);
-    g2.servo_channels.adjust_trim(SRV_Channel::k_flaperon_right, -roll_I);
+    g2.servo_channels.adjust_trim(SRV_Channel::k_flaperon_right, roll_I);
 
     // cope with various dspoiler options
     const int8_t bitmask = g2.crow_flap_options.get();

ArduPlane/system.cpp

@@ -135,8 +135,6 @@ void Plane::init_ardupilot()
 #if LANDING_GEAR_ENABLED == ENABLED
     // initialise landing gear position
     g2.landing_gear.init();
-    gear.last_auto_cmd = -1;
-    gear.last_cmd = -1;
 #endif
 
 #if FENCE_TRIGGERED_PIN > 0
@@ -181,6 +179,9 @@ void Plane::init_ardupilot()
     g2.gripper.init();
 #endif
 
+    // call AP_Vehicle setup code
+    vehicle_setup();
+
     // disable safety if requested
     BoardConfig.init_safety();
 
@@ -223,9 +224,7 @@ void Plane::startup_ground(void)
 #endif
 
 #ifdef ENABLE_SCRIPTING
-    if (!g2.scripting.init()) {
-        gcs().send_text(MAV_SEVERITY_ERROR, "Scripting failed to start");
-    }
+    g2.scripting.init();
 #endif // ENABLE_SCRIPTING
 
     // reset last heartbeat time, so we don't trigger failsafe on slow

ArduPlane/version.h

@@ -6,13 +6,13 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ArduPlane V4.1.0dev"
+#define THISFIRMWARE "ArduPlane V4.0.6"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,1,0,FIRMWARE_VERSION_TYPE_DEV
+#define FIRMWARE_VERSION 4,0,6,FIRMWARE_VERSION_TYPE_OFFICIAL
 
 #define FW_MAJOR 4
-#define FW_MINOR 1
-#define FW_PATCH 0
-#define FW_TYPE FIRMWARE_VERSION_TYPE_DEV
+#define FW_MINOR 0
+#define FW_PATCH 6
+#define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL
 

ArduPlane/wscript

@@ -21,7 +21,6 @@ def build(bld):
             'AC_WPNav',
             'AC_AttitudeControl',
             'AP_Motors',
-            'AC_PID',
             'AP_Stats',
             'AP_Landing',
             'AP_Beacon',

ArduSub/GCS_Mavlink.cpp

@@ -62,7 +62,7 @@ uint32_t GCS_Sub::custom_mode() const
     return sub.control_mode;
 }
 
-MAV_STATE GCS_MAVLINK_Sub::system_status() const
+MAV_STATE GCS_MAVLINK_Sub::vehicle_system_status() const
 {
     // set system as critical if any failsafe have triggered
     if (sub.any_failsafe_triggered())  {

ArduSub/GCS_Mavlink.h

@@ -50,7 +50,7 @@ private:
     bool send_info(void);
 
     MAV_MODE base_mode() const override;
-    MAV_STATE system_status() const override;
+    MAV_STATE vehicle_system_status() const override;
 
     int16_t vfr_hud_throttle() const override;
 

ArduSub/Parameters.cpp

@@ -720,15 +720,5 @@ void Sub::convert_old_parameters()
         AP_Param::convert_old_parameters(&filt_conversion_info[i], 1.0f);
     }
 
-    const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old,  Parameters::k_param_rc_2_old,
-                                      Parameters::k_param_rc_3_old,  Parameters::k_param_rc_4_old,
-                                      Parameters::k_param_rc_5_old,  Parameters::k_param_rc_6_old,
-                                      Parameters::k_param_rc_7_old,  Parameters::k_param_rc_8_old,
-                                      Parameters::k_param_rc_9_old,  Parameters::k_param_rc_10_old,
-                                      Parameters::k_param_rc_11_old, Parameters::k_param_rc_12_old,
-                                      Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old
-                                    };
-    const uint16_t old_aux_chan_mask = 0x3FF0;
-    // note that we don't pass in rcmap as we don't want output channel functions changed based on rcmap
-    SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, nullptr);
+    SRV_Channels::upgrade_parameters();
 }

ArduSub/RC_Channel.h

@@ -19,7 +19,7 @@ public:
 
     RC_Channel_Sub obj_channels[NUM_RC_CHANNELS];
     RC_Channel_Sub *channel(const uint8_t chan) override {
-        if (chan > NUM_RC_CHANNELS) {
+        if (chan >= NUM_RC_CHANNELS) {
             return nullptr;
         }
         return &obj_channels[chan];

ArduSub/system.cpp

@@ -188,9 +188,7 @@ void Sub::init_ardupilot()
     startup_INS_ground();
 
 #ifdef ENABLE_SCRIPTING
-    if (!g2.scripting.init()) {
-        gcs().send_text(MAV_SEVERITY_ERROR, "Scripting failed to start");
-    }
+    g2.scripting.init();
 #endif // ENABLE_SCRIPTING
 
     // we don't want writes to the serial port to cause us to pause

ArduSub/wscript

@@ -8,7 +8,6 @@ def build(bld):
         ap_vehicle=vehicle,
         ap_libraries=bld.ap_common_vehicle_libraries() + [
             'AC_AttitudeControl',
-            'AC_PID',
             'AC_WPNav',
             'AP_Camera',
             'AP_InertialNav',

Tools/AP_Bootloader/bl_protocol.cpp

@@ -462,7 +462,9 @@ bootloader(unsigned timeout)
     uint32_t	first_words[RESERVE_LEAD_WORDS];
     bool done_sync = false;
     bool done_get_device = false;
+    bool done_erase = false;
     static bool done_timer_init;
+    unsigned original_timeout = timeout;
 
     memset(first_words, 0xFF, sizeof(first_words));
 
@@ -596,12 +598,17 @@ bootloader(unsigned timeout)
                 // lower chance of random data on a uart triggering erase
                 goto cmd_bad;
             }
-            
+
             /* expect EOC */
             if (!wait_for_eoc(2)) {
                 goto cmd_bad;
             }
 
+            // once erase is done there is no going back, set timeout
+            // to zero
+            done_erase = true;
+            timeout = 0;
+            
             flash_set_keep_unlocked(true);
 
             // clear the bootloader LED while erasing - it stops blinking at random
@@ -911,7 +918,11 @@ bootloader(unsigned timeout)
             break;
 
 		case PROTO_SET_BAUD: {
-			/* expect arg then EOC */
+            if (!done_sync || !done_get_device) {
+                // prevent timeout going to zero on noise
+                goto cmd_bad;
+            }
+            /* expect arg then EOC */
             uint32_t baud = 0;
 
             if (cin_word(&baud, 100)) {
@@ -957,6 +968,12 @@ bootloader(unsigned timeout)
         sync_response();
         continue;
 cmd_bad:
+        // if we get a bad command it could be line noise on a
+        // uart. Set timeout back to original timeout so we don't get
+        // stuck in the bootloader
+        if (!done_erase) {
+            timeout = original_timeout;
+        }
         // send an 'invalid' response but don't kill the timeout - could be garbage
         invalid_response();
         continue;