Line data Source code
1 : #include "AC_AutoTune_config.h"
2 :
3 : #if AC_AUTOTUNE_ENABLED
4 :
5 : #include "AC_AutoTune_Multi.h"
6 :
7 : #include <AP_Logger/AP_Logger.h>
8 : #include <AP_Scheduler/AP_Scheduler.h>
9 : #include <GCS_MAVLink/GCS.h>
10 : #include <AP_Vehicle/AP_Vehicle_Type.h>
11 :
12 : /*
13 : * autotune support for multicopters
14 : *
15 : *
16 : * Instructions:
17 : * 1) Set up one flight mode switch position to be AltHold.
18 : * 2) Set the Ch7 Opt or Ch8 Opt to AutoTune to allow you to turn the auto tuning on/off with the ch7 or ch8 switch.
19 : * 3) Ensure the ch7 or ch8 switch is in the LOW position.
20 : * 4) Wait for a calm day and go to a large open area.
21 : * 5) Take off and put the vehicle into AltHold mode at a comfortable altitude.
22 : * 6) Set the ch7/ch8 switch to the HIGH position to engage auto tuning:
23 : * a) You will see it twitch about 20 degrees left and right for a few minutes, then it will repeat forward and back.
24 : * b) Use the roll and pitch stick at any time to reposition the copter if it drifts away (it will use the original PID gains during repositioning and between tests).
25 : * When you release the sticks it will continue auto tuning where it left off.
26 : * c) Move the ch7/ch8 switch into the LOW position at any time to abandon the autotuning and return to the origin PIDs.
27 : * d) Make sure that you do not have any trim set on your transmitter or the autotune may not get the signal that the sticks are centered.
28 : * 7) When the tune completes the vehicle will change back to the original PID gains.
29 : * 8) Put the ch7/ch8 switch into the LOW position then back to the HIGH position to test the tuned PID gains.
30 : * 9) Put the ch7/ch8 switch into the LOW position to fly using the original PID gains.
31 : * 10) If you are happy with the autotuned PID gains, leave the ch7/ch8 switch in the HIGH position, land and disarm to save the PIDs permanently.
32 : * If you DO NOT like the new PIDS, switch ch7/ch8 LOW to return to the original PIDs. The gains will not be saved when you disarm
33 : *
34 : * What it's doing during each "twitch":
35 : * a) invokes 90 deg/sec rate request
36 : * b) records maximum "forward" roll rate and bounce back rate
37 : * c) when copter reaches 20 degrees or 1 second has passed, it commands level
38 : * d) tries to keep max rotation rate between 80% ~ 100% of requested rate (90deg/sec) by adjusting rate P
39 : * e) increases rate D until the bounce back becomes greater than 10% of requested rate (90deg/sec)
40 : * f) decreases rate D until the bounce back becomes less than 10% of requested rate (90deg/sec)
41 : * g) increases rate P until the max rotate rate becomes greater than the request rate (90deg/sec)
42 : * h) invokes a 20deg angle request on roll or pitch
43 : * i) increases stab P until the maximum angle becomes greater than 110% of the requested angle (20deg)
44 : * j) decreases stab P by 25%
45 : *
46 : */
47 :
48 : #define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 2000U // timeout for tuning mode's testing step
49 :
50 : #define AUTOTUNE_RD_STEP 0.05 // minimum increment when increasing/decreasing Rate D term
51 : #define AUTOTUNE_RP_STEP 0.05 // minimum increment when increasing/decreasing Rate P term
52 : #define AUTOTUNE_SP_STEP 0.05 // minimum increment when increasing/decreasing Stab P term
53 : #define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1 // I is set 10x smaller than P during testing
54 : #define AUTOTUNE_PI_RATIO_FINAL 1.0 // I is set 1x P after testing
55 : #define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1 // I is set 1x P after testing
56 : #define AUTOTUNE_RD_MAX 0.200 // maximum Rate D value
57 : #define AUTOTUNE_RLPF_MIN 1.0 // minimum Rate Yaw filter value
58 : #define AUTOTUNE_RLPF_MAX 5.0 // maximum Rate Yaw filter value
59 : #define AUTOTUNE_FLTE_MIN 2.5 // minimum Rate Yaw error filter value
60 : #define AUTOTUNE_RP_MIN 0.01 // minimum Rate P value
61 : #define AUTOTUNE_RP_MAX 2.0 // maximum Rate P value
62 :
63 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
64 : #define AUTOTUNE_SP_MAX 10.0 // maximum Stab P value
65 : #else
66 : #define AUTOTUNE_SP_MAX 40.0 // maximum Stab P value
67 : #endif
68 :
69 : #define AUTOTUNE_SP_MIN 0.5 // maximum Stab P value
70 : #define AUTOTUNE_RP_ACCEL_MIN 4000.0 // Minimum acceleration for Roll and Pitch
71 : #define AUTOTUNE_Y_ACCEL_MIN 1000.0 // Minimum acceleration for Yaw
72 : #define AUTOTUNE_Y_FILT_FREQ 10.0 // Autotune filter frequency when testing Yaw
73 : #define AUTOTUNE_D_UP_DOWN_MARGIN 0.2 // The margin below the target that we tune D in
74 : #define AUTOTUNE_ACCEL_RP_BACKOFF 1.0 // back off from maximum acceleration
75 : #define AUTOTUNE_ACCEL_Y_BACKOFF 1.0 // back off from maximum acceleration
76 :
77 : // roll and pitch axes
78 : #define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 18000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step
79 : #define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target min roll/pitch rate during AUTOTUNE_STEP_TWITCHING step
80 :
81 : // yaw axis
82 : #define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step
83 : #define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step
84 :
85 : #define AUTOTUNE_TARGET_ANGLE_MAX_RP_SCALE 1.0 / 2.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
86 : #define AUTOTUNE_TARGET_ANGLE_MAX_Y_SCALE 1.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
87 : #define AUTOTUNE_TARGET_ANGLE_MIN_RP_SCALE 1.0 / 3.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
88 : #define AUTOTUNE_TARGET_ANGLE_MIN_Y_SCALE 1.0 / 6.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
89 : #define AUTOTUNE_ANGLE_ABORT_RP_SCALE 2.5 / 3.0 // maximum allowable angle during testing, as a fraction of angle_max
90 : #define AUTOTUNE_ANGLE_MAX_Y_SCALE 1.0 // maximum allowable angle during testing, as a fraction of angle_max
91 : #define AUTOTUNE_ANGLE_NEG_RP_SCALE 1.0 / 5.0 // maximum allowable angle during testing, as a fraction of angle_max
92 :
93 : // second table of user settable parameters for quadplanes, this
94 : // allows us to go beyond the 64 parameter limit
95 : const AP_Param::GroupInfo AC_AutoTune_Multi::var_info[] = {
96 :
97 : // @Param: AXES
98 : // @DisplayName: Autotune axis bitmask
99 : // @Description: 1-byte bitmap of axes to autotune
100 : // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:YawD
101 : // @User: Standard
102 : AP_GROUPINFO("AXES", 1, AC_AutoTune_Multi, axis_bitmask, 7), // AUTOTUNE_AXIS_BITMASK_DEFAULT
103 :
104 : // @Param: AGGR
105 : // @DisplayName: Autotune aggressiveness
106 : // @Description: Autotune aggressiveness. Defines the bounce back used to detect size of the D term.
107 : // @Range: 0.05 0.10
108 : // @User: Standard
109 : AP_GROUPINFO("AGGR", 2, AC_AutoTune_Multi, aggressiveness, 0.075f),
110 :
111 : // @Param: MIN_D
112 : // @DisplayName: AutoTune minimum D
113 : // @Description: Defines the minimum D gain
114 : // @Range: 0.0001 0.005
115 : // @User: Standard
116 : AP_GROUPINFO("MIN_D", 3, AC_AutoTune_Multi, min_d, 0.0005f),
117 :
118 : // @Param: GMBK
119 : // @DisplayName: AutoTune Gain Margin Backoff
120 : // @Description: Fraction by which tuned P and D gains are reduced after rate and angle AutoTune steps complete. This provides extra stability margin by reducing gains slightly from the optimal values found during tuning. A value of 0.0 applies no reduction. A value of 0.25 reduces tuned gains by 25%.
121 : // @Range: 0.0 0.5
122 : // @User: Standard
123 : AP_GROUPINFO("GMBK", 4, AC_AutoTune_Multi, gain_backoff, 0.25),
124 :
125 : AP_GROUPEND
126 : };
127 :
128 : // constructor
129 1610 : AC_AutoTune_Multi::AC_AutoTune_Multi()
130 : {
131 1610 : tune_seq[0] = TuneType::TUNE_COMPLETE;
132 1610 : AP_Param::setup_object_defaults(this, var_info);
133 1610 : }
134 :
135 1063537 : void AC_AutoTune_Multi::do_gcs_announcements()
136 : {
137 1063537 : const uint32_t now_ms = AP_HAL::millis();
138 1063537 : if (now_ms - last_announce_ms < AUTOTUNE_ANNOUNCE_INTERVAL_MS) {
139 1062153 : return;
140 : }
141 1384 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: %s %s %u%%", get_axis_name(), get_tune_type_name(), (success_counter * (100/AUTOTUNE_SUCCESS_COUNT)));
142 1384 : last_announce_ms = now_ms;
143 : }
144 :
145 : // Prepares all tuning state variables and target values for a new twitch test.
146 2182 : void AC_AutoTune_Multi::test_init()
147 : {
148 : float target_max_rate;
149 2182 : switch (axis) {
150 393 : case AxisType::ROLL:
151 393 : angle_abort = target_angle_max_rp_cd();
152 393 : target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler * AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
153 393 : target_rate = constrain_float(degrees(attitude_control->max_rate_step_bf_roll()) * 100.0, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
154 393 : target_angle = constrain_float(degrees(attitude_control->max_angle_step_bf_roll()) * 100.0, target_angle_min_rp_cd(), target_angle_max_rp_cd());
155 393 : rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz() * 2.0);
156 393 : break;
157 :
158 386 : case AxisType::PITCH:
159 386 : angle_abort = target_angle_max_rp_cd();
160 386 : target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler * AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
161 386 : target_rate = constrain_float(degrees(attitude_control->max_rate_step_bf_pitch()) * 100.0, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
162 386 : target_angle = constrain_float(degrees(attitude_control->max_angle_step_bf_pitch()) * 100.0, target_angle_min_rp_cd(), target_angle_max_rp_cd());
163 386 : rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz() * 2.0);
164 386 : break;
165 :
166 1403 : case AxisType::YAW:
167 : case AxisType::YAW_D:
168 1403 : angle_abort = target_angle_max_y_cd();
169 1403 : target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS);
170 1403 : target_rate = constrain_float(degrees(attitude_control->max_rate_step_bf_yaw() * 0.75) * 100.0, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate);
171 1403 : target_angle = constrain_float(degrees(attitude_control->max_angle_step_bf_yaw() * 0.75) * 100.0, target_angle_min_y_cd(), target_angle_max_y_cd());
172 1403 : if (axis == AxisType::YAW_D) {
173 760 : rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_yaw_pid().filt_D_hz() * 2.0);
174 : } else {
175 643 : rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ);
176 : }
177 1403 : break;
178 : }
179 :
180 2182 : if ((tune_type == TuneType::ANGLE_P_DOWN) || (tune_type == TuneType::ANGLE_P_UP)) {
181 : // TODO: Evaluate whether initializing rotation_rate_filt with start_rate improves results for other axes
182 471 : rotation_rate_filt.reset(start_rate);
183 : } else {
184 1711 : rotation_rate_filt.reset(0.0);
185 : }
186 2182 : angle_step_commanded = false;
187 2182 : test_rate_max = 0.0;
188 2182 : test_rate_min = 0.0;
189 2182 : test_angle_max = 0.0;
190 2182 : test_angle_min = 0.0;
191 2182 : accel_measure_rate_max = 0.0;
192 2182 : }
193 :
194 304666 : void AC_AutoTune_Multi::test_run(AxisType test_axis, const float dir_sign)
195 : {
196 : // hold current attitude
197 :
198 304666 : if ((tune_type == TuneType::ANGLE_P_DOWN) || (tune_type == TuneType::ANGLE_P_UP)) {
199 : // step angle targets on first iteration
200 93747 : if (!angle_step_commanded) {
201 471 : angle_step_commanded = true;
202 : // Testing increasing stabilize P gain so will set lean angle target
203 471 : switch (test_axis) {
204 172 : case AxisType::ROLL:
205 : // request roll to 20deg
206 172 : attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(dir_sign * cd_to_rad(target_angle), 0.0, 0.0);
207 172 : break;
208 172 : case AxisType::PITCH:
209 : // request pitch to 20deg
210 172 : attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(0.0, dir_sign * cd_to_rad(target_angle), 0.0);
211 172 : break;
212 127 : case AxisType::YAW:
213 : case AxisType::YAW_D:
214 : // request yaw to 20deg
215 127 : attitude_control->input_angle_step_bf_roll_pitch_yaw_rad(0.0, 0.0, dir_sign * cd_to_rad(target_angle));
216 127 : break;
217 : }
218 : } else {
219 93276 : attitude_control->input_rate_bf_roll_pitch_yaw_rads(0.0, 0.0, 0.0);
220 : }
221 : } else {
222 : // Testing rate P and D gains so will set body-frame rate targets.
223 : // Rate controller will use existing body-frame rates and convert to motor outputs
224 : // for all axes except the one we override here.
225 210919 : switch (test_axis) {
226 6220 : case AxisType::ROLL:
227 : // override body-frame roll rate
228 6220 : attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(dir_sign * cd_to_rad(target_rate + start_rate), 0.0f, 0.0f);
229 6220 : break;
230 5964 : case AxisType::PITCH:
231 : // override body-frame pitch rate
232 5964 : attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(0.0f, dir_sign * cd_to_rad(target_rate + start_rate), 0.0f);
233 5964 : break;
234 198735 : case AxisType::YAW:
235 : case AxisType::YAW_D:
236 : // override body-frame yaw rate
237 198735 : attitude_control->input_rate_step_bf_roll_pitch_yaw_rads(0.0f, 0.0f, dir_sign * cd_to_rad(target_rate + start_rate));
238 198735 : break;
239 : }
240 : }
241 :
242 : // capture this iteration's rotation rate and lean angle
243 304666 : float gyro_reading = 0;
244 304666 : switch (test_axis) {
245 32495 : case AxisType::ROLL:
246 32495 : gyro_reading = ahrs_view->get_gyro().x;
247 32495 : lean_angle = dir_sign * (ahrs_view->roll_sensor - (int32_t)start_angle);
248 32495 : break;
249 32162 : case AxisType::PITCH:
250 32162 : gyro_reading = ahrs_view->get_gyro().y;
251 32162 : lean_angle = dir_sign * (ahrs_view->pitch_sensor - (int32_t)start_angle);
252 32162 : break;
253 240009 : case AxisType::YAW:
254 : case AxisType::YAW_D:
255 240009 : gyro_reading = ahrs_view->get_gyro().z;
256 240009 : lean_angle = dir_sign * wrap_180_cd(ahrs_view->yaw_sensor-(int32_t)start_angle);
257 240009 : break;
258 : }
259 :
260 : // Add filter to measurements
261 : float filter_value;
262 304666 : switch (tune_type) {
263 93747 : case TuneType::ANGLE_P_DOWN:
264 : case TuneType::ANGLE_P_UP:
265 93747 : filter_value = dir_sign * (degrees(gyro_reading) * 100.0);
266 93747 : break;
267 210919 : default:
268 210919 : filter_value = dir_sign * (degrees(gyro_reading) * 100.0 - start_rate);
269 210919 : break;
270 : }
271 913998 : rotation_rate = rotation_rate_filt.apply(filter_value,
272 304666 : AP::scheduler().get_loop_period_s());
273 :
274 304666 : switch (tune_type) {
275 112073 : case TuneType::RATE_D_UP:
276 : case TuneType::RATE_D_DOWN:
277 112073 : twitching_test_rate(lean_angle, rotation_rate, target_rate, test_rate_min, test_rate_max, test_angle_min);
278 112073 : twitching_measure_acceleration(test_accel_max_cdss, rotation_rate, accel_measure_rate_max);
279 112073 : twitching_abort_rate(lean_angle, rotation_rate, angle_abort, test_rate_min, test_angle_min);
280 112073 : break;
281 98846 : case TuneType::RATE_P_UP:
282 98846 : twitching_test_rate(lean_angle, rotation_rate, target_rate * (1 + 0.5 * aggressiveness), test_rate_min, test_rate_max, test_angle_min);
283 98846 : twitching_measure_acceleration(test_accel_max_cdss, rotation_rate, accel_measure_rate_max);
284 98846 : twitching_abort_rate(lean_angle, rotation_rate, angle_abort, test_rate_min, test_angle_min);
285 98846 : break;
286 93747 : case TuneType::ANGLE_P_DOWN:
287 : case TuneType::ANGLE_P_UP:
288 93747 : twitching_test_angle(lean_angle, rotation_rate, target_angle * (1 + 0.5 * aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max);
289 93747 : twitching_measure_acceleration(test_accel_max_cdss, rotation_rate - dir_sign * start_rate, accel_measure_rate_max);
290 93747 : break;
291 0 : case TuneType::RATE_FF_UP:
292 : case TuneType::MAX_GAINS:
293 : case TuneType::TUNE_CHECK:
294 : // this should never happen
295 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
296 0 : break;
297 0 : default:
298 0 : break;
299 : }
300 304666 : }
301 :
302 : // backup_gains_and_initialise - store current gains as originals
303 : // called before tuning starts to backup original gains
304 5 : void AC_AutoTune_Multi::backup_gains_and_initialise()
305 : {
306 5 : AC_AutoTune::backup_gains_and_initialise();
307 :
308 5 : aggressiveness.set(constrain_float(aggressiveness, 0.05, 0.2));
309 :
310 5 : orig_bf_feedforward = attitude_control->get_bf_feedforward();
311 :
312 : // backup original pids and initialise tuned pid values
313 5 : orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
314 5 : orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
315 5 : orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
316 5 : orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
317 5 : orig_roll_dff = attitude_control->get_rate_roll_pid().kDff();
318 5 : orig_roll_fltt = attitude_control->get_rate_roll_pid().filt_T_hz();
319 5 : orig_roll_smax = attitude_control->get_rate_roll_pid().slew_limit();
320 5 : orig_roll_sp = attitude_control->get_angle_roll_p().kP();
321 5 : orig_roll_accel_radss = attitude_control->get_accel_roll_max_radss();
322 5 : tune_roll_rp = attitude_control->get_rate_roll_pid().kP();
323 5 : tune_roll_rd = attitude_control->get_rate_roll_pid().kD();
324 5 : tune_roll_sp = attitude_control->get_angle_roll_p().kP();
325 5 : tune_roll_accel_radss = attitude_control->get_accel_roll_max_radss();
326 :
327 5 : orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
328 5 : orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
329 5 : orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
330 5 : orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
331 5 : orig_pitch_dff = attitude_control->get_rate_pitch_pid().kDff();
332 5 : orig_pitch_fltt = attitude_control->get_rate_pitch_pid().filt_T_hz();
333 5 : orig_pitch_smax = attitude_control->get_rate_pitch_pid().slew_limit();
334 5 : orig_pitch_sp = attitude_control->get_angle_pitch_p().kP();
335 5 : orig_pitch_accel_radss = attitude_control->get_accel_pitch_max_radss();
336 5 : tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
337 5 : tune_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
338 5 : tune_pitch_sp = attitude_control->get_angle_pitch_p().kP();
339 5 : tune_pitch_accel_radss = attitude_control->get_accel_pitch_max_radss();
340 :
341 5 : orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
342 5 : orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
343 5 : orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
344 5 : orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
345 5 : orig_yaw_dff = attitude_control->get_rate_yaw_pid().kDff();
346 5 : orig_yaw_fltt = attitude_control->get_rate_yaw_pid().filt_T_hz();
347 5 : orig_yaw_smax = attitude_control->get_rate_yaw_pid().slew_limit();
348 5 : orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
349 5 : orig_yaw_accel_radss = attitude_control->get_accel_yaw_max_radss();
350 5 : orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
351 5 : tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
352 5 : tune_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
353 5 : tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
354 5 : if (yaw_d_enabled() && is_zero(tune_yaw_rd)) {
355 1 : tune_yaw_rd = min_d;
356 : }
357 5 : if (yaw_enabled() && is_zero(tune_yaw_rLPF)) {
358 0 : tune_yaw_rLPF = AUTOTUNE_FLTE_MIN;
359 : }
360 5 : tune_yaw_sp = attitude_control->get_angle_yaw_p().kP();
361 5 : tune_yaw_accel_radss = attitude_control->get_accel_yaw_max_radss();
362 :
363 5 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_INITIALISED);
364 5 : }
365 :
366 : // load_orig_gains - set gains to their original values
367 : // called by stop and failed functions
368 11 : void AC_AutoTune_Multi::load_orig_gains()
369 : {
370 11 : attitude_control->use_sqrt_controller(true);
371 11 : attitude_control->bf_feedforward(orig_bf_feedforward);
372 11 : if (roll_enabled()) {
373 9 : if (!is_zero(orig_roll_rp)) {
374 9 : attitude_control->get_rate_roll_pid().set_kP(orig_roll_rp);
375 9 : attitude_control->get_rate_roll_pid().set_kI(orig_roll_ri);
376 9 : attitude_control->get_rate_roll_pid().set_kD(orig_roll_rd);
377 9 : attitude_control->get_rate_roll_pid().set_ff(orig_roll_rff);
378 9 : attitude_control->get_rate_roll_pid().set_kDff(orig_roll_dff);
379 9 : attitude_control->get_rate_roll_pid().set_filt_T_hz(orig_roll_fltt);
380 9 : attitude_control->get_rate_roll_pid().set_slew_limit(orig_roll_smax);
381 9 : attitude_control->get_angle_roll_p().set_kP(orig_roll_sp);
382 9 : attitude_control->set_accel_roll_max_radss(orig_roll_accel_radss);
383 : }
384 : }
385 11 : if (pitch_enabled()) {
386 9 : if (!is_zero(orig_pitch_rp)) {
387 9 : attitude_control->get_rate_pitch_pid().set_kP(orig_pitch_rp);
388 9 : attitude_control->get_rate_pitch_pid().set_kI(orig_pitch_ri);
389 9 : attitude_control->get_rate_pitch_pid().set_kD(orig_pitch_rd);
390 9 : attitude_control->get_rate_pitch_pid().set_ff(orig_pitch_rff);
391 9 : attitude_control->get_rate_pitch_pid().set_kDff(orig_pitch_dff);
392 9 : attitude_control->get_rate_pitch_pid().set_filt_T_hz(orig_pitch_fltt);
393 9 : attitude_control->get_rate_pitch_pid().set_slew_limit(orig_pitch_smax);
394 9 : attitude_control->get_angle_pitch_p().set_kP(orig_pitch_sp);
395 9 : attitude_control->set_accel_pitch_max_radss(orig_pitch_accel_radss);
396 : }
397 : }
398 11 : if (yaw_enabled() || yaw_d_enabled()) {
399 11 : if (!is_zero(orig_yaw_rp)) {
400 11 : attitude_control->get_rate_yaw_pid().set_kP(orig_yaw_rp);
401 11 : attitude_control->get_rate_yaw_pid().set_kI(orig_yaw_ri);
402 11 : attitude_control->get_rate_yaw_pid().set_kD(orig_yaw_rd);
403 11 : attitude_control->get_rate_yaw_pid().set_ff(orig_yaw_rff);
404 11 : attitude_control->get_rate_yaw_pid().set_kDff(orig_yaw_dff);
405 11 : attitude_control->get_rate_yaw_pid().set_filt_E_hz(orig_yaw_rLPF);
406 11 : attitude_control->get_rate_yaw_pid().set_filt_T_hz(orig_yaw_fltt);
407 11 : attitude_control->get_rate_yaw_pid().set_slew_limit(orig_yaw_smax);
408 11 : attitude_control->get_angle_yaw_p().set_kP(orig_yaw_sp);
409 11 : attitude_control->set_accel_yaw_max_radss(orig_yaw_accel_radss);
410 : }
411 : }
412 11 : }
413 :
414 : // load_tuned_gains - load tuned gains
415 3 : void AC_AutoTune_Multi::load_tuned_gains()
416 : {
417 3 : attitude_control->use_sqrt_controller(true);
418 3 : if (!attitude_control->get_bf_feedforward()) {
419 0 : attitude_control->bf_feedforward(true);
420 0 : attitude_control->set_accel_roll_max_radss(0.0);
421 0 : attitude_control->set_accel_pitch_max_radss(0.0);
422 : }
423 3 : if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled() && !is_zero(tune_roll_rp)) {
424 3 : attitude_control->get_rate_roll_pid().set_kP(tune_roll_rp);
425 3 : attitude_control->get_rate_roll_pid().set_kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
426 3 : attitude_control->get_rate_roll_pid().set_kD(tune_roll_rd);
427 3 : attitude_control->get_rate_roll_pid().set_ff(orig_roll_rff);
428 3 : attitude_control->get_rate_roll_pid().set_kDff(orig_roll_dff);
429 3 : attitude_control->get_angle_roll_p().set_kP(tune_roll_sp);
430 3 : attitude_control->set_accel_roll_max_radss(tune_roll_accel_radss);
431 : }
432 3 : if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled() && !is_zero(tune_pitch_rp)) {
433 3 : attitude_control->get_rate_pitch_pid().set_kP(tune_pitch_rp);
434 3 : attitude_control->get_rate_pitch_pid().set_kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
435 3 : attitude_control->get_rate_pitch_pid().set_kD(tune_pitch_rd);
436 3 : attitude_control->get_rate_pitch_pid().set_ff(orig_pitch_rff);
437 3 : attitude_control->get_rate_pitch_pid().set_kDff(orig_pitch_dff);
438 3 : attitude_control->get_angle_pitch_p().set_kP(tune_pitch_sp);
439 3 : attitude_control->set_accel_pitch_max_radss(tune_pitch_accel_radss);
440 : }
441 3 : if ((((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled())
442 6 : || ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW_D) && yaw_d_enabled())) && !is_zero(tune_yaw_rp)) {
443 3 : attitude_control->get_rate_yaw_pid().set_kP(tune_yaw_rp);
444 3 : attitude_control->get_rate_yaw_pid().set_kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
445 3 : if (yaw_d_enabled()) {
446 0 : attitude_control->get_rate_yaw_pid().set_kD(tune_yaw_rd);
447 : }
448 3 : if (yaw_enabled()) {
449 3 : attitude_control->get_rate_yaw_pid().set_filt_E_hz(tune_yaw_rLPF);
450 : }
451 3 : attitude_control->get_rate_yaw_pid().set_ff(orig_yaw_rff);
452 3 : attitude_control->get_rate_yaw_pid().set_kDff(orig_yaw_dff);
453 3 : attitude_control->get_angle_yaw_p().set_kP(tune_yaw_sp);
454 3 : attitude_control->set_accel_yaw_max_radss(tune_yaw_accel_radss);
455 : }
456 3 : }
457 :
458 : // load_intra_test_gains - gains used between tests
459 : // called during testing mode's update-gains step to set gains ahead of return-to-level step
460 2187 : void AC_AutoTune_Multi::load_intra_test_gains()
461 : {
462 : // we are restarting tuning so reset gains to tuning-start gains (i.e. low I term)
463 : // sanity check the gains
464 2187 : attitude_control->use_sqrt_controller(true);
465 2187 : attitude_control->bf_feedforward(true);
466 2187 : if (roll_enabled()) {
467 1295 : attitude_control->get_rate_roll_pid().set_kP(orig_roll_rp);
468 1295 : attitude_control->get_rate_roll_pid().set_kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
469 1295 : attitude_control->get_rate_roll_pid().set_kD(orig_roll_rd);
470 1295 : attitude_control->get_rate_roll_pid().set_ff(orig_roll_rff);
471 1295 : attitude_control->get_rate_roll_pid().set_kDff(orig_roll_dff);
472 1295 : attitude_control->get_rate_roll_pid().set_filt_T_hz(orig_roll_fltt);
473 1295 : attitude_control->get_rate_roll_pid().set_slew_limit(orig_roll_smax);
474 1295 : attitude_control->get_angle_roll_p().set_kP(orig_roll_sp);
475 : }
476 2187 : if (pitch_enabled()) {
477 1295 : attitude_control->get_rate_pitch_pid().set_kP(orig_pitch_rp);
478 1295 : attitude_control->get_rate_pitch_pid().set_kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
479 1295 : attitude_control->get_rate_pitch_pid().set_kD(orig_pitch_rd);
480 1295 : attitude_control->get_rate_pitch_pid().set_ff(orig_pitch_rff);
481 1295 : attitude_control->get_rate_pitch_pid().set_kDff(orig_pitch_dff);
482 1295 : attitude_control->get_rate_pitch_pid().set_filt_T_hz(orig_pitch_fltt);
483 1295 : attitude_control->get_rate_pitch_pid().set_slew_limit(orig_pitch_smax);
484 1295 : attitude_control->get_angle_pitch_p().set_kP(orig_pitch_sp);
485 : }
486 2187 : if (yaw_enabled() || yaw_d_enabled()) {
487 2187 : attitude_control->get_rate_yaw_pid().set_kP(orig_yaw_rp);
488 2187 : attitude_control->get_rate_yaw_pid().set_kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
489 2187 : attitude_control->get_rate_yaw_pid().set_kD(orig_yaw_rd);
490 2187 : attitude_control->get_rate_yaw_pid().set_ff(orig_yaw_rff);
491 2187 : attitude_control->get_rate_yaw_pid().set_kDff(orig_yaw_dff);
492 2187 : attitude_control->get_rate_yaw_pid().set_filt_T_hz(orig_yaw_fltt);
493 2187 : attitude_control->get_rate_yaw_pid().set_slew_limit(orig_yaw_smax);
494 2187 : attitude_control->get_rate_yaw_pid().set_filt_E_hz(orig_yaw_rLPF);
495 2187 : attitude_control->get_angle_yaw_p().set_kP(orig_yaw_sp);
496 : }
497 2187 : }
498 :
499 : // load_test_gains - load the to-be-tested gains for a single axis
500 : // called by control_attitude() just before it beings testing a gain (i.e. just before it twitches)
501 2182 : void AC_AutoTune_Multi::load_test_gains()
502 : {
503 2182 : attitude_control->use_sqrt_controller(false);
504 2182 : switch (axis) {
505 393 : case AxisType::ROLL:
506 393 : attitude_control->get_rate_roll_pid().set_kP(tune_roll_rp);
507 393 : attitude_control->get_rate_roll_pid().set_kI(tune_roll_rp * 0.01);
508 393 : attitude_control->get_rate_roll_pid().set_kD(tune_roll_rd);
509 393 : attitude_control->get_rate_roll_pid().set_ff(0.0);
510 393 : attitude_control->get_rate_roll_pid().set_kDff(0.0);
511 393 : attitude_control->get_rate_roll_pid().set_filt_T_hz(0.0);
512 393 : attitude_control->get_rate_roll_pid().set_slew_limit(0.0);
513 393 : attitude_control->get_angle_roll_p().set_kP(tune_roll_sp);
514 393 : break;
515 386 : case AxisType::PITCH:
516 386 : attitude_control->get_rate_pitch_pid().set_kP(tune_pitch_rp);
517 386 : attitude_control->get_rate_pitch_pid().set_kI(tune_pitch_rp * 0.01);
518 386 : attitude_control->get_rate_pitch_pid().set_kD(tune_pitch_rd);
519 386 : attitude_control->get_rate_pitch_pid().set_ff(0.0);
520 386 : attitude_control->get_rate_pitch_pid().set_kDff(0.0);
521 386 : attitude_control->get_rate_pitch_pid().set_filt_T_hz(0.0);
522 386 : attitude_control->get_rate_pitch_pid().set_slew_limit(0.0);
523 386 : attitude_control->get_angle_pitch_p().set_kP(tune_pitch_sp);
524 386 : break;
525 1403 : case AxisType::YAW:
526 : case AxisType::YAW_D:
527 1403 : attitude_control->get_rate_yaw_pid().set_kP(tune_yaw_rp);
528 1403 : attitude_control->get_rate_yaw_pid().set_kI(tune_yaw_rp * 0.01);
529 1403 : attitude_control->get_rate_yaw_pid().set_ff(0.0);
530 1403 : attitude_control->get_rate_yaw_pid().set_kDff(0.0);
531 1403 : if (axis == AxisType::YAW_D) {
532 760 : attitude_control->get_rate_yaw_pid().set_kD(tune_yaw_rd);
533 : } else {
534 643 : attitude_control->get_rate_yaw_pid().set_kD(0.0);
535 643 : attitude_control->get_rate_yaw_pid().set_filt_E_hz(tune_yaw_rLPF);
536 : }
537 1403 : attitude_control->get_rate_yaw_pid().set_filt_T_hz(0.0);
538 1403 : attitude_control->get_rate_yaw_pid().set_slew_limit(0.0);
539 1403 : attitude_control->get_angle_yaw_p().set_kP(tune_yaw_sp);
540 1403 : break;
541 : }
542 2182 : }
543 :
544 : // save_tuning_gains - save the final tuned gains for each axis
545 : // save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position)
546 3 : void AC_AutoTune_Multi::save_tuning_gains()
547 : {
548 : // see if we successfully completed tuning of at least one axis
549 3 : if (axes_completed == 0) {
550 0 : return;
551 : }
552 :
553 3 : if (!attitude_control->get_bf_feedforward()) {
554 0 : attitude_control->bf_feedforward_save(true);
555 0 : attitude_control->save_accel_roll_max_radss(0.0);
556 0 : attitude_control->save_accel_pitch_max_radss(0.0);
557 : }
558 :
559 : // sanity check the rate P values
560 3 : if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled() && !is_zero(tune_roll_rp)) {
561 : // rate roll gains
562 3 : attitude_control->get_rate_roll_pid().set_kP(tune_roll_rp);
563 3 : attitude_control->get_rate_roll_pid().set_kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
564 3 : attitude_control->get_rate_roll_pid().set_kD(tune_roll_rd);
565 3 : attitude_control->get_rate_roll_pid().set_ff(orig_roll_rff);
566 3 : attitude_control->get_rate_roll_pid().set_kDff(orig_roll_dff);
567 3 : attitude_control->get_rate_roll_pid().set_filt_T_hz(orig_roll_fltt);
568 3 : attitude_control->get_rate_roll_pid().set_slew_limit(orig_roll_smax);
569 3 : attitude_control->get_rate_roll_pid().save_gains();
570 :
571 : // stabilize roll
572 3 : attitude_control->get_angle_roll_p().set_kP(tune_roll_sp);
573 3 : attitude_control->get_angle_roll_p().save_gains();
574 :
575 : // acceleration roll
576 3 : attitude_control->save_accel_roll_max_radss(tune_roll_accel_radss);
577 :
578 : // resave pids to originals in case the autotune is run again
579 3 : orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
580 3 : orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
581 3 : orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
582 3 : orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
583 3 : orig_roll_dff = attitude_control->get_rate_roll_pid().kDff();
584 3 : orig_roll_sp = attitude_control->get_angle_roll_p().kP();
585 3 : orig_roll_accel_radss = attitude_control->get_accel_roll_max_radss();
586 : }
587 :
588 3 : if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled() && !is_zero(tune_pitch_rp)) {
589 : // rate pitch gains
590 3 : attitude_control->get_rate_pitch_pid().set_kP(tune_pitch_rp);
591 3 : attitude_control->get_rate_pitch_pid().set_kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
592 3 : attitude_control->get_rate_pitch_pid().set_kD(tune_pitch_rd);
593 3 : attitude_control->get_rate_pitch_pid().set_ff(orig_pitch_rff);
594 3 : attitude_control->get_rate_pitch_pid().set_kDff(orig_pitch_dff);
595 3 : attitude_control->get_rate_pitch_pid().set_filt_T_hz(orig_pitch_fltt);
596 3 : attitude_control->get_rate_pitch_pid().set_slew_limit(orig_pitch_smax);
597 3 : attitude_control->get_rate_pitch_pid().save_gains();
598 :
599 : // stabilize pitch
600 3 : attitude_control->get_angle_pitch_p().set_kP(tune_pitch_sp);
601 3 : attitude_control->get_angle_pitch_p().save_gains();
602 :
603 : // acceleration pitch
604 3 : attitude_control->save_accel_pitch_max_radss(tune_pitch_accel_radss);
605 :
606 : // resave pids to originals in case the autotune is run again
607 3 : orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
608 3 : orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
609 3 : orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
610 3 : orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
611 3 : orig_pitch_dff = attitude_control->get_rate_pitch_pid().kDff();
612 3 : orig_pitch_sp = attitude_control->get_angle_pitch_p().kP();
613 3 : orig_pitch_accel_radss = attitude_control->get_accel_pitch_max_radss();
614 : }
615 :
616 3 : if ((((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled())
617 6 : || ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW_D) && yaw_d_enabled())) && !is_zero(tune_yaw_rp)) {
618 : // rate yaw gains
619 3 : attitude_control->get_rate_yaw_pid().set_kP(tune_yaw_rp);
620 3 : attitude_control->get_rate_yaw_pid().set_kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
621 3 : attitude_control->get_rate_yaw_pid().set_ff(orig_yaw_rff);
622 3 : attitude_control->get_rate_yaw_pid().set_kDff(orig_yaw_dff);
623 3 : attitude_control->get_rate_yaw_pid().set_filt_T_hz(orig_yaw_fltt);
624 3 : attitude_control->get_rate_yaw_pid().set_slew_limit(orig_yaw_smax);
625 3 : if (yaw_d_enabled()) {
626 0 : attitude_control->get_rate_yaw_pid().set_kD(tune_yaw_rd);
627 : }
628 3 : if (yaw_enabled()) {
629 3 : attitude_control->get_rate_yaw_pid().set_filt_E_hz(tune_yaw_rLPF);
630 : }
631 3 : attitude_control->get_rate_yaw_pid().save_gains();
632 :
633 : // stabilize yaw
634 3 : attitude_control->get_angle_yaw_p().set_kP(tune_yaw_sp);
635 3 : attitude_control->get_angle_yaw_p().save_gains();
636 :
637 : // acceleration yaw
638 3 : attitude_control->save_accel_yaw_max_radss(tune_yaw_accel_radss);
639 :
640 : // resave pids to originals in case the autotune is run again
641 3 : orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
642 3 : orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
643 3 : orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
644 3 : orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
645 3 : orig_yaw_dff = attitude_control->get_rate_yaw_pid().kDff();
646 3 : orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
647 3 : orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
648 3 : orig_yaw_accel_radss = attitude_control->get_accel_yaw_max_radss();
649 : }
650 :
651 : // update GCS and log save gains event
652 3 : update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS);
653 3 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SAVEDGAINS);
654 :
655 3 : reset();
656 : }
657 :
658 : // report final gains for a given axis to GCS
659 14 : void AC_AutoTune_Multi::report_final_gains(AxisType test_axis) const
660 : {
661 14 : switch (test_axis) {
662 4 : case AxisType::ROLL:
663 4 : report_axis_gains("Roll", tune_roll_rp, tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL, tune_roll_rd, tune_roll_sp, tune_roll_accel_radss);
664 4 : break;
665 4 : case AxisType::PITCH:
666 4 : report_axis_gains("Pitch", tune_pitch_rp, tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL, tune_pitch_rd, tune_pitch_sp, tune_pitch_accel_radss);
667 4 : break;
668 5 : case AxisType::YAW:
669 5 : report_axis_gains("Yaw(E)", tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, 0, tune_yaw_sp, tune_yaw_accel_radss);
670 5 : break;
671 1 : case AxisType::YAW_D:
672 1 : report_axis_gains("Yaw(D)", tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_sp, tune_yaw_accel_radss);
673 1 : break;
674 : }
675 14 : }
676 :
677 : // report gain formatting helper
678 14 : void AC_AutoTune_Multi::report_axis_gains(const char* axis_string, float rate_P, float rate_I, float rate_D, float angle_P, float max_accel_radss) const
679 : {
680 14 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s complete", axis_string);
681 14 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s Rate: P:%0.3f, I:%0.3f, D:%0.4f", axis_string, rate_P, rate_I,rate_D);
682 14 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s Angle P:%0.3f, Max Accel:%0.0f", axis_string, angle_P, rad_to_cd(max_accel_radss));
683 14 : }
684 :
685 : // Measures peak angular rates and bounce-back behavior during rate tuning.
686 : // Updates step timer and determines if tuning step is complete.
687 210919 : void AC_AutoTune_Multi::twitching_test_rate(float angle, float rate, float rate_target_max, float &meas_rate_min, float &meas_rate_max, float &meas_angle_min)
688 : {
689 210919 : const uint32_t now_ms = AP_HAL::millis();
690 :
691 : // capture maximum rate
692 210919 : if (rate > meas_rate_max) {
693 : // the measurement is continuing to increase without stopping
694 126105 : meas_rate_max = rate;
695 126105 : meas_rate_min = rate;
696 126105 : meas_angle_min = angle;
697 : }
698 :
699 : // capture minimum measurement after the measurement has peaked (aka "bounce back")
700 210919 : if ((rate < meas_rate_min) && (meas_rate_max > rate_target_max * 0.25)) {
701 : // the measurement is bouncing back
702 62600 : meas_rate_min = rate;
703 62600 : meas_angle_min = angle;
704 : }
705 :
706 : // calculate early stopping time based on the time it takes to get to 63.21%
707 210919 : if (meas_rate_max < rate_target_max * 0.6321) {
708 : // the measurement not reached the 63.21% threshold yet
709 95083 : step_timeout_ms = (now_ms - step_start_time_ms) * 3;
710 95083 : step_timeout_ms = MIN(step_timeout_ms, AUTOTUNE_TESTING_STEP_TIMEOUT_MS);
711 : }
712 :
713 210919 : if (meas_rate_max > rate_target_max) {
714 : // the measured rate has passed the maximum target rate
715 376 : step = Step::UPDATE_GAINS;
716 : }
717 :
718 210919 : if (meas_rate_max - meas_rate_min > meas_rate_max * aggressiveness) {
719 : // the measurement has passed 50% of the maximum rate and bounce back is larger than the threshold
720 707 : step = Step::UPDATE_GAINS;
721 : }
722 :
723 210919 : if (now_ms - step_start_time_ms >= step_timeout_ms) {
724 : // we have passed the maximum stop time
725 540 : step = Step::UPDATE_GAINS;
726 : }
727 210919 : }
728 :
729 : // twitching_test_rate - twitching tests
730 : // update min and max and test for end conditions
731 210919 : void AC_AutoTune_Multi::twitching_abort_rate(float angle, float rate, float angle_max, float meas_rate_min, float angle_min)
732 : {
733 210919 : if (angle >= angle_max) {
734 120 : if (is_equal(rate, meas_rate_min) || (angle_min > 0.95 * angle_max)) {
735 : // we have reached the angle limit before completing the measurement of maximum and minimum
736 : // reduce the maximum target rate
737 85 : if (step_scaler > 0.2f) {
738 85 : step_scaler *= 0.9f;
739 : } else {
740 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
741 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AutoTune: Twitch Size Determination Failed");
742 0 : mode = TuneMode::FAILED;
743 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
744 : }
745 : // ignore result and start test again
746 85 : step = Step::ABORT;
747 : } else {
748 35 : step = Step::UPDATE_GAINS;
749 : }
750 : }
751 210919 : }
752 :
753 : // twitching_test_angle - twitching tests
754 : // update min and max and test for end conditions
755 93747 : void AC_AutoTune_Multi::twitching_test_angle(float angle, float rate, float angle_target_max, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max)
756 : {
757 93747 : const uint32_t now_ms = AP_HAL::millis();
758 :
759 : // capture maximum angle
760 93747 : if (angle > meas_angle_max) {
761 : // the angle still increasing
762 77286 : meas_angle_max = angle;
763 77286 : meas_angle_min = angle;
764 : }
765 :
766 : // capture minimum angle after we have reached a reasonable maximum angle
767 93747 : if ((angle < meas_angle_min) && (meas_angle_max > angle_target_max * 0.25)) {
768 : // the measurement is bouncing back
769 4379 : meas_angle_min = angle;
770 : }
771 :
772 : // capture maximum rate
773 93747 : if (rate > meas_rate_max) {
774 : // the measurement is still increasing
775 18019 : meas_rate_max = rate;
776 18019 : meas_rate_min = rate;
777 : }
778 :
779 : // capture minimum rate after we have reached maximum rate
780 93747 : if (rate < meas_rate_min) {
781 : // the measurement is still decreasing
782 69841 : meas_rate_min = rate;
783 : }
784 :
785 : // calculate early stopping time based on the time it takes to get to 63.21%
786 93747 : if (meas_angle_max < angle_target_max * 0.6321) {
787 : // the measurement not reached the 63.21% threshold yet
788 33216 : step_timeout_ms = (now_ms - step_start_time_ms) * 3;
789 33216 : step_timeout_ms = MIN(step_timeout_ms, AUTOTUNE_TESTING_STEP_TIMEOUT_MS);
790 : }
791 :
792 93747 : if (meas_angle_max > angle_target_max) {
793 : // the measurement has passed the maximum angle
794 32 : step = Step::UPDATE_GAINS;
795 : }
796 :
797 93747 : if (meas_angle_max - meas_angle_min > meas_angle_max * aggressiveness) {
798 : // the measurement has passed 50% of the maximum angle and bounce back is larger than the threshold
799 0 : step = Step::UPDATE_GAINS;
800 : }
801 :
802 93747 : if (now_ms - step_start_time_ms >= step_timeout_ms) {
803 : // we have passed the maximum stop time
804 439 : step = Step::UPDATE_GAINS;
805 : }
806 93747 : }
807 :
808 : // twitching_measure_acceleration - measure rate of change of measurement
809 304666 : void AC_AutoTune_Multi::twitching_measure_acceleration(float &accel_average, float rate, float &rate_max) const
810 : {
811 304666 : if (rate_max < rate) {
812 143210 : rate_max = rate;
813 143210 : accel_average = (1000.0 * rate_max) / (AP_HAL::millis() - step_start_time_ms);
814 : }
815 304666 : }
816 :
817 : // update gains for the rate p up tune type
818 543 : void AC_AutoTune_Multi::updating_rate_p_up_all(AxisType test_axis)
819 : {
820 543 : switch (test_axis) {
821 102 : case AxisType::ROLL:
822 102 : updating_rate_p_up_d_down(tune_roll_rd, min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
823 102 : break;
824 102 : case AxisType::PITCH:
825 102 : updating_rate_p_up_d_down(tune_pitch_rd, min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
826 102 : break;
827 337 : case AxisType::YAW:
828 337 : updating_rate_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max, false);
829 337 : break;
830 2 : case AxisType::YAW_D:
831 2 : updating_rate_p_up_d_down(tune_yaw_rd, min_d, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
832 2 : break;
833 : }
834 543 : }
835 :
836 : // update gains for the rate d up tune type
837 652 : void AC_AutoTune_Multi::updating_rate_d_up_all(AxisType test_axis)
838 : {
839 652 : switch (test_axis) {
840 26 : case AxisType::ROLL:
841 26 : updating_rate_d_up(tune_roll_rd, 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);
842 26 : break;
843 26 : case AxisType::PITCH:
844 26 : updating_rate_d_up(tune_pitch_rd, 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);
845 26 : break;
846 109 : case AxisType::YAW:
847 109 : 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);
848 109 : break;
849 491 : case AxisType::YAW_D:
850 491 : updating_rate_d_up(tune_yaw_rd, min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
851 491 : break;
852 : }
853 652 : }
854 :
855 : // update gains for the rate d down tune type
856 431 : void AC_AutoTune_Multi::updating_rate_d_down_all(AxisType test_axis)
857 : {
858 431 : switch (test_axis) {
859 93 : case AxisType::ROLL:
860 93 : updating_rate_d_down(tune_roll_rd, min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
861 93 : break;
862 86 : case AxisType::PITCH:
863 86 : updating_rate_d_down(tune_pitch_rd, min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
864 86 : break;
865 36 : case AxisType::YAW:
866 36 : updating_rate_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
867 36 : break;
868 216 : case AxisType::YAW_D:
869 216 : updating_rate_d_down(tune_yaw_rd, min_d, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
870 216 : break;
871 : }
872 431 : }
873 :
874 : // update gains for the angle p up tune type
875 378 : void AC_AutoTune_Multi::updating_angle_p_up_all(AxisType test_axis)
876 : {
877 378 : switch (test_axis) {
878 152 : case AxisType::ROLL:
879 152 : updating_angle_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
880 152 : break;
881 152 : case AxisType::PITCH:
882 152 : updating_angle_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
883 152 : break;
884 74 : case AxisType::YAW:
885 : case AxisType::YAW_D:
886 74 : updating_angle_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
887 74 : break;
888 : }
889 378 : }
890 :
891 : // update gains for the angle p down tune type
892 93 : void AC_AutoTune_Multi::updating_angle_p_down_all(AxisType test_axis)
893 : {
894 93 : switch (test_axis) {
895 20 : case AxisType::ROLL:
896 20 : updating_angle_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
897 20 : break;
898 20 : case AxisType::PITCH:
899 20 : updating_angle_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
900 20 : break;
901 53 : case AxisType::YAW:
902 : case AxisType::YAW_D:
903 53 : updating_angle_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
904 53 : break;
905 : }
906 93 : }
907 :
908 : // set gains post tune for the tune type
909 70 : void AC_AutoTune_Multi::set_tuning_gains_with_backoff(AxisType test_axis)
910 : {
911 : // ensure gain_backoff has not been set outside limits
912 70 : gain_backoff.set_and_save_ifchanged(constrain_float(gain_backoff, 0.0f, 0.5f));
913 :
914 70 : switch (tune_type) {
915 14 : case TuneType::RATE_D_UP:
916 14 : break;
917 14 : case TuneType::RATE_D_DOWN:
918 14 : break;
919 14 : case TuneType::RATE_P_UP:
920 : switch (test_axis) {
921 4 : case AxisType::ROLL:
922 4 : tune_roll_rd = tune_roll_rd * (1.0f - gain_backoff);
923 4 : tune_roll_rp = tune_roll_rp * (1.0f - gain_backoff);
924 4 : break;
925 4 : case AxisType::PITCH:
926 4 : tune_pitch_rd = tune_pitch_rd * (1.0f - gain_backoff);
927 4 : tune_pitch_rp = tune_pitch_rp * (1.0f - gain_backoff);
928 4 : break;
929 5 : case AxisType::YAW:
930 5 : tune_yaw_rp = tune_yaw_rp * (1.0f - gain_backoff);
931 5 : break;
932 1 : case AxisType::YAW_D:
933 1 : tune_yaw_rd = tune_yaw_rd * (1.0f - gain_backoff);
934 1 : tune_yaw_rp = tune_yaw_rp * (1.0f - gain_backoff);
935 1 : break;
936 : }
937 14 : break;
938 14 : case TuneType::ANGLE_P_DOWN:
939 14 : break;
940 14 : case TuneType::ANGLE_P_UP:
941 : switch (test_axis) {
942 4 : case AxisType::ROLL:
943 4 : tune_roll_sp = tune_roll_sp * (1.0f - gain_backoff) * (1.0f - aggressiveness);
944 4 : tune_roll_accel_radss = cd_to_rad(MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max_cdss * AUTOTUNE_ACCEL_RP_BACKOFF));
945 4 : break;
946 4 : case AxisType::PITCH:
947 4 : tune_pitch_sp = tune_pitch_sp * (1.0f - gain_backoff) * (1.0f - aggressiveness);
948 4 : tune_pitch_accel_radss = cd_to_rad(MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max_cdss * AUTOTUNE_ACCEL_RP_BACKOFF));
949 4 : break;
950 6 : case AxisType::YAW:
951 : case AxisType::YAW_D:
952 6 : tune_yaw_sp = tune_yaw_sp * (1.0f - gain_backoff) * (1.0f - aggressiveness);
953 6 : tune_yaw_accel_radss = cd_to_rad(MAX(AUTOTUNE_Y_ACCEL_MIN, test_accel_max_cdss * AUTOTUNE_ACCEL_Y_BACKOFF));
954 6 : break;
955 : }
956 14 : break;
957 0 : case TuneType::RATE_FF_UP:
958 : case TuneType::MAX_GAINS:
959 : case TuneType::TUNE_CHECK:
960 : // this should never happen
961 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
962 0 : break;
963 0 : case TuneType::TUNE_COMPLETE:
964 0 : break;
965 : }
966 70 : }
967 :
968 : // updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back
969 : // optimize D term while keeping the maximum just below the target by adjusting P
970 652 : void AC_AutoTune_Multi::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)
971 : {
972 652 : if (meas_rate_max > rate_target) {
973 : // if maximum measurement was higher than target
974 : // reduce P gain (which should reduce maximum)
975 218 : tune_p -= tune_p * tune_p_step_ratio;
976 218 : if (tune_p < tune_p_min) {
977 : // P gain is at minimum so start reducing D
978 0 : tune_p = tune_p_min;
979 0 : tune_d -= tune_d * tune_d_step_ratio;
980 0 : if (tune_d <= tune_d_min) {
981 : // We have reached minimum D gain so stop tuning
982 0 : tune_d = tune_d_min;
983 0 : success_counter = AUTOTUNE_SUCCESS_COUNT;
984 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
985 : // This may be mean AGGR should be increased or MIN_D decreased
986 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Min Rate D limit reached");
987 : }
988 : }
989 434 : } else if ((meas_rate_max < rate_target * (1.0 - AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) {
990 : // we have not achieved a high enough maximum to get a good measurement of bounce back.
991 : // increase P gain (which should increase maximum)
992 299 : tune_p += tune_p * tune_p_step_ratio;
993 299 : if (tune_p >= tune_p_max) {
994 0 : tune_p = tune_p_max;
995 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
996 : }
997 : } else {
998 : // we have a good measurement of bounce back
999 135 : if (meas_rate_max-meas_rate_min > meas_rate_max * aggressiveness) {
1000 : // ignore the next result unless it is the same as this one
1001 36 : ignore_next = true;
1002 : // bounce back is bigger than our threshold so increment the success counter
1003 36 : success_counter++;
1004 : } else {
1005 99 : if (ignore_next == false) {
1006 : // bounce back is smaller than our threshold so decrement the success counter
1007 98 : if (success_counter > 0) {
1008 0 : success_counter--;
1009 : }
1010 : // increase D gain (which should increase bounce back)
1011 98 : tune_d += tune_d*tune_d_step_ratio * 2.0;
1012 : // stop tuning if we hit maximum D
1013 98 : if (tune_d >= tune_d_max) {
1014 5 : tune_d = tune_d_max;
1015 5 : success_counter = AUTOTUNE_SUCCESS_COUNT;
1016 5 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
1017 : }
1018 : } else {
1019 1 : ignore_next = false;
1020 : }
1021 : }
1022 : }
1023 652 : }
1024 :
1025 : // updating_rate_d_down - decrease D and adjust P to optimize the D term for no bounce back
1026 : // optimize D term while keeping the maximum just below the target by adjusting P
1027 431 : void AC_AutoTune_Multi::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)
1028 : {
1029 431 : if (meas_rate_max > rate_target) {
1030 : // if maximum measurement was higher than target
1031 : // reduce P gain (which should reduce maximum)
1032 102 : tune_p -= tune_p*tune_p_step_ratio;
1033 102 : if (tune_p < tune_p_min) {
1034 : // P gain is at minimum so start reducing D gain
1035 0 : tune_p = tune_p_min;
1036 0 : tune_d -= tune_d*tune_d_step_ratio;
1037 0 : if (tune_d <= tune_d_min) {
1038 : // We have reached minimum D so stop tuning
1039 0 : tune_d = tune_d_min;
1040 0 : success_counter = AUTOTUNE_SUCCESS_COUNT;
1041 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
1042 : // This may be mean AGGR should be increased or MIN_D decreased
1043 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Min Rate D limit reached");
1044 : }
1045 : }
1046 329 : } else if ((meas_rate_max < rate_target*(1.0 - AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) {
1047 : // we have not achieved a high enough maximum to get a good measurement of bounce back.
1048 : // increase P gain (which should increase maximum)
1049 124 : tune_p += tune_p * tune_p_step_ratio;
1050 124 : if (tune_p >= tune_p_max) {
1051 0 : tune_p = tune_p_max;
1052 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
1053 : }
1054 : } else {
1055 : // we have a good measurement of bounce back
1056 205 : if (meas_rate_max - meas_rate_min < meas_rate_max * aggressiveness) {
1057 71 : if (ignore_next == false) {
1058 : // bounce back is less than our threshold so increment the success counter
1059 57 : success_counter++;
1060 : } else {
1061 14 : ignore_next = false;
1062 : }
1063 : } else {
1064 : // ignore the next result unless it is the same as this one
1065 134 : ignore_next = true;
1066 : // bounce back is larger than our threshold so decrement the success counter
1067 134 : if (success_counter > 0) {
1068 1 : success_counter--;
1069 : }
1070 : // decrease D gain (which should decrease bounce back)
1071 134 : tune_d -= tune_d * tune_d_step_ratio;
1072 : // stop tuning if we hit minimum D
1073 134 : if (tune_d <= tune_d_min) {
1074 0 : tune_d = tune_d_min;
1075 0 : success_counter = AUTOTUNE_SUCCESS_COUNT;
1076 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
1077 : // This may be mean AGGR should be increased or MIN_D decreased
1078 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Min Rate D limit reached");
1079 : }
1080 : }
1081 : }
1082 431 : }
1083 :
1084 : // updating_rate_p_up_d_down - increase P to ensure the target is reached while checking bounce back isn't increasing
1085 : // P is increased until we achieve our target within a reasonable time while reducing D if bounce back increases above the threshold
1086 543 : void AC_AutoTune_Multi::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, bool fail_min_d)
1087 : {
1088 543 : if (meas_rate_max > rate_target * (1.0 + 0.5 * aggressiveness)) {
1089 : // ignore the next result unless it is the same as this one
1090 56 : ignore_next = true;
1091 : // if maximum measurement was greater than target so increment the success counter
1092 56 : success_counter++;
1093 487 : } else if ((meas_rate_max < rate_target) && (meas_rate_max > rate_target * (1.0 - AUTOTUNE_D_UP_DOWN_MARGIN)) && (meas_rate_max - meas_rate_min > meas_rate_max * aggressiveness) && (tune_d > tune_d_min)) {
1094 : // if bounce back was larger than the threshold so decrement the success counter
1095 189 : if (success_counter > 0) {
1096 0 : success_counter--;
1097 : }
1098 : // Reduce D gain if bounce-back exceeds threshold.
1099 189 : tune_d -= tune_d * tune_d_step_ratio;
1100 : // do not decrease the D term past the minimum
1101 189 : if (tune_d <= tune_d_min) {
1102 4 : tune_d = tune_d_min;
1103 4 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
1104 4 : if (fail_min_d) {
1105 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AutoTune: Rate D Gain Determination Failed");
1106 0 : mode = TuneMode::FAILED;
1107 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
1108 : }
1109 : }
1110 : // decrease P gain to match D gain reduction
1111 189 : tune_p -= tune_p * tune_p_step_ratio;
1112 : // do not decrease the P term past the minimum
1113 189 : if (tune_p <= tune_p_min) {
1114 0 : tune_p = tune_p_min;
1115 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AutoTune: Rate P Gain Determination Failed");
1116 0 : mode = TuneMode::FAILED;
1117 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
1118 : }
1119 : } else {
1120 298 : if (ignore_next == false) {
1121 : // if maximum measurement was lower than target so decrement the success counter
1122 273 : if (success_counter > 0) {
1123 4 : success_counter--;
1124 : }
1125 : // increase P gain (which should increase the maximum)
1126 273 : tune_p += tune_p * tune_p_step_ratio;
1127 : // stop tuning if we hit maximum P
1128 273 : if (tune_p >= tune_p_max) {
1129 1 : tune_p = tune_p_max;
1130 1 : success_counter = AUTOTUNE_SUCCESS_COUNT;
1131 1 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
1132 : }
1133 : } else {
1134 25 : ignore_next = false;
1135 : }
1136 : }
1137 543 : }
1138 :
1139 : // updating_angle_p_down - decrease P until we don't reach the target before time out
1140 : // P is decreased to ensure we are not overshooting the target
1141 93 : void AC_AutoTune_Multi::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)
1142 : {
1143 93 : if (meas_angle_max < angle_target * (1 + 0.5 * aggressiveness)) {
1144 69 : if (ignore_next == false) {
1145 : // if maximum measurement was lower than target so increment the success counter
1146 56 : success_counter++;
1147 : } else {
1148 13 : ignore_next = false;
1149 : }
1150 : } else {
1151 : // ignore the next result unless it is the same as this one
1152 24 : ignore_next = true;
1153 : // if maximum measurement was higher than target so decrement the success counter
1154 24 : if (success_counter > 0) {
1155 0 : success_counter--;
1156 : }
1157 : // decrease P gain (which should decrease the maximum)
1158 24 : tune_p -= tune_p*tune_p_step_ratio;
1159 : // stop tuning if we hit maximum P
1160 24 : if (tune_p <= tune_p_min) {
1161 0 : tune_p = tune_p_min;
1162 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
1163 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AutoTune: Angle P Gain Determination Failed");
1164 0 : mode = TuneMode::FAILED;
1165 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
1166 : }
1167 : }
1168 93 : }
1169 :
1170 : // updating_angle_p_up - increase P to ensure the target is reached
1171 : // P is increased until we achieve our target within a reasonable time
1172 378 : void AC_AutoTune_Multi::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)
1173 : {
1174 472 : if ((meas_angle_max > angle_target * (1 + 0.5 * aggressiveness)) ||
1175 94 : ((meas_angle_max > angle_target) && (meas_rate_min < -meas_rate_max * aggressiveness))) {
1176 : // ignore the next result unless it is the same as this one
1177 24 : ignore_next = true;
1178 : // if maximum measurement was greater than target so increment the success counter
1179 24 : success_counter++;
1180 : } else {
1181 354 : if (ignore_next == false) {
1182 : // if maximum measurement was lower than target so decrement the success counter
1183 351 : if (success_counter > 0) {
1184 0 : success_counter--;
1185 : }
1186 : // increase P gain (which should increase the maximum)
1187 351 : tune_p += tune_p * tune_p_step_ratio;
1188 : // stop tuning if we hit maximum P
1189 351 : if (tune_p >= tune_p_max) {
1190 8 : tune_p = tune_p_max;
1191 8 : success_counter = AUTOTUNE_SUCCESS_COUNT;
1192 8 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
1193 : }
1194 : } else {
1195 3 : ignore_next = false;
1196 : }
1197 : }
1198 378 : }
1199 :
1200 : #if HAL_LOGGING_ENABLED
1201 2097 : void AC_AutoTune_Multi::Log_AutoTune()
1202 : {
1203 2097 : if ((tune_type == TuneType::ANGLE_P_DOWN) || (tune_type == TuneType::ANGLE_P_UP)) {
1204 471 : switch (axis) {
1205 172 : case AxisType::ROLL:
1206 172 : Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max_cdss);
1207 172 : break;
1208 172 : case AxisType::PITCH:
1209 172 : Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max_cdss);
1210 172 : break;
1211 76 : case AxisType::YAW:
1212 76 : Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max_cdss);
1213 76 : break;
1214 51 : case AxisType::YAW_D:
1215 51 : Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max_cdss);
1216 51 : break;
1217 : }
1218 : } else {
1219 1626 : switch (axis) {
1220 221 : case AxisType::ROLL:
1221 221 : Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max_cdss);
1222 221 : break;
1223 214 : case AxisType::PITCH:
1224 214 : Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max_cdss);
1225 214 : break;
1226 482 : case AxisType::YAW:
1227 482 : Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max_cdss);
1228 482 : break;
1229 709 : case AxisType::YAW_D:
1230 709 : Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max_cdss);
1231 709 : break;
1232 : }
1233 : }
1234 :
1235 2097 : }
1236 :
1237 304666 : void AC_AutoTune_Multi::Log_AutoTuneDetails()
1238 : {
1239 304666 : Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
1240 304666 : }
1241 :
1242 : // @LoggerMessage: ATUN
1243 : // @Description: Copter/QuadPlane AutoTune
1244 : // @Vehicles: Copter, Plane
1245 : // @Field: TimeUS: Time since system startup
1246 : // @Field: Axis: which axis is currently being tuned
1247 : // @Field: TuneStep: step in autotune process
1248 : // @Field: Targ: target angle or rate, depending on tuning step
1249 : // @Field: Min: measured minimum target angle or rate
1250 : // @Field: Max: measured maximum target angle or rate
1251 : // @Field: RP: new rate gain P term
1252 : // @Field: RD: new rate gain D term
1253 : // @Field: SP: new angle P term
1254 : // @Field: ddt: maximum measured twitching acceleration
1255 :
1256 : // Write an Autotune data packet
1257 2097 : void AC_AutoTune_Multi::Log_Write_AutoTune(AxisType _axis, TuneType 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)
1258 : {
1259 4194 : AP::logger().Write(
1260 : "ATUN",
1261 : "TimeUS,Axis,TuneStep,Targ,Min,Max,RP,RD,SP,ddt",
1262 : "s--ddd---o",
1263 : "F--000---0",
1264 : "QBBfffffff",
1265 : AP_HAL::micros64(),
1266 : axis,
1267 : tune_step,
1268 2097 : meas_target*0.01,
1269 2097 : meas_min*0.01,
1270 2097 : meas_max*0.01,
1271 : new_gain_rp,
1272 : new_gain_rd,
1273 : new_gain_sp,
1274 : new_ddt);
1275 2097 : }
1276 :
1277 : // Write an Autotune data packet
1278 304666 : void AC_AutoTune_Multi::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds)
1279 : {
1280 : // @LoggerMessage: ATDE
1281 : // @Description: AutoTune data packet
1282 : // @Field: TimeUS: Time since system startup
1283 : // @Field: Angle: current angle
1284 : // @Field: Rate: current angular rate
1285 609332 : AP::logger().WriteStreaming(
1286 : "ATDE",
1287 : "TimeUS,Angle,Rate",
1288 : "sdk",
1289 : "F00",
1290 : "Qff",
1291 : AP_HAL::micros64(),
1292 304666 : angle_cd*0.01,
1293 304666 : rate_cds*0.01);
1294 304666 : }
1295 : #endif // HAL_LOGGING_ENABLED
1296 :
1297 1558 : float AC_AutoTune_Multi::target_angle_max_rp_cd() const
1298 : {
1299 1558 : return attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_MAX_RP_SCALE;
1300 : }
1301 :
1302 2806 : float AC_AutoTune_Multi::target_angle_max_y_cd() const
1303 : {
1304 : // Aircraft with small lean angle will generally benefit from proportional smaller yaw twitch size
1305 2806 : return attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_MAX_Y_SCALE;
1306 : }
1307 :
1308 779 : float AC_AutoTune_Multi::target_angle_min_rp_cd() const
1309 : {
1310 779 : return attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_MIN_RP_SCALE;
1311 : }
1312 :
1313 1403 : float AC_AutoTune_Multi::target_angle_min_y_cd() const
1314 : {
1315 : // Aircraft with small lean angle will generally benefit from proportional smaller yaw twitch size
1316 1403 : return attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_MIN_Y_SCALE;
1317 : }
1318 :
1319 304666 : float AC_AutoTune_Multi::angle_lim_max_rp_cd() const
1320 : {
1321 304666 : return attitude_control->lean_angle_max_cd() * AUTOTUNE_ANGLE_ABORT_RP_SCALE;
1322 : }
1323 :
1324 304666 : float AC_AutoTune_Multi::angle_lim_neg_rpy_cd() const
1325 : {
1326 304666 : return attitude_control->lean_angle_max_cd() * AUTOTUNE_ANGLE_NEG_RP_SCALE;
1327 : }
1328 :
1329 : // get_testing_step_timeout_ms accessor
1330 2182 : uint32_t AC_AutoTune_Multi::get_testing_step_timeout_ms() const
1331 : {
1332 2182 : return AUTOTUNE_TESTING_STEP_TIMEOUT_MS;
1333 : }
1334 :
1335 :
1336 : #endif // AC_AUTOTUNE_ENABLED
|
