From edad43611d96304a9651d93946c7df87456e0fed Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Dec 2012 08:46:36 +1100 Subject: [PATCH] Plane: reset last heartbeat time on startup completion this gives more time for the GCS to send its first heartbeat --- ArduPlane/ArduPlane.pde | 2 -- ArduPlane/GCS_Mavlink.pde | 11 +++++++---- ArduPlane/radio.pde | 2 +- ArduPlane/system.pde | 24 ++++++++++++++++++------ 4 files changed, 26 insertions(+), 13 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index f3001f0237..78dd007f43 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -301,8 +301,6 @@ static bool ch3_failsafe; // A timer used to help recovery from unusual attitudes. If we enter an unusual attitude // while in autonomous flight this variable is used to hold roll at 0 for a recovery period static uint8_t crash_timer; -// A timer used to track how long since we have received the last GCS heartbeat or rc override message -static uint32_t rc_override_fs_timer = 0; // the time when the last HEARTBEAT message arrived from a GCS static uint32_t last_heartbeat_ms; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 778aeb5312..4ea42d3e43 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1775,15 +1775,18 @@ mission_failed: hal.rcin->set_overrides(v, 8); - rc_override_fs_timer = millis(); + // a RC override message is consiered to be a 'heartbeat' from + // the ground station for failsafe purposes + last_heartbeat_ms = millis(); break; } case MAVLINK_MSG_ID_HEARTBEAT: { - // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes - if(msg->sysid != g.sysid_my_gcs) break; - last_heartbeat_ms = rc_override_fs_timer = millis(); + // We keep track of the last time we received a heartbeat from + // our GCS for failsafe purposes + if (msg->sysid != g.sysid_my_gcs) break; + last_heartbeat_ms = millis(); pmTest1++; break; } diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 39f273e240..ba46952e5a 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -134,7 +134,7 @@ static void control_failsafe(uint16_t pwm) // Check for failsafe condition based on loss of GCS control if (rc_override_active) { - if(millis() - rc_override_fs_timer > FAILSAFE_SHORT_TIME) { + if (millis() - last_heartbeat_ms > FAILSAFE_SHORT_TIME) { ch3_failsafe = true; } else { ch3_failsafe = false; diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index a304f81de5..23fd66983c 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -304,6 +304,10 @@ static void startup_ground(void) // ----------------------- demo_servos(3); + // reset last heartbeat time, so we don't trigger failsafe on slow + // startup + last_heartbeat_ms = millis(); + // we don't want writes to the serial port to cause us to pause // mid-flight, so set the serial ports non-blocking once we are // ready to fly @@ -371,23 +375,31 @@ static void set_mode(enum FlightMode mode) static void check_long_failsafe() { + uint32_t tnow = millis(); // only act on changes // ------------------- if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS) { - if(rc_override_active && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) { + if (rc_override_active && tnow - last_heartbeat_ms > FAILSAFE_LONG_TIME) { failsafe_long_on_event(FAILSAFE_LONG); } - if(!rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) { + if(!rc_override_active && failsafe == FAILSAFE_SHORT && + (tnow - ch3_failsafe_timer) > FAILSAFE_LONG_TIME) { failsafe_long_on_event(FAILSAFE_LONG); } - if(g.gcs_heartbeat_fs_enabled && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) { + if (g.gcs_heartbeat_fs_enabled && + (tnow - last_heartbeat_ms) > FAILSAFE_LONG_TIME) { failsafe_long_on_event(FAILSAFE_GCS); } } else { // We do not change state but allow for user to change mode - if(failsafe == FAILSAFE_GCS && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE; - if(failsafe == FAILSAFE_LONG && rc_override_active && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE; - if(failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) failsafe = FAILSAFE_NONE; + if (failsafe == FAILSAFE_GCS && + (tnow - last_heartbeat_ms) < FAILSAFE_SHORT_TIME) + failsafe = FAILSAFE_NONE; + if (failsafe == FAILSAFE_LONG && rc_override_active && + (tnow - last_heartbeat_ms) < FAILSAFE_SHORT_TIME) + failsafe = FAILSAFE_NONE; + if (failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) + failsafe = FAILSAFE_NONE; } }