From 10e6fe43dde28e57868e0698c7a3f09a569599c9 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Tue, 23 Jan 2018 13:54:27 +0100 Subject: [PATCH] Copter: Add a switch option to enable/disable RC_Override --- ArduCopter/Copter.h | 1 + ArduCopter/GCS_Mavlink.cpp | 7 +++++++ ArduCopter/defines.h | 1 + ArduCopter/switches.cpp | 15 +++++++++++++++ 4 files changed, 24 insertions(+) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 261ea101ce..9a6ec29a27 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -288,6 +288,7 @@ private: uint8_t in_arming_delay : 1; // 24 // true while we are armed but waiting to spin motors uint8_t initialised_params : 1; // 25 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done uint8_t compass_init_location : 1; // 26 // true when the compass's initial location has been set + uint8_t rc_override_enable : 1; // 27 // aux switch rc_override is allowed }; uint32_t value; } ap_t; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index fdce6cdc7c..fcd1775ad1 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -761,6 +761,13 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) // or for complete GCS control of switch position // and RC PWM values. if(msg->sysid != copter.g.sysid_my_gcs) break; // Only accept control from our gcs + if (!copter.ap.rc_override_enable) { + if (copter.failsafe.rc_override_active) { // if overrides were active previously, disable them + copter.failsafe.rc_override_active = false; + hal.rcin->clear_overrides(); + } + break; + } mavlink_rc_channels_override_t packet; int16_t v[8]; mavlink_msg_rc_channels_override_decode(msg, &packet); diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index f64256c9c8..5db91a9d9b 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -74,6 +74,7 @@ enum aux_sw_func { AUXSW_INVERTED = 43, // enable inverted flight AUXSW_WINCH_ENABLE = 44, // winch enable/disable AUXSW_WINCH_CONTROL = 45, // winch control + AUXSW_RC_OVERRIDE_ENABLE = 46, // enable RC Override AUXSW_SWITCH_MAX, }; diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index d49c603728..2918a6dc4f 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -198,6 +198,7 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) case AUXSW_AVOID_PROXIMITY: case AUXSW_INVERTED: case AUXSW_WINCH_ENABLE: + case AUXSW_RC_OVERRIDE_ENABLE: do_aux_switch_function(ch_option, ch_flag); break; } @@ -657,6 +658,20 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; } break; + + case AUXSW_RC_OVERRIDE_ENABLE: + // Allow or disallow RC_Override + switch (ch_flag) { + case AUX_SWITCH_HIGH: { + ap.rc_override_enable = true; + break; + } + case AUX_SWITCH_LOW: { + ap.rc_override_enable = false; + break; + } + } + break; } }