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