diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 7242bdac9d..8feee8d183 100644 --- a/ArduCopter/Copter.h +++ b/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); diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 2be1b95f68..85e94de917 100644 --- a/ArduCopter/defines.h +++ 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, }; diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 47b705118b..aa5b20ff20 100644 --- a/ArduCopter/motors.cpp +++ b/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(); } diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index ec8e81d9d3..ad00dc3f22 100644 --- a/ArduCopter/switches.cpp +++ b/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); diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index cd89f5ccc6..769ffbc0d9 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/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 { diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 61f13aa4bc..de1970ff2c 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/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; +} diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index 8d5f2f6789..d80ce54166 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/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); }; diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index a69ee41d18..505cf7bbf6 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/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; diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 752e7be87d..46d9b50732 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/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 diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 36fceebe84..6cf3e97db7 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/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++; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp index cf2088415c..7d4f0aae06 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareI2C.cpp +++ b/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(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp index 50906c1f04..b97abc3886 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp +++ b/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(); diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 7ca2bf4528..7f9b63a789 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/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 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;