/*
this gives the user control of the aircraft in stabilization modes
*/
static void stabilize_stick_mixing()
{
if (!stick_mixing_enabled() ||
control_mode == FLY_BY_WIRE_A ||
control_mode == FLY_BY_WIRE_B ||
control_mode == TRAINING) {
return;
}
// do stick mixing on aileron/elevator
float ch1_inf;
float ch2_inf;
ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
ch1_inf = fabs(ch1_inf);
ch1_inf = min(ch1_inf, 400.0);
ch1_inf = ((400.0 - ch1_inf) /400.0);
ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim;
ch2_inf = fabs(ch2_inf);
ch2_inf = min(ch2_inf, 400.0);
ch2_inf = ((400.0 - ch2_inf) /400.0);
// scale the sensor input based on the stick input
// -----------------------------------------------
g.channel_roll.servo_out *= ch1_inf;
g.channel_pitch.servo_out *= ch2_inf;
// Mix in stick inputs
// -------------------
g.channel_roll.servo_out += g.channel_roll.pwm_to_angle();
g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle();
}
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);
}
/*
calculate yaw control for ground steering with specific course
*/
void Plane::calc_nav_yaw_course(void)
{
// holding a specific navigation course on the ground. Used in
// auto-takeoff and landing
int32_t bearing_error_cd = nav_controller->bearing_error_cd();
steering_control.steering = steerController.get_steering_out_angle_error(bearing_error_cd);
if (stick_mixing_enabled()) {
stick_mix_channel(channel_rudder, steering_control.steering);
}
steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500);
}
/*
this gives the user control of the aircraft in stabilization modes
*/
void Plane::stabilize_stick_mixing_direct()
{
if (!stick_mixing_enabled() ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == TRAINING) {
return;
}
stick_mix_channel(channel_roll, channel_roll->servo_out);
stick_mix_channel(channel_pitch, channel_pitch->servo_out);
}
/*
this gives the user control of the aircraft in stabilization modes
using FBW style controls
*/
void Plane::stabilize_stick_mixing_fbw()
{
if (!stick_mixing_enabled() ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == QRTL ||
control_mode == TRAINING ||
(control_mode == AUTO && g.auto_fbw_steer == 42)) {
return;
}
// do FBW style stick mixing. We don't treat it linearly
// however. For inputs up to half the maximum, we use linear
// addition to the nav_roll and nav_pitch. Above that it goes
// non-linear and ends up as 2x the maximum, to ensure that
// the user can direct the plane in any direction with stick
// mixing.
float roll_input = channel_roll->norm_input();
if (roll_input > 0.5f) {
roll_input = (3*roll_input - 1);
} else if (roll_input < -0.5f) {
roll_input = (3*roll_input + 1);
}
nav_roll_cd += roll_input * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
float pitch_input = channel_pitch->norm_input();
if (pitch_input > 0.5f) {
pitch_input = (3*pitch_input - 1);
} else if (pitch_input < -0.5f) {
pitch_input = (3*pitch_input + 1);
}
if (fly_inverted()) {
pitch_input = -pitch_input;
}
if (pitch_input > 0) {
nav_pitch_cd += pitch_input * aparm.pitch_limit_max_cd;
} else {
nav_pitch_cd += -(pitch_input * pitch_limit_min_cd);
}
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
}
/*
this gives the user control of the aircraft in stabilization modes
*/
void Plane::stabilize_stick_mixing_direct()
{
if (!stick_mixing_enabled() ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == QRTL ||
control_mode == TRAINING) {
return;
}
stick_mix_channel(channel_roll);
stick_mix_channel(channel_pitch);
}
/*
stabilize the yaw axis
*/
static void stabilize_yaw(float speed_scaler)
{
float ch4_inf = 1.0;
if (stick_mixing_enabled()) {
// stick mixing performed for rudder for all cases including FBW
// important for steering on the ground during landing
// -----------------------------------------------
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
ch4_inf = fabs(ch4_inf);
ch4_inf = min(ch4_inf, 400.0);
ch4_inf = ((400.0 - ch4_inf) /400.0);
}
// Apply output to Rudder
calc_nav_yaw(speed_scaler, ch4_inf);
g.channel_rudder.servo_out *= ch4_inf;
g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle();
}
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.
return 0;
}
if ((g2.flight_options & FlightOptions::DIRECT_RUDDER_ONLY) &&
!(control_mode == MANUAL || control_mode == STABILIZE || control_mode == ACRO)) {
// 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;
}
/*
this gives the user control of the aircraft in stabilization modes
*/
void Plane::stabilize_stick_mixing_direct()
{
if (!stick_mixing_enabled() ||
control_mode == ACRO ||
control_mode == FLY_BY_WIRE_A ||
control_mode == AUTOTUNE ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == QRTL ||
control_mode == TRAINING) {
return;
}
int16_t aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
stick_mix_channel(channel_roll, aileron);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
int16_t elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
stick_mix_channel(channel_pitch, elevator);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
}
void Plane::read_radio()
{
if (!hal.rcin->new_input()) {
control_failsafe(channel_throttle->get_radio_in());
return;
}
if(!failsafe.ch3_failsafe)
{
failsafe.AFS_last_valid_rc_ms = millis();
}
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->get_radio_in());
channel_throttle->set_servo_out(channel_throttle->get_control_in());
if (g.throttle_nudge && channel_throttle->get_servo_out() > 50 && geofence_stickmixing()) {
float nudge = (channel_throttle->get_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 if (stick_mixing_enabled()) {
rudder_input = channel_rudder->get_control_in();
} else {
// no stick mixing
rudder_input = 0;
}
// check for transmitter tuning changes
tuning.check_input(control_mode);
}
