diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 08333da789..e1179094cd 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -381,7 +381,6 @@ private: int8_t radio_counter; // number of iterations with throttle below throttle_fs_value - uint8_t rc_override_active : 1; // true if rc control are overwritten by ground station uint8_t radio : 1; // A status flag for the radio failsafe uint8_t gcs : 1; // A status flag for the ground station failsafe uint8_t ekf : 1; // true if ekf failsafe has occurred diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 4d0156a947..ebd59755d9 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1052,13 +1052,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) if(msg->sysid != copter.g.sysid_my_gcs) { break; // Only accept control from our gcs } - if (!rc().gcs_overrides_enabled()) { - if (copter.failsafe.rc_override_active) { // if overrides were active previously, disable them - copter.failsafe.rc_override_active = false; - RC_Channels::clear_overrides(); - } - break; - } + uint32_t tnow = AP_HAL::millis(); mavlink_rc_channels_override_t packet; @@ -1073,9 +1067,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) RC_Channels::set_override(6, packet.chan7_raw, tnow); RC_Channels::set_override(7, packet.chan8_raw, tnow); - // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation - copter.failsafe.rc_override_active = RC_Channels::has_active_overrides(); - // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes copter.failsafe.last_heartbeat_ms = tnow; break; @@ -1110,9 +1101,6 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) RC_Channels::set_override(uint8_t(copter.rcmap.throttle() - 1), throttle, tnow); RC_Channels::set_override(uint8_t(copter.rcmap.yaw() - 1), yaw, tnow); - // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation - copter.failsafe.rc_override_active = RC_Channels::has_active_overrides(); - // a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes copter.failsafe.last_heartbeat_ms = tnow; break; diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 64a2ad76d6..ff02e062ef 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -92,7 +92,7 @@ void Copter::failsafe_gcs_check() // return immediately if gcs failsafe is disabled, gcs has never been connected or we are not overriding rc controls from the gcs and we are not in guided mode // this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state. - if ((!failsafe.gcs)&&(g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0 || (!failsafe.rc_override_active && control_mode != GUIDED))) { + if ((!failsafe.gcs)&&(g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0 || (!RC_Channels::has_active_overrides() && control_mode != GUIDED))) { return; } @@ -122,7 +122,6 @@ void Copter::failsafe_gcs_check() // clear overrides so that RC control can be regained with radio. RC_Channels::clear_overrides(); - failsafe.rc_override_active = false; if (should_disarm_on_failsafe()) { init_disarm_motors(); diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 89db9527f1..00e22bbcaa 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -90,7 +90,7 @@ void Copter::enable_motor_output() void Copter::read_radio() { - uint32_t tnow_ms = millis(); + const uint32_t tnow_ms = millis(); if (rc().read_input()) { ap.new_radio_frame = true; @@ -98,9 +98,10 @@ void Copter::read_radio() set_throttle_and_failsafe(channel_throttle->get_radio_in()); set_throttle_zero_flag(channel_throttle->get_control_in()); - // flag we must have an rc receiver attached - if (!failsafe.rc_override_active) { - ap.rc_receiver_present = true; + if (!ap.rc_receiver_present) { + // RC receiver must be attached if we've just go input and + // there are no overrides + ap.rc_receiver_present = !RC_Channels::has_active_overrides(); } // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters) @@ -112,7 +113,8 @@ void Copter::read_radio() }else{ uint32_t elapsed = tnow_ms - last_radio_update_ms; // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE - if (((!failsafe.rc_override_active && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (failsafe.rc_override_active && (elapsed >= FS_RADIO_RC_OVERRIDE_TIMEOUT_MS))) && + const bool has_active_overrides = RC_Channels::has_active_overrides(); + if (((!has_active_overrides && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (has_active_overrides && (elapsed >= FS_RADIO_RC_OVERRIDE_TIMEOUT_MS))) && (g.failsafe_throttle && (ap.rc_receiver_present||motors->armed()) && !failsafe.radio)) { Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME); set_failsafe_radio(true);