/*
a special stabilization function for training mode
*/
void Plane::stabilize_training(float speed_scaler)
{
if (training_manual_roll) {
channel_roll->set_servo_out(channel_roll->get_control_in());
} else {
// calculate what is needed to hold
stabilize_roll(speed_scaler);
if ((nav_roll_cd > 0 && channel_roll->get_control_in() < channel_roll->get_servo_out()) ||
(nav_roll_cd < 0 && channel_roll->get_control_in() > channel_roll->get_servo_out())) {
// allow user to get out of the roll
channel_roll->set_servo_out(channel_roll->get_control_in());
}
}
if (training_manual_pitch) {
channel_pitch->set_servo_out(channel_pitch->get_control_in());
} else {
stabilize_pitch(speed_scaler);
if ((nav_pitch_cd > 0 && channel_pitch->get_control_in() < channel_pitch->get_servo_out()) ||
(nav_pitch_cd < 0 && channel_pitch->get_control_in() > channel_pitch->get_servo_out())) {
// allow user to get back to level
channel_pitch->set_servo_out(channel_pitch->get_control_in());
}
}
stabilize_yaw(speed_scaler);
}
/*
a special stabilization function for training mode
*/
void Plane::stabilize_training(float speed_scaler)
{
if (training_manual_roll) {
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in());
} else {
// calculate what is needed to hold
stabilize_roll(speed_scaler);
if ((nav_roll_cd > 0 && channel_roll->get_control_in() < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
(nav_roll_cd < 0 && channel_roll->get_control_in() > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
// allow user to get out of the roll
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, channel_roll->get_control_in());
}
}
if (training_manual_pitch) {
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in());
} else {
stabilize_pitch(speed_scaler);
if ((nav_pitch_cd > 0 && channel_pitch->get_control_in() < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
(nav_pitch_cd < 0 && channel_pitch->get_control_in() > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
// allow user to get back to level
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, channel_pitch->get_control_in());
}
}
stabilize_yaw(speed_scaler);
}
/*
main stabilization function for all 3 axes
*/
void Plane::stabilize()
{
if (control_mode == MANUAL) {
// nothing to do
return;
}
float speed_scaler = get_speed_scaler();
if (quadplane.in_tailsitter_vtol_transition()) {
/*
during transition to vtol in a tailsitter try to raise the
nose rapidly while keeping the wings level
*/
nav_pitch_cd = constrain_float((quadplane.tailsitter.transition_angle+5)*100, 5500, 8500),
nav_roll_cd = 0;
}
if (control_mode == TRAINING) {
stabilize_training(speed_scaler);
} else if (control_mode == ACRO) {
stabilize_acro(speed_scaler);
} else if ((control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == QRTL) &&
!quadplane.in_tailsitter_vtol_transition()) {
quadplane.control_run();
} else {
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {
stabilize_stick_mixing_fbw();
}
stabilize_roll(speed_scaler);
stabilize_pitch(speed_scaler);
if (g.stick_mixing == STICK_MIXING_DIRECT || control_mode == STABILIZE) {
stabilize_stick_mixing_direct();
}
stabilize_yaw(speed_scaler);
}
/*
see if we should zero the attitude controller integrators.
*/
if (channel_throttle->get_control_in() == 0 &&
fabsf(relative_altitude) < 5.0f &&
fabsf(barometer.get_climb_rate()) < 0.5f &&
gps.ground_speed() < 3) {
// we are low, with no climb rate, and zero throttle, and very
// low ground speed. Zero the attitude controller
// integrators. This prevents integrator buildup pre-takeoff.
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();
// if moving very slowly also zero the steering integrator
if (gps.ground_speed() < 1) {
steerController.reset_I();
}
}
}
/*
a special stabilization function for training mode
*/
static void stabilize_training(float speed_scaler)
{
if (training_manual_roll) {
g.channel_roll.servo_out = g.channel_roll.control_in;
} else {
// calculate what is needed to hold
stabilize_roll(speed_scaler);
if ((nav_roll_cd > 0 && g.channel_roll.control_in < g.channel_roll.servo_out) ||
(nav_roll_cd < 0 && g.channel_roll.control_in > g.channel_roll.servo_out)) {
// allow user to get out of the roll
g.channel_roll.servo_out = g.channel_roll.control_in;
}
}
if (training_manual_pitch) {
g.channel_pitch.servo_out = g.channel_pitch.control_in;
} else {
stabilize_pitch(speed_scaler);
if ((nav_pitch_cd > 0 && g.channel_pitch.control_in < g.channel_pitch.servo_out) ||
(nav_pitch_cd < 0 && g.channel_pitch.control_in > g.channel_pitch.servo_out)) {
// allow user to get back to level
g.channel_pitch.servo_out = g.channel_pitch.control_in;
}
}
stabilize_stick_mixing();
stabilize_yaw(speed_scaler);
}
/*
main stabilization function for all 3 axes
*/
static void stabilize()
{
float speed_scaler = get_speed_scaler();
if (control_mode == TRAINING) {
stabilize_training(speed_scaler);
} else {
stabilize_roll(speed_scaler);
stabilize_pitch(speed_scaler);
stabilize_stick_mixing();
stabilize_yaw(speed_scaler);
}
}
/*
main stabilization function for all 3 axes
*/
void Plane::stabilize()
{
if (control_mode == MANUAL) {
// nothing to do
return;
}
float speed_scaler = get_speed_scaler();
if (control_mode == TRAINING) {
stabilize_training(speed_scaler);
} else if (control_mode == ACRO) {
stabilize_acro(speed_scaler);
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == QLAND ||
control_mode == QRTL) {
quadplane.control_run();
} else {
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {
stabilize_stick_mixing_fbw();
}
stabilize_roll(speed_scaler);
stabilize_pitch(speed_scaler);
if (g.stick_mixing == STICK_MIXING_DIRECT || control_mode == STABILIZE) {
stabilize_stick_mixing_direct();
}
stabilize_yaw(speed_scaler);
}
/*
see if we should zero the attitude controller integrators.
*/
if (channel_throttle->get_control_in() == 0 &&
relative_altitude_abs_cm() < 500 &&
fabsf(barometer.get_climb_rate()) < 0.5f &&
gps.ground_speed() < 3) {
// we are low, with no climb rate, and zero throttle, and very
// low ground speed. Zero the attitude controller
// integrators. This prevents integrator buildup pre-takeoff.
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();
// if moving very slowly also zero the steering integrator
if (gps.ground_speed() < 1) {
steerController.reset_I();
}
}
}
