Merge branch 'apm4.0.3' into v4.0.5-dev-08

# Conflicts:
#	ArduCopter/AP_Arming.cpp
#	ArduCopter/AP_Arming.h
#	ArduCopter/Parameters.cpp
#	ArduCopter/version.h
#	Tools/scripts/build_autotest.sh
#	libraries/AP_Logger/AP_Logger.cpp
#	libraries/AP_UAVCAN/AP_UAVCAN.cpp
#	libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp
#	libraries/GCS_MAVLink/GCS_Common.cpp

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

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

ArduCopter/AP_Arming.cpp

@@ -1,3 +1,4 @@
+<<<<<<< HEAD
 #include "Copter.h"
 
 // performs pre-arm checks. expects to be called at 1hz.
@@ -869,3 +870,852 @@ bool AP_Arming_Copter::disarm()
 
     return true;
 }
+=======
+#include "Copter.h"
+
+// performs pre-arm checks. expects to be called at 1hz.
+void AP_Arming_Copter::update(void)
+{
+    // perform pre-arm checks & display failures every 30 seconds
+    static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2;
+    pre_arm_display_counter++;
+    bool display_fail = false;
+    if (pre_arm_display_counter >= PREARM_DISPLAY_PERIOD) {
+        display_fail = true;
+        pre_arm_display_counter = 0;
+    }
+
+    pre_arm_checks(display_fail);
+}
+
+bool AP_Arming_Copter::pre_arm_checks(bool display_failure)
+{
+    const bool passed = run_pre_arm_checks(display_failure);
+    set_pre_arm_check(passed);
+    return passed;
+}
+
+// perform pre-arm checks
+//  return true if the checks pass successfully
+bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure)
+{
+    // exit immediately if already armed
+    if (copter.motors->armed()) {
+        return true;
+    }
+
+    // check if motor interlock and Emergency Stop aux switches are used
+    // at the same time.  This cannot be allowed.
+    if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) &&
+        rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP)){
+        check_failed(display_failure, "Interlock/E-Stop Conflict");
+        return false;
+    }
+
+    // check if motor interlock aux switch is in use
+    // if it is, switch needs to be in disabled position to arm
+    // otherwise exit immediately.  This check to be repeated,
+    // as state can change at any time.
+    if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) {
+        check_failed(display_failure, "Motor Interlock Enabled");
+    }
+
+    // if pre arm checks are disabled run only the mandatory checks
+    if (checks_to_perform == 0) {
+        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)
+        & gcs_failsafe_check(display_failure) &
+        AP_Arming::pre_arm_checks(display_failure);
+}
+
+bool AP_Arming_Copter::barometer_checks(bool display_failure)
+{
+    if (!AP_Arming::barometer_checks(display_failure)) {
+        return false;
+    }
+
+    bool ret = true;
+    // check Baro
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_BARO)) {
+        // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
+        // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
+        // that may differ from the baro height due to baro drift.
+        nav_filter_status filt_status = copter.inertial_nav.get_filter_status();
+        bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
+        if (using_baro_ref) {
+            if (fabsf(copter.inertial_nav.get_altitude() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
+                check_failed(ARMING_CHECK_BARO, display_failure, "Altitude disparity");
+                ret = false;
+            }
+        }
+    }
+    return ret;
+}
+
+bool AP_Arming_Copter::compass_checks(bool display_failure)
+{
+    bool ret = AP_Arming::compass_checks(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");
+            ret = false;
+        }
+    }
+
+    return ret;
+}
+
+bool AP_Arming_Copter::ins_checks(bool display_failure)
+{
+    bool ret = AP_Arming::ins_checks(display_failure);
+
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) {
+
+        // get ekf attitude (if bad, it's usually the gyro biases)
+        if (!pre_arm_ekf_attitude_check()) {
+            check_failed(ARMING_CHECK_INS, display_failure, "EKF attitude is bad");
+            ret = false;
+        }
+    }
+
+    return ret;
+}
+
+bool AP_Arming_Copter::board_voltage_checks(bool display_failure)
+{
+    if (!AP_Arming::board_voltage_checks(display_failure)) {
+        return false;
+    }
+
+    // check battery voltage
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) {
+        if (copter.battery.has_failsafed()) {
+            check_failed(ARMING_CHECK_VOLTAGE, display_failure, "Battery failsafe");
+            return false;
+        }
+
+        // call parent battery checks
+        if (!AP_Arming::battery_checks(display_failure)) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+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)) {
+
+        // ensure all rc channels have different functions
+        if (rc().duplicate_options_exist()) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Duplicate Aux Switch Options");
+            return false;
+        }
+
+        // failsafe parameter checks
+        if (copter.g.failsafe_throttle) {
+            // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900
+            if (copter.channel_throttle->get_radio_min() <= copter.g.failsafe_throttle_value+10 || copter.g.failsafe_throttle_value < 910) {
+                check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check FS_THR_VALUE");
+                return false;
+            }
+        }
+
+        // lean angle parameter check
+        if (copter.aparm.angle_max < 1000 || copter.aparm.angle_max > 8000) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check ANGLE_MAX");
+            return false;
+        }
+
+        // acro balance parameter check
+#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
+        if ((copter.g.acro_balance_roll > copter.attitude_control->get_angle_roll_p().kP()) || (copter.g.acro_balance_pitch > copter.attitude_control->get_angle_pitch_p().kP())) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "ACRO_BAL_ROLL/PITCH");
+            return false;
+        }
+#endif
+
+        // pilot-speed-up parameter check
+        if (copter.g.pilot_speed_up <= 0) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check PILOT_SPEED_UP");
+            return false;
+        }
+
+        #if FRAME_CONFIG == HELI_FRAME
+        if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD &&
+            copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_DUAL &&
+            copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Invalid Heli FRAME_CLASS");
+            return false;
+        }
+
+        // check helicopter parameters
+        if (!copter.motors->parameter_check(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) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Inverted flight option not supported");
+            return false;
+        }
+        // Ensure an Aux Channel is configured for motor interlock
+        if (rc().find_channel_for_option(RC_Channel::aux_func_t::MOTOR_INTERLOCK) == nullptr) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Motor Interlock not configured");
+            return false;
+        }
+
+        #else
+        if (copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_QUAD ||
+            copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_DUAL ||
+            copter.g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Invalid MultiCopter FRAME_CLASS");
+            return false;
+        }
+        #endif // HELI_FRAME
+
+        // check for missing terrain data
+        if (!pre_arm_terrain_check(display_failure)) {
+            return false;
+        }
+
+        // check adsb avoidance failsafe
+#if ADSB_ENABLED == ENABLE
+        if (copter.failsafe.adsb) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "ADSB threat detected");
+            return false;
+        }
+#endif
+
+        // ensure controllers are OK with us arming:
+        char failure_msg[50];
+        if (!copter.pos_control->pre_arm_checks("PSC", failure_msg, ARRAY_SIZE(failure_msg))) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
+            return false;
+        }
+        if (!copter.attitude_control->pre_arm_checks("ATC", failure_msg, ARRAY_SIZE(failure_msg))) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Bad parameter: %s", failure_msg);
+            return false;
+        }
+    }
+
+    return true;
+}
+
+// check motor setup was successful
+bool AP_Arming_Copter::motor_checks(bool display_failure)
+{
+    // check motors initialised  correctly
+    if (!copter.motors->initialised_ok()) {
+        check_failed(display_failure, "check firmware or FRAME_CLASS");
+        return false;
+    }
+
+    // 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;
+    const uint8_t num_drivers = AP::can().get_num_drivers();
+    for (uint8_t i = 0; i < num_drivers; i++) {
+        if (AP::can().get_protocol_type(i) == AP_BoardConfig_CAN::Protocol_Type_ToshibaCAN) {
+            tcan_active = true;
+        }
+    }
+    if (tcan_active) {
+        if (copter.motors->get_pwm_output_min() != 1000) {
+            check_failed(display_failure, "TCAN ESCs require MOT_PWM_MIN=1000");
+            return false;
+        }
+        if (copter.motors->get_pwm_output_max() != 2000) {
+            check_failed(display_failure, "TCAN ESCs require MOT_PWM_MAX=2000");
+            return false;
+        }
+    }
+#endif
+
+    return true;
+}
+
+bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure)
+{
+    // check throttle is above failsafe throttle
+    // this is near the bottom to allow other failures to be displayed before checking pilot throttle
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) {
+        if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) {
+            #if FRAME_CONFIG == HELI_FRAME
+            const char *failmsg = "Collective below Failsafe";
+            #else
+            const char *failmsg = "Throttle below Failsafe";
+            #endif
+            check_failed(ARMING_CHECK_RC, display_failure, "%s", failmsg);
+            return false;
+        }
+    }
+
+    return true;
+}
+
+bool AP_Arming_Copter::oa_checks(bool display_failure)
+{
+#if AC_OAPATHPLANNER_ENABLED == ENABLED
+    char failure_msg[50];
+    if (copter.g2.oa.pre_arm_check(failure_msg, ARRAY_SIZE(failure_msg))) {
+        return true;
+    }
+    // display failure
+    if (strlen(failure_msg) == 0) {
+        check_failed(display_failure, "%s", "Check Object Avoidance");
+    } else {
+        check_failed(display_failure, "%s", failure_msg);
+    }
+    return false;
+#else
+    return true;
+#endif
+}
+
+bool AP_Arming_Copter::rc_calibration_checks(bool display_failure)
+{
+    const RC_Channel *channels[] = {
+        copter.channel_roll,
+        copter.channel_pitch,
+        copter.channel_throttle,
+        copter.channel_yaw
+    };
+
+    copter.ap.pre_arm_rc_check = rc_checks_copter_sub(display_failure, channels)
+        & AP_Arming::rc_calibration_checks(display_failure);
+
+    return copter.ap.pre_arm_rc_check;
+}
+
+// performs pre_arm gps related checks and returns true if passed
+bool AP_Arming_Copter::gps_checks(bool display_failure)
+{
+    // run mandatory gps checks first
+    if (!mandatory_gps_checks(display_failure)) {
+        AP_Notify::flags.pre_arm_gps_check = false;
+        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) {
+        AP_Notify::flags.pre_arm_gps_check = true;
+        return true;
+    }
+
+    // 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;
+        return true;
+    }
+
+    // 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;
+    }
+
+    // if we got here all must be ok
+    AP_Notify::flags.pre_arm_gps_check = true;
+    return true;
+}
+
+// check ekf attitude is acceptable
+bool AP_Arming_Copter::pre_arm_ekf_attitude_check()
+{
+    // get ekf filter status
+    nav_filter_status filt_status = copter.inertial_nav.get_filter_status();
+
+    return filt_status.flags.attitude;
+}
+
+// check we have required terrain data
+bool AP_Arming_Copter::pre_arm_terrain_check(bool display_failure)
+{
+#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
+    // succeed if not using terrain data
+    if (!copter.terrain_use()) {
+        return true;
+    }
+
+    // check if terrain following is enabled, using a range finder but RTL_ALT is higher than rangefinder's max range
+    // To-Do: modify RTL return path to fly at or above the RTL_ALT and remove this check
+
+    if (copter.rangefinder_state.enabled && (copter.g.rtl_altitude > copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270))) {
+        check_failed(ARMING_CHECK_PARAMETERS, display_failure, "RTL_ALT above rangefinder max range");
+        return false;
+    }
+
+    // show terrain statistics
+    uint16_t terr_pending, terr_loaded;
+    copter.terrain.get_statistics(terr_pending, terr_loaded);
+    bool have_all_data = (terr_pending <= 0);
+    if (!have_all_data) {
+        check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Waiting for Terrain data");
+    }
+    return have_all_data;
+#else
+    return true;
+#endif
+}
+
+// check nothing is too close to vehicle
+bool AP_Arming_Copter::proximity_checks(bool display_failure) const
+{
+#if PROXIMITY_ENABLED == ENABLED
+
+    if (!AP_Arming::proximity_checks(display_failure)) {
+        return false;
+    }
+
+    if (!((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS))) {
+        // check is disabled
+        return true;
+    }
+
+    // get closest object if we might use it for avoidance
+#if AC_AVOID_ENABLED == ENABLED
+    float angle_deg, distance;
+    if (copter.avoid.proximity_avoidance_enabled() && copter.g2.proximity.get_closest_object(angle_deg, distance)) {
+        // display error if something is within 60cm
+        if (distance <= 0.6f) {
+            check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Proximity %d deg, %4.2fm", (int)angle_deg, (double)distance);
+            return false;
+        }
+    }
+#endif
+
+#endif
+    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
+bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
+{
+    const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
+
+    // always check if inertial nav has started and is ready
+    if (!ahrs.healthy()) {
+        check_failed(true, "AHRS not healthy");
+        return false;
+    }
+
+#ifndef ALLOW_ARM_NO_COMPASS
+    // if external source of heading is available, we can skip compass health check
+    if (!ahrs.is_ext_nav_used_for_yaw()) {
+        const Compass &_compass = AP::compass();
+        // check compass health
+        if (!_compass.healthy()) {
+            check_failed(true, "Compass not healthy");
+            return false;
+        }
+    }
+#endif
+
+    Mode::Number control_mode = copter.control_mode;
+
+    // always check if the current mode allows arming
+    if (!copter.flightmode->allows_arming(method == AP_Arming::Method::MAVLINK)) {
+        check_failed(true, "Mode not armable");
+        return false;
+    }
+
+    // always check motors
+    if (!motor_checks(true)) {
+        return false;
+    }
+
+    // if we are using motor interlock switch and it's enabled, fail to arm
+    // skip check in Throw mode which takes control of the motor interlock
+    if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) {
+        check_failed(true, "Motor Interlock Enabled");
+        return false;
+    }
+
+    // if we are not using Emergency Stop switch option, force Estop false to ensure motors
+    // can run normally
+    if (!rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP)){
+        SRV_Channels::set_emergency_stop(false);
+        // if we are using motor Estop switch, it must not be in Estop position
+    } else if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP) && SRV_Channels::get_emergency_stop()){
+        gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped");
+        return false;
+    }
+
+    // succeed if arming checks are disabled
+    if (checks_to_perform == 0) {
+        return true;
+    }
+
+    // check lean angle
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) {
+        if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > copter.aparm.angle_max) {
+            check_failed(ARMING_CHECK_INS, true, "Leaning");
+            return false;
+        }
+    }
+
+    // check adsb
+#if ADSB_ENABLED == ENABLE
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) {
+        if (copter.failsafe.adsb) {
+            check_failed(ARMING_CHECK_PARAMETERS, true, "ADSB threat detected");
+            return false;
+        }
+    }
+#endif
+
+    // check throttle
+    if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) {
+         #if FRAME_CONFIG == HELI_FRAME
+        const char *rc_item = "Collective";
+        #else
+        const char *rc_item = "Throttle";
+        #endif
+        // check throttle is not too low - must be above failsafe throttle
+        if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.g.failsafe_throttle_value) {
+            check_failed(ARMING_CHECK_RC, true, "%s below failsafe", rc_item);
+            return false;
+        }
+
+        // check throttle is not too high - skips checks if arming from GCS in Guided
+        if (!(method == AP_Arming::Method::MAVLINK && (control_mode == Mode::Number::GUIDED || control_mode == Mode::Number::GUIDED_NOGPS))) {
+            // above top of deadband is too always high
+            if (copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()) > 0.0f) {
+                check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item);
+                return false;
+            }
+            // in manual modes throttle must be at zero
+            #if FRAME_CONFIG != HELI_FRAME
+            if ((copter.flightmode->has_manual_throttle() || control_mode == Mode::Number::DRIFT) && copter.channel_throttle->get_control_in() > 0) {
+                check_failed(ARMING_CHECK_RC, true, "%s too high", rc_item);
+                return false;
+            }
+            #endif
+        }
+    }
+
+    // check if safety switch has been pushed
+    if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
+        check_failed(true, "Safety Switch");
+        return false;
+    }
+
+    // superclass method should always be the last thing called; it
+    // has side-effects which would need to be cleaned up if one of
+    // our arm checks failed
+    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;
+    AP_Notify::flags.pre_arm_check = b;
+}
+
+bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_checks)
+{
+    static bool in_arm_motors = false;
+
+    // exit immediately if already in this function
+    if (in_arm_motors) {
+        return false;
+    }
+    in_arm_motors = true;
+
+    // return true if already armed
+    if (copter.motors->armed()) {
+        in_arm_motors = false;
+        return true;
+    }
+
+    if (!AP_Arming::arm(method, do_arming_checks)) {
+        AP_Notify::events.arming_failed = true;
+        in_arm_motors = false;
+        return false;
+    }
+
+    // let logger know that we're armed (it may open logs e.g.)
+    AP::logger().set_vehicle_armed(true);
+
+    // disable cpu failsafe because initialising everything takes a while
+    copter.failsafe_disable();
+
+    // notify that arming will occur (we do this early to give plenty of warning)
+    AP_Notify::flags.armed = true;
+    // call notify update a few times to ensure the message gets out
+    for (uint8_t i=0; i<=10; i++) {
+        AP::notify().update();
+    }
+
+#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    gcs().send_text(MAV_SEVERITY_INFO, "Arming motors");
+#endif
+
+    // Remember Orientation
+    // --------------------
+    copter.init_simple_bearing();
+
+    AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
+
+    copter.initial_armed_bearing = ahrs.yaw_sensor;
+
+    if (!ahrs.home_is_set()) {
+        // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
+        ahrs.resetHeightDatum();
+        AP::logger().Write_Event(DATA_EKF_ALT_RESET);
+
+        // we have reset height, so arming height is zero
+        copter.arming_altitude_m = 0;
+    } else if (!ahrs.home_is_locked()) {
+        // Reset home position if it has already been set before (but not locked)
+        if (!copter.set_home_to_current_location(false)) {
+            // ignore failure
+        }
+
+        // remember the height when we armed
+        copter.arming_altitude_m = copter.inertial_nav.get_altitude() * 0.01;
+    }
+    copter.update_super_simple_bearing(false);
+
+    // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point
+#if MODE_SMARTRTL_ENABLED == ENABLED
+    copter.g2.smart_rtl.set_home(copter.position_ok());
+#endif
+
+    // enable gps velocity based centrefugal force compensation
+    ahrs.set_correct_centrifugal(true);
+    hal.util->set_soft_armed(true);
+
+#if SPRAYER_ENABLED == ENABLED
+    // turn off sprayer's test if on
+    copter.sprayer.test_pump(false);
+#endif
+
+    // enable output to motors
+    copter.enable_motor_output();
+
+    // finally actually arm the motors
+    copter.motors->armed(true);
+
+    AP::logger().Write_Event(DATA_ARMED);
+
+    // log flight mode in case it was changed while vehicle was disarmed
+    AP::logger().Write_Mode((uint8_t)copter.control_mode, copter.control_mode_reason);
+
+    // re-enable failsafe
+    copter.failsafe_enable();
+
+    // perf monitor ignores delay due to arming
+    AP::scheduler().perf_info.ignore_this_loop();
+
+    // flag exiting this function
+    in_arm_motors = false;
+
+    // Log time stamp of arming event
+    copter.arm_time_ms = millis();
+
+    // Start the arming delay
+    copter.ap.in_arming_delay = true;
+
+    // assumed armed without a arming, switch. Overridden in switches.cpp
+    copter.ap.armed_with_switch = false;
+
+    // return success
+    return true;
+}
+
+// arming.disarm - disarm motors
+bool AP_Arming_Copter::disarm()
+{
+    // return immediately if we are already disarmed
+    if (!copter.motors->armed()) {
+        return true;
+    }
+
+    if (!AP_Arming::disarm()) {
+        return false;
+    }
+
+#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors");
+#endif
+
+    AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
+
+    // save compass offsets learned by the EKF if enabled
+    Compass &compass = AP::compass();
+    if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
+        for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+            Vector3f magOffsets;
+            if (ahrs.getMagOffsets(i, magOffsets)) {
+                compass.set_and_save_offsets(i, magOffsets);
+            }
+        }
+    }
+
+#if AUTOTUNE_ENABLED == ENABLED
+    // save auto tuned parameters
+    if (copter.flightmode == &copter.mode_autotune) {
+        copter.mode_autotune.save_tuning_gains();
+    } else {
+        copter.mode_autotune.reset();
+    }
+#endif
+
+    // we are not in the air
+    copter.set_land_complete(true);
+    copter.set_land_complete_maybe(true);
+
+    AP::logger().Write_Event(DATA_DISARMED);
+
+    // send disarm command to motors
+    copter.motors->armed(false);
+
+#if MODE_AUTO_ENABLED == ENABLED
+    // reset the mission
+    copter.mode_auto.mission.reset();
+#endif
+
+    AP::logger().set_vehicle_armed(false);
+
+    // disable gps velocity based centrefugal force compensation
+    ahrs.set_correct_centrifugal(false);
+    hal.util->set_soft_armed(false);
+
+    copter.ap.in_arming_delay = false;
+
+    return true;
+}
+>>>>>>> apm4.0.3

