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

Plane: move failsafe variables into a structure

Browse Source 
this makes the logic a bit easier to follow
This commit is contained in:
Andrew Tridgell 2013-07-19 14:11:16 +10:00
parent e2fbc00b52
commit 51b9cf3e76
8 changed files with 77 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
ArduPlane/ArduPlane.pde
View File

@ -334,24 +334,32 @@ static struct {
ch2_temp : 1500
};
// A flag if GCS joystick control is in use
static bool rc_override_active = false;
////////////////////////////////////////////////////////////////////////////////
// Failsafe
////////////////////////////////////////////////////////////////////////////////
// A tracking variable for type of failsafe active
// Used for failsafe based on loss of RC signal or GCS signal
static int16_t failsafe;
// Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
// RC receiver should be set up to output a low throttle value when signal is lost
static bool ch3_failsafe;
static struct {
// A flag if GCS joystick control is in use
bool rc_override_active;
// the time when the last HEARTBEAT message arrived from a GCS
static uint32_t last_heartbeat_ms;
// A tracking variable for type of failsafe active
// Used for failsafe based on loss of RC signal or GCS signal
int16_t state;
// Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
// RC receiver should be set up to output a low throttle value when signal is lost
bool ch3_failsafe;
// number of low ch3 values
uint8_t ch3_counter;
// the time when the last HEARTBEAT message arrived from a GCS
uint32_t last_heartbeat_ms;
// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
uint32_t ch3_timer_ms;
} failsafe;
// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
static uint32_t ch3_failsafe_timer = 0;
////////////////////////////////////////////////////////////////////////////////
// LED output

2
ArduPlane/Attitude.pde
View File

@ -42,7 +42,7 @@ static bool stick_mixing_enabled(void)
// we're in an auto mode. Check the stick mixing flag
if (g.stick_mixing != STICK_MIXING_DISABLED &&
geofence_stickmixing() &&
failsafe == FAILSAFE_NONE &&
failsafe.state == FAILSAFE_NONE &&
(g.throttle_fs_enabled == 0 ||
channel_throttle->radio_in >= g.throttle_fs_value)) {
// we're in an auto mode, and haven't triggered failsafe

6
ArduPlane/GCS_Mavlink.pde
View File

@ -28,7 +28,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
if (failsafe != FAILSAFE_NONE) {
if (failsafe.state != FAILSAFE_NONE) {
system_status = MAV_STATE_CRITICAL;
}
@ -1911,7 +1911,7 @@ mission_failed:
// a RC override message is consiered to be a 'heartbeat' from
// the ground station for failsafe purposes
last_heartbeat_ms = millis();
failsafe.last_heartbeat_ms = millis();
break;
}
@ -1920,7 +1920,7 @@ mission_failed:
// 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();
failsafe.last_heartbeat_ms = millis();
break;
}

2
ArduPlane/control_modes.pde
View File

