ardupilot/ArduPlane/radio.cpp

420 lines
14 KiB
C++
Raw Normal View History

2015-05-13 03:09:36 -03:00
#include "Plane.h"
//Function that will read the radio data, limit servos and trigger a failsafe
// ----------------------------------------------------------------------------
/*
allow for runtime change of control channel ordering
*/
2015-05-13 03:09:36 -03:00
void Plane::set_control_channels(void)
{
if (g.rudder_only) {
// in rudder only mode the roll and rudder channels are the
// same.
2016-10-22 07:27:57 -03:00
channel_roll = RC_Channels::rc_channel(rcmap.yaw()-1);
} else {
2016-10-22 07:27:57 -03:00
channel_roll = RC_Channels::rc_channel(rcmap.roll()-1);
}
2016-10-22 07:27:57 -03:00
channel_pitch = RC_Channels::rc_channel(rcmap.pitch()-1);
channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1);
channel_rudder = RC_Channels::rc_channel(rcmap.yaw()-1);
2012-08-21 23:19:50 -03:00
// set rc channel ranges
channel_roll->set_angle(SERVO_MAX);
channel_pitch->set_angle(SERVO_MAX);
channel_rudder->set_angle(SERVO_MAX);
if (!have_reverse_thrust()) {
// normal operation
2016-10-22 07:27:57 -03:00
channel_throttle->set_range(100);
} else {
// reverse thrust
if (have_reverse_throttle_rc_option) {
// when we have a reverse throttle RC option setup we use throttle
// as a range, and rely on the RC switch to get reverse thrust
channel_throttle->set_range(100);
} else {
channel_throttle->set_angle(100);
}
SRV_Channels::set_angle(SRV_Channel::k_throttle, 100);
SRV_Channels::set_angle(SRV_Channel::k_throttleLeft, 100);
SRV_Channels::set_angle(SRV_Channel::k_throttleRight, 100);
}
// update flap and airbrake channel assignment
channel_flap = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FLAP);
channel_airbrake = rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRBRAKE);
// update manual forward throttle channel assignment
quadplane.rc_fwd_thr_ch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::FWD_THR);
if (!arming.is_armed() && arming.arming_required() == AP_Arming::Required::YES_MIN_PWM) {
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, have_reverse_thrust()?SRV_Channel::Limit::TRIM:SRV_Channel::Limit::MIN);
}
2014-11-20 03:32:18 -04:00
if (!quadplane.enable) {
2019-10-17 03:03:48 -03:00
// setup correct scaling for ESCs like the UAVCAN ESCs which
// take a proportion of speed. For quadplanes we use AP_Motors
// scaling
g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle);
}
}
2012-08-21 23:19:50 -03:00
/*
initialise RC input channels
*/
2015-05-13 03:09:36 -03:00
void Plane::init_rc_in()
{
2012-08-21 23:19:50 -03:00
// set rc dead zones
channel_roll->set_default_dead_zone(30);
channel_pitch->set_default_dead_zone(30);
channel_rudder->set_default_dead_zone(30);
channel_throttle->set_default_dead_zone(30);
}
/*
initialise RC output for main channels. This is done early to allow
for BRD_SAFETYENABLE=0 and early servo control
*/
void Plane::init_rc_out_main()
{
/*
change throttle trim to minimum throttle. This prevents a
configuration error where the user sets CH3_TRIM incorrectly and
the motor may start on power up
*/
if (!have_reverse_thrust()) {
2016-10-22 07:27:57 -03:00
SRV_Channels::set_trim_to_min_for(SRV_Channel::k_throttle);
}
SRV_Channels::set_failsafe_limit(SRV_Channel::k_aileron, SRV_Channel::Limit::TRIM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_elevator, SRV_Channel::Limit::TRIM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::TRIM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_rudder, SRV_Channel::Limit::TRIM);
2016-10-22 07:27:57 -03:00
2019-10-17 03:03:48 -03:00
// setup flight controller to output the min throttle when safety off if arming
// is setup for min on disarm
if (arming.arming_required() == AP_Arming::Required::YES_MIN_PWM) {
SRV_Channels::set_safety_limit(SRV_Channel::k_throttle, have_reverse_thrust()?SRV_Channel::Limit::TRIM:SRV_Channel::Limit::MIN);
}
}
/*
initialise RC output channels for aux channels
*/
void Plane::init_rc_out_aux()
{
2016-10-22 07:27:57 -03:00
SRV_Channels::enable_aux_servos();
2012-08-04 13:39:20 -03:00
servos_output();
// setup PWM values to send if the FMU firmware dies
// allows any VTOL motors to shut off
SRV_Channels::setup_failsafe_trim_all_non_motors();
}
/*
check for pilot input on rudder stick for arming/disarming
*/
void Plane::rudder_arm_disarm_check()
2013-12-11 02:01:37 -04:00
{
if (!arming.is_armed()) {
// when not armed, full right rudder starts arming counter
ArduPlane: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
if (channel_rudder->get_control_in() > 4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to arm!
arming.arm(AP_Arming::Method::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full right rudder
rudder_arm_timer = 0;
}
} else {
// full left rudder starts disarming counter
ArduPlane: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
if (channel_rudder->get_control_in() < -4000) {
uint32_t now = millis();
if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) {
if (rudder_arm_timer == 0) {
rudder_arm_timer = now;
}
} else {
//time to disarm!
2020-02-21 09:09:58 -04:00
arming.disarm(AP_Arming::Method::RUDDER);
rudder_arm_timer = 0;
}
} else {
// not at full left rudder
rudder_arm_timer = 0;
}
}
2013-11-27 22:19:34 -04:00
}
2015-05-13 03:09:36 -03:00
void Plane::read_radio()
{
if (!rc().read_input()) {
control_failsafe();
return;
}
if (!failsafe.rc_failsafe)
{
failsafe.AFS_last_valid_rc_ms = millis();
}
if (rc_throttle_value_ok()) {
failsafe.last_valid_rc_ms = millis();
}
if (control_mode == &mode_training) {
// in training mode we don't want to use a deadzone, as we
// want manual pass through when not exceeding attitude limits
2018-04-10 19:14:57 -03:00
channel_roll->recompute_pwm_no_deadzone();
channel_pitch->recompute_pwm_no_deadzone();
channel_throttle->recompute_pwm_no_deadzone();
channel_rudder->recompute_pwm_no_deadzone();
2012-08-21 23:19:50 -03:00
}
control_failsafe();
2012-08-21 23:19:50 -03:00
2020-12-14 08:47:05 -04:00
#if AC_FENCE == ENABLED
const bool stickmixing = fence_stickmixing();
#else
const bool stickmixing = true;
#endif
airspeed_nudge_cm = 0;
throttle_nudge = 0;
2020-12-14 08:47:05 -04:00
if (g.throttle_nudge
&& channel_throttle->get_control_in() > 50
&& stickmixing) {
float nudge = (channel_throttle->get_control_in() - 50) * 0.02f;
if (ahrs.airspeed_sensor_enabled()) {
2016-11-17 21:07:10 -04:00
airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge;
2012-08-21 23:19:50 -03:00
} else {
throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
2012-08-21 23:19:50 -03:00
}
}
2013-11-27 22:19:34 -04:00
rudder_arm_disarm_check();
2018-08-20 21:38:41 -03:00
// potentially swap inputs for tailsitters
quadplane.tailsitter_check_input();
// check for transmitter tuning changes
tuning.check_input(control_mode->mode_number());
2018-08-20 21:38:41 -03:00
}
int16_t Plane::rudder_input(void)
{
if (g.rudder_only != 0) {
// in rudder only mode we discard rudder input and get target
// attitude from the roll channel.
2018-08-20 21:38:41 -03:00
return 0;
}
2018-08-20 21:38:41 -03:00
if ((g2.flight_options & FlightOptions::DIRECT_RUDDER_ONLY) &&
!(control_mode == &mode_manual || control_mode == &mode_stabilize || control_mode == &mode_acro)) {
2018-08-20 21:38:41 -03:00
// the user does not want any input except in these modes
return 0;
}
if (stick_mixing_enabled()) {
return channel_rudder->get_control_in();
}
return 0;
}
void Plane::control_failsafe()
{
if (rc_failsafe_active()) {
// we do not have valid RC input. Set all primary channel
2014-03-08 02:22:24 -04:00
// control inputs to the trim value and throttle to min
ArduPlane: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
channel_roll->set_radio_in(channel_roll->get_radio_trim());
channel_pitch->set_radio_in(channel_pitch->get_radio_trim());
channel_rudder->set_radio_in(channel_rudder->get_radio_trim());
// note that we don't set channel_throttle->radio_in to radio_trim,
// as that would cause throttle failsafe to not activate
ArduPlane: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
channel_roll->set_control_in(0);
channel_pitch->set_control_in(0);
channel_rudder->set_control_in(0);
airspeed_nudge_cm = 0;
throttle_nudge = 0;
switch (control_mode->mode_number()) {
case Mode::Number::QSTABILIZE:
case Mode::Number::QHOVER:
case Mode::Number::QLOITER:
case Mode::Number::QLAND: // throttle is ignored, but reset anyways
case Mode::Number::QRTL: // throttle is ignored, but reset anyways
case Mode::Number::QACRO:
case Mode::Number::QAUTOTUNE:
if (quadplane.available() && quadplane.motors->get_desired_spool_state() > AP_Motors::DesiredSpoolState::GROUND_IDLE) {
// set half throttle to avoid descending at maximum rate, still has a slight descent due to throttle deadzone
channel_throttle->set_control_in(channel_throttle->get_range() / 2);
break;
}
FALLTHROUGH;
default:
channel_throttle->set_control_in(0);
break;
}
}
if (ThrFailsafe(g.throttle_fs_enabled.get()) != ThrFailsafe::Enabled) {
2012-08-21 23:19:50 -03:00
return;
2019-08-20 13:24:01 -03:00
}
2012-08-21 23:19:50 -03:00
2019-08-20 13:24:01 -03:00
if (rc_failsafe_active()) {
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafe.throttle_counter++;
if (failsafe.throttle_counter == 10) {
gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe on");
failsafe.rc_failsafe = true;
AP_Notify::flags.failsafe_radio = true;
}
if (failsafe.throttle_counter > 10) {
failsafe.throttle_counter = 10;
}
} else if(failsafe.throttle_counter > 0) {
// we are no longer in failsafe condition
// but we need to recover quickly
failsafe.throttle_counter--;
if (failsafe.throttle_counter > 3) {
failsafe.throttle_counter = 3;
}
if (failsafe.throttle_counter == 1) {
gcs().send_text(MAV_SEVERITY_WARNING, "Throttle failsafe off");
} else if(failsafe.throttle_counter == 0) {
failsafe.rc_failsafe = false;
AP_Notify::flags.failsafe_radio = false;
2012-08-21 23:19:50 -03:00
}
}
}
bool Plane::trim_radio()
{
if (failsafe.rc_failsafe) {
// can't trim if we don't have valid input
return false;
}
ArduPlane: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
int16_t trim_roll_range = (channel_roll->get_radio_max() - channel_roll->get_radio_min())/5;
int16_t trim_pitch_range = (channel_pitch->get_radio_max() - channel_pitch->get_radio_min())/5;
if (channel_roll->get_radio_in() < channel_roll->get_radio_min()+trim_roll_range ||
channel_roll->get_radio_in() > channel_roll->get_radio_max()-trim_roll_range ||
channel_pitch->get_radio_in() < channel_pitch->get_radio_min()+trim_pitch_range ||
channel_pitch->get_radio_in() > channel_pitch->get_radio_max()-trim_pitch_range) {
// don't trim for extreme values - if we attempt to trim so
// there is less than 20 percent range left then assume the
// sticks are not properly centered. This also prevents
// problems with starting APM with the TX off
return false;
}
// trim main surfaces
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_aileron);
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_elevator);
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_rudder);
// trim elevons
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_elevon_left);
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_elevon_right);
// trim vtail
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_vtail_left);
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_vtail_right);
if (SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) == 0) {
// trim differential spoilers if no rudder input
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerLeft1);
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerLeft2);
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerRight1);
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerRight2);
}
if (SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto) == 0 &&
SRV_Channels::get_output_scaled(SRV_Channel::k_flap) == 0) {
// trim flaperons if no flap input
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_left);
SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_right);
}
2012-08-21 23:19:50 -03:00
// now save input trims, as these have been moved to the outputs
channel_roll->set_and_save_trim();
channel_pitch->set_and_save_trim();
channel_rudder->set_and_save_trim();
2012-08-21 23:19:50 -03:00
return true;
}
/*
check if throttle value is within allowed range
*/
bool Plane::rc_throttle_value_ok(void) const
{
if (ThrFailsafe(g.throttle_fs_enabled.get()) == ThrFailsafe::Disabled) {
return true;
}
if (channel_throttle->get_reverse()) {
return channel_throttle->get_radio_in() < g.throttle_fs_value;
}
return channel_throttle->get_radio_in() > g.throttle_fs_value;
}
/*
return true if throttle level is below throttle failsafe threshold
or RC input is invalid
*/
2018-08-20 21:38:41 -03:00
bool Plane::rc_failsafe_active(void) const
{
if (!rc_throttle_value_ok()) {
return true;
}
if (millis() - failsafe.last_valid_rc_ms > 1000) {
// we haven't had a valid RC frame for 1 seconds
return true;
}
return false;
}
/*
expo handling for MANUAL, ACRO and TRAINING modes
*/
static float channel_expo(RC_Channel *chan, int8_t expo, bool use_dz)
{
if (chan == nullptr) {
return 0;
}
float rin = use_dz? chan->get_control_in() : chan->get_control_in_zero_dz();
return SERVO_MAX * expo_curve(constrain_float(expo*0.01, 0, 1), rin/SERVO_MAX);
}
float Plane::roll_in_expo(bool use_dz) const
{
return channel_expo(channel_roll, g2.man_expo_roll, use_dz);
}
float Plane::pitch_in_expo(bool use_dz) const
{
return channel_expo(channel_pitch, g2.man_expo_roll, use_dz);
}
float Plane::rudder_in_expo(bool use_dz) const
{
return channel_expo(channel_rudder, g2.man_expo_roll, use_dz);
}