ArduCopter/AP_Arming.h

@@ -1,3 +1,4 @@
+<<<<<<< HEAD
 #pragma once
 
 #include <AP_Arming/AP_Arming.h>
@@ -60,3 +61,68 @@ private:
     bool run_pre_arm_checks(bool display_failure);
 
 };
+=======
+#pragma once
+
+#include <AP_Arming/AP_Arming.h>
+
+class AP_Arming_Copter : public AP_Arming
+{
+public:
+    friend class Copter;
+    friend class ToyMode;
+
+    AP_Arming_Copter() : AP_Arming()
+    {
+        // default REQUIRE parameter to 1 (Copter does not have an
+        // actual ARMING_REQUIRE parameter)
+        require.set_default((uint8_t)Required::YES_MIN_PWM);
+    }
+
+    /* Do not allow copies */
+    AP_Arming_Copter(const AP_Arming_Copter &other) = delete;
+    AP_Arming_Copter &operator=(const AP_Arming_Copter&) = delete;
+
+    void update(void);
+
+    bool rc_calibration_checks(bool display_failure) override;
+
+    bool disarm() override;
+    bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
+
+protected:
+
+    bool pre_arm_checks(bool display_failure) override;
+    bool pre_arm_ekf_attitude_check();
+    bool pre_arm_terrain_check(bool display_failure);
+    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;
+    bool gps_checks(bool display_failure) override;
+    bool barometer_checks(bool display_failure) override;
+    bool board_voltage_checks(bool display_failure) override;
+
+    // NOTE! the following check functions *DO NOT* call into AP_Arming!
+    bool parameter_checks(bool display_failure);
+    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);
+
+private:
+
+    // actually contains the pre-arm checks.  This is wrapped so that
+    // we can store away success/failure of the checks.
+    bool run_pre_arm_checks(bool display_failure);
+
+};
+>>>>>>> apm4.0.3

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/Copter.h

