ardupilot/ArduPlane/events.cpp

158 lines
4.0 KiB
C++
Raw Normal View History

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2015-05-13 03:09:36 -03:00
#include "Plane.h"
2015-05-13 03:09:36 -03:00
void Plane::failsafe_short_on_event(enum failsafe_state fstype)
{
2012-08-16 21:50:15 -03:00
// This is how to handle a short loss of control signal failsafe.
failsafe.state = fstype;
failsafe.ch3_timer_ms = millis();
2015-11-18 15:17:50 -04:00
gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on, ");
2012-08-16 21:50:15 -03:00
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
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);
}
2012-08-16 21:50:15 -03:00
break;
case QSTABILIZE:
case QLOITER:
case QHOVER:
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
set_mode(QLAND);
break;
2012-08-16 21:50:15 -03:00
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);
}
2012-08-16 21:50:15 -03:00
}
break;
case CIRCLE:
case RTL:
case QLAND:
case QRTL:
2012-08-16 21:50:15 -03:00
default:
break;
}
2015-11-18 15:17:50 -04:00
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
}
2015-05-13 03:09:36 -03:00
void Plane::failsafe_long_on_event(enum failsafe_state fstype)
{
2012-08-16 21:50:15 -03:00
// This is how to handle a long loss of control signal failsafe.
2015-11-18 15:17:50 -04:00
gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on, ");
2012-12-04 18:22:21 -04:00
// If the GCS is locked up we allow control to revert to RC
hal.rcin->clear_overrides();
failsafe.state = fstype;
2012-08-16 21:50:15 -03:00
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case ACRO:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CRUISE:
case TRAINING:
2012-08-16 21:50:15 -03:00
case CIRCLE:
2016-01-11 11:29:03 -04:00
if(g.long_fs_action == 3) {
#if PARACHUTE == ENABLED
parachute_release();
#endif
} else if (g.long_fs_action == 2) {
set_mode(FLY_BY_WIRE_A);
} else {
set_mode(RTL);
}
2012-08-16 21:50:15 -03:00
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
set_mode(QLAND);
break;
2012-08-16 21:50:15 -03:00
case AUTO:
case GUIDED:
case LOITER:
2016-01-11 11:29:03 -04:00
if(g.long_fs_action == 3) {
#if PARACHUTE == ENABLED
parachute_release();
#endif
} else if (g.long_fs_action == 2) {
set_mode(FLY_BY_WIRE_A);
} else if (g.long_fs_action == 1) {
2012-08-16 21:50:15 -03:00
set_mode(RTL);
}
break;
case RTL:
case QLAND:
case QRTL:
2012-08-16 21:50:15 -03:00
default:
break;
}
if (fstype == FAILSAFE_GCS) {
2015-11-18 15:17:50 -04:00
gcs_send_text(MAV_SEVERITY_CRITICAL, "No GCS heartbeat");
}
2015-11-18 15:17:50 -04:00
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
}
2015-05-13 03:09:36 -03:00
void Plane::failsafe_short_off_event()
{
2012-08-16 21:50:15 -03:00
// We're back in radio contact
2015-11-18 15:17:50 -04:00
gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off");
failsafe.state = FAILSAFE_NONE;
2012-08-16 21:50:15 -03:00
// 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);
}
}
2015-05-13 03:09:36 -03:00
void Plane::low_battery_event(void)
{
2013-09-29 09:54:39 -03:00
if (failsafe.low_battery) {
return;
}
2015-11-18 15:17:50 -04:00
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Low battery %.2fV used %.0f mAh",
(double)battery.voltage(), (double)battery.current_total_mah());
if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
2016-01-30 02:32:42 -04:00
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
set_mode(RTL);
aparm.throttle_cruise.load();
}
2013-09-29 09:54:39 -03:00
failsafe.low_battery = true;
AP_Notify::flags.failsafe_battery = true;
}
2015-05-13 03:09:36 -03:00
void Plane::update_events(void)
{
2014-01-20 00:36:31 -04:00
ServoRelayEvents.update_events();
}