void Copter::AutoTune::run()
{
// apply SIMPLE mode transform to pilot inputs
copter.update_simple_mode();
// reset target lean angles and heading while landed
if (copter.ap.land_complete) {
// we are landed, shut down
float target_climb_rate = get_pilot_desired_climb_rate_cms();
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if (target_climb_rate < 0.0f) {
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
} else {
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
copter.attitude_control->reset_rate_controller_I_terms();
copter.attitude_control->set_yaw_target_to_current_heading();
int32_t target_roll, target_pitch, target_yaw_rate;
get_pilot_desired_rp_yrate_cd(target_roll, target_pitch, target_yaw_rate);
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
copter.pos_control->relax_alt_hold_controllers(0.0f);
copter.pos_control->update_z_controller();
} else {
// run autotune mode
AC_AutoTune::run();
}
}
/*
control QHOVER mode
*/
void QuadPlane::control_hover(void)
{
if (throttle_wait) {
attitude_control->set_throttle_out_unstabilized(0, true, 0);
pos_control->relax_alt_hold_controllers(0);
} else {
hold_hover(get_pilot_desired_climb_rate_cms());
}
}
// run quadplane loiter controller
void QuadPlane::control_loiter()
{
if (throttle_wait) {
attitude_control->set_throttle_out_unstabilized(0, true, 0);
pos_control->relax_alt_hold_controllers(0);
wp_nav->init_loiter_target();
return;
}
if (should_relax()) {
wp_nav->loiter_soften_for_landing();
}
if (millis() - last_loiter_ms > 500) {
wp_nav->init_loiter_target();
}
last_loiter_ms = millis();
// initialize vertical speed and acceleration
pos_control->set_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max);
pos_control->set_accel_z(pilot_accel_z);
// process pilot's roll and pitch input
wp_nav->set_pilot_desired_acceleration(plane.channel_roll->control_in,
plane.channel_pitch->control_in);
// Update EKF speed limit - used to limit speed when we are using optical flow
float ekfGndSpdLimit, ekfNavVelGainScaler;
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(),
wp_nav->get_pitch(),
get_desired_yaw_rate_cds());
// nav roll and pitch are controller by loiter controller
plane.nav_roll_cd = wp_nav->get_roll();
plane.nav_pitch_cd = wp_nav->get_pitch();
// update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false);
pos_control->update_z_controller();
}