Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: new failsafe logic 

this obeys FS_TIMEOUT and FS_ACTION
This commit is contained in:
Andrew Tridgell 2013-03-29 10:25:53 +11:00
parent 0ed3061d32
commit caaf32211e
8 changed files with 73 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
APMrover2/APMrover2.pde
View File

@ -293,16 +293,14 @@ 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;
// 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;
// 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;
// Used for failsafe based on loss of RC signal or GCS signal. See
// FAILSAFE_EVENT_*
static struct {
uint8_t bits;
uint32_t rc_override_timer;
uint32_t start_time;
uint8_t triggered;
} failsafe;
////////////////////////////////////////////////////////////////////////////////
// LED output
@ -667,6 +665,7 @@ static void medium_loop()
// This case deals with the GPS
//-------------------------------
case 0:
failsafe_trigger(FAILSAFE_EVENT_GCS, last_heartbeat_ms != 0 && (millis() - last_heartbeat_ms) > 2000);
medium_loopCounter++;
update_GPS();
@ -744,7 +743,6 @@ static void slow_loop()
switch (slow_loopCounter){
case 0:
slow_loopCounter++;
check_long_failsafe();
superslow_loopCounter++;
if(superslow_loopCounter >=200) { // 200 = Execute every minute
#if HIL_MODE != HIL_MODE_ATTITUDE

8
APMrover2/GCS_Mavlink.pde
View File

@ -25,7 +25,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.triggered != 0) {
system_status = MAV_STATE_CRITICAL;
}
@ -1631,7 +1631,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
hal.rcin->set_overrides(v, 8);
rc_override_fs_timer = millis();
failsafe.rc_override_timer = millis();
failsafe_trigger(FAILSAFE_EVENT_RC, false);
break;
}
@ -1639,7 +1640,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
// 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();
last_heartbeat_ms = failsafe.rc_override_timer = millis();
failsafe_trigger(FAILSAFE_EVENT_GCS, false);
pmTest1++;
break;
}

6
APMrover2/Parameters.pde
View File

@ -241,16 +241,16 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: FS_ACTION
// @DisplayName: Failsafe Action
// @Description: What to do on a failsafe event
// @Values: 0:Nothing,1:RTL,2:STOP
// @Values: 0:Nothing,1:RTL,2:HOLD
// @User: Standard
GSCALAR(fs_action, "FS_ACTION", 0),
GSCALAR(fs_action, "FS_ACTION", 1),
// @Param: FS_TIMEOUT
// @DisplayName: Failsafe timeout
// @Description: How long a failsafe event need to happen for before we trigger the failsafe action
// @Units: seconds
// @User: Standard
GSCALAR(fs_timeout, "FS_TIMEOUT", 10),
GSCALAR(fs_timeout, "FS_TIMEOUT", 5),
// @Param: FS_THR_ENABLE
// @DisplayName: Throttle Failsafe Enable

10
APMrover2/Steering.pde
View File

@ -157,11 +157,21 @@ static void set_servos(void)
// do a direct pass through of radio values
g.channel_steer.radio_out = hal.rcin->read(CH_STEER);
g.channel_throttle.radio_out = hal.rcin->read(CH_THROTTLE);
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
// suppress throttle if in failsafe and manual
g.channel_throttle.radio_out = g.channel_throttle.radio_trim;
}
} else {
g.channel_steer.calc_pwm();
g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out,
g.throttle_min.get(),
g.throttle_max.get());
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
// suppress throttle if in failsafe
g.channel_throttle.servo_out = 0;
}
// convert 0 to 100% into PWM
g.channel_throttle.calc_pwm();

15
APMrover2/defines.h
View File