@@ -761,6 +761,7 @@ 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

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:

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

ArduCopter/ReleaseNotes.txt

@@ -1,5 +1,84 @@
 ArduPilot Copter Release Notes:
 ------------------------------------------------------------------
+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)

ArduCopter/heli.cpp

@@ -131,6 +131,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 +150,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;

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/mode.h

@@ -597,6 +597,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
 };
 
 
@@ -1359,7 +1360,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

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_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 {
@@ -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();
+    }
+
+    // Loiter State Machine Determination
+    AltHoldModeState althold_state = get_alt_hold_state(target_climb_rate);
+
+    // althold state machine
+    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
+        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;
+
+    case AltHold_Takeoff:
+        // initiate take-off
+        if (!takeoff.running()) {
+            takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
+        }
 
-    // run loiter controller
-    loiter_nav->update();
+        // get takeoff adjusted pilot and takeoff climb rates
+        takeoff.get_climb_rates(target_climb_rate, takeoff_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);
+        // get avoidance adjusted climb rate
+        target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
 
-    // adjust climb rate using rangefinder
-    target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
+        // run loiter controller
+        loiter_nav->update();
 
-    // get avoidance adjusted climb rate
-    target_climb_rate = get_avoidance_adjusted_climbrate(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);
 
-    // update altitude target and call position controller
-    pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
+        // 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;
 
