2011-09-08 22:29:39 -03:00
|
|
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
|
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;
|
2015-05-13 19:05:32 -03:00
|
|
|
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:
|
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;
|
|
|
|
failsafe.saved_mode_set = 1;
|
2013-09-13 21:30:13 -03:00
|
|
|
if(g.short_fs_action == 2) {
|
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:
|
2015-12-26 03:45:42 -04:00
|
|
|
failsafe.saved_mode = control_mode;
|
|
|
|
failsafe.saved_mode_set = 1;
|
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:
|
2013-09-13 21:30:13 -03:00
|
|
|
if(g.short_fs_action != 0) {
|
2013-07-20 04:08:58 -03:00
|
|
|
failsafe.saved_mode = control_mode;
|
|
|
|
failsafe.saved_mode_set = 1;
|
2013-09-13 21:30:13 -03:00
|
|
|
if(g.short_fs_action == 2) {
|
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;
|
|
|
|
}
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text_fmt(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.
|
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();
|
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:
|
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) {
|
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:
|
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:
|
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) {
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode(FLY_BY_WIRE_A, reason);
|
2013-09-13 21:30:13 -03:00
|
|
|
} else if (g.long_fs_action == 1) {
|
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;
|
|
|
|
}
|
2014-07-15 16:25:31 -03:00
|
|
|
if (fstype == FAILSAFE_GCS) {
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_CRITICAL, "No GCS heartbeat");
|
2014-07-15 16:25:31 -03:00
|
|
|
}
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text_fmt(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
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text(MAV_SEVERITY_WARNING, "Failsafe. Short event off");
|
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) {
|
|
|
|
failsafe.saved_mode_set = 0;
|
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
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::low_battery_event(void)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2013-09-29 09:54:39 -03:00
|
|
|
if (failsafe.low_battery) {
|
2013-05-22 07:33:57 -03:00
|
|
|
return;
|
|
|
|
}
|
2015-11-18 15:17:50 -04:00
|
|
|
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Low battery %.2fV used %.0f mAh",
|
2015-05-03 03:00:05 -03:00
|
|
|
(double)battery.voltage(), (double)battery.current_total_mah());
|
2015-06-20 18:26:31 -03:00
|
|
|
if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
|
2016-01-30 02:32:42 -04:00
|
|
|
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
|
2015-06-20 18:26:31 -03:00
|
|
|
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
|
2016-08-13 04:54:37 -03:00
|
|
|
set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
|
2015-06-20 18:26:31 -03:00
|
|
|
aparm.throttle_cruise.load();
|
|
|
|
}
|
2013-09-29 09:54:39 -03:00
|
|
|
failsafe.low_battery = true;
|
2013-09-17 21:50:34 -03:00
|
|
|
AP_Notify::flags.failsafe_battery = true;
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::update_events(void)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2014-01-20 00:36:31 -04:00
|
|
|
ServoRelayEvents.update_events();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|