@ -10,7 +10,7 @@ static void read_control_switch()
// If we get this value we do not want to change modes.
if(switchPosition == 255) return;
if (ch3_failsafe) {
if (failsafe.ch3_failsafe) {
// when we are in ch3_failsafe mode then RC input is not
// working, and we need to ignore the mode switch channel
return;

10
ArduPlane/defines.h
View File

@ -18,10 +18,12 @@
// failsafe
// ----------------------
#define FAILSAFE_NONE 0
#define FAILSAFE_SHORT 1
#define FAILSAFE_LONG 2
#define FAILSAFE_GCS 3
enum failsafe_state {
FAILSAFE_NONE=0,
FAILSAFE_SHORT=1,
FAILSAFE_LONG=2,
FAILSAFE_GCS=3
};
// active altitude sensor

12
ArduPlane/events.pde
View File

@ -1,11 +1,11 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void failsafe_short_on_event(int16_t fstype)
static void failsafe_short_on_event(enum failsafe_state fstype)
{
// This is how to handle a short loss of control signal failsafe.
failsafe = fstype;
ch3_failsafe_timer = millis();
failsafe.state = fstype;
failsafe.ch3_timer_ms = millis();
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
switch(control_mode)
{
@ -35,13 +35,13 @@ static void failsafe_short_on_event(int16_t fstype)
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
}
static void failsafe_long_on_event(int16_t fstype)
static void failsafe_long_on_event(enum failsafe_state fstype)
{
// This is how to handle a long loss of control signal failsafe.
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, "));
// If the GCS is locked up we allow control to revert to RC
hal.rcin->clear_overrides();
failsafe = fstype;
failsafe.state = fstype;
switch(control_mode)
{
case MANUAL:
@ -74,7 +74,7 @@ static void failsafe_short_off_event()
{
// We're back in radio contact
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off"));
failsafe = FAILSAFE_NONE;
failsafe.state = FAILSAFE_NONE;
// re-read the switch so we can return to our preferred mode
// --------------------------------------------------------

35
ArduPlane/radio.pde
View File

@ -2,7 +2,6 @@
//Function that will read the radio data, limit servos and trigger a failsafe
// ----------------------------------------------------------------------------
static uint8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
/*
allow for runtime change of control channel ordering
@ -127,11 +126,11 @@ static void control_failsafe(uint16_t pwm)
return;
// Check for failsafe condition based on loss of GCS control
if (rc_override_active) {
if (millis() - last_heartbeat_ms > g.short_fs_timeout*1000) {
ch3_failsafe = true;
if (failsafe.rc_override_active) {
if (millis() - failsafe.last_heartbeat_ms > g.short_fs_timeout*1000) {
failsafe.ch3_failsafe = true;
} else {
ch3_failsafe = false;
failsafe.ch3_failsafe = false;
}
//Check for failsafe and debounce funky reads
@ -139,26 +138,26 @@ static void control_failsafe(uint16_t pwm)
if (pwm < (unsigned)g.throttle_fs_value) {
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafeCounter++;
if (failsafeCounter == 9) {
failsafe.ch3_counter++;
if (failsafe.ch3_counter == 9) {
gcs_send_text_fmt(PSTR("MSG FS ON %u"), (unsigned)pwm);
}else if(failsafeCounter == 10) {
ch3_failsafe = true;
}else if (failsafeCounter > 10) {
failsafeCounter = 11;
}else if(failsafe.ch3_counter == 10) {
failsafe.ch3_failsafe = true;
}else if (failsafe.ch3_counter > 10) {
failsafe.ch3_counter = 11;
}
}else if(failsafeCounter > 0) {
}else if(failsafe.ch3_counter > 0) {
// we are no longer in failsafe condition
// but we need to recover quickly
failsafeCounter--;
if (failsafeCounter > 3) {
failsafeCounter = 3;
failsafe.ch3_counter--;
if (failsafe.ch3_counter > 3) {
failsafe.ch3_counter = 3;
}
if (failsafeCounter == 1) {
if (failsafe.ch3_counter == 1) {
gcs_send_text_fmt(PSTR("MSG FS OFF %u"), (unsigned)pwm);
} else if(failsafeCounter == 0) {
ch3_failsafe = false;
} else if(failsafe.ch3_counter == 0) {
failsafe.ch3_failsafe = false;
}
}
}

42
ArduPlane/system.pde
View File

@ -306,7 +306,7 @@ static void startup_ground(void)
// reset last heartbeat time, so we don't trigger failsafe on slow
// startup
last_heartbeat_ms = millis();
failsafe.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
@ -411,27 +411,31 @@ static void check_long_failsafe()
uint32_t tnow = millis();
// only act on changes
// -------------------
if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS) {
if (rc_override_active && (tnow - last_heartbeat_ms) > g.long_fs_timeout*1000) {
if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS) {
if (failsafe.rc_override_active && (tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_LONG);
} else if (!rc_override_active && failsafe == FAILSAFE_SHORT &&
(tnow - ch3_failsafe_timer) > g.long_fs_timeout*1000) {
} else if (!failsafe.rc_override_active &&
failsafe.state == FAILSAFE_SHORT &&
(tnow - failsafe.ch3_timer_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_LONG);
} else if (g.gcs_heartbeat_fs_enabled &&
last_heartbeat_ms != 0 &&
(tnow - last_heartbeat_ms) > g.long_fs_timeout*1000) {
failsafe.last_heartbeat_ms != 0 &&
(tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_GCS);
}
} else {
// We do not change state but allow for user to change mode
if (failsafe == FAILSAFE_GCS &&
(tnow - last_heartbeat_ms) < g.short_fs_timeout*1000) {
failsafe = FAILSAFE_NONE;
} else if (failsafe == FAILSAFE_LONG && rc_override_active &&
(tnow - last_heartbeat_ms) < g.short_fs_timeout*1000) {
failsafe = FAILSAFE_NONE;
} else if (failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) {
failsafe = FAILSAFE_NONE;
if (failsafe.state == FAILSAFE_GCS &&
(tnow - failsafe.last_heartbeat_ms) < g.short_fs_timeout*1000) {
failsafe.state = FAILSAFE_NONE;
} else if (failsafe.state == FAILSAFE_LONG &&
failsafe.rc_override_active &&
(tnow - failsafe.last_heartbeat_ms) < g.short_fs_timeout*1000) {
failsafe.state = FAILSAFE_NONE;
} else if (failsafe.state == FAILSAFE_LONG &&
!failsafe.rc_override_active &&
!failsafe.ch3_failsafe) {
failsafe.state = FAILSAFE_NONE;
}
}
}
@ -440,14 +444,14 @@ static void check_short_failsafe()
{
// only act on changes
// -------------------
if(failsafe == FAILSAFE_NONE) {
if(ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde
if(failsafe.state == FAILSAFE_NONE) {
if(failsafe.ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde
failsafe_short_on_event(FAILSAFE_SHORT);
}
}
if(failsafe == FAILSAFE_SHORT) {
if(!ch3_failsafe) {
if(failsafe.state == FAILSAFE_SHORT) {
if(!failsafe.ch3_failsafe) {
failsafe_short_off_event();
}
}