/*
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();
}
}
}
/*
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();
}
}
}