3 #if AUTOTUNE_ENABLED == ENABLED 40 #define AUTOTUNE_AXIS_BITMASK_ROLL 1 41 #define AUTOTUNE_AXIS_BITMASK_PITCH 2 42 #define AUTOTUNE_AXIS_BITMASK_YAW 4 44 #define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds 45 #define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 1000 // timeout for tuning mode's testing step 46 #define AUTOTUNE_LEVEL_ANGLE_CD 500 // angle which qualifies as level 47 #define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch 48 #define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw 49 #define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the copter to be level 50 #define AUTOTUNE_RD_STEP 0.05f // minimum increment when increasing/decreasing Rate D term 51 #define AUTOTUNE_RP_STEP 0.05f // minimum increment when increasing/decreasing Rate P term 52 #define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term 53 #define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing 54 #define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing 55 #define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing 56 #define AUTOTUNE_RD_MAX 0.200f // maximum Rate D value 57 #define AUTOTUNE_RLPF_MIN 1.0f // minimum Rate Yaw filter value 58 #define AUTOTUNE_RLPF_MAX 5.0f // maximum Rate Yaw filter value 59 #define AUTOTUNE_RP_MIN 0.01f // minimum Rate P value 60 #define AUTOTUNE_RP_MAX 2.0f // maximum Rate P value 61 #define AUTOTUNE_SP_MAX 20.0f // maximum Stab P value 62 #define AUTOTUNE_SP_MIN 0.5f // maximum Stab P value 63 #define AUTOTUNE_RP_ACCEL_MIN 4000.0f // Minimum acceleration for Roll and Pitch 64 #define AUTOTUNE_Y_ACCEL_MIN 1000.0f // Minimum acceleration for Yaw 65 #define AUTOTUNE_Y_FILT_FREQ 10.0f // Autotune filter frequency when testing Yaw 66 #define AUTOTUNE_SUCCESS_COUNT 4 // The number of successful iterations we need to freeze at current gains 67 #define AUTOTUNE_D_UP_DOWN_MARGIN 0.2f // The margin below the target that we tune D in 68 #define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning 69 #define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning 70 #define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning 71 #define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration 72 #define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration 75 #define AUTOTUNE_TARGET_ANGLE_RLLPIT_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step 76 #define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 18000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step 77 #define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step 78 #define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step 81 #define AUTOTUNE_TARGET_ANGLE_YAW_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step 82 #define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step 83 #define AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD 500 // minimum target angle during TESTING_RATE step that will cause us to move to next step 84 #define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step 87 #define AUTOTUNE_MESSAGE_STARTED 0 88 #define AUTOTUNE_MESSAGE_STOPPED 1 89 #define AUTOTUNE_MESSAGE_SUCCESS 2 90 #define AUTOTUNE_MESSAGE_FAILED 3 91 #define AUTOTUNE_MESSAGE_SAVED_GAINS 4 93 #define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000 109 success =
start(
false);
122 success =
start(
false);
174 if (
ap.throttle_zero) {
179 if (!
motors->armed() || !
ap.auto_armed ||
ap.land_complete) {
220 gcs().
send_text(MAV_SEVERITY_INFO,
"AutoTune: Paused: Pilot Override Active");
228 gcs().
send_text(MAV_SEVERITY_INFO,
"AutoTune: UPDATING_GAINS");
231 gcs().
send_text(MAV_SEVERITY_INFO,
"AutoTune: TWITCHING");
234 gcs().
send_text(MAV_SEVERITY_INFO,
"AutoTune: unknown step");
243 return "Rate D Down";
247 return "Angle P Down";
256 const uint32_t now =
millis();
260 float tune_rp = 0.0f;
261 float tune_rd = 0.0f;
262 float tune_sp = 0.0f;
263 float tune_accel = 0.0f;
264 char axis_char =
'?';
301 gcs().
send_text(MAV_SEVERITY_INFO,
"AutoTune: p=%f d=%f", (
double)tune_rp, (
double)tune_rd);
305 gcs().
send_text(MAV_SEVERITY_INFO,
"AutoTune: p=%f accel=%f", (
double)tune_sp, (
double)tune_accel);
317 float target_roll, target_pitch;
318 float target_yaw_rate;
319 int16_t target_climb_rate;
327 if (!
motors->armed() || !
ap.auto_armed || !
motors->get_interlock()) {
349 if (
ap.land_complete && target_climb_rate > 0) {
357 if (
ap.land_complete) {
359 if (target_climb_rate < 0.0
f) {
366 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
371 bool zero_rp_input =
is_zero(target_roll) &&
is_zero(target_pitch);
372 if (!zero_rp_input || !
is_zero(target_yaw_rate) || target_climb_rate != 0) {
381 if (!zero_rp_input) {
406 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
422 if (current > maximum) {
572 attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0
f, direction_sign * target_angle, 0.0
f);
576 attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0
f, 0.0
f, direction_sign * target_angle);
653 #if LOGGING_ENABLED == ENABLED 664 #if LOGGING_ENABLED == ENABLED 698 updating_rate_d_up(
tune_roll_rd,
g.
autotune_min_d,
AUTOTUNE_RD_MAX,
AUTOTUNE_RD_STEP,
tune_roll_rp,
AUTOTUNE_RP_MIN,
AUTOTUNE_RP_MAX,
AUTOTUNE_RP_STEP,
target_rate,
test_rate_min,
test_rate_max);
701 updating_rate_d_up(
tune_pitch_rd,
g.
autotune_min_d,
AUTOTUNE_RD_MAX,
AUTOTUNE_RD_STEP,
tune_pitch_rp,
AUTOTUNE_RP_MIN,
AUTOTUNE_RP_MAX,
AUTOTUNE_RP_STEP,
target_rate,
test_rate_min,
test_rate_max);
704 updating_rate_d_up(
tune_yaw_rLPF,
AUTOTUNE_RLPF_MIN,
AUTOTUNE_RLPF_MAX,
AUTOTUNE_RD_STEP,
tune_yaw_rp,
AUTOTUNE_RP_MIN,
AUTOTUNE_RP_MAX,
AUTOTUNE_RP_STEP,
target_rate,
test_rate_min,
test_rate_max);
816 bool complete =
false;
1154 switch (message_id) {
1156 gcs().
send_text(MAV_SEVERITY_INFO,
"AutoTune: Started");
1159 gcs().
send_text(MAV_SEVERITY_INFO,
"AutoTune: Stopped");
1162 gcs().
send_text(MAV_SEVERITY_INFO,
"AutoTune: Success");
1165 gcs().
send_text(MAV_SEVERITY_NOTICE,
"AutoTune: Failed");
1168 gcs().
send_text(MAV_SEVERITY_INFO,
"AutoTune: Saved gains");
1191 if (rate > meas_rate_max) {
1193 meas_rate_max = rate;
1194 meas_rate_min = rate;
1198 if ((rate < meas_rate_min) && (meas_rate_max > rate_target_max * 0.5
f)) {
1200 meas_rate_min = rate;
1204 if (meas_rate_max < rate_target_max * 0.75f) {
1210 if (meas_rate_max > rate_target_max) {
1231 if (angle > meas_angle_max) {
1233 meas_angle_max = angle;
1234 meas_angle_min = angle;
1238 if ((angle < meas_angle_min) && (meas_angle_max > angle_target_max * 0.5
f)) {
1240 meas_angle_min = angle;
1244 if (rate > meas_rate_max) {
1246 meas_rate_max = rate;
1247 meas_rate_min = rate;
1251 if (rate < meas_rate_min) {
1253 meas_rate_min = rate;
1257 if (meas_angle_max < angle_target_max * 0.75f) {
1263 if (meas_angle_max > angle_target_max) {
1282 if (rate_measurement_max < rate_measurement) {
1283 rate_measurement_max = rate_measurement;
1290 void Copter::ModeAutoTune::updating_rate_d_up(
float &tune_d,
float tune_d_min,
float tune_d_max,
float tune_d_step_ratio,
float &tune_p,
float tune_p_min,
float tune_p_max,
float tune_p_step_ratio,
float rate_target,
float meas_rate_min,
float meas_rate_max)
1292 if (meas_rate_max > rate_target) {
1295 tune_p -= tune_p*tune_p_step_ratio;
1296 if (tune_p < tune_p_min) {
1298 tune_p = tune_p_min;
1299 tune_d -= tune_d*tune_d_step_ratio;
1300 if (tune_d <= tune_d_min) {
1302 tune_d = tune_d_min;
1310 tune_p += tune_p*tune_p_step_ratio;
1311 if (tune_p >= tune_p_max) {
1312 tune_p = tune_p_max;
1329 tune_d += tune_d*tune_d_step_ratio*2.0f;
1331 if (tune_d >= tune_d_max) {
1332 tune_d = tune_d_max;
1345 void Copter::ModeAutoTune::updating_rate_d_down(
float &tune_d,
float tune_d_min,
float tune_d_step_ratio,
float &tune_p,
float tune_p_min,
float tune_p_max,
float tune_p_step_ratio,
float rate_target,
float meas_rate_min,
float meas_rate_max)
1347 if (meas_rate_max > rate_target) {
1350 tune_p -= tune_p*tune_p_step_ratio;
1351 if (tune_p < tune_p_min) {
1353 tune_p = tune_p_min;
1354 tune_d -= tune_d*tune_d_step_ratio;
1355 if (tune_d <= tune_d_min) {
1357 tune_d = tune_d_min;
1365 tune_p += tune_p*tune_p_step_ratio;
1366 if (tune_p >= tune_p_max) {
1367 tune_p = tune_p_max;
1387 tune_d -= tune_d*tune_d_step_ratio;
1389 if (tune_d <= tune_d_min) {
1390 tune_d = tune_d_min;
1413 tune_d -= tune_d*tune_d_step_ratio;
1415 if (tune_d <= tune_d_min) {
1416 tune_d = tune_d_min;
1420 tune_p -= tune_p*tune_p_step_ratio;
1422 if (tune_p <= tune_p_min) {
1423 tune_p = tune_p_min;
1435 tune_p += tune_p*tune_p_step_ratio;
1437 if (tune_p >= tune_p_max) {
1438 tune_p = tune_p_max;
1467 tune_p -= tune_p*tune_p_step_ratio;
1469 if (tune_p <= tune_p_min) {
1470 tune_p = tune_p_min;
1494 tune_p += tune_p*tune_p_step_ratio;
1496 if (tune_p >= tune_p_max) {
1497 tune_p = tune_p_max;
1510 roll_cd_out = pitch_cd_out = 0;
1518 if (!
copter.position_ok()) {
1528 const float angle_max_cd = 1000;
1531 const float dist_limit_cm = 2000;
1535 const float yaw_dist_limit_cm = 500;
1539 float dist_cm = pdiff.
length();
1550 angle_ne *= scaling / dist_cm;
1556 if (dist_cm < yaw_dist_limit_cm) {
1567 float target_yaw_cd =
degrees(atan2f(pdiff.
y, pdiff.
x)) * 100;
1571 target_yaw_cd += 9000;
1574 if (fabsf(yaw_cd_out - target_yaw_cd) > 9500) {
1575 target_yaw_cd += 18000;
1578 yaw_cd_out = target_yaw_cd;
1581 #endif // AUTOTUNE_ENABLED == ENABLED
#define AUTOTUNE_RLPF_MIN
void load_intra_test_gains()
void zero_throttle_and_relax_ac()
virtual float get_velocity_z() const=0
void get_poshold_attitude(float &roll_cd, float &pitch_cd, float &yaw_cd)
bool start(bool ignore_checks)
void updating_angle_p_up(float &tune_p, float tune_p_max, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max)
AP_Int8 autotune_axis_bitmask
#define AUTOTUNE_TARGET_ANGLE_RLLPIT_CD
virtual const Vector3f & get_position() const=0
#define AUTOTUNE_TARGET_ANGLE_YAW_CD
#define AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD
virtual const Vector3f & get_gyro(void) const=0
#define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS
void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt)
#define AUTOTUNE_LEVEL_RATE_Y_CD
void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max)
float get_avoidance_adjusted_climbrate(float target_rate)
bool check_level(const enum LEVEL_ISSUE issue, const float current, const float maximum)
void update_z_controller()
void twitching_measure_acceleration(float &rate_of_change, float rate_measurement, float &rate_measurement_max)
void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
void Log_Write_Event(uint8_t id)
#define AUTOTUNE_SUCCESS_COUNT
#define AUTOTUNE_TESTING_STEP_TIMEOUT_MS
struct Copter::ModeAutoTune::@8 level_problem
uint16_t get_pilot_speed_dn(void)
void update_simple_mode(void)
#define AUTOTUNE_RP_ACCEL_MIN
#define AUTOTUNE_MESSAGE_SAVED_GAINS
#define AUTOTUNE_MESSAGE_STARTED
#define AUTOTUNE_LEVEL_ANGLE_CD
AC_AttitudeControl_t *& attitude_control
#define AUTOTUNE_TARGET_RATE_RLLPIT_CDS
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f))
float get_pilot_desired_climb_rate(float throttle_control)
#define AUTOTUNE_PI_RATIO_FOR_TESTING
float get_pilot_desired_yaw_rate(int16_t stick_angle)
#define DATA_AUTOTUNE_OFF
AP_Float autotune_aggressiveness
#define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS
void backup_gains_and_initialise()
void set_cutoff_frequency(float cutoff_freq)
#define AUTOTUNE_ACCEL_RP_BACKOFF
#define DATA_AUTOTUNE_PILOT_TESTING
#define DATA_AUTOTUNE_SAVEDGAINS
void twitching_test_rate(float rate, float rate_target, float &meas_rate_min, float &meas_rate_max)
#define AUTOTUNE_ACCEL_Y_BACKOFF
#define AUTOTUNE_ANNOUNCE_INTERVAL_MS
void updating_angle_p_down(float &tune_p, float tune_p_min, float tune_p_step_ratio, float angle_target, float meas_angle_max, float meas_rate_min, float meas_rate_max)
int16_t get_control_in() const
void relax_alt_hold_controllers(float throttle_setting)
void do_gcs_announcements()
bool is_zero(const T fVal1)
void updating_rate_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max)
#define DATA_AUTOTUNE_SUCCESS
#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD
void set_desired_velocity_z(float vel_z_cms)
void set_speed_z(float speed_down, float speed_up)
#define AUTOTUNE_AXIS_BITMASK_YAW
#define AUTOTUNE_SP_BACKOFF
#define AUTOTUNE_RP_BACKOFF
void set_throttle_takeoff(void)
const char * level_issue_string() const
void set_accel_z(float accel_cmss)
#define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS
uint32_t autotune_complete
void send_text(MAV_SEVERITY severity, const char *fmt,...)
#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS
void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
bool init(bool ignore_checks) override
DESIRED_THROTTLE_UNLIMITED
void autotune_attitude_control()
virtual void set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
AC_PosControl *& pos_control
#define AUTOTUNE_D_UP_DOWN_MARGIN
void update_gcs(uint8_t message_id)
#define AUTOTUNE_MESSAGE_FAILED
#define AUTOTUNE_Y_FILT_FREQ
void updating_rate_d_up(float &tune_d, float tune_d_min, float tune_d_max, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max)
float constrain_float(const float amt, const float low, const float high)
const char * type_string() const
#define AUTOTUNE_PI_RATIO_FINAL
void updating_rate_p_up_d_down(float &tune_d, float tune_d_min, float tune_d_step_ratio, float &tune_p, float tune_p_min, float tune_p_max, float tune_p_step_ratio, float rate_target, float meas_rate_min, float meas_rate_max)
AP_HAL_MAIN_CALLBACKS & copter
#define AUTOTUNE_Y_ACCEL_MIN
#define AUTOTUNE_AXIS_BITMASK_PITCH
uint32_t autotune_next_axis
#define DATA_AUTOTUNE_REACHED_LIMIT
#define DATA_AUTOTUNE_RESTART
#define AUTOTUNE_TARGET_RATE_YAW_CDS
#define AUTOTUNE_RD_BACKOFF
LowPassFilterFloat rotation_rate_filt
AP_InertialNav & inertial_nav
#define AUTOTUNE_MESSAGE_SUCCESS
#define AUTOTUNE_AXIS_BITMASK_ROLL
#define DATA_AUTOTUNE_INITIALISED
#define AUTOTUNE_LEVEL_RATE_RP_CD
RC_Channel *& channel_yaw
static struct notify_events_type events
#define AUTOTUNE_YAW_PI_RATIO_FINAL
#define AUTOTUNE_RLPF_MAX
void set_land_complete(bool b)
RC_Channel *& channel_throttle
#define AUTOTUNE_MESSAGE_STOPPED
float apply(float sample, float dt)
void set_alt_target_to_current_alt()