-    // adjusts target up or down using a climb rate
-    pos_control->update_z_controller();
+    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();
+        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/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,19 @@
 
 #include "ap_version.h"
 
+<<<<<<< HEAD
 #define THISFIRMWARE "ZRUAV v4.0.5-rc2"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,0,5,FIRMWARE_VERSION_TYPE_OFFICIAL
+=======
+#define THISFIRMWARE "ArduCopter V4.0.3"
+
+// the following line is parsed by the autotest scripts
+#define FIRMWARE_VERSION 4,0,3,FIRMWARE_VERSION_TYPE_OFFICIAL
+>>>>>>> apm4.0.3
 
 #define FW_MAJOR 4
 #define FW_MINOR 0
-#define FW_PATCH 0
+#define FW_PATCH 3
 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL

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;

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
 };
 
@@ -1377,6 +1386,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 +1394,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 +1447,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;
     }
@@ -3137,3 +3172,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

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

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

Tools/Frame_params/Solo_Copter-4.param

@@ -19,11 +19,16 @@ ATC_RAT_PIT_D,0.0088
 ATC_RAT_PIT_I,0.103
 ATC_RAT_PIT_IMAX,0.666
 ATC_RAT_PIT_P,0.103
+ATC_RAT_PIT_FLTT,20
+ATC_RAT_PIT_FLTD,20
+ATC_RAT_PIT_FLTE,0
 ATC_RAT_RLL_D,0.0065
 ATC_RAT_RLL_I,0.091
 ATC_RAT_RLL_IMAX,0.666
 ATC_RAT_RLL_P,0.091
