mirror of https://github.com/ArduPilot/ardupilot
133 lines
4.2 KiB
Plaintext
133 lines
4.2 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
/*
|
|
* This event will be called when the failsafe changes
|
|
* boolean failsafe reflects the current state
|
|
*/
|
|
static void failsafe_on_event()
|
|
{
|
|
// if motors are not armed there is nothing to do
|
|
if( !motors.armed() ) {
|
|
return;
|
|
}
|
|
|
|
// This is how to handle a failsafe.
|
|
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 && home_distance >= FS_THR_RTL_MIN_DISTANCE) {
|
|
set_mode(RTL);
|
|
}else{
|
|
// We have no GPS or are very close to home so we will land
|
|
set_mode(LAND);
|
|
}
|
|
break;
|
|
case AUTO:
|
|
// failsafe_throttle is 1 do RTL, 2 means continue with the mission
|
|
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
|
|
if(home_distance >= FS_THR_RTL_MIN_DISTANCE) {
|
|
set_mode(RTL);
|
|
}else{
|
|
// We are very close to home so we will land
|
|
set_mode(LAND);
|
|
}
|
|
}
|
|
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
|
|
break;
|
|
default:
|
|
if(ap.home_is_set == true && home_distance >= FS_THR_RTL_MIN_DISTANCE) {
|
|
set_mode(RTL);
|
|
}else{
|
|
// We have no GPS or are very close to home so we will land
|
|
set_mode(LAND);
|
|
}
|
|
break;
|
|
}
|
|
|
|
// log the error to the dataflash
|
|
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE, ERROR_CODE_FAILSAFE_THROTTLE);
|
|
|
|
}
|
|
|
|
// 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()
|
|
{
|
|
// 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);
|
|
}
|
|
|
|
static void low_battery_event(void)
|
|
{
|
|
// failsafe check
|
|
if (g.failsafe_battery_enabled && !ap.low_battery && motors.armed()) {
|
|
switch(control_mode) {
|
|
case STABILIZE:
|
|
case ACRO:
|
|
// if throttle is zero disarm motors
|
|
if (g.rc_3.control_in == 0) {
|
|
init_disarm_motors();
|
|
}else{
|
|
set_mode(LAND);
|
|
}
|
|
break;
|
|
case AUTO:
|
|
if(ap.home_is_set == true && home_distance >= FS_THR_RTL_MIN_DISTANCE) {
|
|
set_mode(RTL);
|
|
}else{
|
|
// We have no GPS or are very close to home so we will land
|
|
set_mode(LAND);
|
|
}
|
|
break;
|
|
default:
|
|
set_mode(LAND);
|
|
break;
|
|
}
|
|
}
|
|
|
|
// set the low battery flag
|
|
set_low_battery(true);
|
|
|
|
// 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);
|
|
|
|
#if COPTER_LEDS == ENABLED
|
|
if ( bitRead(g.copter_leds_mode, 3) ) { // Only Activate if a battery is connected to avoid alarm on USB only
|
|
piezo_on();
|
|
}
|
|
#endif // COPTER_LEDS
|
|
}
|
|
|
|
|
|
static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
|
|
{
|
|
if(event_repeat == 0 || (millis() - event_timer) < event_delay)
|
|
return;
|
|
|
|
if(event_repeat != 0) { // event_repeat = -1 means repeat forever
|
|
event_timer = millis();
|
|
|
|
if (event_id >= CH_5 && event_id <= CH_8) {
|
|
if(event_repeat%2) {
|
|
hal.rcout->write(event_id, event_value); // send to Servos
|
|
} else {
|
|
hal.rcout->write(event_id, event_undo_value);
|
|
}
|
|
}
|
|
|
|
if (event_id == RELAY_TOGGLE) {
|
|
relay.toggle();
|
|
}
|
|
if (event_repeat > 0) {
|
|
event_repeat--;
|
|
}
|
|
}
|
|
}
|
|
|