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