-ATC_RAT_YAW_FILT,7.6
+ATC_RAT_RLL_FLTT,20
+ATC_RAT_RLL_FLTD,20
+ATC_RAT_RLL_FLTE,0
 ATC_RAT_YAW_FLTE,7.6
 ATC_RAT_YAW_I,0.0594
 ATC_RAT_YAW_IMAX,0.222
@@ -39,7 +44,7 @@ BATT_LOW_MAH,520
 BATT_LOW_TIMER,10
 BATT_LOW_VOLT,14
 BATT_MONITOR,5
-PWM_VOLT_SEL,1
+BRD_PWM_VOLT_SEL,1
 BRD_SAFETY_MASK,16368
 BRD_SAFETYENABLE,0
 BRD_TYPE,3
@@ -64,12 +69,15 @@ FLTMODE6,5
 FRAME_CLASS,1
 FRAME_TYPE,1
 FS_EKF_ACTION,2
+FS_GCS_ENABLE,0
 FS_THR_VALUE,950
 GPS_HDOP_GOOD,200
 GPS_NAVFILTER,6
 GPS_TYPE,1
 INS_ACC_BODYFIX,3
 INS_FAST_SAMPLE,3
+INS_GYR_FILTER,40
+INS_ACCEL_FILTER,10
 INS_HNTCH_ATT,40
 INS_HNTCH_ENABLE,1
 INS_HNTCH_REF,0.26
