Added switches for 3.6.7 for failure testing DO NOT MERGE

ArduCopter/Copter.h

@@ -675,6 +675,10 @@ class Copter : public AP_HAL::HAL::Callbacks {
                   "_failsafe_priorities is missing the sentinel");
 
 
+    struct {
+        bool kill_all;
+        bool kill_mot[4];
+    } motkill;
 
     // AP_State.cpp
     void set_auto_armed(bool b);

ArduCopter/defines.h

@@ -78,6 +78,15 @@ enum aux_sw_func {
     AUXSW_USER_FUNC1 =          47, // user function #1
     AUXSW_USER_FUNC2 =          48, // user function #2
     AUXSW_USER_FUNC3 =          49, // user function #3
+    AUXSW_KILL_ALL   =         110,
+    AUXSW_KILL_MOT1  =         111,
+    AUXSW_KILL_MOT2  =         112,
+    AUXSW_KILL_MOT3  =         113,
+    AUXSW_KILL_MOT4  =         114,
+    AUXSW_KILL_MAGPRIMARY =    115,
+    AUXSW_KILL_RANGEFINDER =   116,
+    AUXSW_KILL_GPS1 =          117,
+    AUXSW_KILL_GPS2 =          118,
     AUXSW_SWITCH_MAX,
 };
 

ArduCopter/motors.cpp

@@ -343,6 +343,13 @@ void Copter::motors_output()
         motors->output();
     }
 
+    // implement motor kill for parachute testing
+    for (uint8_t i=0; i<4; i++) {
+        if (motkill.kill_all || motkill.kill_mot[i]) {
+            hal.rcout->write(i, 1000);
+        }
+    }
+
     // push all channels
     SRV_Channels::push();
 }

ArduCopter/switches.cpp

@@ -235,6 +235,11 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
         case AUXSW_INVERTED:
         case AUXSW_WINCH_ENABLE:
         case AUXSW_RC_OVERRIDE_ENABLE:
+        case AUXSW_KILL_ALL:
+        case AUXSW_KILL_MOT1:
+        case AUXSW_KILL_MOT2:
+        case AUXSW_KILL_MOT3:
+        case AUXSW_KILL_MOT4:
             do_aux_switch_function(ch_option, ch_flag);
             break;
     }
@@ -761,7 +766,35 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
                 }
             }
             break;
-
+
+    case AUXSW_KILL_ALL:
+        motkill.kill_all = (ch_flag == AUX_SWITCH_HIGH);
+        break;
+    case AUXSW_KILL_MOT1:
+        motkill.kill_mot[0] = (ch_flag == AUX_SWITCH_HIGH);
+        break;
+    case AUXSW_KILL_MOT2:
+        motkill.kill_mot[1] = (ch_flag == AUX_SWITCH_HIGH);
+        break;
+    case AUXSW_KILL_MOT3:
+        motkill.kill_mot[2] = (ch_flag == AUX_SWITCH_HIGH);
+        break;
+    case AUXSW_KILL_MOT4:
+        motkill.kill_mot[3] = (ch_flag == AUX_SWITCH_HIGH);
+        break;
+    case AUXSW_KILL_MAGPRIMARY:
+        compass.set_kill_primary(ch_flag == AUX_SWITCH_HIGH);
+        break;
+    case AUXSW_KILL_RANGEFINDER:
+        rangefinder.set_kill(ch_flag == AUX_SWITCH_HIGH);
+        break;
+    case AUXSW_KILL_GPS1:
+        gps.set_kill(0, ch_flag == AUX_SWITCH_HIGH);
+        break;
+    case AUXSW_KILL_GPS2:
+        gps.set_kill(1, ch_flag == AUX_SWITCH_HIGH);
+        break;
+
 #ifdef USERHOOK_AUXSWITCH
         case AUXSW_USER_FUNC1:
             userhook_auxSwitch1(ch_flag);

libraries/AP_Compass/AP_Compass.h

@@ -340,6 +340,11 @@ friend class AP_Compass_Backend;
     MAV_RESULT mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
                                  float lat_deg, float lon_deg);
 
+    // option to kill primary compass
+    void set_kill_primary(bool set) {
+        _kill_primary = set;
+    }
+
 private:
     static Compass *_singleton;
     /// Register a new compas driver, allocating an instance number
@@ -498,6 +503,9 @@ friend class AP_Compass_Backend;
     AP_Int32 _driver_type_mask;
 
     AP_Int8 _filter_range;
+
+    // enable killing of primary sensor
+    bool _kill_primary;
 };
 
 namespace AP {

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -33,6 +33,9 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
 
 void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint8_t instance)
 {
+    if (is_disabled(instance)) {
+        return;
+    }
     Compass::mag_state &state = _compass._state[instance];
 
     // note that we do not set last_update_usec here as otherwise the
@@ -106,6 +109,9 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i)
  */
 void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t instance)
 {
+    if (is_disabled(instance)) {
+        return;
+    }
     Compass::mag_state &state = _compass._state[instance];
 
     state.field = mag;
@@ -116,6 +122,9 @@ void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag, uint8_t ins
 
 void AP_Compass_Backend::set_last_update_usec(uint32_t last_update, uint8_t instance)
 {
+    if (is_disabled(instance)) {
+        return;
+    }
     Compass::mag_state &state = _compass._state[instance];
     state.last_update_usec = last_update;
 }
@@ -209,3 +218,8 @@ bool AP_Compass_Backend::field_ok(const Vector3f &field)
     return ret;
 }
 
