2015-05-13 03:09:36 -03:00
|
|
|
#include "Plane.h"
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2016-08-13 04:54:37 -03:00
|
|
|
void Plane::failsafe_short_on_event(enum failsafe_state fstype, mode_reason_t reason)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2012-08-16 21:50:15 -03:00
|
|
|
// This is how to handle a short loss of control signal failsafe.
|
2013-07-19 01:11:16 -03:00
|
|
|
failsafe.state = fstype;
|
2017-10-27 17:21:28 -03:00
|
|
|
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:
|
2013-07-10 10:25:38 -03:00
|
|
|
case ACRO:
|
2012-12-04 02:32:37 -04:00
|
|
|
case FLY_BY_WIRE_A:
|
2014-04-12 01:12:14 -03:00
|
|
|
case AUTOTUNE:
|
2012-12-04 02:32:37 -04:00
|
|
|
case FLY_BY_WIRE_B:
|
2013-07-13 07:05:53 -03:00
|
|
|
case CRUISE:
|
2012-12-04 02:32:37 -04:00
|
|
|
case TRAINING:
|
2013-07-20 04:08:58 -03:00
|
|
|
failsafe.saved_mode = control_mode;
|
2017-12-06 19:02:11 -04:00
|
|
|
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);
|
2013-09-13 21:30:13 -03:00
|
|
|
} else {
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode(CIRCLE, reason);
|
2013-09-13 21:30:13 -03:00
|
|
|
}
|
2012-08-16 21:50:15 -03:00
|
|
|
break;
|
|
|
|
|
2015-12-26 03:45:42 -04:00
|
|
|
case QSTABILIZE:
|
2015-12-26 04:51:05 -04:00
|
|
|
case QLOITER:
|
2016-03-09 03:20:41 -04:00
|
|
|
case QHOVER:
|
2018-12-07 03:52:05 -04:00
|
|
|
case QAUTOTUNE:
|
2015-12-26 03:45:42 -04:00
|
|
|
failsafe.saved_mode = control_mode;
|
2017-12-06 19:02:11 -04:00
|
|
|
failsafe.saved_mode_set = true;
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode(QLAND, reason);
|
2015-12-26 03:45:42 -04:00
|
|
|
break;
|
|
|
|
|
2012-08-16 21:50:15 -03:00
|
|
|
case AUTO:
|
2016-08-12 17:27:48 -03:00
|
|
|
case AVOID_ADSB:
|
2012-08-16 21:50:15 -03:00
|
|
|
case GUIDED:
|
|
|
|
case LOITER:
|
2017-12-06 19:02:11 -04:00
|
|
|
if(g.fs_action_short != FS_ACTION_SHORT_BESTGUESS) {
|
2013-07-20 04:08:58 -03:00
|
|
|
failsafe.saved_mode = control_mode;
|
2017-12-06 19:02:11 -04:00
|
|
|
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);
|
2013-09-13 21:30:13 -03:00
|
|
|
} else {
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode(CIRCLE, reason);
|
2013-09-13 21:30:13 -03:00
|
|
|
}
|
2012-08-16 21:50:15 -03:00
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
case CIRCLE:
|
|
|
|
case RTL:
|
2016-03-09 03:20:41 -04:00
|
|
|
case QLAND:
|
2016-04-29 02:31:08 -03:00
|
|
|
case QRTL:
|
2012-08-16 21:50:15 -03:00
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
2017-07-08 22:15:58 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2016-08-13 04:54:37 -03:00
|
|
|
void Plane::failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
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
|
2018-04-03 23:17:05 -03:00
|
|
|
RC_Channels::clear_overrides();
|
2013-07-19 01:11:16 -03:00
|
|
|
failsafe.state = fstype;
|
2012-08-16 21:50:15 -03:00
|
|
|
switch(control_mode)
|
|
|
|
{
|
|
|
|
case MANUAL:
|
|
|
|
case STABILIZE:
|
2013-07-10 10:25:38 -03:00
|
|
|
case ACRO:
|
2012-12-04 02:32:37 -04:00
|
|
|
case FLY_BY_WIRE_A:
|
2014-04-12 01:12:14 -03:00
|
|
|
case AUTOTUNE:
|
2012-12-04 02:32:37 -04:00
|
|
|
case FLY_BY_WIRE_B:
|
2013-07-13 07:05:53 -03:00
|
|
|
case CRUISE:
|
2012-12-04 02:32:37 -04:00
|
|
|
case TRAINING:
|
2012-08-16 21:50:15 -03:00
|
|
|
case CIRCLE:
|
2017-12-06 19:02:11 -04:00
|
|
|
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
|
2016-01-11 11:29:03 -04:00
|
|
|
#if PARACHUTE == ENABLED
|
|
|
|
parachute_release();
|
|
|
|
#endif
|
2017-12-06 19:02:11 -04:00
|
|
|
} 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);
|
2013-09-13 21:30:13 -03:00
|
|
|
} else {
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode(RTL, reason);
|
2013-09-13 21:30:13 -03:00
|
|
|
}
|
2012-08-16 21:50:15 -03:00
|
|
|
break;
|
|
|
|
|
2015-12-26 03:45:42 -04:00
|
|
|
case QSTABILIZE:
|
2016-03-09 03:20:41 -04:00
|
|
|
case QHOVER:
|
2015-12-26 04:51:05 -04:00
|
|
|
case QLOITER:
|
2018-12-07 03:52:05 -04:00
|
|
|
case QAUTOTUNE:
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode(QLAND, reason);
|
2015-12-26 03:45:42 -04:00
|
|
|
break;
|
|
|
|
|
2012-08-16 21:50:15 -03:00
|
|
|
case AUTO:
|
2016-08-12 17:27:48 -03:00
|
|
|
case AVOID_ADSB:
|
2012-08-16 21:50:15 -03:00
|
|
|
case GUIDED:
|
|
|
|
case LOITER:
|
2017-12-06 19:02:11 -04:00
|
|
|
if(g.fs_action_long == FS_ACTION_LONG_PARACHUTE) {
|
2016-01-11 11:29:03 -04:00
|
|
|
#if PARACHUTE == ENABLED
|
|
|
|
parachute_release();
|
|
|
|
#endif
|
2017-12-06 19:02:11 -04:00
|
|
|
} 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);
|
2017-12-06 19:02:11 -04:00
|
|
|
} 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:
|
2016-03-09 03:20:41 -04:00
|
|
|
case QLAND:
|
2016-04-29 02:31:08 -03:00
|
|
|
case QRTL:
|
2012-08-16 21:50:15 -03:00
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
2017-07-08 22:15:58 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Flight mode = %u", (unsigned)control_mode);
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2016-08-13 04:54:37 -03:00
|
|
|
void Plane::failsafe_short_off_event(mode_reason_t reason)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
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);
|
2013-07-19 01:11:16 -03:00
|
|
|
failsafe.state = FAILSAFE_NONE;
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-16 21:50:15 -03:00
|
|
|
// re-read the switch so we can return to our preferred mode
|
|
|
|
// --------------------------------------------------------
|
2013-07-20 04:08:58 -03:00
|
|
|
if (control_mode == CIRCLE && failsafe.saved_mode_set) {
|
2017-12-06 19:02:11 -04:00
|
|
|
failsafe.saved_mode_set = false;
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode(failsafe.saved_mode, reason);
|
2012-03-02 21:50:46 -04:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
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)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2017-11-09 18:34:12 -04:00
|
|
|
switch ((Failsafe_Action)action) {
|
2018-11-23 19:00:31 -04:00
|
|
|
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:
|
2018-11-23 19:00:31 -04:00
|
|
|
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()) {
|
2018-11-23 19:00:31 -04:00
|
|
|
plane.set_mode(AUTO, MODE_REASON_BATTERY_FAILSAFE);
|
2017-11-09 18:34:12 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
FALLTHROUGH;
|
|
|
|
case Failsafe_Action_RTL:
|
2018-11-23 19:00:31 -04:00
|
|
|
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;
|
2015-06-20 18:26:31 -03:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|