2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2014-02-02 03:57:34 -04:00
|
|
|
/*
|
2018-12-12 23:53:56 -04:00
|
|
|
autotune mode is a wrapper around the AC_AutoTune library
|
2014-02-02 03:57:34 -04:00
|
|
|
*/
|
|
|
|
|
2018-12-12 23:53:56 -04:00
|
|
|
#if AUTOTUNE_ENABLED == ENABLED
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
bool AutoTune::init()
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
2020-02-10 22:06:57 -04:00
|
|
|
// only allow AutoTune from some flight modes, for example Stabilize, AltHold, PosHold or Loiter modes
|
|
|
|
if (!copter.flightmode->allows_autotune()) {
|
2014-02-02 03:57:34 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// ensure throttle is above zero
|
2018-12-12 23:53:56 -04:00
|
|
|
if (copter.ap.throttle_zero) {
|
2014-02-02 03:57:34 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// ensure we are flying
|
2018-12-12 23:53:56 -04:00
|
|
|
if (!copter.motors->armed() || !copter.ap.auto_armed || copter.ap.land_complete) {
|
2014-02-02 03:57:34 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2021-04-29 23:29:00 -03:00
|
|
|
// use position hold while tuning if we were in QLOITER
|
|
|
|
bool position_hold = (copter.flightmode->mode_number() == Mode::Number::LOITER || copter.flightmode->mode_number() == Mode::Number::POSHOLD);
|
|
|
|
|
|
|
|
return init_internals(position_hold,
|
|
|
|
copter.attitude_control,
|
|
|
|
copter.pos_control,
|
|
|
|
copter.ahrs_view,
|
|
|
|
&copter.inertial_nav);
|
2017-02-07 20:35:31 -04:00
|
|
|
}
|
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
void AutoTune::run()
|
2017-02-07 20:35:31 -04:00
|
|
|
{
|
2014-02-02 03:57:34 -04:00
|
|
|
// apply SIMPLE mode transform to pilot inputs
|
2018-12-12 23:53:56 -04:00
|
|
|
copter.update_simple_mode();
|
2014-02-02 03:57:34 -04:00
|
|
|
|
|
|
|
// reset target lean angles and heading while landed
|
2018-12-12 23:53:56 -04:00
|
|
|
if (copter.ap.land_complete) {
|
|
|
|
// we are landed, shut down
|
|
|
|
float target_climb_rate = get_pilot_desired_climb_rate_cms();
|
|
|
|
|
2016-07-31 23:12:16 -03:00
|
|
|
// 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) {
|
2019-04-09 09:16:58 -03:00
|
|
|
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
|
2016-07-31 23:12:16 -03:00
|
|
|
} else {
|
2019-04-09 09:16:58 -03:00
|
|
|
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
2020-12-20 17:40:58 -04:00
|
|
|
copter.attitude_control->reset_rate_controller_I_terms_smoothly();
|
2021-05-24 10:42:19 -03:00
|
|
|
copter.attitude_control->reset_yaw_target_and_rate();
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2019-02-28 07:10:05 -04:00
|
|
|
float target_roll, target_pitch, target_yaw_rate;
|
2018-12-12 23:53:56 -04:00
|
|
|
get_pilot_desired_rp_yrate_cd(target_roll, target_pitch, target_yaw_rate);
|
2016-01-17 23:53:02 -04:00
|
|
|
|
2018-12-12 23:53:56 -04:00
|
|
|
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
|
2021-05-11 01:42:02 -03:00
|
|
|
copter.pos_control->relax_z_controller(0.0f);
|
2018-12-12 23:53:56 -04:00
|
|
|
copter.pos_control->update_z_controller();
|
|
|
|
} else {
|
|
|
|
// run autotune mode
|
|
|
|
AC_AutoTune::run();
|
2017-02-12 21:49:55 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2018-12-12 23:53:56 -04:00
|
|
|
/*
|
|
|
|
get stick input climb rate
|
|
|
|
*/
|
2019-05-09 23:18:49 -03:00
|
|
|
float AutoTune::get_pilot_desired_climb_rate_cms(void) const
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
2018-12-12 23:53:56 -04:00
|
|
|
float target_climb_rate = copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in());
|
2014-02-02 03:57:34 -04:00
|
|
|
|
2018-12-12 23:53:56 -04:00
|
|
|
// get avoidance adjusted climb rate
|
2020-06-16 03:50:37 -03:00
|
|
|
target_climb_rate = copter.mode_autotune.get_avoidance_adjusted_climbrate(target_climb_rate);
|
2014-12-05 08:38:28 -04:00
|
|
|
|
2018-12-12 23:53:56 -04:00
|
|
|
return target_climb_rate;
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
|
2018-12-12 23:53:56 -04:00
|
|
|
/*
|
|
|
|
get stick roll, pitch and yaw rate
|
|
|
|
*/
|
2019-05-09 23:18:49 -03:00
|
|
|
void AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitch_cd, float &yaw_rate_cds)
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
2019-02-28 07:10:05 -04:00
|
|
|
copter.mode_autotune.get_pilot_desired_lean_angles(des_roll_cd, des_pitch_cd, copter.aparm.angle_max,
|
2021-09-06 06:44:18 -03:00
|
|
|
copter.attitude_control->get_althold_lean_angle_max_cd());
|
2021-09-17 02:54:19 -03:00
|
|
|
yaw_rate_cds = copter.mode_autotune.get_pilot_desired_yaw_rate(copter.channel_yaw->norm_input_dz());
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
|
2018-12-12 23:53:56 -04:00
|
|
|
/*
|
|
|
|
setup z controller velocity and accel limits
|
|
|
|
*/
|
2019-05-09 23:18:49 -03:00
|
|
|
void AutoTune::init_z_limits()
|
2014-02-02 03:57:34 -04:00
|
|
|
{
|
2021-05-19 11:07:38 -03:00
|
|
|
// set vertical speed and acceleration limits
|
2021-05-11 01:42:02 -03:00
|
|
|
copter.pos_control->set_max_speed_accel_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up, copter.g.pilot_accel_z);
|
2021-07-08 01:17:41 -03:00
|
|
|
copter.pos_control->set_correction_speed_accel_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up, copter.g.pilot_accel_z);
|
2014-02-02 03:57:34 -04:00
|
|
|
}
|
|
|
|
|
2023-07-13 21:58:09 -03:00
|
|
|
#if HAL_LOGGING_ENABLED
|
2019-05-09 23:18:49 -03:00
|
|
|
void AutoTune::log_pids()
|
2018-12-21 22:26:45 -04:00
|
|
|
{
|
2019-01-18 00:24:08 -04:00
|
|
|
copter.logger.Write_PID(LOG_PIDR_MSG, copter.attitude_control->get_rate_roll_pid().get_pid_info());
|
|
|
|
copter.logger.Write_PID(LOG_PIDP_MSG, copter.attitude_control->get_rate_pitch_pid().get_pid_info());
|
|
|
|
copter.logger.Write_PID(LOG_PIDY_MSG, copter.attitude_control->get_rate_yaw_pid().get_pid_info());
|
2018-12-21 22:26:45 -04:00
|
|
|
}
|
2023-07-13 21:58:09 -03:00
|
|
|
#endif
|
2018-12-21 22:26:45 -04:00
|
|
|
|
2018-12-12 23:53:56 -04:00
|
|
|
/*
|
|
|
|
check if we have a good position estimate
|
|
|
|
*/
|
2019-05-09 23:18:49 -03:00
|
|
|
bool AutoTune::position_ok()
|
2018-04-01 02:41:49 -03:00
|
|
|
{
|
2018-12-12 23:53:56 -04:00
|
|
|
return copter.position_ok();
|
2018-04-01 02:41:49 -03:00
|
|
|
}
|
|
|
|
|
2018-12-12 23:53:56 -04:00
|
|
|
/*
|
|
|
|
initialise autotune mode
|
|
|
|
*/
|
2019-05-09 23:18:49 -03:00
|
|
|
bool ModeAutoTune::init(bool ignore_checks)
|
2014-12-05 08:38:28 -04:00
|
|
|
{
|
2021-03-24 21:15:20 -03:00
|
|
|
return autotune.init();
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
void ModeAutoTune::run()
|
2014-12-05 08:38:28 -04:00
|
|
|
{
|
2021-03-24 21:15:20 -03:00
|
|
|
autotune.run();
|
2018-04-01 02:41:49 -03:00
|
|
|
}
|
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
void ModeAutoTune::save_tuning_gains()
|
2018-04-01 02:41:49 -03:00
|
|
|
{
|
2021-03-24 21:15:20 -03:00
|
|
|
autotune.save_tuning_gains();
|
2014-12-05 08:38:28 -04:00
|
|
|
}
|
|
|
|
|
2021-04-20 09:36:36 -03:00
|
|
|
void ModeAutoTune::exit()
|
2017-01-01 00:52:07 -04:00
|
|
|
{
|
2021-03-24 21:15:20 -03:00
|
|
|
autotune.stop();
|
2017-01-01 00:52:07 -04:00
|
|
|
}
|
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
void ModeAutoTune::reset()
|
2019-05-01 09:27:40 -03:00
|
|
|
{
|
2021-03-24 21:15:20 -03:00
|
|
|
autotune.reset();
|
2019-05-01 09:27:40 -03:00
|
|
|
}
|
|
|
|
|
2014-02-02 03:57:34 -04:00
|
|
|
#endif // AUTOTUNE_ENABLED == ENABLED
|