@@ -97,7 +105,7 @@ MNT_RC_IN_TILT,6
 MNT_TYPE,2
 MOT_BAT_CURR_MAX,40
 MOT_BAT_VOLT_MAX,16.8
-MOT_BAT_VOLT_MIN,12
+MOT_BAT_VOLT_MIN,13.2
 MOT_PWM_MAX,1900
 MOT_PWM_MIN,1000
 MOT_SLEW_UP_TIME,0.25
@@ -105,7 +113,7 @@ MOT_SPIN_ARM,0.08
 MOT_SPIN_MIN,0.12
 MOT_SPIN_MAX,0.965
 MOT_THST_EXPO,0.8
-NTF_LED_TYPES,215
+NTF_LED_TYPES,22
 NTF_OREO_THEME,1
 PHLD_BRAKE_ANGLE,2500
 PHLD_BRAKE_RATE,6
@@ -144,7 +152,7 @@ SERVO1_FUNCTION,33
 SERVO2_FUNCTION,34
 SERVO3_FUNCTION,35
 SERVO4_FUNCTION,36
-SR1_PARAMS,30
+SR1_PARAMS,100
 THR_DZ,10
 WP_YAW_BEHAVIOR,3
 WPNAV_SPEED,300

Tools/Frame_params/Solo_Copter-4_GreenCube.param

@@ -19,12 +19,17 @@ ATC_RAT_PIT_D,0.0075
 ATC_RAT_PIT_I,0.270
 ATC_RAT_PIT_IMAX,0.444
 ATC_RAT_PIT_P,0.270
+ATC_RAT_PIT_FLTT,20
+ATC_RAT_PIT_FLTD,20
+ATC_RAT_PIT_FLTE,0
 ATC_RAT_RLL_D,0.005
 ATC_RAT_RLL_I,0.200
 ATC_RAT_RLL_IMAX,0.444
 ATC_RAT_RLL_P,0.200
-ATC_RAT_YAW_FILT,5
-ATC_RAT_YAW_FLTE,5
+ATC_RAT_RLL_FLTT,20
+ATC_RAT_RLL_FLTD,20
+ATC_RAT_RLL_FLTE,0
+ATC_RAT_YAW_FLTE,7.6
 ATC_RAT_YAW_I,0.05
 ATC_RAT_YAW_IMAX,0.222
 ATC_RAT_YAW_P,0.5
@@ -39,7 +44,7 @@ BATT_LOW_MAH,520
 BATT_LOW_TIMER,10
 BATT_LOW_VOLT,14
 BATT_MONITOR,5
-PWM_VOLT_SEL,1
+BRD_PWM_VOLT_SEL,1
 BRD_SAFETY_MASK,16368
 BRD_SAFETYENABLE,0
 CIRCLE_RADIUS,3000
@@ -63,12 +68,15 @@ FLTMODE6,5
 FRAME_CLASS,1
 FRAME_TYPE,1
 FS_EKF_ACTION,2
+FS_GCS_ENABLE,0
 FS_THR_VALUE,950
 GPS_HDOP_GOOD,200
 GPS_NAVFILTER,6
 GPS_TYPE,1
 INS_ACC_BODYFIX,3
 INS_FAST_SAMPLE,3
+INS_GYR_FILTER,40
+INS_ACCEL_FILTER,10
 INS_HNTCH_ATT,40
 INS_HNTCH_ENABLE,1
 INS_HNTCH_REF,0.26
@@ -96,7 +104,7 @@ MNT_RC_IN_TILT,6
 MNT_TYPE,2
 MOT_BAT_CURR_MAX,50
 MOT_BAT_VOLT_MAX,16.8
-MOT_BAT_VOLT_MIN,12
+MOT_BAT_VOLT_MIN,13.2
 MOT_PWM_MAX,1900
 MOT_PWM_MIN,1000
 MOT_SLEW_UP_TIME,0
@@ -105,7 +113,7 @@ MOT_SPIN_ARM,0.08
 MOT_SPIN_MIN,0.12
 MOT_SPIN_MAX,0.965
 MOT_THST_EXPO,0.8
-NTF_LED_TYPES,215
+NTF_LED_TYPES,22
 NTF_OREO_THEME,1
 PHLD_BRAKE_ANGLE,2500
 PHLD_BRAKE_RATE,6
