mirror of https://github.com/ArduPilot/ardupilot
155 lines
4.1 KiB
Plaintext
155 lines
4.1 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
|
|
static void failsafe_short_on_event(enum failsafe_state fstype)
|
|
{
|
|
// This is how to handle a short loss of control signal failsafe.
|
|
failsafe.state = fstype;
|
|
failsafe.ch3_timer_ms = millis();
|
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
|
|
switch(control_mode)
|
|
{
|
|
case MANUAL:
|
|
case STABILIZE:
|
|
case ACRO:
|
|
case FLY_BY_WIRE_A:
|
|
case FLY_BY_WIRE_B:
|
|
case CRUISE:
|
|
case TRAINING:
|
|
failsafe.saved_mode = control_mode;
|
|
failsafe.saved_mode_set = 1;
|
|
if(g.short_fs_action == 2) {
|
|
set_mode(FLY_BY_WIRE_A);
|
|
} else {
|
|
set_mode(CIRCLE);
|
|
}
|
|
break;
|
|
|
|
case AUTO:
|
|
case GUIDED:
|
|
case LOITER:
|
|
if(g.short_fs_action != 0) {
|
|
failsafe.saved_mode = control_mode;
|
|
failsafe.saved_mode_set = 1;
|
|
if(g.short_fs_action == 2) {
|
|
set_mode(FLY_BY_WIRE_A);
|
|
} else {
|
|
set_mode(CIRCLE);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case CIRCLE:
|
|
case RTL:
|
|
default:
|
|
break;
|
|
}
|
|
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
|
|
}
|
|
|
|
static void failsafe_long_on_event(enum failsafe_state fstype)
|
|
{
|
|
// This is how to handle a long loss of control signal failsafe.
|
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, "));
|
|
// If the GCS is locked up we allow control to revert to RC
|
|
hal.rcin->clear_overrides();
|
|
failsafe.state = fstype;
|
|
switch(control_mode)
|
|
{
|
|
case MANUAL:
|
|
case STABILIZE:
|
|
case ACRO:
|
|
case FLY_BY_WIRE_A:
|
|
case FLY_BY_WIRE_B:
|
|
case CRUISE:
|
|
case TRAINING:
|
|
case CIRCLE:
|
|
if(g.long_fs_action == 2) {
|
|
set_mode(FLY_BY_WIRE_A);
|
|
} else {
|
|
set_mode(RTL);
|
|
}
|
|
break;
|
|
|
|
case AUTO:
|
|
case GUIDED:
|
|
case LOITER:
|
|
if(g.long_fs_action == 2) {
|
|
set_mode(FLY_BY_WIRE_A);
|
|
} else if (g.long_fs_action == 1) {
|
|
set_mode(RTL);
|
|
}
|
|
break;
|
|
|
|
case RTL:
|
|
default:
|
|
break;
|
|
}
|
|
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
|
|
}
|
|
|
|
static void failsafe_short_off_event()
|
|
{
|
|
// We're back in radio contact
|
|
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off"));
|
|
failsafe.state = FAILSAFE_NONE;
|
|
|
|
// re-read the switch so we can return to our preferred mode
|
|
// --------------------------------------------------------
|
|
if (control_mode == CIRCLE && failsafe.saved_mode_set) {
|
|
failsafe.saved_mode_set = 0;
|
|
set_mode(failsafe.saved_mode);
|
|
}
|
|
}
|
|
|
|
void low_battery_event(void)
|
|
{
|
|
if (failsafe.low_battery) {
|
|
return;
|
|
}
|
|
gcs_send_text_fmt(PSTR("Low Battery %.2fV Used %.0f mAh"),
|
|
battery.voltage(), battery.current_total_mah());
|
|
set_mode(RTL);
|
|
aparm.throttle_cruise.load();
|
|
failsafe.low_battery = true;
|
|
AP_Notify::flags.failsafe_battery = true;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
// repeating event control
|
|
|
|
/*
|
|
update state for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
|
|
*/
|
|
static void update_events(void)
|
|
{
|
|
if (event_state.repeat == 0 || (millis() - event_state.start_time_ms) < event_state.delay_ms) {
|
|
return;
|
|
}
|
|
|
|
// event_repeat = -1 means repeat forever
|
|
if (event_state.repeat != 0) {
|
|
event_state.start_time_ms = millis();
|
|
|
|
switch (event_state.type) {
|
|
case EVENT_TYPE_SERVO:
|
|
hal.rcout->enable_ch(event_state.rc_channel);
|
|
if (event_state.repeat & 1) {
|
|
servo_write(event_state.rc_channel, event_state.undo_value);
|
|
} else {
|
|
servo_write(event_state.rc_channel, event_state.servo_value);
|
|
}
|
|
break;
|
|
|
|
case EVENT_TYPE_RELAY:
|
|
gcs_send_text_fmt(PSTR("Relay toggle"));
|
|
relay.toggle();
|
|
break;
|
|
}
|
|
|
|
if (event_state.repeat > 0) {
|
|
event_state.repeat--;
|
|
}
|
|
}
|
|
}
|