From 6167e6ac0bcd90dcfd02eaf29ffb30e4ca45a5ce Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 31 Oct 2018 14:27:56 +0900 Subject: [PATCH] Rover: gcs fs not triggered if rc-overrides arrive also not triggered if manual-control messages arrive also minor restructure of gcs_failsafe_check (non-functional) also remove unnecessary failsafe_trigger call when heartbeats arrive --- APMrover2/APMrover2.cpp | 8 ++++++-- APMrover2/GCS_Mavlink.cpp | 8 +++++--- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index cc071702a0..6030ee0429 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -196,9 +196,13 @@ void Rover::ahrs_update() */ void Rover::gcs_failsafe_check(void) { - if (g.fs_gcs_enabled) { - failsafe_trigger(FAILSAFE_EVENT_GCS, failsafe.last_heartbeat_ms != 0 && (millis() - failsafe.last_heartbeat_ms) > 2000); + if (!g.fs_gcs_enabled) { + // gcs failsafe disabled + return; } + + // check for updates from GCS within 2 seconds + failsafe_trigger(FAILSAFE_EVENT_GCS, failsafe.last_heartbeat_ms != 0 && (millis() - failsafe.last_heartbeat_ms) > 2000); } /* diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 272c8d4ccc..6162524f0d 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -780,6 +780,8 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) RC_Channels::set_override(6, packet.chan7_raw, tnow); RC_Channels::set_override(7, packet.chan8_raw, tnow); + // an RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes + rover.failsafe.last_heartbeat_ms = tnow; break; } @@ -803,18 +805,18 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) RC_Channels::set_override(uint8_t(rover.rcmap.roll() - 1), roll, tnow); RC_Channels::set_override(uint8_t(rover.rcmap.throttle() - 1), throttle, tnow); + // a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes + rover.failsafe.last_heartbeat_ms = tnow; break; } case MAVLINK_MSG_ID_HEARTBEAT: { - // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes + // we keep track of the last time we received a heartbeat from our GCS for failsafe purposes if (msg->sysid != rover.g.sysid_my_gcs) { break; } - rover.failsafe.last_heartbeat_ms = AP_HAL::millis(); - rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false); break; }