@@ -116,8 +124,8 @@ PILOT_THR_BHV,7
 PILOT_THR_FILT,2
 PILOT_TKOFF_ALT,214
 PILOT_TKOFF_DZ,250
-PSC_ACCZ_I,1.5
-PSC_ACCZ_P,0.75
+PSC_ACCZ_I,1.0
+PSC_ACCZ_P,0.5
 PSC_VELXY_I,0.51
 PSC_VELXY_P,1.41
 RC1_MAX,2000
@@ -144,7 +152,7 @@ SERVO1_FUNCTION,33
 SERVO2_FUNCTION,34
 SERVO3_FUNCTION,35
 SERVO4_FUNCTION,36
-SR1_PARAMS,30
+SR1_PARAMS,100
 THR_DZ,10
 WP_YAW_BEHAVIOR,3
 WPNAV_SPEED,300

Tools/ardupilotwaf/chibios.py

@@ -155,7 +155,8 @@ class generate_apj(Task.Task):
             "version": "0.1",
             "image_size": len(img),
             "git_identity": self.generator.bld.git_head_hash(short=True),
-            "board_revision": 0
+            "board_revision": 0,
+            "USBID": self.env.USBID
         }
         if self.env.build_dates:
             # we omit build_time when we don't have build_dates so that apj

Tools/bootloaders/CUAVv5Nano_bl.hex

@@ -991,7 +991,7 @@
 :103DD00024010001042402020524060001070582D3
 :103DE000030800FF09040100020A000000070501A2
 :103DF00002400000070581024000000012000000A0
-:103E0000043E0008120110010200004083044057E4
+:103E0000043E00081201100102000040091241574F
 :103E100000020102030100000403090425424F418E
 :103E2000524425004355415676354E616E6F003041
 :103E30003132333435363738394142434445460010

Tools/bootloaders/CUAVv5_bl.hex

@@ -991,7 +991,7 @@
 :103DD000042402020524060001070582030800FFEF
 :103DE00009040100020A000000070501024000006A
 :103DF000070581024000000012000000003E00089C
-:103E00001201100102000040830440570002010229
+:103E00001201100102000040091241570002010294
 :103E1000030100000403090425424F4152442500D8
 :103E200043554156763500303132333435363738E4
 :103E300039414243444546000080000000800000B4

Tools/bootloaders/F35Lightning_bl.hex

@@ -974,7 +974,7 @@
 :103CC000042402020524060001070582030800FF00
 :103CD00009040100020A000000070501024000007B
 :103CE000070581024000000012000000F03C0008BF
-:103CF000120110010200004083044057000201023B
+:103CF00012011001020000400912415700020102A6
 :103D0000030100000403090425424F4152442500E9
 :103D10004633354C696768746E696E6700303132BE
 :103D20003334353637383941424344454600000084

Tools/bootloaders/F4BY_bl.hex

@@ -959,7 +959,7 @@
 :103BD000042402020524060001070582030800FFF1
 :103BE00009040100020A000000070501024000006C
 :103BF000070581024000000012000000003C0008A0
-:103C0000120110010200004083044057000201022B
+:103C00001201100102000040091241570002010296
 :103C1000030100000403090425424F4152442500DA
 :103C20004634425900303132333435363738394131
 :103C300042434445460000000040000000400000B0

Tools/bootloaders/KakuteF4_bl.hex

@@ -849,7 +849,7 @@
 :1034F00024010001042402020524060001070582BC
 :10350000030800FF09040100020A0000000705018A
 :103510000240000007058102400000001200000088
-:1035200024350008120110010200004083044057B6
+:103520002435000812011001020000400912415721
 :1035300000020102030100000403090425424F4177
 :10354000524425004B616B7574654634003031324E
 :10355000333435363738394142434445460000005C

Tools/bootloaders/KakuteF7Mini_bl.hex

@@ -872,7 +872,7 @@
 :1036600001070582030800FF09040100020A0000A7
 :10367000000705010240000007058102400000002C
 :1036800012000000883600081201100102000040FC
-:1036900083044057000201020301000004030904EF
+:10369000091241570002010203010000040309045A
 :1036A00025424F41524425004B616B757465463786
 :1036B0004D696E690030313233343536373839412F
 :1036C00042434445460000000080000000800000A6

Tools/bootloaders/KakuteF7_bl.hex

@@ -871,7 +871,7 @@
 :10365000240100010424020205240600010705825A
 :10366000030800FF09040100020A00000007050129
 :103670000240000007058102400000001200000027
-:1036800084360008120110010200004083044057F4
+:10368000843600081201100102000040091241575F
 :1036900000020102030100000403090425424F4116
 :1036A000524425004B616B757465463700303132EA
 :1036B00033343536373839414243444546000000FB

Tools/bootloaders/MatekF405-STD_bl.hex

@@ -853,7 +853,7 @@
 :10353000240100010424020205240600010705827B
 :10354000030800FF09040100020A0000000705014A
 :103550000240000007058102400000001200000048
-:103560006435000812011001020000408304405736
+:1035600064350008120110010200004009124157A1
 :1035700000020102030100000403090425424F4137
 :10358000524425004D6174656B463430352D5354DB
 :1035900044003031323334353637383941424344D0

Tools/bootloaders/MatekF405-Wing_bl.hex

