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
// 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;

11
ArduPlane/GCS_Mavlink.pde
View File

@ -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;
}

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
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;

24
ArduPlane/system.pde
View File

@ -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;
}
}