/*
return true if the current settings and mode should allow for stick mixing
*/
static bool stick_mixing_enabled(void)
{
if (control_mode == CIRCLE || control_mode > FLY_BY_WIRE_B) {
// we're in an auto mode. Check the stick mixing flag
if (g.stick_mixing &&
geofence_stickmixing() &&
failsafe == FAILSAFE_NONE) {
// we're in an auto mode, and haven't triggered failsafe
return true;
} else {
return false;
}
}
// non-auto mode. Always do stick mixing
return true;
}
void Plane::read_radio()
{
if (!rc().read_input()) {
control_failsafe();
return;
}
if(!failsafe.rc_failsafe)
{
failsafe.AFS_last_valid_rc_ms = millis();
}
failsafe.last_valid_rc_ms = millis();
if (control_mode == TRAINING) {
// in training mode we don't want to use a deadzone, as we
// want manual pass through when not exceeding attitude limits
channel_roll->recompute_pwm_no_deadzone();
channel_pitch->recompute_pwm_no_deadzone();
channel_throttle->recompute_pwm_no_deadzone();
channel_rudder->recompute_pwm_no_deadzone();
}
control_failsafe();
if (g.throttle_nudge && channel_throttle->get_control_in() > 50 && geofence_stickmixing()) {
float nudge = (channel_throttle->get_control_in() - 50) * 0.02f;
if (ahrs.airspeed_sensor_enabled()) {
airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge;
} else {
throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
}
} else {
airspeed_nudge_cm = 0;
throttle_nudge = 0;
}
rudder_arm_disarm_check();
// potentially swap inputs for tailsitters
quadplane.tailsitter_check_input();
// check for transmitter tuning changes
tuning.check_input(control_mode);
}
/*
return true if the current settings and mode should allow for stick mixing
*/
bool Plane::stick_mixing_enabled(void)
{
if (auto_throttle_mode) {
// we're in an auto mode. Check the stick mixing flag
if (g.stick_mixing != STICK_MIXING_DISABLED &&
geofence_stickmixing() &&
failsafe.state == FAILSAFE_NONE &&
!rc_failsafe_active()) {
// we're in an auto mode, and haven't triggered failsafe
return true;
} else {
return false;
}
}
if (failsafe.ch3_failsafe && g.short_fs_action == 2) {
// don't do stick mixing in FBWA glide mode
return false;
}
// non-auto mode. Always do stick mixing
return true;
}
void Plane::read_radio()
{
if (!hal.rcin->new_input()) {
control_failsafe(channel_throttle->radio_in);
return;
}
failsafe.last_valid_rc_ms = millis();
elevon.ch1_temp = channel_roll->read();
elevon.ch2_temp = channel_pitch->read();
uint16_t pwm_roll, pwm_pitch;
if (g.mix_mode == 0) {
pwm_roll = elevon.ch1_temp;
pwm_pitch = elevon.ch2_temp;
}else{
pwm_roll = BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500;
pwm_pitch = (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500;
}
RC_Channel::set_pwm_all();
if (control_mode == TRAINING) {
// in training mode we don't want to use a deadzone, as we
// want manual pass through when not exceeding attitude limits
channel_roll->set_pwm_no_deadzone(pwm_roll);
channel_pitch->set_pwm_no_deadzone(pwm_pitch);
channel_throttle->set_pwm_no_deadzone(channel_throttle->read());
channel_rudder->set_pwm_no_deadzone(channel_rudder->read());
} else {
channel_roll->set_pwm(pwm_roll);
channel_pitch->set_pwm(pwm_pitch);
}
control_failsafe(channel_throttle->radio_in);
channel_throttle->servo_out = channel_throttle->control_in;
if (g.throttle_nudge && channel_throttle->servo_out > 50 && geofence_stickmixing()) {
float nudge = (channel_throttle->servo_out - 50) * 0.02f;
if (ahrs.airspeed_sensor_enabled()) {
airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
} else {
throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
}
} else {
airspeed_nudge_cm = 0;
throttle_nudge = 0;
}
rudder_arm_disarm_check();
if (g.rudder_only != 0) {
// in rudder only mode we discard rudder input and get target
// attitude from the roll channel.
rudder_input = 0;
} else {
rudder_input = channel_rudder->control_in;
}
}
void Plane::read_radio()
{
if (!RC_Channels::read_input()) {
control_failsafe();
return;
}
if(!failsafe.rc_failsafe)
{
failsafe.AFS_last_valid_rc_ms = millis();
}
failsafe.last_valid_rc_ms = millis();
if (control_mode == TRAINING) {
// in training mode we don't want to use a deadzone, as we
// want manual pass through when not exceeding attitude limits
channel_roll->recompute_pwm_no_deadzone();
channel_pitch->recompute_pwm_no_deadzone();
channel_throttle->recompute_pwm_no_deadzone();
channel_rudder->recompute_pwm_no_deadzone();
}
control_failsafe();
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
if (g.throttle_nudge && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 50 && geofence_stickmixing()) {
float nudge = (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) - 50) * 0.02f;
if (ahrs.airspeed_sensor_enabled()) {
airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge;
} else {
throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
}
} else {
airspeed_nudge_cm = 0;
throttle_nudge = 0;
}
rudder_arm_disarm_check();
if (g.rudder_only != 0) {
// in rudder only mode we discard rudder input and get target
// attitude from the roll channel.
rudder_input = 0;
} else if (stick_mixing_enabled()) {
rudder_input = channel_rudder->get_control_in();
} else {
// no stick mixing
rudder_input = 0;
}
// potentially swap inputs for tailsitters
quadplane.tailsitter_check_input();
// check for transmitter tuning changes
tuning.check_input(control_mode);
}