@@ -991,7 +991,7 @@
 :103DD000042402020524060001070582030800FFEF
 :103DE00009040100020A000000070501024000006A
 :103DF000070581024000000012000000003E00089C
-:103E00001201100102000040830440570002010229
+:103E00001201100102000040091241570002010294
 :103E1000030100000403090425424F4152442500D8
 :103E20004D6174656B463430352D57696E670030CF
 :103E30003132333435363738394142434445460010

Tools/bootloaders/MatekF405_bl.hex

@@ -853,7 +853,7 @@
 :10353000042402020524060001070582030800FF97
 :1035400009040100020A0000000705010240000012
 :1035500007058102400000001200000060350008ED
-:1035600012011001020000408304405700020102D2
+:10356000120110010200004009124157000201023D
 :10357000030100000403090425424F415244250081
 :103580004D6174656B46343035003031323334353B
 :103590003637383941424344454600000040000078

Tools/bootloaders/MatekF765-Wing_bl.hex

@@ -980,7 +980,7 @@
 :103D200001070582030800FF09040100020A0000E0
 :103D30000007050102400000070581024000000065
 :103D400012000000483D000812011001020000406E
-:103D50008304405700020102030100000403090428
+:103D50000912415700020102030100000403090493
 :103D600025424F41524425004D6174656B463736FC
 :103D7000352D57696E670030313233343536373878
 :103D80003941424344454600008000000080000065

Tools/bootloaders/NucleoH743_bl.hex

@@ -1025,7 +1025,7 @@
 :103FF00001070582030800FF09040100020A00000E
 :104000000007050102400000070581024000000092
 :1040100012000000184000081201100102000040C8
-:104020008304405700020102030100000403090455
+:1040200009124157000201020301000004030904C0
 :1040300025424F41524425004E75636C656F4837E9
 :104040003433003031323334353637383941424336
 :1040500044454600000000001D150008FD1700083B

Tools/bootloaders/OMNIBUSF7V2_bl.hex

@@ -872,7 +872,7 @@
 :103660000524060001070582030800FF0904010084
 :10367000020A000000070501024000000705810260
 :1036800040000000120000008C36000812011001FA
-:1036900002000040830440570002010203010000C1
+:10369000020000400912415700020102030100002C
 :1036A0000403090425424F41524425004F4D4E4921
 :1036B000425553463756320030313233343536377F
 :1036C0003839414243444546000000000080000074

Tools/bootloaders/OmnibusNanoV6_bl.hex

@@ -850,7 +850,7 @@
 :10350000042402020524060001070582030800FFC7
 :1035100009040100020A0000000705010240000042
 :10352000070581024000000012000000303500084D
-:103530001201100102000040830440570002010202
+:10353000120110010200004009124157000201026D
 :10354000030100000403090425424F4152442500B1
 :103550004F6D6E696275734E616E6F563600303115
 :10356000323334353637383941424344454600001A

Tools/bootloaders/PH4-mini_bl.hex

@@ -991,7 +991,7 @@
 :103DD000042402020524060001070582030800FFEF
 :103DE00009040100020A000000070501024000006A
 :103DF000070581024000000012000000003E00089C
-:103E00001201100102000040830440570002010229
+:103E00001201100102000040091241570002010294
 :103E1000030100000403090425424F4152442500D8
 :103E20005048342D6D696E69003031323334353687
 :103E300037383941424344454600000000800000C5

Tools/bootloaders/Pixhawk4_bl.hex

@@ -991,7 +991,7 @@
 :103DD000042402020524060001070582030800FFEF
 :103DE00009040100020A000000070501024000006A
 :103DF000070581024000000012000000003E00089C
-:103E00001201100102000040830440570002010229
+:103E00001201100102000040091241570002010294
 :103E1000030100000403090425424F4152442500D8
 :103E20005069786861776B3400303132333435361D
 :103E300037383941424344454600000000800000C5

Tools/bootloaders/Pixracer_bl.hex

@@ -969,7 +969,7 @@
 :103C700001070582030800FF09040100020A000091
 :103C80000007050102400000070581024000000016
 :103C900012000000983C00081201100102000040D0
-:103CA00083044057000201020301000004030904D9
+:103CA0000912415700020102030100000403090444
 :103CB00025424F4152442500506978726163657214
 :103CC0000030313233343536373839414243444598
 :103CD00046000000004000000040000000400000DE

Tools/bootloaders/TBS-Colibri-F7_bl.hex

@@ -992,7 +992,7 @@
 :103DE00001070582030800FF09040100020A000020
 :103DF00000070501024000000705810240000000A5
 :103E000012000000083E00081201100102000040EC
-:103E10008304405700020102030100000403090467
+:103E100009124157000201020301000004030904D2
 :103E200025424F41524425005442532D436F6C6943
 :103E30006272692D463700303132333435363738C7
 :103E400039414243444546000080000000800000A4

Tools/bootloaders/airbotf4_bl.hex

@@ -849,7 +849,7 @@
 :1034F00024010001042402020524060001070582BC
 :10350000030800FF09040100020A0000000705018A
 :103510000240000007058102400000001200000088
-:1035200024350008120110010200004083044057B6
+:103520002435000812011001020000400912415721
 :1035300000020102030100000403090425424F4177
 :1035400052442500616972626F7466340030313212
 :10355000333435363738394142434445460000005C