diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 75e8e65fcc..34a9ab1b1f 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -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 diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 30aa3d9285..e7c7e0272e 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -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; } diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 8e2b7a7a80..390d06b670 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -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 diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde index dfadba63e4..3144bee240 100644 --- a/APMrover2/Steering.pde +++ b/APMrover2/Steering.pde @@ -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(); diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 05e97f762f..0330bf737f 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -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 diff --git a/APMrover2/events.pde b/APMrover2/events.pde index 15d7974fb3..4b08195906 100644 --- a/APMrover2/events.pde +++ b/APMrover2/events.pde @@ -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) diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde index dde3bdf390..8250d3d9a2 100644 --- a/APMrover2/radio.pde +++ b/APMrover2/radio.pde @@ -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); } } diff --git a/APMrover2/system.pde b/APMrover2/system.pde index 22846abf0b..bee5b90e29 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -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)