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

ArduCopter: failsafe improvements 

resolves momentary throttle drop to zero before failsafe engages
resolves motor cut after 30seconds if flying in stabilize without GPS (now switches to ALT_HOLD with target altitude zero)
disables motors if throttle was zero before failsafe event
This commit is contained in:
rmackay9 2012-11-11 22:11:12 +09:00
parent 95771692f0
commit 41c0cee292
3 changed files with 87 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/config.h
View File

@ -427,9 +427,13 @@
#ifndef THROTTE_FS_VALUE
# define THROTTLE_FS_VALUE 975
#endif
#define THROTTLE_FAILSAFE_ACTION_ALWAYS_RTL 1
#define THROTTLE_FAILSAFE_ACTION_CONTINUE_MISSION 2
#ifndef THROTTLE_FAILSAFE_ACTION
# define THROTTLE_FAILSAFE_ACTION 2
# define THROTTLE_FAILSAFE_ACTION THROTTLE_FAILSAFE_ACTION_CONTINUE_MISSION
#endif
#ifndef MINIMUM_THROTTLE
# define MINIMUM_THROTTLE 130
#endif

70
ArduCopter/events.pde
View File

@ -7,59 +7,53 @@
static void failsafe_on_event()
{
// This is how to handle a failsafe.
switch(control_mode)
{
case AUTO:
if (g.throttle_fs_action == 1) {
// do_rtl sets the altitude to the current altitude by default
switch(control_mode) {
case STABILIZE:
case ACRO:
// if throttle is zero disarm motors
if (g.rc_3.control_in == 0) {
init_disarm_motors();
}else if(ap.home_is_set == true) {
set_mode(RTL);
// We add an additional 10m to the current altitude
//next_WP.alt += 1000;
set_new_altitude(next_WP.alt + 1000);
}else{
// We have no GPS so we will crash land in alt hold mode
// the low throttle will bring us down at the maximum
// the low throttle will bring us down at the maximum descent speed
// To-Do: make LAND a throttle mode so it can operate without GPS
set_mode(ALT_HOLD);
set_new_altitude(0);
}
//if (g.throttle_fs_action == 2)
// Stay in AUTO and ignore failsafe
break;
case AUTO:
// throttle_fs_action is 1 do RTL, everything else means continue with the mission
if (g.throttle_fs_action == THROTTLE_FAILSAFE_ACTION_ALWAYS_RTL) {
set_mode(RTL);
}
// if throttle_fs_action is 2 (i.e. THROTTLE_FAILSAFE_ACTION_CONTINUE_MISSION) no need to do anything
break;
default:
if(ap.home_is_set) {
if(ap.home_is_set == true) {
// same as above ^
// do_rtl sets the altitude to the current altitude by default
set_mode(RTL);
// We add an additional 10m to the current altitude
//next_WP.alt += 1000;
set_new_altitude(next_WP.alt + 1000);
}else{
// We have no GPS so we must land
set_mode(LAND);
// We have no GPS so we will crash land in alt hold mode
// the low throttle will bring us down at the maximum descent speed
// To-Do: make LAND a throttle mode so it can operate without GPS
set_mode(ALT_HOLD);
set_new_altitude(0);
}
break;
}
}
// 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()
{
// If we are in AUTO, no need to do anything
if(control_mode == AUTO)
return;
if (g.throttle_fs_action == 2) {
// We're back in radio contact
// return to AP
// ---------------------------
// re-read the switch so we can return to our preferred mode
// --------------------------------------------------------
reset_control_switch();
}else if (g.throttle_fs_action == 1) {
// We're back in radio contact
// return to Home
// we should already be in RTL and throttle set to cruise
// ------------------------------------------------------
set_mode(RTL);
}
// no need to do anything
// user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
}
static void low_battery_event(void)

78
ArduCopter/radio.pde
View File

@ -1,9 +1,7 @@
// -*- 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
// Function that will read the radio data, limit servos and trigger a failsafe
// ----------------------------------------------------------------------------
static int8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
extern RC_Channel* rc_ch[NUM_CHANNELS];
@ -132,7 +130,9 @@ static void read_radio()
system.new_radio_frame = true;
g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
set_throttle_and_failsafe(APM_RC.InputCh(CH_3));
g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
@ -143,55 +143,51 @@ static void read_radio()
// limit our input to 800 so we can still pitch and roll
g.rc_3.control_in = min(g.rc_3.control_in, MAXIMUM_THROTTLE);
#endif
throttle_failsafe(g.rc_3.radio_in);
}
}
#define FS_COUNTER 3
static void throttle_failsafe(uint16_t pwm)
static void set_throttle_and_failsafe(uint16_t throttle_pwm)
{
// Don't enter Failsafe if not enabled by user
if(g.throttle_fs_enabled == 0)
static int8_t failsafe_counter = 0;
// if failsafe not enabled pass through throttle and exit
if(g.throttle_fs_enabled == 0) {
g.rc_3.set_pwm(throttle_pwm);
return;
}
//check for failsafe and debounce funky reads
// ------------------------------------------
if (pwm < (unsigned)g.throttle_fs_value) {
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafeCounter++;
if (failsafeCounter == FS_COUNTER-1) {
// called right before trigger
// do nothing
}else if(failsafeCounter == FS_COUNTER) {
// Don't enter Failsafe if we are not armed
// home distance is in meters
// This is to prevent accidental RTL
if(motors.armed() && ap.takeoff_complete) {
Serial.print_P(PSTR("MSG FS ON "));
Serial.println(pwm, DEC);
//check for low throttle value
if (throttle_pwm < (uint16_t)g.throttle_fs_value) {
// if we are already in failsafe or motors not armed pass through throttle and exit
if (ap.failsafe || !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
set_failsafe(true);
g.rc_3.set_pwm(throttle_pwm); // pass through failsafe throttle
}
}else if (failsafeCounter > FS_COUNTER) {
failsafeCounter = FS_COUNTER+1;
}
}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
}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) {
Serial.print_P(PSTR("MSG FS OFF "));
Serial.println(pwm, DEC);
}else if(failsafeCounter == 0) {
// disengage failsafe after three (nearly) consecutive valid throttle values
if (ap.failsafe) {
set_failsafe(false);
}else if (failsafeCounter <0) {
failsafeCounter = -1;
}
}
// pass through throttle
g.rc_3.set_pwm(throttle_pwm);
}
}
static void trim_radio()