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

Copter: rename failsafe to failsafe_radio 

Makes way for separation of failsafes for throttle, gps and gcs
This commit is contained in:
Randy Mackay 2013-03-16 17:14:21 +09:00
parent f263e81ed7
commit f9539384a1
11 changed files with 54 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/AP_State.pde
View File

@ -55,24 +55,24 @@ void set_simple_mode(bool b)
}
// ---------------------------------------------
static void set_failsafe(bool mode)
static void set_failsafe_radio(bool mode)
{
// only act on changes
// -------------------
if(ap.failsafe != mode) {
if(ap.failsafe_radio != mode) {
// store the value so we don't trip the gate twice
// -----------------------------------------------
ap.failsafe = mode;
ap.failsafe_radio = mode;
if (ap.failsafe == false) {
if (ap.failsafe_radio == false) {
// We've regained radio contact
// ----------------------------
failsafe_off_event();
failsafe_radio_off_event();
}else{
// We've lost radio contact
// ------------------------
failsafe_on_event();
failsafe_radio_on_event();
}
}
}

14
ArduCopter/ArduCopter.pde
View File

@ -358,12 +358,14 @@ static union {
uint8_t armed : 1; // 6
uint8_t auto_armed : 1; // 7
uint8_t failsafe : 1; // 8 // A status flag for the failsafe state
uint8_t do_flip : 1; // 9 // Used to enable flip code
uint8_t takeoff_complete : 1; // 10
uint8_t land_complete : 1; // 11
uint8_t compass_status : 1; // 12
uint8_t gps_status : 1; // 13
uint8_t failsafe_radio : 1; // 8 // A status flag for the radio failsafe
uint8_t failsafe_batt : 1; // 9 // A status flag for the battery failsafe
uint8_t failsafe_gps : 1; // 10 // A status flag for the gps failsafe
uint8_t do_flip : 1; // 11 // Used to enable flip code
uint8_t takeoff_complete : 1; // 12
uint8_t land_complete : 1; // 13
uint8_t compass_status : 1; // 14
uint8_t gps_status : 1; // 15
};
uint16_t value;
} ap;

16
ArduCopter/Attitude.pde
View File

@ -138,7 +138,7 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
angle_error = constrain(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
}
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe_radio)) {
angle_error = 0;
}
@ -174,7 +174,7 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
angle_error = constrain(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
}
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe_radio)) {
angle_error = 0;
}
@ -205,7 +205,7 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
// limit the maximum overshoot
angle_error = constrain(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe_radio)) {
angle_error = 0;
}
@ -883,7 +883,7 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
int16_t desired_rate = 0;
// throttle failsafe check
if( ap.failsafe ) {
if( ap.failsafe_radio ) {
return 0;
}
@ -916,7 +916,7 @@ static int16_t get_pilot_desired_acceleration(int16_t throttle_control)
int32_t desired_accel = 0;
// throttle failsafe check
if( ap.failsafe ) {
if( ap.failsafe_radio ) {
return 0;
}
@ -944,8 +944,8 @@ static int32_t get_pilot_desired_direct_alt(int16_t throttle_control)
{
int32_t desired_alt = 0;
// throttle failsafe check
if( ap.failsafe ) {
// radio failsafe check
if( ap.failsafe_radio ) {
return 0;
}
@ -1104,7 +1104,7 @@ get_throttle_land()
land_detector++;
}else{
set_land_complete(true);
if( g.rc_3.control_in == 0 || ap.failsafe ) {
if( g.rc_3.control_in == 0 || ap.failsafe_radio ) {
init_disarm_motors();
}
}

2
ArduCopter/GCS_Mavlink.pde
View File

@ -68,7 +68,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
uint8_t system_status = MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
if (ap.failsafe == true) {
if (ap.failsafe_radio == true) {
system_status = MAV_STATE_CRITICAL;
}

10
ArduCopter/Log.pde
View File

@ -1208,8 +1208,14 @@ static void Log_Read_Error()
case ERROR_SUBSYSTEM_OPTFLOW:
cliSerial->print_P(PSTR("OF"));
break;
case ERROR_SUBSYSTEM_FAILSAFE:
cliSerial->print_P(PSTR("FS"));
case ERROR_SUBSYSTEM_FAILSAFE_RADIO:
cliSerial->print_P(PSTR("FSRADIO"));
break;
case ERROR_SUBSYSTEM_FAILSAFE_BATT:
cliSerial->print_P(PSTR("FSBATT"));
break;
case ERROR_SUBSYSTEM_FAILSAFE_GPS:
cliSerial->print_P(PSTR("FSGPS"));
break;
default:
// if undefined print subsytem as a number

2
ArduCopter/commands_logic.pde
View File

@ -556,7 +556,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) {
if(g.rtl_alt_final == 0 || ap.failsafe_radio) {
// land
do_land();
// override landing location (do_land defaults to current location)

2
ArduCopter/control_modes.pde
View File

@ -14,7 +14,7 @@ static void read_control_switch()
switch_counter = 0;
// ignore flight mode changes if in failsafe
if( !ap.failsafe ) {
if( !ap.failsafe_radio ) {
set_mode(flight_modes[switchPosition]);
if(g.ch7_option != CH7_SIMPLE_MODE) {

11
ArduCopter/defines.h
View File

@ -442,16 +442,17 @@ enum gcs_severity {
#define ERROR_SUBSYSTEM_RADIO 2
#define ERROR_SUBSYSTEM_COMPASS 3
#define ERROR_SUBSYSTEM_OPTFLOW 4
#define ERROR_SUBSYSTEM_FAILSAFE 5
#define ERROR_SUBSYSTEM_FAILSAFE_RADIO 5
#define ERROR_SUBSYSTEM_FAILSAFE_BATT 6
#define ERROR_SUBSYSTEM_FAILSAFE_GPS 7
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1
// subsystem specific error codes -- radio
#define ERROR_CODE_RADIO_LATE_FRAME 2
// subsystem specific error codes -- failsafe
#define ERROR_CODE_FAILSAFE_THROTTLE 2
#define ERROR_CODE_FAILSAFE_BATTERY 3
#define ERROR_CODE_FAILSAFE_WATCHDOG 4
// subsystem specific error codes -- failsafe_thr, batt, gps
#define ERROR_CODE_FAILSAFE_RESOLVED 0
#define ERROR_CODE_FAILSAFE_OCCURRED 1

10
ArduCopter/events.pde
View File

@ -4,7 +4,7 @@
* This event will be called when the failsafe changes
* boolean failsafe reflects the current state
*/
static void failsafe_on_event()
static void failsafe_radio_on_event()
{
// if motors are not armed there is nothing to do
if( !motors.armed() ) {
@ -48,18 +48,18 @@ static void failsafe_on_event()
}
// log the error to the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE, ERROR_CODE_FAILSAFE_THROTTLE);
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_OCCURRED);
}
// failsafe_off_event - respond to radio contact being regained
// we must be in AUTO, LAND or RTL modes
// or Stabilize or ACRO mode but with motors disarmed
static void failsafe_off_event()
static void failsafe_radio_off_event()
{
// no need to do anything except log the error as resolved
// user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE, ERROR_CODE_ERROR_RESOLVED);
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_RESOLVED);
}
static void low_battery_event(void)
@ -95,7 +95,7 @@ static void low_battery_event(void)
// warn the ground station and log to dataflash
gcs_send_text_P(SEVERITY_LOW,PSTR("Low Battery!"));
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE, ERROR_CODE_FAILSAFE_BATTERY);
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
#if COPTER_LEDS == ENABLED
if ( bitRead(g.copter_leds_mode, 3) ) { // Only Activate if a battery is connected to avoid alarm on USB only

16
ArduCopter/radio.pde
View File

@ -123,7 +123,7 @@ void output_min()
motors.output_min();
}
#define RADIO_FS_TIMEOUT_MS 2000 // 2 seconds
#define FAILSAFE_RADIO_TIMEOUT_MS 2000 // 2 seconds
static void read_radio()
{
static uint32_t last_update = 0;
@ -150,10 +150,10 @@ static void read_radio()
}else{
uint32_t elapsed = millis() - last_update;
// turn on throttle failsafe if no update from ppm encoder for 2 seconds
if ((elapsed >= RADIO_FS_TIMEOUT_MS)
&& g.failsafe_throttle && motors.armed() && !ap.failsafe) {
if ((elapsed >= FAILSAFE_RADIO_TIMEOUT_MS)
&& g.failsafe_throttle && motors.armed() && !ap.failsafe_radio) {
Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME);
set_failsafe(true);
set_failsafe_radio(true);
}
}
}
@ -173,7 +173,7 @@ 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 || !motors.armed()) {
if (ap.failsafe_radio || !motors.armed()) {
g.rc_3.set_pwm(throttle_pwm);
return;
}
@ -183,7 +183,7 @@ static void set_throttle_and_failsafe(uint16_t throttle_pwm)
failsafe_counter++;
if( failsafe_counter >= FS_COUNTER ) {
failsafe_counter = FS_COUNTER; // check to ensure we don't overflow the counter
set_failsafe(true);
set_failsafe_radio(true);
g.rc_3.set_pwm(throttle_pwm); // pass through failsafe throttle
}
}else{
@ -193,8 +193,8 @@ static void set_throttle_and_failsafe(uint16_t throttle_pwm)
failsafe_counter = 0; // check to ensure we don't underflow the counter
// disengage failsafe after three (nearly) consecutive valid throttle values
if (ap.failsafe) {
set_failsafe(false);
if (ap.failsafe_radio) {
set_failsafe_radio(false);
}
}
// pass through throttle

4
ArduCopter/system.pde
View File

@ -384,8 +384,8 @@ static void set_mode(uint8_t mode)
// used to stop fly_aways
// set to false if we have low throttle
motors.auto_armed(g.rc_3.control_in > 0 || ap.failsafe);
set_auto_armed(g.rc_3.control_in > 0 || ap.failsafe);
motors.auto_armed(g.rc_3.control_in > 0 || ap.failsafe_radio);
set_auto_armed(g.rc_3.control_in > 0 || ap.failsafe_radio);
// if we change modes, we must clear landed flag
set_land_complete(false);