@ -14,16 +14,6 @@
#define DEBUG 0
#define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel.
// failsafe
// ----------------------
#define FAILSAFE_NONE 0
#define FAILSAFE_SHORT 1
#define FAILSAFE_LONG 2
#define FAILSAFE_GCS 3
#define FAILSAFE_SHORT_TIME 1500 // Miliiseconds
#define FAILSAFE_LONG_TIME 20000 // Miliiseconds
// active altitude sensor
// ----------------------
#define SONAR 0
@ -73,6 +63,11 @@ enum mode {
INITIALISING=16
};
// types of failsafe events
#define FAILSAFE_EVENT_THROTTLE (1<<0)
#define FAILSAFE_EVENT_GCS (1<<1)
#define FAILSAFE_EVENT_RC (1<<2)
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
#define CMD_BLANK 0 // there is no command stored in the mem location requested
#define NO_COMMAND 0

27
APMrover2/events.pde
View File

@ -1,33 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void failsafe_long_on_event(int fstype)
{
// This is how to handle a long loss of control signal failsafe.
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, "));
hal.rcin->clear_overrides();
failsafe = fstype;
switch(control_mode)
{
case MANUAL:
case LEARNING:
case STEERING:
set_mode(RTL);
break;
case AUTO:
case GUIDED:
set_mode(RTL);
break;
case RTL:
case HOLD:
default:
break;
}
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
}
static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
{
if(event_repeat == 0 || (millis() - event_timer) < event_delay)

41
APMrover2/radio.pde
View File

@ -1,10 +1,5 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//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 rover circling
static void init_rc_in()
{
// set rc channel ranges
@ -117,41 +112,9 @@ 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) {
ch3_failsafe = true;
} else {
ch3_failsafe = false;
}
//Check for failsafe and debounce funky reads
failsafe_trigger(FAILSAFE_EVENT_RC, (millis() - failsafe.rc_override_timer) > 1500);
} else if (g.fs_throttle_enabled) {
if (pwm < (unsigned)g.fs_throttle_value){
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafeCounter++;
if (failsafeCounter == 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(failsafeCounter > 0){
// we are no longer in failsafe condition
// but we need to recover quickly
failsafeCounter--;
if (failsafeCounter > 3){
failsafeCounter = 3;
}
if (failsafeCounter == 1){
gcs_send_text_fmt(PSTR("MSG FS OFF %u"), (unsigned)pwm);
}else if(failsafeCounter == 0) {
ch3_failsafe = false;
}else if (failsafeCounter <0){
failsafeCounter = -1;
}
}
failsafe_trigger(FAILSAFE_EVENT_THROTTLE, pwm < (uint16_t)g.fs_throttle_value);
}
}

58
APMrover2/system.pde
View File

@ -337,26 +337,46 @@ static void set_mode(enum mode mode)
Log_Write_Mode(control_mode);
}
static void check_long_failsafe()
/*
called to set/unset a failsafe event.
*/
static void failsafe_trigger(uint8_t failsafe_type, bool on)
{
// only act on changes
// -------------------
if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS){
if(rc_override_active && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) {
failsafe_long_on_event(FAILSAFE_LONG);
}
if(! rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) {
failsafe_long_on_event(FAILSAFE_LONG);
}
if (g.fs_gcs_enabled && millis() - rc_override_fs_timer > 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;
}
uint8_t old_bits = failsafe.bits;
if (on) {
failsafe.bits |= failsafe_type;
} else {
failsafe.bits &= ~failsafe_type;
}
if (old_bits == 0 && failsafe.bits != 0) {
// a failsafe event has started
failsafe.start_time = millis();
}
if (failsafe.triggered != 0 && failsafe.bits == 0) {
// a failsafe event has ended
gcs_send_text_fmt(PSTR("Failsafe ended"));
}
failsafe.triggered &= failsafe.bits;
if (failsafe.triggered == 0 &&
failsafe.bits != 0 &&
millis() - failsafe.start_time > g.fs_timeout*1000 &&
control_mode != RTL &&
control_mode != HOLD) {
failsafe.triggered = failsafe.bits;
gcs_send_text_fmt(PSTR("Failsafe trigger 0x%x"), (unsigned)failsafe.triggered);
switch (g.fs_action) {
case 0:
break;
case 1:
set_mode(RTL);
break;
case 2:
set_mode(HOLD);
break;
}
}
}
static void startup_INS_ground(bool force_accel_level)