+// return true if this is disabled using kill_primary
+bool AP_Compass_Backend::is_disabled(uint8_t instance)
+{
+    return instance == _compass.get_primary() && _compass._kill_primary;
+}

libraries/AP_Compass/AP_Compass_Backend.h

@@ -123,4 +123,7 @@ class AP_Compass_Backend
     float _mean_field_length;
     // number of dropped samples. Not used for now, but can be usable to choose more reliable sensor
     uint32_t _error_count;
+
+    // return true if this is disabled using kill_primary
+    bool is_disabled(uint8_t instance);
 };

libraries/AP_GPS/AP_GPS.cpp

@@ -655,7 +655,9 @@ void AP_GPS::update_instance(uint8_t instance)
     bool data_should_be_logged = false;
     if (!result) {
         if (tnow - timing[instance].last_message_time_ms > GPS_TIMEOUT_MS) {
+            bool disabled = state[instance].disabled;
             memset(&state[instance], 0, sizeof(state[instance]));
+            state[instance].disabled = disabled;
             state[instance].instance = instance;
             state[instance].hdop = GPS_UNKNOWN_DOP;
             state[instance].vdop = GPS_UNKNOWN_DOP;

libraries/AP_GPS/AP_GPS.h

@@ -160,6 +160,9 @@ class AP_GPS
         int32_t  rtk_baseline_z_mm;        ///< Current baseline in ECEF z or NED down component in mm
         uint32_t rtk_accuracy;             ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999)
         int32_t  rtk_iar_num_hypotheses;   ///< Current number of integer ambiguity hypotheses
+
+        // disabled by user switch
+        bool disabled;
     };
 
     /// Startup initialisation.
@@ -426,6 +429,10 @@ class AP_GPS
     // handle possibly fragmented RTCM injection data
     void handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len);
 
+    void set_kill(uint8_t instance, bool set) {
+        state[instance].disabled = set;
+    }
+
 protected:
 
     // configuration parameters

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -387,6 +387,9 @@ AP_GPS_UBLOX::read(void)
 
         // read the next byte
         data = port->read();
+        if (state.disabled) {
+            continue;
+        }
 
         _stats.total_bytes++;
 

libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp

@@ -476,6 +476,10 @@ void AP_RangeFinder_LightWareI2C::update(void)
 
 void AP_RangeFinder_LightWareI2C::legacy_timer(void)
 {
+    if (state.disabled) {
+        set_status(RangeFinder::RangeFinder_NoData);
+        return;
+    }
     if (legacy_get_reading(state.distance_cm)) {
         // update range_valid state based on distance measured
         update_status();
@@ -486,6 +490,10 @@ void AP_RangeFinder_LightWareI2C::legacy_timer(void)
 
 void AP_RangeFinder_LightWareI2C::sf20_timer(void)
 {
+    if (state.disabled) {
+        set_status(RangeFinder::RangeFinder_NoData);
+        return;
+    }
     if (sf20_get_reading(state.distance_cm)) {
         // update range_valid state based on distance measured
         update_status();

libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp

@@ -115,7 +115,9 @@ void AP_RangeFinder_analog::update(void)
     if (dist_m < 0) {
         dist_m = 0;
     }
-    state.distance_cm = dist_m * 100.0f;  
+    if (!state.disabled) {
+        state.distance_cm = dist_m * 100.0f;
+    }
 
     // update range_valid state based on distance measured
     update_status();

libraries/AP_RangeFinder/RangeFinder.h

@@ -106,6 +106,9 @@ class RangeFinder
         AP_Vector3f pos_offset; // position offset in body frame
         AP_Int8  orientation;
         const struct AP_Param::GroupInfo *var_info;
+
+        // disabled by user switch
+        bool disabled;
     };
 
     static const struct AP_Param::GroupInfo *backend_var_info[RANGEFINDER_MAX_INSTANCES];
@@ -169,6 +172,12 @@ class RangeFinder
 
     static RangeFinder *get_singleton(void) { return _singleton; }
 
+    // allow for killing of rangefinders
+    void set_kill(bool set) {
+        for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
+            state[i].disabled = set;
+        }
+    }
 
 private:
     static RangeFinder *_singleton;

libraries/AP_RangeFinder/RangeFinder_Backend.cpp

@@ -33,6 +33,10 @@ AP_RangeFinder_Backend::AP_RangeFinder_Backend(RangeFinder::RangeFinder_State &_
 // update status based on distance measurement
 void AP_RangeFinder_Backend::update_status()
 {
+    if (state.disabled) {
+        state.status = RangeFinder::RangeFinder_NoData;
+        return;
+    }
     // check distance
     if ((int16_t)state.distance_cm > state.max_distance_cm) {
         set_status(RangeFinder::RangeFinder_OutOfRangeHigh);
@@ -46,6 +50,10 @@ void AP_RangeFinder_Backend::update_status()
 // set status and update valid count
 void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_Status _status)
 {
+    if (state.disabled) {
+        state.status = RangeFinder::RangeFinder_NoData;
+        return;
+    }
     state.status = _status;
 
     // update valid count
@@ -66,6 +74,9 @@ void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_Status _status)
  */
 void AP_RangeFinder_Backend::update_pre_arm_check()
 {
+    if (state.disabled) {
+        return;
+    }
     // return immediately if already passed or no sensor data
     if (state.pre_arm_check || state.status == RangeFinder::RangeFinder_NotConnected || state.status == RangeFinder::RangeFinder_NoData) {
         return;