2015-05-13 03:09:36 -03:00
|
|
|
#include "Plane.h"
|
2012-08-01 00:59:20 -03:00
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
Mode *Plane::mode_from_mode_num(const enum Mode::Number num)
|
|
|
|
{
|
|
|
|
Mode *ret = nullptr;
|
|
|
|
switch (num) {
|
|
|
|
case Mode::Number::MANUAL:
|
|
|
|
ret = &mode_manual;
|
|
|
|
break;
|
|
|
|
case Mode::Number::CIRCLE:
|
|
|
|
ret = &mode_circle;
|
|
|
|
break;
|
|
|
|
case Mode::Number::STABILIZE:
|
|
|
|
ret = &mode_stabilize;
|
|
|
|
break;
|
|
|
|
case Mode::Number::TRAINING:
|
|
|
|
ret = &mode_training;
|
|
|
|
break;
|
|
|
|
case Mode::Number::ACRO:
|
|
|
|
ret = &mode_acro;
|
|
|
|
break;
|
|
|
|
case Mode::Number::FLY_BY_WIRE_A:
|
|
|
|
ret = &mode_fbwa;
|
|
|
|
break;
|
|
|
|
case Mode::Number::FLY_BY_WIRE_B:
|
|
|
|
ret = &mode_fbwb;
|
|
|
|
break;
|
|
|
|
case Mode::Number::CRUISE:
|
|
|
|
ret = &mode_cruise;
|
|
|
|
break;
|
|
|
|
case Mode::Number::AUTOTUNE:
|
|
|
|
ret = &mode_autotune;
|
|
|
|
break;
|
|
|
|
case Mode::Number::AUTO:
|
|
|
|
ret = &mode_auto;
|
|
|
|
break;
|
|
|
|
case Mode::Number::RTL:
|
|
|
|
ret = &mode_rtl;
|
|
|
|
break;
|
|
|
|
case Mode::Number::LOITER:
|
|
|
|
ret = &mode_loiter;
|
|
|
|
break;
|
|
|
|
case Mode::Number::AVOID_ADSB:
|
2020-09-22 14:00:39 -03:00
|
|
|
#if HAL_ADSB_ENABLED
|
2019-01-15 13:46:13 -04:00
|
|
|
ret = &mode_avoidADSB;
|
|
|
|
break;
|
2020-09-22 14:00:39 -03:00
|
|
|
#endif
|
|
|
|
// if ADSB is not compiled in then fallthrough to guided
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::GUIDED:
|
|
|
|
ret = &mode_guided;
|
|
|
|
break;
|
|
|
|
case Mode::Number::INITIALISING:
|
|
|
|
ret = &mode_initializing;
|
|
|
|
break;
|
|
|
|
case Mode::Number::QSTABILIZE:
|
|
|
|
ret = &mode_qstabilize;
|
|
|
|
break;
|
|
|
|
case Mode::Number::QHOVER:
|
|
|
|
ret = &mode_qhover;
|
|
|
|
break;
|
|
|
|
case Mode::Number::QLOITER:
|
|
|
|
ret = &mode_qloiter;
|
|
|
|
break;
|
|
|
|
case Mode::Number::QLAND:
|
|
|
|
ret = &mode_qland;
|
|
|
|
break;
|
|
|
|
case Mode::Number::QRTL:
|
|
|
|
ret = &mode_qrtl;
|
|
|
|
break;
|
|
|
|
case Mode::Number::QACRO:
|
|
|
|
ret = &mode_qacro;
|
|
|
|
break;
|
|
|
|
case Mode::Number::QAUTOTUNE:
|
|
|
|
ret = &mode_qautotune;
|
|
|
|
break;
|
2019-05-04 06:56:07 -03:00
|
|
|
case Mode::Number::TAKEOFF:
|
|
|
|
ret = &mode_takeoff;
|
|
|
|
break;
|
2020-09-16 04:46:56 -03:00
|
|
|
case Mode::Number::THERMAL:
|
2020-09-23 05:16:45 -03:00
|
|
|
#if HAL_SOARING_ENABLED
|
2020-09-16 04:46:56 -03:00
|
|
|
ret = &mode_thermal;
|
|
|
|
#endif
|
|
|
|
break;
|
2019-01-15 13:46:13 -04:00
|
|
|
}
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::read_control_switch()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2012-08-16 21:50:15 -03:00
|
|
|
static bool switch_debouncer;
|
2012-12-04 18:22:21 -04:00
|
|
|
uint8_t switchPosition = readSwitch();
|
2012-08-16 21:50:15 -03:00
|
|
|
|
|
|
|
// If switchPosition = 255 this indicates that the mode control channel input was out of range
|
|
|
|
// If we get this value we do not want to change modes.
|
|
|
|
if(switchPosition == 255) return;
|
2011-12-18 18:42:32 -04:00
|
|
|
|
2017-10-27 17:21:28 -03:00
|
|
|
if (failsafe.rc_failsafe || failsafe.throttle_counter > 0) {
|
|
|
|
// when we are in rc_failsafe mode then RC input is not
|
2013-02-20 20:21:39 -04:00
|
|
|
// working, and we need to ignore the mode switch channel
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2015-05-13 19:05:32 -03:00
|
|
|
if (millis() - failsafe.last_valid_rc_ms > 100) {
|
2014-03-08 01:49:57 -04:00
|
|
|
// only use signals that are less than 0.1s old.
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2011-12-18 18:42:32 -04:00
|
|
|
// we look for changes in the switch position. If the
|
|
|
|
// RST_SWITCH_CH parameter is set, then it is a switch that can be
|
|
|
|
// used to force re-reading of the control switch. This is useful
|
|
|
|
// when returning to the previous mode after a failsafe or fence
|
|
|
|
// breach. This channel is best used on a momentary switch (such
|
|
|
|
// as a spring loaded trainer switch).
|
2012-08-16 21:50:15 -03:00
|
|
|
if (oldSwitchPosition != switchPosition ||
|
|
|
|
(g.reset_switch_chan != 0 &&
|
2018-04-03 23:17:05 -03:00
|
|
|
RC_Channels::get_radio_in(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) {
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-01 00:59:20 -03:00
|
|
|
if (switch_debouncer == false) {
|
|
|
|
// this ensures that mode switches only happen if the
|
|
|
|
// switch changes for 2 reads. This prevents momentary
|
|
|
|
// spikes in the mode control channel from causing a mode
|
|
|
|
// switch
|
|
|
|
switch_debouncer = true;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2019-10-17 00:49:32 -03:00
|
|
|
set_mode_by_number((enum Mode::Number)flight_modes[switchPosition].get(), ModeReason::RC_COMMAND);
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-16 21:50:15 -03:00
|
|
|
oldSwitchPosition = switchPosition;
|
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-16 21:50:15 -03:00
|
|
|
if (g.reset_mission_chan != 0 &&
|
2018-04-03 23:17:05 -03:00
|
|
|
RC_Channels::get_radio_in(g.reset_mission_chan-1) > RESET_SWITCH_CHAN_PWM) {
|
2014-03-13 23:06:40 -03:00
|
|
|
mission.start();
|
2014-03-16 01:53:10 -03:00
|
|
|
prev_WP_loc = current_loc;
|
2012-08-08 21:33:16 -03:00
|
|
|
}
|
|
|
|
|
2012-08-01 00:59:20 -03:00
|
|
|
switch_debouncer = false;
|
|
|
|
|
2016-06-01 21:24:45 -03:00
|
|
|
#if PARACHUTE == ENABLED
|
2015-10-26 20:46:01 -03:00
|
|
|
if (g.parachute_channel > 0) {
|
2021-02-03 13:21:39 -04:00
|
|
|
if (RC_Channels::get_radio_in(g.parachute_channel-1) >= RC_Channel::AUX_PWM_TRIGGER_HIGH) {
|
2015-10-26 20:46:01 -03:00
|
|
|
parachute_manual_release();
|
|
|
|
}
|
|
|
|
}
|
2016-06-01 21:24:45 -03:00
|
|
|
#endif
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2021-02-01 12:26:22 -04:00
|
|
|
uint8_t Plane::readSwitch(void) const
|
2014-03-08 01:49:57 -04:00
|
|
|
{
|
2018-04-03 23:17:05 -03:00
|
|
|
uint16_t pulsewidth = RC_Channels::get_radio_in(g.flight_mode_channel - 1);
|
2014-03-08 01:49:57 -04:00
|
|
|
if (pulsewidth <= 900 || pulsewidth >= 2200) return 255; // This is an error condition
|
2016-08-17 13:46:34 -03:00
|
|
|
if (pulsewidth <= 1230) return 0;
|
|
|
|
if (pulsewidth <= 1360) return 1;
|
|
|
|
if (pulsewidth <= 1490) return 2;
|
|
|
|
if (pulsewidth <= 1620) return 3;
|
|
|
|
if (pulsewidth <= 1749) return 4; // Software Manual
|
|
|
|
return 5; // Hardware Manual
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::reset_control_switch()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2013-12-16 23:13:32 -04:00
|
|
|
oldSwitchPosition = 254;
|
2012-08-16 21:50:15 -03:00
|
|
|
read_control_switch();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2014-04-12 01:12:14 -03:00
|
|
|
/*
|
|
|
|
called when entering autotune
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::autotune_start(void)
|
2014-04-12 01:12:14 -03:00
|
|
|
{
|
|
|
|
rollController.autotune_start();
|
|
|
|
pitchController.autotune_start();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
called when exiting autotune
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::autotune_restore(void)
|
2014-04-12 01:12:14 -03:00
|
|
|
{
|
|
|
|
rollController.autotune_restore();
|
|
|
|
pitchController.autotune_restore();
|
|
|
|
}
|
2014-06-05 03:12:10 -03:00
|
|
|
|
2015-06-13 06:16:02 -03:00
|
|
|
/*
|
|
|
|
enable/disable autotune for AUTO modes
|
|
|
|
*/
|
|
|
|
void Plane::autotune_enable(bool enable)
|
|
|
|
{
|
|
|
|
if (enable) {
|
|
|
|
autotune_start();
|
|
|
|
} else {
|
|
|
|
autotune_restore();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-06-05 03:12:10 -03:00
|
|
|
/*
|
|
|
|
are we flying inverted?
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
bool Plane::fly_inverted(void)
|
2014-06-05 03:12:10 -03:00
|
|
|
{
|
2019-01-15 13:46:13 -04:00
|
|
|
if (control_mode == &plane.mode_manual) {
|
2018-08-11 08:12:35 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (inverted_flight) {
|
|
|
|
// controlled with aux switch
|
2014-06-05 03:12:10 -03:00
|
|
|
return true;
|
|
|
|
}
|
2019-01-15 13:46:13 -04:00
|
|
|
if (control_mode == &mode_auto && auto_state.inverted_flight) {
|
2014-06-05 03:12:10 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|