aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: reset last heartbeat time on startup completion

Browse Source 
this gives more time for the GCS to send its first heartbeat
This commit is contained in:
Andrew Tridgell 2012-12-26 08:46:36 +11:00
parent 85e4f67e68
commit edad43611d
4 changed files with 26 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.pde
View File

@ -301,8 +301,6 @@ static bool ch3_failsafe;
// A timer used to help recovery from unusual attitudes. If we enter an unusual attitude // 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 // while in autonomous flight this variable is used to hold roll at 0 for a recovery period
static uint8_t crash_timer; 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 // the time when the last HEARTBEAT message arrived from a GCS
static uint32_t last_heartbeat_ms; static uint32_t last_heartbeat_ms;

11
ArduPlane/GCS_Mavlink.pde
View File

@ -1775,15 +1775,18 @@ mission_failed:
hal.rcin->set_overrides(v, 8); 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; break;
} }
case MAVLINK_MSG_ID_HEARTBEAT: 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
if(msg->sysid != g.sysid_my_gcs) break; // our GCS for failsafe purposes
last_heartbeat_ms = rc_override_fs_timer = millis(); if (msg->sysid != g.sysid_my_gcs) break;
last_heartbeat_ms = millis();
pmTest1++; pmTest1++;
break; break;
} }

2
ArduPlane/radio.pde
View File

@ -134,7 +134,7 @@ static void control_failsafe(uint16_t pwm)
// Check for failsafe condition based on loss of GCS control // Check for failsafe condition based on loss of GCS control
if (rc_override_active) { if (rc_override_active) {
if(millis() - rc_override_fs_timer > FAILSAFE_SHORT_TIME) { if (millis() - last_heartbeat_ms > FAILSAFE_SHORT_TIME) {
ch3_failsafe = true; ch3_failsafe = true;
} else { } else {
ch3_failsafe = false; ch3_failsafe = false;

24
ArduPlane/system.pde
View File

@ -304,6 +304,10 @@ static void startup_ground(void)
// ----------------------- // -----------------------
demo_servos(3); 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 // 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 // mid-flight, so set the serial ports non-blocking once we are
// ready to fly // ready to fly
@ -371,23 +375,31 @@ static void set_mode(enum FlightMode mode)
static void check_long_failsafe() static void check_long_failsafe()
{ {
uint32_t tnow = millis();
// only act on changes // only act on changes
// ------------------- // -------------------
if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS) { 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); 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); 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); failsafe_long_on_event(FAILSAFE_GCS);
} }
} else { } else {
// We do not change state but allow for user to change mode // 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_GCS &&
if(failsafe == FAILSAFE_LONG && rc_override_active && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE; (tnow - last_heartbeat_ms) < FAILSAFE_SHORT_TIME)
if(failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) failsafe = FAILSAFE_NONE; 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;
} }
} }