ardupilot/ArduPlane/events.cpp

177 lines
5.4 KiB
C++
Raw Normal View History

2015-05-13 03:09:36 -03:00
#include "Plane.h"
2016-08-13 04:54:37 -03:00
void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t reason)
{
2012-08-16 21:50:15 -03:00
// This is how to handle a short loss of control signal failsafe.
failsafe.state = fstype;
failsafe.short_timer_ms = millis();
2017-09-22 00:31:44 -03:00
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event on: type=%u/reason=%u", fstype, reason);
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 = true;
if(g.fs_action_short == FS_ACTION_SHORT_FBWA) {
2016-08-13 04:54:37 -03:00
set_mode(FLY_BY_WIRE_A, reason);
} else {
2016-08-13 04:54:37 -03:00
set_mode(CIRCLE, reason);
}
2012-08-16 21:50:15 -03:00
break;
case QSTABILIZE:
case QLOITER:
case QHOVER:
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = true;
2016-08-13 04:54:37 -03:00
set_mode(QLAND, reason);
break;
2012-08-16 21:50:15 -03:00
case AUTO:
case AVOID_ADSB:
2012-08-16 21:50:15 -03:00
case GUIDED:
case LOITER:
if(g.fs_action_short != FS_ACTION_SHORT_BESTGUESS) {
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = true;
if(g.fs_action_short == FS_ACTION_SHORT_FBWA) {
2016-08-13 04:54:37 -03:00
set_mode(FLY_BY_WIRE_A, reason);
} else {
2016-08-13 04:54:37 -03:00
set_mode(CIRCLE, reason);
}
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;
}
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
}
2016-08-13 04:54:37 -03:00
void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason)
{
2012-08-16 21:50:15 -03:00
// This is how to handle a long loss of control signal failsafe.
2017-09-22 00:31:44 -03:00
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event on: type=%u/reason=%u", fstype, reason);
2012-12-04 18:22:21 -04:00
// If the GCS is locked up we allow control to revert to RC
RC_Channels::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:
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
2016-01-11 11:29:03 -04:00
#if PARACHUTE == ENABLED
parachute_release();
#endif
} else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
2016-08-13 04:54:37 -03:00
set_mode(FLY_BY_WIRE_A, reason);
} else {
2016-08-13 04:54:37 -03:00
set_mode(RTL, reason);
}
2012-08-16 21:50:15 -03:00
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
2016-08-13 04:54:37 -03:00
set_mode(QLAND, reason);
break;
2012-08-16 21:50:15 -03:00
case AUTO:
case AVOID_ADSB:
2012-08-16 21:50:15 -03:00
case GUIDED:
case LOITER:
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
2016-01-11 11:29:03 -04:00
#if PARACHUTE == ENABLED
parachute_release();
#endif
} else if (g.fs_action_long == FS_ACTION_LONG_GLIDE) {
2016-08-13 04:54:37 -03:00
set_mode(FLY_BY_WIRE_A, reason);
} else if (g.fs_action_long == FS_ACTION_LONG_RTL) {
2016-08-13 04:54:37 -03:00
set_mode(RTL, reason);
2012-08-16 21:50:15 -03:00
}
break;
case RTL:
case QLAND:
case QRTL:
2012-08-16 21:50:15 -03:00
default:
break;
}
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
}
2016-08-13 04:54:37 -03:00
void Plane::failsafe_short_off_event(mode_reason_t reason)
{
2012-08-16 21:50:15 -03:00
// We're back in radio contact
2017-09-22 00:31:44 -03:00
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off: reason=%u", reason);
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 = false;
2016-08-13 04:54:37 -03:00
set_mode(failsafe.saved_mode, reason);
}
}
2017-09-22 00:31:44 -03:00
void Plane::failsafe_long_off_event(mode_reason_t reason)
{
// We're back in radio contact
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe. Long event off: reason=%u", reason);
failsafe.state = FAILSAFE_NONE;
}
2017-11-09 18:34:12 -04:00
void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
{
2017-11-09 18:34:12 -04:00
switch ((Failsafe_Action)action) {
case Failsafe_Action_QLand:
if (quadplane.available()) {
plane.set_mode(QLAND, MODE_REASON_BATTERY_FAILSAFE);
break;
}
FALLTHROUGH;
2017-11-09 18:34:12 -04:00
case Failsafe_Action_Land:
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND || control_mode == QLAND) {
2017-11-09 18:34:12 -04:00
// never stop a landing if we were already committed
if (plane.mission.jump_to_landing_sequence()) {
plane.set_mode(AUTO, MODE_REASON_BATTERY_FAILSAFE);
2017-11-09 18:34:12 -04:00
break;
}
}
FALLTHROUGH;
case Failsafe_Action_RTL:
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND || control_mode == QLAND ) {
2017-11-09 18:34:12 -04:00
// never stop a landing if we were already committed
set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
aparm.throttle_cruise.load();
}
break;
case Failsafe_Action_Terminate:
char battery_type_str[17];
snprintf(battery_type_str, 17, "%s battery", type_str);
afs.gcs_terminate(true, battery_type_str);
break;
case Failsafe_Action_None:
// don't actually do anything, however we should still flag the system as having hit a failsafe
// and ensure all appropriate flags are going off to the user
break;
}
}