2016-07-22 05:14:53 -03:00
|
|
|
/*
|
|
|
|
plane specific AP_AdvancedFailsafe class
|
|
|
|
*/
|
|
|
|
|
|
|
|
#include "Plane.h"
|
|
|
|
|
2019-01-08 21:05:40 -04:00
|
|
|
#if ADVANCED_FAILSAFE == ENABLED
|
2016-07-22 05:14:53 -03:00
|
|
|
// Constructor
|
2019-06-14 00:13:55 -03:00
|
|
|
AP_AdvancedFailsafe_Plane::AP_AdvancedFailsafe_Plane(AP_Mission &_mission) :
|
|
|
|
AP_AdvancedFailsafe(_mission)
|
2016-07-22 05:14:53 -03:00
|
|
|
{}
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
setup radio_out values for all channels to termination values
|
|
|
|
*/
|
|
|
|
void AP_AdvancedFailsafe_Plane::terminate_vehicle(void)
|
|
|
|
{
|
2016-10-22 07:27:57 -03:00
|
|
|
plane.g2.servo_channels.disable_passthrough(true);
|
2016-10-12 00:41:34 -03:00
|
|
|
|
2017-07-25 14:58:24 -03:00
|
|
|
if (_terminate_action == TERMINATE_ACTION_LAND) {
|
|
|
|
plane.landing.terminate();
|
|
|
|
} else {
|
|
|
|
// aerodynamic termination is the default approach to termination
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 100);
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 100);
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, SERVO_MAX);
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, SERVO_MAX);
|
|
|
|
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, SERVO_MAX);
|
2018-11-09 18:38:43 -04:00
|
|
|
if (plane.have_reverse_thrust()) {
|
2018-09-12 17:38:25 -03:00
|
|
|
// configured for reverse thrust, use TRIM
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
|
|
|
} else {
|
|
|
|
// use MIN
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
|
|
|
|
}
|
2017-07-25 14:58:24 -03:00
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
|
|
|
SRV_Channels::set_output_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
|
|
|
}
|
2016-07-22 05:36:28 -03:00
|
|
|
|
2017-06-15 04:00:42 -03:00
|
|
|
plane.servos_output();
|
|
|
|
|
2016-09-26 22:46:51 -03:00
|
|
|
plane.quadplane.afs_terminate();
|
|
|
|
|
|
|
|
// also disarm to ensure that ignition is cut
|
2019-05-03 20:35:12 -03:00
|
|
|
plane.arming.disarm();
|
2016-07-22 05:14:53 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void AP_AdvancedFailsafe_Plane::setup_IO_failsafe(void)
|
|
|
|
{
|
2016-10-22 07:27:57 -03:00
|
|
|
// all aux channels
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_flap_auto, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_flap, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_aileron, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_rudder, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator, SRV_Channel::SRV_CHANNEL_LIMIT_MAX);
|
2018-11-09 18:38:43 -04:00
|
|
|
if (plane.have_reverse_thrust()) {
|
2018-09-12 17:38:25 -03:00
|
|
|
// configured for reverse thrust, use TRIM
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
|
|
|
} else {
|
|
|
|
// normal throttle, use MIN
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleLeft, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttleRight, SRV_Channel::SRV_CHANNEL_LIMIT_MIN);
|
|
|
|
}
|
2016-10-22 07:27:57 -03:00
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
|
|
|
SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
2016-07-22 05:36:28 -03:00
|
|
|
|
|
|
|
if (plane.quadplane.available()) {
|
|
|
|
// setup AP_Motors outputs for failsafe
|
|
|
|
uint16_t mask = plane.quadplane.motors->get_motor_mask();
|
|
|
|
hal.rcout->set_failsafe_pwm(mask, plane.quadplane.thr_min_pwm);
|
|
|
|
}
|
2016-07-22 05:14:53 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
return an AFS_MODE for current control mode
|
|
|
|
*/
|
|
|
|
AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Plane::afs_mode(void)
|
|
|
|
{
|
|
|
|
if (plane.auto_throttle_mode) {
|
|
|
|
return AP_AdvancedFailsafe::AFS_AUTO;
|
|
|
|
}
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.control_mode == &plane.mode_manual) {
|
2016-07-22 05:14:53 -03:00
|
|
|
return AP_AdvancedFailsafe::AFS_MANUAL;
|
|
|
|
}
|
|
|
|
return AP_AdvancedFailsafe::AFS_STABILIZED;
|
|
|
|
}
|
2019-01-08 21:05:40 -04:00
|
|
|
#endif // ADVANCED_FAILSAFE
|