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

Copter: move failsafe flags into structure

This commit is contained in:
Randy Mackay 2013-09-26 17:54:33 +09:00
parent f144de21b4
commit 7cc8a9038f
10 changed files with 45 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/AP_State.pde
View File

@ -46,13 +46,13 @@ static void set_failsafe_radio(bool b)
{
// only act on changes
// -------------------
if(ap.failsafe_radio != b) {
if(failsafe.radio != b) {
// store the value so we don't trip the gate twice
// -----------------------------------------------
ap.failsafe_radio = b;
failsafe.radio = b;
if (ap.failsafe_radio == false) {
if (failsafe.radio == false) {
// We've regained radio contact
// ----------------------------
failsafe_radio_off_event();
@ -79,7 +79,7 @@ void set_low_battery(bool b)
// ---------------------------------------------
static void set_failsafe_gps(bool b)
{
ap.failsafe_gps = b;
failsafe.gps = b;
// update AP_Notify
AP_Notify::flags.failsafe_gps = b;
@ -88,7 +88,7 @@ static void set_failsafe_gps(bool b)
// ---------------------------------------------
static void set_failsafe_gcs(bool b)
{
ap.failsafe_gcs = b;
failsafe.gcs = b;
}
// ---------------------------------------------

21
ArduCopter/ArduCopter.pde
View File

@ -392,11 +392,6 @@ static union {
uint8_t logging_started : 1; // 8 // true if dataflash logging has started
uint8_t low_battery : 1; // 9 // Used to track if the battery is low - LED output flashes when the batt is low
uint8_t failsafe_radio : 1; // 10 // A status flag for the radio failsafe
uint8_t failsafe_batt : 1; // 11 // A status flag for the battery failsafe
uint8_t failsafe_gps : 1; // 12 // A status flag for the gps failsafe
uint8_t failsafe_gcs : 1; // 13 // A status flag for the ground station failsafe
uint8_t rc_override_active : 1; // 14 // true if rc control are overwritten by ground station
uint8_t do_flip : 1; // 15 // Used to enable flip code
uint8_t takeoff_complete : 1; // 16
uint8_t land_complete : 1; // 17 // true if we have detected a landing
@ -433,6 +428,20 @@ static RCMapper rcmap;
// receiver RSSI
static uint8_t receiver_rssi;
////////////////////////////////////////////////////////////////////////////////
// Failsafe
////////////////////////////////////////////////////////////////////////////////
static struct {
uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station
uint8_t radio : 1; // 1 // A status flag for the radio failsafe
uint8_t batt : 1; // 2 // A status flag for the battery failsafe
uint8_t gps : 1; // 3 // A status flag for the gps failsafe
uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe
int8_t radio_counter; // number of iterations with throttle below throttle_fs_value
uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
} failsafe;
////////////////////////////////////////////////////////////////////////////////
// Motor Output
@ -798,8 +807,6 @@ static int16_t superslow_loopCounter;
static uint32_t rtl_loiter_start_time;
// prevents duplicate GPS messages from entering system
static uint32_t last_gps_time;
// the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
static uint32_t last_heartbeat_ms;
// Used to exit the roll and pitch auto trim function
static uint8_t auto_trim_counter;

4
ArduCopter/Attitude.pde
View File

@ -1094,7 +1094,7 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
int16_t desired_rate = 0;
// throttle failsafe check
if( ap.failsafe_radio ) {
if( failsafe.radio ) {
return 0;
}
@ -1298,7 +1298,7 @@ get_throttle_land()
get_throttle_rate_stabilized(-abs(g.land_speed));
// disarm when the landing detector says we've landed and throttle is at min (or we're in failsafe so we have no pilot thorottle input)
if( ap.land_complete && (g.rc_3.control_in == 0 || ap.failsafe_radio) ) {
if( ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio) ) {
init_disarm_motors();
}
}

8
ArduCopter/GCS_Mavlink.pde
View File

@ -47,7 +47,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
uint8_t system_status = ap.land_complete ? MAV_STATE_STANDBY : MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
if (ap.failsafe_radio == true) {
if (failsafe.radio == true) {
system_status = MAV_STATE_CRITICAL;
}
@ -1873,9 +1873,9 @@ mission_failed:
hal.rcin->set_overrides(v, 8);
// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
ap.rc_override_active = true;
failsafe.rc_override_active = true;
// 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;
}
@ -1940,7 +1940,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();
pmTest1++;
break;
}

2
ArduCopter/commands_logic.pde
View File

@ -655,7 +655,7 @@ static bool verify_RTL()
// check if we've loitered long enough
if( millis() - rtl_loiter_start_time > (uint32_t)g.rtl_loiter_time.get() ) {
// initiate landing or descent
if(g.rtl_alt_final == 0 || ap.failsafe_radio) {
if(g.rtl_alt_final == 0 || failsafe.radio) {
// switch to loiter which restores horizontal control to pilot
// To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands
set_roll_pitch_mode(ROLL_PITCH_LOITER);

2
ArduCopter/control_modes.pde
View File

@ -9,7 +9,7 @@ static void read_control_switch()
// has switch moved?
// ignore flight mode changes if in failsafe
if (oldSwitchPosition != switchPosition && !ap.failsafe_radio) {
if (oldSwitchPosition != switchPosition && !failsafe.radio) {
switch_counter++;
if(switch_counter >= CONTROL_SWITCH_COUNTER) {
oldSwitchPosition = switchPosition;

12
ArduCopter/events.pde
View File

@ -143,7 +143,7 @@ static void failsafe_gps_check()
// check if all is well
if( last_gps_update_ms < FAILSAFE_GPS_TIMEOUT_MS) {
// check for recovery from gps failsafe
if( ap.failsafe_gps ) {
if( failsafe.gps ) {
failsafe_gps_off_event();
set_failsafe_gps(false);
}
@ -151,7 +151,7 @@ static void failsafe_gps_check()
}
// do nothing if gps failsafe already triggered or motors disarmed
if( ap.failsafe_gps || !motors.armed()) {
if( failsafe.gps || !motors.armed()) {
return;
}
@ -186,17 +186,17 @@ static void failsafe_gcs_check()
uint32_t last_gcs_update_ms;
// return immediately if gcs failsafe is disabled, gcs has never been connected or we are not overriding rc controls from the gcs
if( g.failsafe_gcs == FS_GCS_DISABLED || last_heartbeat_ms == 0 || !ap.rc_override_active) {
if( g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0 || !failsafe.rc_override_active) {
return;
}
// calc time since last gcs update
last_gcs_update_ms = millis() - last_heartbeat_ms;
last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms;
// check if all is well
if( last_gcs_update_ms < FS_GCS_TIMEOUT_MS) {
// check for recovery from gcs failsafe
if( ap.failsafe_gcs ) {
if (failsafe.gcs) {
failsafe_gcs_off_event();
set_failsafe_gcs(false);
}
@ -204,7 +204,7 @@ static void failsafe_gcs_check()
}
// do nothing if gcs failsafe already triggered or motors disarmed
if( ap.failsafe_gcs || !motors.armed()) {
if( failsafe.gcs || !motors.armed()) {
return;
}

2
ArduCopter/fence.pde
View File

@ -32,7 +32,7 @@ void fence_check()
// disarm immediately if we think we are on the ground
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !ap.failsafe_radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
init_disarm_motors();
}else{
// if we have a GPS

22
ArduCopter/radio.pde
View File

@ -139,18 +139,16 @@ static void read_radio()
uint32_t elapsed = millis() - last_update;
// turn on throttle failsafe if no update from ppm encoder for 2 seconds
if ((elapsed >= FAILSAFE_RADIO_TIMEOUT_MS)
&& g.failsafe_throttle && motors.armed() && !ap.failsafe_radio) {
&& g.failsafe_throttle && motors.armed() && !failsafe.radio) {
Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME);
set_failsafe_radio(true);
}
}
}
#define FS_COUNTER 3
#define FS_COUNTER 3 // radio failsafe kicks in after 3 consecutive throttle values below failsafe_throttle_value
static void set_throttle_and_failsafe(uint16_t throttle_pwm)
{
static int8_t failsafe_counter = 0;
// if failsafe not enabled pass through throttle and exit
if(g.failsafe_throttle == FS_THR_DISABLED) {
g.rc_3.set_pwm(throttle_pwm);
@ -161,27 +159,27 @@ static void set_throttle_and_failsafe(uint16_t throttle_pwm)
if (throttle_pwm < (uint16_t)g.failsafe_throttle_value) {
// if we are already in failsafe or motors not armed pass through throttle and exit
if (ap.failsafe_radio || !motors.armed()) {
if (failsafe.radio || !motors.armed()) {
g.rc_3.set_pwm(throttle_pwm);
return;
}
// check for 3 low throttle values
// Note: we do not pass through the low throttle until 3 low throttle values are recieved
failsafe_counter++;
if( failsafe_counter >= FS_COUNTER ) {
failsafe_counter = FS_COUNTER; // check to ensure we don't overflow the counter
failsafe.radio_counter++;
if( failsafe.radio_counter >= FS_COUNTER ) {
failsafe.radio_counter = FS_COUNTER; // check to ensure we don't overflow the counter
set_failsafe_radio(true);
g.rc_3.set_pwm(throttle_pwm); // pass through failsafe throttle
}
}else{
// we have a good throttle so reduce failsafe counter
failsafe_counter--;
if( failsafe_counter <= 0 ) {
failsafe_counter = 0; // check to ensure we don't underflow the counter
failsafe.radio_counter--;
if( failsafe.radio_counter <= 0 ) {
failsafe.radio_counter = 0; // check to ensure we don't underflow the counter
// disengage failsafe after three (nearly) consecutive valid throttle values
if (ap.failsafe_radio) {
if (failsafe.radio) {
set_failsafe_radio(false);
}
}

2
ArduCopter/system.pde
View File

@ -506,7 +506,7 @@ static void update_auto_armed()
return;
}
// if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !ap.failsafe_radio) {
if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !failsafe.radio) {
set_auto_armed(false);
}
}else{