From 4e7e84fc996d39f02cf00f7b5063869ab19656e7 Mon Sep 17 00:00:00 2001 From: James Stoyell Date: Thu, 24 Mar 2016 19:17:41 +0000 Subject: [PATCH] Plane: Modified last_valid_rc_ms AFS failsafe input to work when standard failsafe is on Ran into a bug on our physical plane where failsafe.last_valid_rc_ms was not recognizing that the transmitter had failed. This is likely due to how the standard failsafe works in receiving lower-than-possible throttle values. So in order to account for this, I added a new variable to the failsafe, AFS_last_valid_rc_ms, and I update it only if the ch3_failsafe (the throttle failsafe) is not on. If the throttle failsafe is on, that means that the plane has indeed lost transmitter input, so the AFS needs to recognize that. --- ArduPlane/ArduPlane.cpp | 2 +- ArduPlane/GCS_Mavlink.cpp | 1 + ArduPlane/Plane.h | 4 ++++ ArduPlane/radio.cpp | 5 +++++ 4 files changed, 11 insertions(+), 1 deletion(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 145d880535..463f7c2f6f 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -294,7 +294,7 @@ void Plane::obc_fs_check(void) { #if OBC_FAILSAFE == ENABLED // perform OBC failsafe checks - obc.check(OBC_MODE(control_mode), failsafe.last_heartbeat_ms, geofence_breached(), failsafe.last_valid_rc_ms); + obc.check(OBC_MODE(control_mode), failsafe.last_heartbeat_ms, geofence_breached(), failsafe.AFS_last_valid_rc_ms); #endif } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 443f70d315..87089fcea3 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1848,6 +1848,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (hal.rcin->set_overrides(v, 8)) { plane.failsafe.last_valid_rc_ms = AP_HAL::millis(); + plane.failsafe.AFS_last_valid_rc_ms = plane.failsafe.last_valid_rc_ms; } // a RC override message is consiered to be a 'heartbeat' from diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 507b5360f7..507a5ac342 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -345,6 +345,10 @@ private: uint32_t ch3_timer_ms; uint32_t last_valid_rc_ms; + + //keeps track of the last valid rc as it relates to the AFS system + //Does not count rc inputs as valid if the standard failsafe is on + uint32_t AFS_last_valid_rc_ms; } failsafe; // A counter used to count down valid gps fixes to allow the gps estimate to settle diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 81bc6acd91..6f8c60e77d 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -162,6 +162,11 @@ void Plane::read_radio() return; } + if(!failsafe.ch3_failsafe) + { + failsafe.AFS_last_valid_rc_ms = millis(); + } + failsafe.last_valid_rc_ms = millis(); elevon.ch1_temp = channel_roll->read();