Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : support for autotune of helicopters. Based on original autotune code from ArduCopter, written by Leonard Hall
17 : Converted to a library by Andrew Tridgell, and rewritten to include helicopters by Bill Geyer
18 : */
19 :
20 : #include "AC_AutoTune_config.h"
21 :
22 : #if AC_AUTOTUNE_ENABLED
23 :
24 : #include "AC_AutoTune_Heli.h"
25 :
26 : #include <AP_Logger/AP_Logger.h>
27 : #include <GCS_MAVLink/GCS.h>
28 :
29 : #define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 5000U // timeout for tuning mode's testing step
30 :
31 : #define AUTOTUNE_RD_STEP 0.0005f // minimum increment when increasing/decreasing Rate D term
32 : #define AUTOTUNE_RP_STEP 0.005f // minimum increment when increasing/decreasing Rate P term
33 : #define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term
34 : #define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing
35 : #define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing
36 : #define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing
37 : #define AUTOTUNE_RD_MAX 0.020f // maximum Rate D value
38 : #define AUTOTUNE_RP_MIN 0.02f // minimum Rate P value
39 : #define AUTOTUNE_RP_MAX 0.3f // maximum Rate P value
40 : #define AUTOTUNE_SP_MAX 10.0f // maximum Stab P value
41 : #define AUTOTUNE_SP_MIN 3.0f // maximum Stab P value
42 : #define AUTOTUNE_RFF_MAX 0.5f // maximum Stab P value
43 : #define AUTOTUNE_RFF_MIN 0.025f // maximum Stab P value
44 : #define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning
45 : #define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning
46 : #define AUTOTUNE_SP_BACKOFF 1.0f // Stab P gains are reduced to 90% of their maximum value discovered during tuning
47 : #define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration
48 : #define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration
49 :
50 : #define AUTOTUNE_HELI_TARGET_ANGLE_RLLPIT_CD 1500 // target roll/pitch angle during AUTOTUNE FeedForward rate test
51 : #define AUTOTUNE_HELI_TARGET_RATE_RLLPIT_CDS 5000 // target roll/pitch rate during AUTOTUNE FeedForward rate test
52 : #define AUTOTUNE_FFI_RATIO_FOR_TESTING 0.5f // I is set 2x smaller than VFF during testing
53 : #define AUTOTUNE_FFI_RATIO_FINAL 0.5f // I is set 0.5x VFF after testing
54 : #define AUTOTUNE_RP_ACCEL_MIN 20000.0f // Minimum acceleration for Roll and Pitch
55 : #define AUTOTUNE_Y_ACCEL_MIN 10000.0f // Minimum acceleration for Yaw
56 :
57 : #define AUTOTUNE_SEQ_BITMASK_VFF 1
58 : #define AUTOTUNE_SEQ_BITMASK_RATE_D 2
59 : #define AUTOTUNE_SEQ_BITMASK_ANGLE_P 4
60 : #define AUTOTUNE_SEQ_BITMASK_MAX_GAIN 8
61 : #define AUTOTUNE_SEQ_BITMASK_TUNE_CHECK 16
62 :
63 : // angle limits preserved from previous behaviour as Multi changed:
64 : #define AUTOTUNE_ANGLE_TARGET_MAX_RP_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step
65 : #define AUTOTUNE_ANGLE_TARGET_MIN_RP_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step
66 : #define AUTOTUNE_ANGLE_TARGET_MAX_Y_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step
67 : #define AUTOTUNE_ANGLE_TARGET_MIN_Y_CD 500 // target angle during TESTING_RATE step that will cause us to move to next step
68 : #define AUTOTUNE_ANGLE_MAX_RP_CD 3000 // maximum allowable angle in degrees during testing
69 : #define AUTOTUNE_ANGLE_NEG_RPY_CD 1000 // maximum allowable angle in degrees during testing
70 :
71 : const AP_Param::GroupInfo AC_AutoTune_Heli::var_info[] = {
72 :
73 : // @Param: AXES
74 : // @DisplayName: Autotune axis bitmask
75 : // @Description: 1-byte bitmap of axes to autotune
76 : // @Bitmask: 0:Roll,1:Pitch,2:Yaw
77 : // @User: Standard
78 : AP_GROUPINFO("AXES", 1, AC_AutoTune_Heli, axis_bitmask, 1),
79 :
80 : // @Param: SEQ
81 : // @DisplayName: AutoTune Sequence Bitmask
82 : // @Description: 2-byte bitmask to select what tuning should be performed. Max gain automatically performed if Rate D is selected. Values: 7:All,1:VFF Only,2:Rate D/Rate P Only(incl max gain),4:Angle P Only,8:Max Gain Only,16:Tune Check,3:VFF and Rate D/Rate P(incl max gain),5:VFF and Angle P,6:Rate D/Rate P(incl max gain) and angle P
83 : // @Bitmask: 0:VFF,1:Rate D/Rate P(incl max gain),2:Angle P,3:Max Gain Only,4:Tune Check
84 : // @User: Standard
85 : AP_GROUPINFO("SEQ", 2, AC_AutoTune_Heli, seq_bitmask, 3),
86 :
87 : // @Param: FRQ_MIN
88 : // @DisplayName: AutoTune minimum sweep frequency
89 : // @Description: Defines the start frequency for sweeps and dwells
90 : // @Range: 10 30
91 : // @User: Standard
92 : AP_GROUPINFO("FRQ_MIN", 3, AC_AutoTune_Heli, min_sweep_freq, 10.0f),
93 :
94 : // @Param: FRQ_MAX
95 : // @DisplayName: AutoTune maximum sweep frequency
96 : // @Description: Defines the end frequency for sweeps and dwells
97 : // @Range: 50 120
98 : // @User: Standard
99 : AP_GROUPINFO("FRQ_MAX", 4, AC_AutoTune_Heli, max_sweep_freq, 70.0f),
100 :
101 : // @Param: GN_MAX
102 : // @DisplayName: AutoTune maximum response gain
103 : // @Description: Defines the response gain (output/input) to tune
104 : // @Range: 1 2.5
105 : // @User: Standard
106 : AP_GROUPINFO("GN_MAX", 5, AC_AutoTune_Heli, max_resp_gain, 1.0f),
107 :
108 : // @Param: VELXY_P
109 : // @DisplayName: AutoTune velocity xy P gain
110 : // @Description: Velocity xy P gain used to hold position during Max Gain, Rate P, and Rate D frequency sweeps
111 : // @Range: 0 1
112 : // @User: Standard
113 : AP_GROUPINFO("VELXY_P", 6, AC_AutoTune_Heli, vel_hold_gain, 0.1f),
114 :
115 : // @Param: ACC_MAX
116 : // @DisplayName: AutoTune maximum allowable angular acceleration
117 : // @Description: maximum angular acceleration in deg/s/s allowed during autotune maneuvers
118 : // @Range: 1 4000
119 : // @User: Standard
120 : // @Units: deg/s/s
121 : AP_GROUPINFO("ACC_MAX", 7, AC_AutoTune_Heli, accel_max_degss, 0.0f),
122 :
123 : // @Param: RAT_MAX
124 : // @DisplayName: Autotune maximum allowable angular rate
125 : // @Description: maximum angular rate in deg/s allowed during autotune maneuvers
126 : // @Range: 0 500
127 : // @User: Standard
128 : // @Units: deg/s
129 : AP_GROUPINFO("RAT_MAX", 8, AC_AutoTune_Heli, rate_max_degs, 0.0f),
130 :
131 : AP_GROUPEND
132 : };
133 :
134 : // constructor
135 83 : AC_AutoTune_Heli::AC_AutoTune_Heli()
136 : {
137 83 : tune_seq[0] = TuneType::TUNE_COMPLETE;
138 83 : AP_Param::setup_object_defaults(this, var_info);
139 83 : }
140 :
141 : // Prepares all tuning state variables and target values for a new test.
142 147 : void AC_AutoTune_Heli::test_init()
143 : {
144 147 : AC_AutoTune_FreqResp::ResponseType resp_type = AC_AutoTune_FreqResp::ResponseType::RATE;
145 147 : FreqRespCalcType calc_type = RATE;
146 147 : FreqRespInput freq_resp_input = TARGET;
147 147 : float freq_resp_amplitude = 5.0f; // amplitude in deg
148 147 : float filter_freq = 10.0f;
149 147 : switch (tune_type) {
150 35 : case TuneType::RATE_FF_UP:
151 35 : if (!is_positive(next_test_freq)) {
152 3 : start_freq = 0.25f * M_2PI;
153 : } else {
154 32 : start_freq = next_test_freq;
155 : }
156 35 : stop_freq = start_freq;
157 35 : filter_freq = start_freq;
158 :
159 35 : attitude_control->bf_feedforward(false);
160 :
161 : // variables needed to initialize frequency response object and test method
162 35 : resp_type = AC_AutoTune_FreqResp::ResponseType::RATE;
163 35 : calc_type = RATE;
164 35 : freq_resp_input = TARGET;
165 35 : pre_calc_cycles = 1.0f;
166 35 : num_dwell_cycles = 3;
167 35 : break;
168 21 : case TuneType::MAX_GAINS:
169 : // initialize start frequency for sweep
170 21 : if (!is_positive(next_test_freq)) {
171 3 : start_freq = min_sweep_freq;
172 3 : stop_freq = max_sweep_freq;
173 3 : sweep_complete = true;
174 : } else {
175 18 : start_freq = next_test_freq;
176 18 : stop_freq = start_freq;
177 18 : test_accel_max_cdss = 0.0f;
178 : }
179 21 : filter_freq = start_freq;
180 :
181 21 : attitude_control->bf_feedforward(false);
182 :
183 : // variables needed to initialize frequency response object and test method
184 21 : resp_type = AC_AutoTune_FreqResp::ResponseType::RATE;
185 21 : calc_type = RATE;
186 21 : freq_resp_input = MOTOR;
187 21 : pre_calc_cycles = 6.25f;
188 21 : num_dwell_cycles = 6;
189 21 : break;
190 75 : case TuneType::RATE_P_UP:
191 : case TuneType::RATE_D_UP:
192 : // initialize start frequency
193 75 : if (!is_positive(next_test_freq)) {
194 : // continue using frequency where testing left off with RATE_D_UP completed
195 6 : if (curr_data.phase > 150.0f && curr_data.phase < 180.0f && tune_type == TuneType::RATE_P_UP) {
196 3 : start_freq = curr_data.freq;
197 : // start with freq found for sweep where phase was 180 deg
198 3 : } else if (!is_zero(sweep_tgt.ph180.freq)) {
199 3 : start_freq = sweep_tgt.ph180.freq;
200 : // otherwise start at min freq to step up in dwell frequency until phase > 160 deg
201 : } else {
202 0 : start_freq = min_sweep_freq;
203 : }
204 : } else {
205 69 : start_freq = next_test_freq;
206 : }
207 75 : stop_freq = start_freq;
208 75 : filter_freq = start_freq;
209 :
210 75 : attitude_control->bf_feedforward(false);
211 :
212 : // variables needed to initialize frequency response object and test method
213 75 : resp_type = AC_AutoTune_FreqResp::ResponseType::RATE;
214 75 : calc_type = RATE;
215 75 : freq_resp_input = TARGET;
216 75 : pre_calc_cycles = 6.25f;
217 75 : num_dwell_cycles = 6;
218 75 : break;
219 13 : case TuneType::ANGLE_P_UP:
220 : // initialize start frequency for sweep
221 13 : if (!is_positive(next_test_freq)) {
222 3 : start_freq = min_sweep_freq;
223 3 : stop_freq = max_sweep_freq;
224 3 : sweep_complete = true;
225 : } else {
226 10 : start_freq = next_test_freq;
227 10 : stop_freq = start_freq;
228 10 : test_accel_max_cdss = 0.0f;
229 : }
230 13 : filter_freq = start_freq;
231 13 : attitude_control->bf_feedforward(false);
232 :
233 : // variables needed to initialize frequency response object and test method
234 13 : resp_type = AC_AutoTune_FreqResp::ResponseType::ANGLE;
235 13 : calc_type = DRB;
236 13 : freq_resp_input = TARGET;
237 13 : pre_calc_cycles = 6.25f;
238 13 : num_dwell_cycles = 6;
239 13 : break;
240 3 : case TuneType::TUNE_CHECK:
241 : // initialize start frequency
242 3 : start_freq = min_sweep_freq;
243 3 : stop_freq = max_sweep_freq;
244 3 : test_accel_max_cdss = 0.0f;
245 3 : filter_freq = start_freq;
246 :
247 : // variables needed to initialize frequency response object and test method
248 3 : resp_type = AC_AutoTune_FreqResp::ResponseType::ANGLE;
249 3 : calc_type = ANGLE;
250 3 : freq_resp_input = TARGET;
251 3 : break;
252 0 : default:
253 0 : break;
254 : }
255 :
256 147 : if (!is_equal(start_freq,stop_freq)) {
257 9 : input_type = AC_AutoTune_FreqResp::InputType::SWEEP;
258 : } else {
259 138 : input_type = AC_AutoTune_FreqResp::InputType::DWELL;
260 : }
261 :
262 :
263 : // initialize dwell test method
264 147 : dwell_test_init(start_freq, stop_freq, freq_resp_amplitude, filter_freq, freq_resp_input, calc_type, resp_type, input_type);
265 147 : }
266 :
267 : // run tests for each tune type
268 550704 : void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign)
269 : {
270 : // if tune complete or beyond frequency range or no max allowed gains then exit testing
271 1652112 : if (tune_type == TuneType::TUNE_COMPLETE ||
272 1101408 : ((tune_type == TuneType::RATE_P_UP || tune_type == TuneType::RATE_D_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) ||
273 550704 : ((tune_type == TuneType::MAX_GAINS || tune_type == TuneType::RATE_P_UP || tune_type == TuneType::RATE_D_UP || tune_type == TuneType::ANGLE_P_UP) && exceeded_freq_range(start_freq))){
274 :
275 0 : load_gains(GainType::ORIGINAL);
276 :
277 : // hold level attitude
278 0 : attitude_control->input_euler_angle_roll_pitch_yaw_rad(desired_roll_rad, desired_pitch_rad, desired_yaw_rad, true);
279 :
280 0 : if ((tune_type == TuneType::RATE_P_UP || tune_type == TuneType::RATE_D_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) {
281 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Max Gain Determination Failed");
282 0 : mode = TuneMode::FAILED;
283 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
284 0 : update_gcs(AUTOTUNE_MESSAGE_FAILED);
285 0 : } else if ((tune_type == TuneType::MAX_GAINS || tune_type == TuneType::RATE_P_UP || tune_type == TuneType::RATE_D_UP || tune_type == TuneType::ANGLE_P_UP) && exceeded_freq_range(start_freq)){
286 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Exceeded frequency range");
287 0 : mode = TuneMode::FAILED;
288 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
289 0 : update_gcs(AUTOTUNE_MESSAGE_FAILED);
290 0 : } else if (tune_type == TuneType::TUNE_COMPLETE) {
291 0 : success_counter = AUTOTUNE_SUCCESS_COUNT;
292 0 : step = Step::UPDATE_GAINS;
293 : }
294 0 : return;
295 : }
296 :
297 550704 : dwell_test_run(curr_data);
298 :
299 : }
300 :
301 : // heli specific gcs announcements
302 715598 : void AC_AutoTune_Heli::do_gcs_announcements()
303 : {
304 715598 : const uint32_t now_ms = AP_HAL::millis();
305 715598 : if (now_ms - last_announce_ms < AUTOTUNE_ANNOUNCE_INTERVAL_MS) {
306 714700 : return;
307 : }
308 :
309 898 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: %s %s", get_axis_name(), get_tune_type_name());
310 898 : send_step_string();
311 898 : switch (tune_type) {
312 898 : case TuneType::RATE_FF_UP:
313 : case TuneType::RATE_D_UP:
314 : case TuneType::RATE_P_UP:
315 : case TuneType::MAX_GAINS:
316 : case TuneType::ANGLE_P_UP:
317 : case TuneType::TUNE_CHECK:
318 898 : if (is_equal(start_freq,stop_freq)) {
319 782 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Dwell");
320 : } else {
321 116 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Sweep");
322 116 : if (settle_time == 0) {
323 116 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f phase=%f", (double)(curr_test.freq), (double)(curr_test.gain), (double)(curr_test.phase));
324 : }
325 : }
326 898 : break;
327 0 : default:
328 0 : break;
329 : }
330 :
331 898 : last_announce_ms = now_ms;
332 : }
333 :
334 : // send post test updates to user
335 147 : void AC_AutoTune_Heli::do_post_test_gcs_announcements() {
336 147 : float tune_rp = 0.0f;
337 147 : float tune_rd = 0.0f;
338 147 : float tune_rff = 0.0f;
339 147 : float tune_sp = 0.0f;
340 147 : float tune_accel_radss = 0.0f;
341 :
342 147 : switch (axis) {
343 45 : case AxisType::ROLL:
344 45 : tune_rp = tune_roll_rp;
345 45 : tune_rd = tune_roll_rd;
346 45 : tune_rff = tune_roll_rff;
347 45 : tune_sp = tune_roll_sp;
348 45 : tune_accel_radss = tune_roll_accel_radss;
349 45 : break;
350 47 : case AxisType::PITCH:
351 47 : tune_rp = tune_pitch_rp;
352 47 : tune_rd = tune_pitch_rd;
353 47 : tune_rff = tune_pitch_rff;
354 47 : tune_sp = tune_pitch_sp;
355 47 : tune_accel_radss = tune_pitch_accel_radss;
356 47 : break;
357 55 : case AxisType::YAW:
358 : case AxisType::YAW_D:
359 55 : tune_rp = tune_yaw_rp;
360 55 : tune_rd = tune_yaw_rd;
361 55 : tune_rff = tune_yaw_rff;
362 55 : tune_sp = tune_yaw_sp;
363 55 : tune_accel_radss = tune_yaw_accel_radss;
364 55 : break;
365 : }
366 :
367 147 : if (step == Step::UPDATE_GAINS) {
368 147 : switch (tune_type) {
369 144 : case TuneType::RATE_FF_UP:
370 : case TuneType::RATE_P_UP:
371 : case TuneType::RATE_D_UP:
372 : case TuneType::ANGLE_P_UP:
373 : case TuneType::MAX_GAINS:
374 144 : if (is_equal(start_freq,stop_freq)) {
375 : // announce results of dwell
376 138 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: freq=%f gain=%f", (double)(curr_data.freq), (double)(curr_data.gain));
377 138 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: ph=%f", (double)(curr_data.phase));
378 138 : if (tune_type == TuneType::RATE_P_UP) {
379 35 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: rate_p=%f", (double)(tune_rp));
380 103 : } else if (tune_type == TuneType::RATE_D_UP) {
381 40 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: rate_d=%f", (double)(tune_rd));
382 63 : } else if (tune_type == TuneType::RATE_FF_UP) {
383 35 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: rate_ff=%f", (double)(tune_rff));
384 28 : } else if (tune_type == TuneType::ANGLE_P_UP) {
385 10 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: angle_p=%f tune_accel=%f max_accel=%f", (double)(tune_sp), (double)(rad_to_cd(tune_accel_radss)), (double)(test_accel_max_cdss));
386 : }
387 : } else {
388 6 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: max_freq=%f max_gain=%f", (double)(sweep_tgt.maxgain.freq), (double)(sweep_tgt.maxgain.gain));
389 6 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: ph180_freq=%f ph180_gain=%f", (double)(sweep_tgt.ph180.freq), (double)(sweep_tgt.ph180.gain));
390 : }
391 144 : break;
392 3 : default:
393 3 : break;
394 : }
395 : }
396 147 : }
397 :
398 : // backup_gains_and_initialise - store current gains as originals
399 : // called before tuning starts to backup original gains
400 7 : void AC_AutoTune_Heli::backup_gains_and_initialise()
401 : {
402 7 : AC_AutoTune::backup_gains_and_initialise();
403 :
404 : // initializes dwell test sequence for rate_p_up and rate_d_up tests for tradheli
405 7 : next_test_freq = 0.0f;
406 7 : start_freq = 0.0f;
407 7 : stop_freq = 0.0f;
408 :
409 7 : orig_bf_feedforward = attitude_control->get_bf_feedforward();
410 :
411 : // backup original pids and initialise tuned pid values
412 7 : orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
413 7 : orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
414 7 : orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
415 7 : orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
416 7 : orig_roll_fltt = attitude_control->get_rate_roll_pid().filt_T_hz();
417 7 : orig_roll_smax = attitude_control->get_rate_roll_pid().slew_limit();
418 7 : orig_roll_sp = attitude_control->get_angle_roll_p().kP();
419 7 : orig_roll_accel_radss = attitude_control->get_accel_roll_max_radss();
420 7 : orig_roll_rate_rads = attitude_control->get_ang_vel_roll_max_rads();
421 7 : tune_roll_rp = attitude_control->get_rate_roll_pid().kP();
422 7 : tune_roll_rd = attitude_control->get_rate_roll_pid().kD();
423 7 : tune_roll_rff = attitude_control->get_rate_roll_pid().ff();
424 7 : tune_roll_sp = attitude_control->get_angle_roll_p().kP();
425 7 : tune_roll_accel_radss = attitude_control->get_accel_roll_max_radss();
426 :
427 7 : orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
428 7 : orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
429 7 : orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
430 7 : orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
431 7 : orig_pitch_fltt = attitude_control->get_rate_pitch_pid().filt_T_hz();
432 7 : orig_pitch_smax = attitude_control->get_rate_pitch_pid().slew_limit();
433 7 : orig_pitch_sp = attitude_control->get_angle_pitch_p().kP();
434 7 : orig_pitch_accel_radss = attitude_control->get_accel_pitch_max_radss();
435 7 : orig_pitch_rate_rads = attitude_control->get_ang_vel_pitch_max_rads();
436 7 : tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
437 7 : tune_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
438 7 : tune_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
439 7 : tune_pitch_sp = attitude_control->get_angle_pitch_p().kP();
440 7 : tune_pitch_accel_radss = attitude_control->get_accel_pitch_max_radss();
441 :
442 7 : orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
443 7 : orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
444 7 : orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
445 7 : orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
446 7 : orig_yaw_fltt = attitude_control->get_rate_yaw_pid().filt_T_hz();
447 7 : orig_yaw_smax = attitude_control->get_rate_yaw_pid().slew_limit();
448 7 : orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
449 7 : orig_yaw_accel_radss = attitude_control->get_accel_yaw_max_radss();
450 7 : orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
451 7 : orig_yaw_rate_rads = attitude_control->get_ang_vel_yaw_max_rads();
452 7 : tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
453 7 : tune_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
454 7 : tune_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
455 7 : tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
456 7 : tune_yaw_sp = attitude_control->get_angle_yaw_p().kP();
457 7 : tune_yaw_accel_radss = attitude_control->get_accel_yaw_max_radss();
458 :
459 7 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_INITIALISED);
460 7 : }
461 :
462 : // load_orig_gains - set gains to their original values
463 : // called by stop and failed functions
464 14 : void AC_AutoTune_Heli::load_orig_gains()
465 : {
466 14 : attitude_control->use_sqrt_controller(true);
467 14 : attitude_control->bf_feedforward(orig_bf_feedforward);
468 14 : if (roll_enabled()) {
469 8 : load_gain_set(AxisType::ROLL, orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel_radss, orig_roll_fltt, 0.0f, orig_roll_smax, orig_roll_rate_rads);
470 : }
471 14 : if (pitch_enabled()) {
472 8 : load_gain_set(AxisType::PITCH, orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel_radss, orig_pitch_fltt, 0.0f, orig_pitch_smax, orig_pitch_rate_rads);
473 : }
474 14 : if (yaw_enabled()) {
475 6 : load_gain_set(AxisType::YAW, orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_sp, orig_yaw_accel_radss, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax, orig_yaw_rate_rads);
476 : }
477 14 : }
478 :
479 : // load_tuned_gains - load tuned gains
480 0 : void AC_AutoTune_Heli::load_tuned_gains()
481 : {
482 0 : attitude_control->use_sqrt_controller(true);
483 0 : if (!attitude_control->get_bf_feedforward()) {
484 0 : attitude_control->bf_feedforward(true);
485 0 : attitude_control->set_accel_roll_max_radss(0.0f);
486 0 : attitude_control->set_accel_pitch_max_radss(0.0f);
487 : }
488 0 : if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled()) {
489 0 : load_gain_set(AxisType::ROLL, tune_roll_rp, tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_roll_rd, tune_roll_rff, tune_roll_sp, tune_roll_accel_radss, orig_roll_fltt, 0.0f, orig_roll_smax, orig_roll_rate_rads);
490 : }
491 0 : if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled()) {
492 0 : load_gain_set(AxisType::PITCH, tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel_radss, orig_pitch_fltt, 0.0f, orig_pitch_smax, orig_pitch_rate_rads);
493 : }
494 0 : if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) {
495 0 : load_gain_set(AxisType::YAW, tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel_radss, orig_yaw_fltt, tune_yaw_rLPF, orig_yaw_smax, orig_yaw_rate_rads);
496 : }
497 0 : }
498 :
499 : // load_intra_test_gains - gains used between tests
500 : // called during testing mode's update-gains step to set gains ahead of return-to-level step
501 154 : void AC_AutoTune_Heli::load_intra_test_gains()
502 : {
503 : // we are restarting tuning so reset gains to tuning-start gains (i.e. low I term)
504 : // sanity check the gains
505 154 : attitude_control->use_sqrt_controller(true);
506 154 : attitude_control->bf_feedforward(true);
507 154 : if (roll_enabled()) {
508 63 : load_gain_set(AxisType::ROLL, orig_roll_rp, orig_roll_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel_radss, orig_roll_fltt, 0.0f, orig_roll_smax, orig_roll_rate_rads);
509 : }
510 154 : if (pitch_enabled()) {
511 60 : load_gain_set(AxisType::PITCH, orig_pitch_rp, orig_pitch_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel_radss, orig_pitch_fltt, 0.0f, orig_pitch_smax, orig_pitch_rate_rads);
512 : }
513 154 : if (yaw_enabled()) {
514 60 : load_gain_set(AxisType::YAW, orig_yaw_rp, orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING, orig_yaw_rd, orig_yaw_rff, orig_yaw_sp, orig_yaw_accel_radss, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax, orig_yaw_rate_rads);
515 : }
516 154 : }
517 :
518 : // load_test_gains - load the to-be-tested gains for a single axis
519 : // called by control_attitude() just before it beings testing a gain
520 147 : void AC_AutoTune_Heli::load_test_gains()
521 : {
522 147 : attitude_control->use_sqrt_controller(true);
523 : float rate_p, rate_i, rate_d, rate_test_max_rads, accel_test_max_radss;
524 147 : switch (axis) {
525 45 : case AxisType::ROLL:
526 :
527 45 : if (tune_type == TuneType::TUNE_CHECK) {
528 1 : rate_test_max_rads = orig_roll_rate_rads;
529 1 : accel_test_max_radss = tune_roll_accel_radss;
530 : } else {
531 : // have attitude controller not perform rate limiting and angle P scaling based on accel max
532 44 : rate_test_max_rads = 0.0;
533 44 : accel_test_max_radss = 0.0;
534 : }
535 45 : if (tune_type == TuneType::ANGLE_P_UP || tune_type == TuneType::TUNE_CHECK) {
536 5 : rate_i = tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL;
537 : } else {
538 : // freeze integrator to hold trim by making i term small during rate controller tuning
539 40 : rate_i = 0.01f * orig_roll_ri;
540 : }
541 45 : if (tune_type == TuneType::MAX_GAINS && !is_zero(tune_roll_rff)) {
542 8 : rate_p = 0.0f;
543 8 : rate_d = 0.0f;
544 : } else {
545 37 : rate_p = tune_roll_rp;
546 37 : rate_d = tune_roll_rd;
547 : }
548 45 : load_gain_set(AxisType::ROLL, rate_p, rate_i, rate_d, tune_roll_rff, tune_roll_sp, accel_test_max_radss, orig_roll_fltt, 0.0f, 0.0f, rate_test_max_rads);
549 45 : break;
550 47 : case AxisType::PITCH:
551 47 : if (tune_type == TuneType::TUNE_CHECK) {
552 1 : rate_test_max_rads = orig_pitch_rate_rads;
553 1 : accel_test_max_radss = tune_pitch_accel_radss;
554 : } else {
555 : // have attitude controller not perform rate limiting and angle P scaling based on accel max
556 46 : rate_test_max_rads = 0.0;
557 46 : accel_test_max_radss = 0.0;
558 : }
559 47 : if (tune_type == TuneType::ANGLE_P_UP || tune_type == TuneType::TUNE_CHECK) {
560 5 : rate_i = tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL;
561 : } else {
562 : // freeze integrator to hold trim by making i term small during rate controller tuning
563 42 : rate_i = 0.01f * orig_pitch_ri;
564 : }
565 47 : if (tune_type == TuneType::MAX_GAINS && !is_zero(tune_pitch_rff)) {
566 6 : rate_p = 0.0f;
567 6 : rate_d = 0.0f;
568 : } else {
569 41 : rate_p = tune_pitch_rp;
570 41 : rate_d = tune_pitch_rd;
571 : }
572 47 : load_gain_set(AxisType::PITCH, rate_p, rate_i, rate_d, tune_pitch_rff, tune_pitch_sp, accel_test_max_radss, orig_pitch_fltt, 0.0f, 0.0f, rate_test_max_rads);
573 47 : break;
574 55 : case AxisType::YAW:
575 : case AxisType::YAW_D:
576 55 : if (tune_type == TuneType::TUNE_CHECK) {
577 1 : rate_test_max_rads = orig_yaw_rate_rads;
578 1 : accel_test_max_radss = tune_yaw_accel_radss;
579 : } else {
580 : // have attitude controller not perform rate limiting and angle P scaling based on accel max
581 54 : rate_test_max_rads = 0.0;
582 54 : accel_test_max_radss = 0.0;
583 : }
584 55 : if (tune_type == TuneType::ANGLE_P_UP || tune_type == TuneType::TUNE_CHECK) {
585 6 : rate_i = tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL;
586 : } else {
587 : // freeze integrator to hold trim by making i term small during rate controller tuning
588 49 : rate_i = 0.01f * orig_yaw_ri;
589 : }
590 55 : load_gain_set(AxisType::YAW, tune_yaw_rp, rate_i, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, accel_test_max_radss, orig_yaw_fltt, tune_yaw_rLPF, 0.0f, rate_test_max_rads);
591 55 : break;
592 : }
593 147 : }
594 :
595 : // load gains
596 360 : void AC_AutoTune_Heli::load_gain_set(AxisType s_axis, float rate_p, float rate_i, float rate_d, float rate_ff, float angle_p, float max_accel_radss, float rate_fltt, float rate_flte, float smax, float max_rate_rads)
597 : {
598 360 : switch (s_axis) {
599 119 : case AxisType::ROLL:
600 119 : attitude_control->get_rate_roll_pid().set_kP(rate_p);
601 119 : attitude_control->get_rate_roll_pid().set_kI(rate_i);
602 119 : attitude_control->get_rate_roll_pid().set_kD(rate_d);
603 119 : attitude_control->get_rate_roll_pid().set_ff(rate_ff);
604 119 : attitude_control->get_rate_roll_pid().set_filt_T_hz(rate_fltt);
605 119 : attitude_control->get_rate_roll_pid().set_slew_limit(smax);
606 119 : attitude_control->get_angle_roll_p().set_kP(angle_p);
607 119 : attitude_control->set_accel_roll_max_radss(max_accel_radss);
608 119 : attitude_control->set_ang_vel_roll_max_rads(max_rate_rads);
609 119 : break;
610 118 : case AxisType::PITCH:
611 118 : attitude_control->get_rate_pitch_pid().set_kP(rate_p);
612 118 : attitude_control->get_rate_pitch_pid().set_kI(rate_i);
613 118 : attitude_control->get_rate_pitch_pid().set_kD(rate_d);
614 118 : attitude_control->get_rate_pitch_pid().set_ff(rate_ff);
615 118 : attitude_control->get_rate_pitch_pid().set_filt_T_hz(rate_fltt);
616 118 : attitude_control->get_rate_pitch_pid().set_slew_limit(smax);
617 118 : attitude_control->get_angle_pitch_p().set_kP(angle_p);
618 118 : attitude_control->set_accel_pitch_max_radss(max_accel_radss);
619 118 : attitude_control->set_ang_vel_pitch_max_rads(max_rate_rads);
620 118 : break;
621 123 : case AxisType::YAW:
622 : case AxisType::YAW_D:
623 123 : attitude_control->get_rate_yaw_pid().set_kP(rate_p);
624 123 : attitude_control->get_rate_yaw_pid().set_kI(rate_i);
625 123 : attitude_control->get_rate_yaw_pid().set_kD(rate_d);
626 123 : attitude_control->get_rate_yaw_pid().set_ff(rate_ff);
627 123 : attitude_control->get_rate_yaw_pid().set_filt_T_hz(rate_fltt);
628 123 : attitude_control->get_rate_yaw_pid().set_slew_limit(smax);
629 123 : attitude_control->get_rate_yaw_pid().set_filt_E_hz(rate_flte);
630 123 : attitude_control->get_angle_yaw_p().set_kP(angle_p);
631 123 : attitude_control->set_accel_yaw_max_radss(max_accel_radss);
632 123 : attitude_control->set_ang_vel_yaw_max_rads(max_rate_rads);
633 123 : break;
634 : }
635 360 : }
636 :
637 : // save_tuning_gains - save the final tuned gains for each axis
638 : // save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position)
639 6 : void AC_AutoTune_Heli::save_tuning_gains()
640 : {
641 : // see if we successfully completed tuning of at least one axis
642 6 : if (axes_completed == 0) {
643 0 : return;
644 : }
645 :
646 6 : if (!attitude_control->get_bf_feedforward()) {
647 0 : attitude_control->bf_feedforward_save(true);
648 0 : attitude_control->save_accel_roll_max_radss(0.0f);
649 0 : attitude_control->save_accel_pitch_max_radss(0.0f);
650 : }
651 :
652 : // sanity check the rate P values
653 6 : if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled()) {
654 3 : load_gain_set(AxisType::ROLL, tune_roll_rp, tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_roll_rd, tune_roll_rff, tune_roll_sp, tune_roll_accel_radss, orig_roll_fltt, 0.0f, orig_roll_smax, orig_roll_rate_rads);
655 : // save rate roll gains
656 3 : attitude_control->get_rate_roll_pid().save_gains();
657 :
658 : // save stabilize roll
659 3 : attitude_control->get_angle_roll_p().save_gains();
660 :
661 : // resave pids to originals in case the autotune is run again
662 3 : orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
663 3 : orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
664 3 : orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
665 3 : orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
666 3 : orig_roll_sp = attitude_control->get_angle_roll_p().kP();
667 3 : orig_roll_accel_radss = attitude_control->get_accel_roll_max_radss();
668 : }
669 :
670 6 : if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled()) {
671 3 : load_gain_set(AxisType::PITCH, tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel_radss, orig_pitch_fltt, 0.0f, orig_pitch_smax, orig_pitch_rate_rads);
672 : // save rate pitch gains
673 3 : attitude_control->get_rate_pitch_pid().save_gains();
674 :
675 : // save stabilize pitch
676 3 : attitude_control->get_angle_pitch_p().save_gains();
677 :
678 : // resave pids to originals in case the autotune is run again
679 3 : orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
680 3 : orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
681 3 : orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
682 3 : orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
683 3 : orig_pitch_sp = attitude_control->get_angle_pitch_p().kP();
684 3 : orig_pitch_accel_radss = attitude_control->get_accel_pitch_max_radss();
685 : }
686 :
687 6 : if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) {
688 2 : load_gain_set(AxisType::YAW, tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel_radss, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_smax, orig_yaw_rate_rads);
689 : // save rate yaw gains
690 2 : attitude_control->get_rate_yaw_pid().save_gains();
691 :
692 : // save stabilize yaw
693 2 : attitude_control->get_angle_yaw_p().save_gains();
694 :
695 : // resave pids to originals in case the autotune is run again
696 2 : orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
697 2 : orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
698 2 : orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
699 2 : orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
700 2 : orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
701 2 : orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
702 2 : orig_yaw_accel_radss = attitude_control->get_accel_yaw_max_radss();
703 : }
704 :
705 : // update GCS and log save gains event
706 6 : update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS);
707 6 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SAVEDGAINS);
708 :
709 6 : reset();
710 : }
711 :
712 : // report final gains for a given axis to GCS
713 11 : void AC_AutoTune_Heli::report_final_gains(AxisType test_axis) const
714 : {
715 11 : switch (test_axis) {
716 4 : case AxisType::ROLL:
717 4 : report_axis_gains("Roll", tune_roll_rp, tune_roll_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_roll_rd, tune_roll_rff, tune_roll_sp, tune_roll_accel_radss);
718 4 : break;
719 4 : case AxisType::PITCH:
720 4 : report_axis_gains("Pitch", tune_pitch_rp, tune_pitch_rff*AUTOTUNE_FFI_RATIO_FINAL, tune_pitch_rd, tune_pitch_rff, tune_pitch_sp, tune_pitch_accel_radss);
721 4 : break;
722 3 : case AxisType::YAW:
723 : case AxisType::YAW_D:
724 3 : report_axis_gains("Yaw", tune_yaw_rp, tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL, tune_yaw_rd, tune_yaw_rff, tune_yaw_sp, tune_yaw_accel_radss);
725 3 : break;
726 : }
727 11 : }
728 :
729 : // report gain formatting helper
730 11 : void AC_AutoTune_Heli::report_axis_gains(const char* axis_string, float rate_P, float rate_I, float rate_D, float rate_ff, float angle_P, float max_accel_radss) const
731 : {
732 11 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s complete", axis_string);
733 11 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s Rate: P:%0.4f, I:%0.4f, D:%0.5f, FF:%0.4f", axis_string, rate_P, rate_I, rate_D, rate_ff);
734 11 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s Angle P:%0.2f, Max Accel:%0.0f", axis_string, angle_P, rad_to_cd(max_accel_radss));
735 11 : }
736 :
737 147 : void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float amplitude, float filt_freq, FreqRespInput freq_resp_input, FreqRespCalcType calc_type, AC_AutoTune_FreqResp::ResponseType resp_type, AC_AutoTune_FreqResp::InputType waveform_input_type)
738 : {
739 147 : test_input_type = waveform_input_type;
740 147 : test_freq_resp_input = freq_resp_input;
741 147 : test_calc_type = calc_type;
742 147 : test_start_freq = start_frq;
743 : //target attitude magnitude
744 147 : tgt_attitude = radians(amplitude);
745 :
746 : // initialize frequency response object
747 147 : if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
748 9 : step_timeout_ms = sweep_time_ms + 500;
749 9 : reset_sweep_variables();
750 9 : curr_test.gain = 0.0f;
751 9 : curr_test.phase = 0.0f;
752 9 : chirp_input.init(0.001f * sweep_time_ms, start_frq / M_2PI, stop_frq / M_2PI, 0.0f, 0.0001f * sweep_time_ms, 0.0f);
753 : } else {
754 138 : if (!is_zero(start_frq)) {
755 : // time limit set by adding the pre calc cycles with the dwell cycles. 500 ms added to account for settling with buffer.
756 138 : step_timeout_ms = (uint32_t) (2000 + ((float)num_dwell_cycles + pre_calc_cycles + 2.0f) * 1000.0f * M_2PI / start_frq);
757 : }
758 138 : chirp_input.init(0.001f * step_timeout_ms, start_frq / M_2PI, stop_frq / M_2PI, 0.0f, 0.0001f * step_timeout_ms, 0.0f);
759 : }
760 :
761 147 : freqresp_tgt.init(test_input_type, resp_type, num_dwell_cycles);
762 147 : freqresp_mtr.init(test_input_type, resp_type, num_dwell_cycles);
763 :
764 147 : dwell_start_time_ms = 0.0f;
765 147 : settle_time = 200;
766 :
767 147 : rotation_rate_filt.set_cutoff_frequency(filt_freq);
768 147 : command_filt.set_cutoff_frequency(filt_freq);
769 147 : target_rate_filt.set_cutoff_frequency(filt_freq);
770 :
771 147 : rotation_rate_filt.reset(0);
772 147 : command_filt.reset(0);
773 147 : target_rate_filt.reset(0);
774 147 : rotation_rate = 0.0f;
775 147 : command_out = 0.0f;
776 147 : filt_target_rate = 0.0f;
777 :
778 : // filter at lower frequency to remove steady state
779 147 : filt_command_reading.set_cutoff_frequency(0.2f * filt_freq);
780 147 : filt_gyro_reading.set_cutoff_frequency(0.05f * filt_freq);
781 147 : filt_tgt_rate_reading.set_cutoff_frequency(0.05f * filt_freq);
782 147 : filt_att_fdbk_from_velxy_cd.set_cutoff_frequency(0.2f * filt_freq);
783 :
784 147 : curr_test_mtr = {};
785 147 : curr_test_tgt = {};
786 147 : cycle_complete_tgt = false;
787 147 : cycle_complete_mtr = false;
788 147 : sweep_complete = false;
789 :
790 147 : }
791 :
792 550704 : void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data)
793 : {
794 550704 : float gyro_reading = 0.0f;
795 550704 : float command_reading = 0.0f;
796 550704 : float tgt_rate_reading = 0.0f;
797 550704 : const uint32_t now_ms = AP_HAL::millis();
798 550704 : float target_angle_cd = 0.0f;
799 550704 : float dwell_freq = test_start_freq;
800 :
801 550704 : float cycle_time_ms = 0;
802 550704 : if (!is_zero(dwell_freq)) {
803 550704 : cycle_time_ms = 1000.0f * M_2PI / dwell_freq;
804 : }
805 :
806 : // body frame calculation of velocity
807 550704 : Vector3f velocity_ned, velocity_bf;
808 550704 : if (ahrs_view->get_velocity_NED(velocity_ned)) {
809 550704 : velocity_bf.x = velocity_ned.x * ahrs_view->cos_yaw() + velocity_ned.y * ahrs_view->sin_yaw();
810 550704 : velocity_bf.y = -velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw();
811 : }
812 :
813 550704 : if (settle_time == 0) {
814 521304 : dwell_freq = chirp_input.get_frequency_rads();
815 521304 : float tgt_att_limited = tgt_attitude;
816 521304 : if (is_positive(dwell_freq)) {
817 521303 : float tgt_att_temp = tgt_attitude;
818 521303 : if (is_positive(rate_max_degs)) {
819 0 : float ang_limit_rate = radians(rate_max_degs) / dwell_freq;
820 0 : tgt_att_temp = MIN(ang_limit_rate, tgt_attitude);
821 : }
822 521303 : if (is_positive(accel_max_degss)) {
823 0 : float ang_limit_accel_radss = radians(accel_max_degss) / sq(dwell_freq);
824 0 : tgt_att_limited = MIN(ang_limit_accel_radss, tgt_att_temp);
825 : } else {
826 521303 : tgt_att_limited = tgt_att_temp;
827 : }
828 : }
829 521304 : target_angle_cd = -chirp_input.update((now_ms - dwell_start_time_ms) * 0.001, degrees(tgt_att_limited) * 100.0f);
830 521304 : dwell_freq = chirp_input.get_frequency_rads();
831 : const Vector2f att_fdbk {
832 521304 : -5730.0f * vel_hold_gain * velocity_bf.y,
833 1042608 : 5730.0f * vel_hold_gain * velocity_bf.x
834 521304 : };
835 521304 : filt_att_fdbk_from_velxy_cd.apply(att_fdbk, AP::scheduler().get_loop_period_s());
836 : } else {
837 29400 : target_angle_cd = 0.0f;
838 29400 : trim_yaw_tgt_reading_cd = (float)attitude_control->get_att_target_euler_cd().z;
839 29400 : trim_yaw_heading_reading_cd = (float)ahrs_view->yaw_sensor;
840 29400 : dwell_start_time_ms = now_ms;
841 29400 : filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f));
842 29400 : settle_time--;
843 : }
844 :
845 : const Vector2f trim_angle_cd {
846 550704 : constrain_float(filt_att_fdbk_from_velxy_cd.get().x, -2000.0f, 2000.0f),
847 550704 : constrain_float(filt_att_fdbk_from_velxy_cd.get().y, -2000.0f, 2000.0f)
848 1101408 : };
849 :
850 550704 : switch (axis) {
851 103649 : case AxisType::ROLL:
852 103649 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(cd_to_rad(target_angle_cd + trim_angle_cd.x), cd_to_rad(trim_angle_cd.y), 0.0f);
853 103649 : command_reading = motors->get_roll();
854 103649 : if (test_calc_type == DRB) {
855 14443 : tgt_rate_reading = cd_to_rad(target_angle_cd);
856 14443 : gyro_reading = cd_to_rad((float)ahrs_view->roll_sensor + trim_angle_cd.x - target_angle_cd);
857 89206 : } else if (test_calc_type == RATE) {
858 79926 : tgt_rate_reading = attitude_control->rate_bf_targets().x;
859 79926 : gyro_reading = ahrs_view->get_gyro().x;
860 : } else {
861 9280 : tgt_rate_reading = cd_to_rad((float)attitude_control->get_att_target_euler_cd().x);
862 9280 : gyro_reading = cd_to_rad((float)ahrs_view->roll_sensor);
863 : }
864 103649 : break;
865 160234 : case AxisType::PITCH:
866 160234 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(cd_to_rad(trim_angle_cd.x), cd_to_rad(target_angle_cd + trim_angle_cd.y), 0.0f);
867 160234 : command_reading = motors->get_pitch();
868 160234 : if (test_calc_type == DRB) {
869 15948 : tgt_rate_reading = cd_to_rad(target_angle_cd);
870 15948 : gyro_reading = cd_to_rad((float)ahrs_view->pitch_sensor + trim_angle_cd.y - target_angle_cd);
871 144286 : } else if (test_calc_type == RATE) {
872 135006 : tgt_rate_reading = attitude_control->rate_bf_targets().y;
873 135006 : gyro_reading = ahrs_view->get_gyro().y;
874 : } else {
875 9280 : tgt_rate_reading = cd_to_rad((float)attitude_control->get_att_target_euler_cd().y);
876 9280 : gyro_reading = cd_to_rad((float)ahrs_view->pitch_sensor);
877 : }
878 160234 : break;
879 286821 : case AxisType::YAW:
880 : case AxisType::YAW_D:
881 286821 : attitude_control->input_euler_angle_roll_pitch_yaw_rad(cd_to_rad(trim_angle_cd.x), cd_to_rad(trim_angle_cd.y), cd_to_rad(wrap_180_cd(trim_yaw_tgt_reading_cd + target_angle_cd)), false);
882 286821 : command_reading = motors->get_yaw();
883 286821 : if (test_calc_type == DRB) {
884 23241 : tgt_rate_reading = cd_to_rad(target_angle_cd);
885 23241 : gyro_reading = cd_to_rad(wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading_cd - target_angle_cd));
886 263580 : } else if (test_calc_type == RATE) {
887 254300 : tgt_rate_reading = attitude_control->rate_bf_targets().z;
888 254300 : gyro_reading = ahrs_view->get_gyro().z;
889 : } else {
890 9280 : tgt_rate_reading = cd_to_rad(wrap_180_cd((float)attitude_control->get_att_target_euler_cd().z - trim_yaw_tgt_reading_cd));
891 9280 : gyro_reading = cd_to_rad((wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading_cd)));
892 : }
893 286821 : break;
894 : }
895 :
896 550704 : if (settle_time == 0) {
897 521451 : filt_command_reading.apply(command_reading, AP::scheduler().get_loop_period_s());
898 521451 : filt_gyro_reading.apply(gyro_reading, AP::scheduler().get_loop_period_s());
899 521451 : filt_tgt_rate_reading.apply(tgt_rate_reading, AP::scheduler().get_loop_period_s());
900 : } else {
901 29253 : filt_command_reading.reset(command_reading);
902 29253 : filt_gyro_reading.reset(gyro_reading);
903 29253 : filt_tgt_rate_reading.reset(tgt_rate_reading);
904 : }
905 :
906 : // looks at gain and phase of input rate to output rate
907 550704 : rotation_rate = rotation_rate_filt.apply((gyro_reading - filt_gyro_reading.get()),
908 550704 : AP::scheduler().get_loop_period_s());
909 550704 : filt_target_rate = target_rate_filt.apply((tgt_rate_reading - filt_tgt_rate_reading.get()),
910 550704 : AP::scheduler().get_loop_period_s());
911 550704 : command_out = command_filt.apply((command_reading - filt_command_reading.get()),
912 550704 : AP::scheduler().get_loop_period_s());
913 :
914 550704 : float dwell_gain_mtr = 0.0f;
915 550704 : float dwell_phase_mtr = 0.0f;
916 550704 : float dwell_gain_tgt = 0.0f;
917 550704 : float dwell_phase_tgt = 0.0f;
918 : // wait for dwell to start before determining gain and phase
919 550704 : if ((float)(now_ms - dwell_start_time_ms) > pre_calc_cycles * cycle_time_ms || (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP && settle_time == 0)) {
920 401130 : freqresp_mtr.update(command_out,command_out,rotation_rate, dwell_freq);
921 401130 : freqresp_tgt.update(command_out,filt_target_rate,rotation_rate, dwell_freq);
922 :
923 401130 : if (freqresp_mtr.is_cycle_complete()) {
924 950 : if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
925 812 : if (is_zero(curr_test_mtr.freq) && freqresp_mtr.get_freq() < test_start_freq) {
926 : // don't set data since captured frequency is below the start frequency
927 : } else {
928 804 : curr_test_mtr.freq = freqresp_mtr.get_freq();
929 804 : curr_test_mtr.gain = freqresp_mtr.get_gain();
930 804 : curr_test_mtr.phase = freqresp_mtr.get_phase();
931 : }
932 : // reset cycle_complete to allow indication of next cycle
933 812 : freqresp_mtr.reset_cycle_complete();
934 : #if HAL_LOGGING_ENABLED
935 : // log sweep data
936 812 : Log_AutoTuneSweep();
937 : #endif
938 : } else {
939 138 : dwell_gain_mtr = freqresp_mtr.get_gain();
940 138 : dwell_phase_mtr = freqresp_mtr.get_phase();
941 138 : cycle_complete_mtr = true;
942 : }
943 : }
944 :
945 401130 : if (freqresp_tgt.is_cycle_complete()) {
946 1145 : if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
947 809 : if (is_zero(curr_test_tgt.freq) && freqresp_tgt.get_freq() < test_start_freq) {
948 : // don't set data since captured frequency is below the start frequency
949 : } else {
950 782 : curr_test_tgt.freq = freqresp_tgt.get_freq();
951 782 : curr_test_tgt.gain = freqresp_tgt.get_gain();
952 782 : curr_test_tgt.phase = freqresp_tgt.get_phase();
953 782 : if (test_calc_type == DRB) {test_accel_max_cdss = freqresp_tgt.get_accel_max();}
954 : }
955 : // reset cycle_complete to allow indication of next cycle
956 809 : freqresp_tgt.reset_cycle_complete();
957 : #if HAL_LOGGING_ENABLED
958 : // log sweep data
959 809 : Log_AutoTuneSweep();
960 : #endif
961 : } else {
962 336 : dwell_gain_tgt = freqresp_tgt.get_gain();
963 336 : dwell_phase_tgt = freqresp_tgt.get_phase();
964 336 : if (test_calc_type == DRB) {test_accel_max_cdss = freqresp_tgt.get_accel_max();}
965 336 : cycle_complete_tgt = true;
966 : }
967 : }
968 :
969 401130 : if (test_freq_resp_input == TARGET) {
970 361029 : if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
971 54487 : curr_test = curr_test_tgt;
972 : } else {
973 306542 : test_data.freq = test_start_freq;
974 306542 : test_data.gain = dwell_gain_tgt;
975 306542 : test_data.phase = dwell_phase_tgt;
976 : }
977 : } else {
978 40101 : if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
979 27243 : curr_test = curr_test_mtr;
980 : } else {
981 12858 : test_data.freq = test_start_freq;
982 12858 : test_data.gain = dwell_gain_mtr;
983 12858 : test_data.phase = dwell_phase_mtr;
984 : }
985 : }
986 : }
987 :
988 : // set sweep data if a frequency sweep is being conducted
989 550704 : if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP && (float)(now_ms - dwell_start_time_ms) > 2.5f * cycle_time_ms) {
990 : // track sweep phase to prevent capturing 180 deg and 270 deg data after phase has wrapped.
991 74185 : if (curr_test_tgt.phase > 180.0f && sweep_tgt.progress == 0) {
992 9 : sweep_tgt.progress = 1;
993 74176 : } else if (curr_test_tgt.phase > 270.0f && sweep_tgt.progress == 1) {
994 6 : sweep_tgt.progress = 2;
995 : }
996 74185 : if (curr_test_tgt.phase <= 160.0f && curr_test_tgt.phase >= 150.0f && sweep_tgt.progress == 0) {
997 4322 : sweep_tgt.ph180 = curr_test_tgt;
998 : }
999 74185 : if (curr_test_tgt.phase <= 250.0f && curr_test_tgt.phase >= 240.0f && sweep_tgt.progress == 1) {
1000 917 : sweep_tgt.ph270 = curr_test_tgt;
1001 : }
1002 74185 : if (curr_test_tgt.gain > sweep_tgt.maxgain.gain) {
1003 189 : sweep_tgt.maxgain = curr_test_tgt;
1004 : }
1005 : // Determine sweep info for motor input to response output
1006 74185 : if (curr_test_mtr.phase > 180.0f && sweep_mtr.progress == 0) {
1007 9 : sweep_mtr.progress = 1;
1008 74176 : } else if (curr_test_mtr.phase > 270.0f && sweep_mtr.progress == 1) {
1009 7 : sweep_mtr.progress = 2;
1010 : }
1011 74185 : if (curr_test_mtr.phase <= 160.0f && curr_test_mtr.phase >= 150.0f && sweep_mtr.progress == 0) {
1012 3378 : sweep_mtr.ph180 = curr_test_mtr;
1013 : }
1014 74185 : if (curr_test_mtr.phase <= 250.0f && curr_test_mtr.phase >= 240.0f && sweep_mtr.progress == 1) {
1015 1398 : sweep_mtr.ph270 = curr_test_mtr;
1016 : }
1017 74185 : if (curr_test_mtr.gain > sweep_mtr.maxgain.gain) {
1018 79 : sweep_mtr.maxgain = curr_test_mtr;
1019 : }
1020 :
1021 74185 : if (now_ms - step_start_time_ms >= sweep_time_ms + 200) {
1022 : // we have passed the maximum stop time
1023 9 : sweep_complete = true;
1024 9 : step = Step::UPDATE_GAINS;
1025 : }
1026 : } else {
1027 476519 : if (now_ms - step_start_time_ms >= step_timeout_ms || (freqresp_tgt.is_cycle_complete() && freqresp_mtr.is_cycle_complete())) {
1028 138 : if (now_ms - step_start_time_ms >= step_timeout_ms) {
1029 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Step time limit exceeded");
1030 : }
1031 138 : cycle_complete_tgt = false;
1032 138 : cycle_complete_tgt = false;
1033 : // we have passed the maximum stop time
1034 138 : step = Step::UPDATE_GAINS;
1035 : }
1036 : }
1037 550704 : }
1038 :
1039 : // update gains for the rate p up tune type
1040 35 : void AC_AutoTune_Heli::updating_rate_p_up_all(AxisType test_axis)
1041 : {
1042 35 : switch (test_axis) {
1043 21 : case AxisType::ROLL:
1044 21 : updating_rate_p_up(tune_roll_rp, curr_data, next_test_freq, max_rate_p);
1045 21 : break;
1046 8 : case AxisType::PITCH:
1047 8 : updating_rate_p_up(tune_pitch_rp, curr_data, next_test_freq, max_rate_p);
1048 8 : break;
1049 6 : case AxisType::YAW:
1050 : case AxisType::YAW_D:
1051 6 : updating_rate_p_up(tune_yaw_rp, curr_data, next_test_freq, max_rate_p);
1052 6 : break;
1053 : }
1054 35 : }
1055 :
1056 : // update gains for the rate d up tune type
1057 40 : void AC_AutoTune_Heli::updating_rate_d_up_all(AxisType test_axis)
1058 : {
1059 40 : switch (test_axis) {
1060 8 : case AxisType::ROLL:
1061 8 : updating_rate_d_up(tune_roll_rd, curr_data, next_test_freq, max_rate_d);
1062 8 : break;
1063 20 : case AxisType::PITCH:
1064 20 : updating_rate_d_up(tune_pitch_rd, curr_data, next_test_freq, max_rate_d);
1065 20 : break;
1066 12 : case AxisType::YAW:
1067 : case AxisType::YAW_D:
1068 12 : updating_rate_d_up(tune_yaw_rd, curr_data, next_test_freq, max_rate_d);
1069 12 : break;
1070 : }
1071 40 : }
1072 :
1073 : // update gains for the rate ff up tune type
1074 35 : void AC_AutoTune_Heli::updating_rate_ff_up_all(AxisType test_axis)
1075 : {
1076 35 : switch (test_axis) {
1077 3 : case AxisType::ROLL:
1078 3 : updating_rate_ff_up(tune_roll_rff, curr_data, next_test_freq);
1079 3 : break;
1080 8 : case AxisType::PITCH:
1081 8 : updating_rate_ff_up(tune_pitch_rff, curr_data, next_test_freq);
1082 8 : break;
1083 24 : case AxisType::YAW:
1084 : case AxisType::YAW_D:
1085 24 : updating_rate_ff_up(tune_yaw_rff, curr_data, next_test_freq);
1086 : // TODO make FF updating routine determine when to set rff gain to zero based on A/C response
1087 24 : if (tune_yaw_rff <= AUTOTUNE_RFF_MIN && success_counter == AUTOTUNE_SUCCESS_COUNT) {
1088 0 : tune_yaw_rff = 0.0f;
1089 : }
1090 24 : break;
1091 : }
1092 35 : }
1093 :
1094 : // update gains for the angle p up tune type
1095 13 : void AC_AutoTune_Heli::updating_angle_p_up_all(AxisType test_axis)
1096 : {
1097 13 : attitude_control->bf_feedforward(orig_bf_feedforward);
1098 :
1099 : // sweep doesn't require gain update so return immediately after setting next test freq
1100 : // determine next_test_freq for dwell testing
1101 13 : if (sweep_complete && input_type == AC_AutoTune_FreqResp::InputType::SWEEP){
1102 : // if a max gain frequency was found then set the start of the dwells to that freq otherwise start at min frequency
1103 3 : if (!is_zero(sweep_tgt.maxgain.freq)) {
1104 3 : next_test_freq = constrain_float(sweep_tgt.maxgain.freq, min_sweep_freq, max_sweep_freq);
1105 3 : freq_max = next_test_freq;
1106 3 : sp_prev_gain = sweep_tgt.maxgain.gain;
1107 3 : phase_max = sweep_tgt.maxgain.phase;
1108 3 : found_max_gain_freq = true;
1109 : } else {
1110 0 : next_test_freq = min_sweep_freq;
1111 : }
1112 3 : return;
1113 : }
1114 :
1115 10 : switch (test_axis) {
1116 3 : case AxisType::ROLL:
1117 3 : updating_angle_p_up(tune_roll_sp, curr_data, next_test_freq);
1118 3 : break;
1119 3 : case AxisType::PITCH:
1120 3 : updating_angle_p_up(tune_pitch_sp, curr_data, next_test_freq);
1121 3 : break;
1122 4 : case AxisType::YAW:
1123 : case AxisType::YAW_D:
1124 4 : updating_angle_p_up(tune_yaw_sp, curr_data, next_test_freq);
1125 4 : break;
1126 : }
1127 : }
1128 :
1129 : // update gains for the max gain tune type
1130 21 : void AC_AutoTune_Heli::updating_max_gains_all(AxisType test_axis)
1131 : {
1132 : // sweep doesn't require gain update so return immediately after setting next test freq
1133 : // determine next_test_freq for dwell testing
1134 21 : if (sweep_complete && input_type == AC_AutoTune_FreqResp::InputType::SWEEP) {
1135 : // if a max gain frequency was found then set the start of the dwells to that freq otherwise start at min frequency
1136 3 : if (!is_zero(sweep_mtr.ph180.freq)) {
1137 3 : next_test_freq = constrain_float(sweep_mtr.ph180.freq, min_sweep_freq, max_sweep_freq);
1138 3 : reset_maxgains_update_gain_variables();
1139 : } else {
1140 0 : next_test_freq = min_sweep_freq;
1141 : }
1142 3 : return;
1143 : }
1144 :
1145 18 : switch (test_axis) {
1146 7 : case AxisType::ROLL:
1147 7 : updating_max_gains(curr_data, next_test_freq, max_rate_p, max_rate_d, tune_roll_rp, tune_roll_rd);
1148 7 : break;
1149 5 : case AxisType::PITCH:
1150 5 : updating_max_gains(curr_data, next_test_freq, max_rate_p, max_rate_d, tune_pitch_rp, tune_pitch_rd);
1151 5 : break;
1152 6 : case AxisType::YAW:
1153 : case AxisType::YAW_D:
1154 6 : updating_max_gains(curr_data, next_test_freq, max_rate_p, max_rate_d, tune_yaw_rp, tune_yaw_rd);
1155 : // rate P and rate D can be non zero for yaw and need to be included in the max allowed gain
1156 6 : if (!is_zero(max_rate_p.max_allowed) && success_counter == AUTOTUNE_SUCCESS_COUNT) {
1157 1 : max_rate_p.max_allowed += tune_yaw_rp;
1158 : }
1159 6 : if (!is_zero(max_rate_d.max_allowed) && success_counter == AUTOTUNE_SUCCESS_COUNT) {
1160 1 : max_rate_d.max_allowed += tune_yaw_rd;
1161 : }
1162 6 : break;
1163 : }
1164 : }
1165 :
1166 : // set gains post tune for the tune type
1167 18 : void AC_AutoTune_Heli::set_tuning_gains_with_backoff(AxisType test_axis)
1168 : {
1169 18 : switch (tune_type) {
1170 3 : case TuneType::RATE_D_UP:
1171 : switch (test_axis) {
1172 1 : case AxisType::ROLL:
1173 1 : tune_roll_rd = MAX(0.0f, tune_roll_rd * AUTOTUNE_RD_BACKOFF);
1174 1 : break;
1175 1 : case AxisType::PITCH:
1176 1 : tune_pitch_rd = MAX(0.0f, tune_pitch_rd * AUTOTUNE_RD_BACKOFF);
1177 1 : break;
1178 1 : case AxisType::YAW:
1179 : case AxisType::YAW_D:
1180 1 : tune_yaw_rd = MAX(0.0f, tune_yaw_rd * AUTOTUNE_RD_BACKOFF);
1181 1 : break;
1182 : }
1183 3 : break;
1184 3 : case TuneType::RATE_P_UP:
1185 : switch (test_axis) {
1186 1 : case AxisType::ROLL:
1187 1 : tune_roll_rp = MAX(0.0f, tune_roll_rp * AUTOTUNE_RP_BACKOFF);
1188 1 : break;
1189 1 : case AxisType::PITCH:
1190 1 : tune_pitch_rp = MAX(0.0f, tune_pitch_rp * AUTOTUNE_RP_BACKOFF);
1191 1 : break;
1192 1 : case AxisType::YAW:
1193 : case AxisType::YAW_D:
1194 1 : tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF);
1195 1 : break;
1196 : }
1197 3 : break;
1198 3 : case TuneType::ANGLE_P_UP:
1199 : switch (test_axis) {
1200 1 : case AxisType::ROLL:
1201 1 : tune_roll_sp = MAX(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF);
1202 1 : break;
1203 1 : case AxisType::PITCH:
1204 1 : tune_pitch_sp = MAX(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF);
1205 1 : break;
1206 1 : case AxisType::YAW:
1207 : case AxisType::YAW_D:
1208 1 : tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF);
1209 1 : break;
1210 : }
1211 3 : break;
1212 3 : case TuneType::RATE_FF_UP:
1213 3 : break;
1214 6 : default:
1215 6 : break;
1216 : }
1217 18 : }
1218 :
1219 : // updating_rate_ff_up - adjust FF to ensure the target is reached
1220 : // FF is adjusted until rate requested is achieved
1221 35 : void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, sweep_info &test_data, float &next_freq)
1222 : {
1223 35 : float tune_tgt = 0.95;
1224 35 : float tune_tol = 0.025;
1225 35 : next_freq = test_data.freq;
1226 :
1227 : // handle axes where FF gain is initially zero
1228 35 : if (test_data.gain < tune_tgt - tune_tol && !is_positive(tune_ff)) {
1229 0 : tune_ff = 0.03f;
1230 0 : return;
1231 : }
1232 :
1233 35 : if (test_data.gain < tune_tgt - 0.2 || test_data.gain > tune_tgt + 0.2) {
1234 3 : tune_ff = tune_ff * constrain_float(tune_tgt / test_data.gain, 0.75, 1.25); //keep changes less than 25%
1235 32 : } else if (test_data.gain < tune_tgt - 0.1 || test_data.gain > tune_tgt + 0.1) {
1236 1 : if (test_data.gain < tune_tgt - 0.1) {
1237 1 : tune_ff *= 1.05;
1238 : } else {
1239 0 : tune_ff *= 0.95;
1240 : }
1241 31 : } else if (test_data.gain < tune_tgt - tune_tol || test_data.gain > tune_tgt + tune_tol) {
1242 28 : if (test_data.gain < tune_tgt - tune_tol) {
1243 28 : tune_ff *= 1.02;
1244 : } else {
1245 0 : tune_ff *= 0.98;
1246 : }
1247 3 : } else if (test_data.gain >= tune_tgt - tune_tol && test_data.gain <= tune_tgt + tune_tol) {
1248 3 : success_counter = AUTOTUNE_SUCCESS_COUNT;
1249 : // reset next_freq for next test
1250 3 : next_freq = 0.0f;
1251 3 : tune_ff = constrain_float(tune_ff,0.0f,1.0f);
1252 : }
1253 : }
1254 :
1255 : // updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not
1256 : // exceed max response gain. A phase of 161 deg is used to conduct the tuning as this phase is where analytically
1257 : // max gain to 6db gain margin is determined for a unity feedback controller.
1258 35 : void AC_AutoTune_Heli::updating_rate_p_up(float &tune_p, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p)
1259 : {
1260 35 : float test_freq_incr = 0.25f * M_2PI;
1261 35 : next_freq = test_data.freq;
1262 :
1263 : sweep_info data_at_ph161;
1264 : float sugg_freq;
1265 35 : if (freq_search_for_phase(test_data, 161.0f, test_freq_incr, data_at_ph161, sugg_freq)) {
1266 16 : if (data_at_ph161.gain < max_resp_gain && tune_p < 0.6f * max_gain_p.max_allowed) {
1267 13 : tune_p += 0.05f * max_gain_p.max_allowed;
1268 13 : next_freq = data_at_ph161.freq;
1269 : } else {
1270 3 : success_counter = AUTOTUNE_SUCCESS_COUNT;
1271 : // reset next_freq for next test
1272 3 : next_freq = 0.0f;
1273 3 : tune_p -= 0.05f * max_gain_p.max_allowed;
1274 3 : tune_p = constrain_float(tune_p,0.0f,0.6f * max_gain_p.max_allowed);
1275 : }
1276 : } else {
1277 19 : next_freq = sugg_freq;
1278 : }
1279 35 : }
1280 :
1281 : // updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response
1282 : // gain is at a minimum. A phase of 161 deg is used to conduct the tuning as this phase is where analytically
1283 : // max gain to 6db gain margin is determined for a unity feedback controller.
1284 40 : void AC_AutoTune_Heli::updating_rate_d_up(float &tune_d, sweep_info &test_data, float &next_freq, max_gain_data &max_gain_d)
1285 : {
1286 40 : float test_freq_incr = 0.25f * M_2PI; // set for 1/4 hz increments
1287 40 : next_freq = test_data.freq;
1288 :
1289 : sweep_info data_at_ph161;
1290 : float sugg_freq;
1291 40 : if (freq_search_for_phase(test_data, 161.0f, test_freq_incr, data_at_ph161, sugg_freq)) {
1292 18 : if ((data_at_ph161.gain < rd_prev_gain || is_zero(rd_prev_gain)) && tune_d < 0.6f * max_gain_d.max_allowed) {
1293 15 : tune_d += 0.05f * max_gain_d.max_allowed;
1294 15 : rd_prev_gain = data_at_ph161.gain;
1295 15 : next_freq = data_at_ph161.freq;
1296 : } else {
1297 3 : success_counter = AUTOTUNE_SUCCESS_COUNT;
1298 : // reset next freq and rd_prev_gain for next test
1299 3 : next_freq = 0;
1300 3 : rd_prev_gain = 0.0f;
1301 3 : tune_d -= 0.05f * max_gain_d.max_allowed;
1302 3 : tune_d = constrain_float(tune_d,0.0f,0.6f * max_gain_d.max_allowed);
1303 : }
1304 : } else {
1305 22 : next_freq = sugg_freq;
1306 : }
1307 40 : }
1308 :
1309 : // updating_angle_p_up - determines maximum angle p gain for pitch and roll. This is accomplished by determining the frequency
1310 : // for the maximum response gain that is the disturbance rejection peak.
1311 10 : void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, sweep_info &test_data, float &next_freq)
1312 : {
1313 10 : float test_freq_incr = 0.5f * M_2PI;
1314 10 : float gain_incr = 0.5f;
1315 :
1316 10 : if (is_zero(test_data.phase)) {
1317 : // bad test point. increase slightly in hope of getting better result
1318 0 : next_freq += 0.5f * test_freq_incr;
1319 0 : return;
1320 : }
1321 :
1322 10 : if (!found_max_gain_freq) {
1323 0 : if (test_data.gain > max_resp_gain && tune_p > AUTOTUNE_SP_MIN) {
1324 : // exceeded max response gain already, reduce tuning gain to remain under max response gain
1325 0 : tune_p -= gain_incr;
1326 : // force counter to stay on frequency
1327 0 : next_freq = test_data.freq;
1328 0 : return;
1329 0 : } else if (test_data.gain > max_resp_gain && tune_p <= AUTOTUNE_SP_MIN) {
1330 : // exceeded max response gain at the minimum allowable tuning gain. terminate testing.
1331 0 : tune_p = AUTOTUNE_SP_MIN;
1332 0 : success_counter = AUTOTUNE_SUCCESS_COUNT;
1333 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
1334 0 : } else if (test_data.gain > sp_prev_gain) {
1335 0 : freq_max = test_data.freq;
1336 0 : phase_max = test_data.phase;
1337 0 : sp_prev_gain = test_data.gain;
1338 0 : next_freq = test_data.freq + test_freq_incr;
1339 0 : return;
1340 : // Gain is expected to continue decreasing past gain peak. declare max gain freq found and refine search.
1341 0 : } else if (test_data.gain < 0.95f * sp_prev_gain) {
1342 0 : found_max_gain_freq = true;
1343 0 : next_freq = freq_max + 0.5 * test_freq_incr;
1344 0 : return;
1345 : } else {
1346 0 : next_freq = test_data.freq + test_freq_incr;
1347 0 : return;
1348 : }
1349 : }
1350 :
1351 : // refine peak
1352 10 : if (!found_peak) {
1353 : // look at frequency above max gain freq found
1354 3 : if (test_data.freq > freq_max && test_data.gain > sp_prev_gain) {
1355 : // found max at frequency greater than initial max gain frequency
1356 0 : found_peak = true;
1357 3 : } else if (test_data.freq > freq_max && test_data.gain < sp_prev_gain) {
1358 : // look at frequency below initial max gain frequency
1359 0 : next_freq = test_data.freq - 0.5 * test_freq_incr;
1360 0 : return;
1361 3 : } else if (test_data.freq < freq_max && test_data.gain > sp_prev_gain) {
1362 : // found max at frequency less than initial max gain frequency
1363 0 : found_peak = true;
1364 : } else {
1365 3 : found_peak = true;
1366 3 : test_data.freq = freq_max;
1367 3 : test_data.gain = sp_prev_gain;
1368 : }
1369 3 : sp_prev_gain = test_data.gain;
1370 : }
1371 :
1372 : // start increasing gain
1373 10 : if (found_max_gain_freq && found_peak) {
1374 10 : if (test_data.gain < max_resp_gain && tune_p < AUTOTUNE_SP_MAX) {
1375 7 : tune_p += gain_incr;
1376 7 : next_freq = test_data.freq;
1377 7 : if (tune_p >= AUTOTUNE_SP_MAX) {
1378 0 : tune_p = AUTOTUNE_SP_MAX;
1379 0 : success_counter = AUTOTUNE_SUCCESS_COUNT;
1380 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT);
1381 : }
1382 7 : sp_prev_gain = test_data.gain;
1383 3 : } else if (test_data.gain > 1.1f * max_resp_gain && tune_p > AUTOTUNE_SP_MIN) {
1384 0 : tune_p -= gain_incr;
1385 : } else {
1386 : // adjust tuning gain so max response gain is not exceeded
1387 3 : if (sp_prev_gain < max_resp_gain && test_data.gain > max_resp_gain) {
1388 3 : float adj_factor = (max_resp_gain - test_data.gain) / (test_data.gain - sp_prev_gain);
1389 3 : tune_p = tune_p + gain_incr * adj_factor;
1390 : }
1391 3 : success_counter = AUTOTUNE_SUCCESS_COUNT;
1392 : }
1393 : }
1394 10 : if (success_counter == AUTOTUNE_SUCCESS_COUNT) {
1395 3 : next_freq = 0.0f; //initializes next test that uses dwell test
1396 3 : sweep_complete = false;
1397 3 : reset_sweep_variables();
1398 : }
1399 : }
1400 :
1401 : // updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur. This uses the frequency
1402 : // response of motor class input to rate response to determine the max allowable gain for rate P gain. A phase of 161 deg is used to
1403 : // determine analytically the max gain to 6db gain margin for a unity feedback controller. Since acceleration can be more noisy, the
1404 : // response of the motor class input to rate response to determine the max allowable gain for rate D gain. A phase of 251 deg is used
1405 : // to determine analytically the max gain to 6db gain margin for a unity feedback controller.
1406 18 : void AC_AutoTune_Heli::updating_max_gains(sweep_info &test_data, float &next_freq, max_gain_data &max_gain_p, max_gain_data &max_gain_d, float &tune_p, float &tune_d)
1407 : {
1408 18 : float test_freq_incr = 0.5f * M_2PI;
1409 18 : next_freq = test_data.freq;
1410 :
1411 : sweep_info data_at_phase;
1412 : float sugg_freq;
1413 18 : if (!found_max_p) {
1414 8 : if (freq_search_for_phase(test_data, 161.0f, test_freq_incr, data_at_phase, sugg_freq)) {
1415 3 : max_gain_p.freq = data_at_phase.freq;
1416 3 : max_gain_p.gain = data_at_phase.gain;
1417 3 : max_gain_p.phase = data_at_phase.phase;
1418 3 : max_gain_p.max_allowed = powf(10.0f,-1 * (log10f(max_gain_p.gain) * 20.0f + 2.42) / 20.0f);
1419 : // limit max gain allowed to be no more than 2x the max p gain limit to keep initial gains bounded
1420 3 : max_gain_p.max_allowed = constrain_float(max_gain_p.max_allowed, 0.0f, 2.0f * AUTOTUNE_RP_MAX);
1421 3 : found_max_p = true;
1422 3 : if (!is_zero(sweep_mtr.ph270.freq)) {
1423 3 : next_freq = sweep_mtr.ph270.freq;
1424 : } else {
1425 0 : next_freq = data_at_phase.freq;
1426 : }
1427 : } else {
1428 5 : next_freq = sugg_freq;
1429 : }
1430 10 : } else if (!found_max_d) {
1431 10 : if (freq_search_for_phase(test_data, 251.0f, test_freq_incr, data_at_phase, sugg_freq)) {
1432 3 : max_gain_d.freq = data_at_phase.freq;
1433 3 : max_gain_d.gain = data_at_phase.gain;
1434 3 : max_gain_d.phase = data_at_phase.phase;
1435 3 : max_gain_d.max_allowed = powf(10.0f,-1 * (log10f(max_gain_d.freq * max_gain_d.gain) * 20.0f + 2.42) / 20.0f);
1436 : // limit max gain allowed to be no more than 2x the max d gain limit to keep initial gains bounded
1437 3 : max_gain_d.max_allowed = constrain_float(max_gain_d.max_allowed, 0.0f, 2.0f * AUTOTUNE_RD_MAX);
1438 3 : found_max_d = true;
1439 : } else {
1440 7 : next_freq = sugg_freq;
1441 : }
1442 : }
1443 :
1444 18 : if (found_max_p && found_max_d) {
1445 3 : success_counter = AUTOTUNE_SUCCESS_COUNT;
1446 : // reset variables for next test
1447 3 : next_freq = 0.0f; //initializes next test that uses dwell test
1448 3 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Max rate P freq=%f gain=%f", (double)(max_rate_p.freq), (double)(max_rate_p.gain));
1449 3 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_p=%f", (double)(max_rate_p.phase), (double)(max_rate_p.max_allowed));
1450 3 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Max Rate D freq=%f gain=%f", (double)(max_rate_d.freq), (double)(max_rate_d.gain));
1451 3 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: ph=%f rate_d=%f", (double)(max_rate_d.phase), (double)(max_rate_d.max_allowed));
1452 : }
1453 :
1454 18 : }
1455 :
1456 0 : float AC_AutoTune_Heli::target_angle_max_rp_cd() const
1457 : {
1458 0 : return AUTOTUNE_ANGLE_TARGET_MAX_RP_CD;
1459 : }
1460 :
1461 0 : float AC_AutoTune_Heli::target_angle_max_y_cd() const
1462 : {
1463 0 : return AUTOTUNE_ANGLE_TARGET_MAX_Y_CD;
1464 : }
1465 :
1466 0 : float AC_AutoTune_Heli::target_angle_min_rp_cd() const
1467 : {
1468 0 : return AUTOTUNE_ANGLE_TARGET_MIN_RP_CD;
1469 : }
1470 :
1471 0 : float AC_AutoTune_Heli::target_angle_min_y_cd() const
1472 : {
1473 0 : return AUTOTUNE_ANGLE_TARGET_MIN_Y_CD;
1474 : }
1475 :
1476 550704 : float AC_AutoTune_Heli::angle_lim_max_rp_cd() const
1477 : {
1478 550704 : return AUTOTUNE_ANGLE_MAX_RP_CD;
1479 : }
1480 :
1481 550704 : float AC_AutoTune_Heli::angle_lim_neg_rpy_cd() const
1482 : {
1483 550704 : return AUTOTUNE_ANGLE_NEG_RPY_CD;
1484 : }
1485 :
1486 : // freq_search_for_phase: general search strategy for specified phase. interpolation done once specified phase has been bounded.
1487 93 : bool AC_AutoTune_Heli::freq_search_for_phase(sweep_info test, float desired_phase, float freq_incr, sweep_info &est_data, float &new_freq)
1488 : {
1489 93 : new_freq = test.freq;
1490 93 : float phase_delta = 20.0f; // delta from desired phase below and above which full steps are taken
1491 93 : if (is_zero(test.phase)) {
1492 : // bad test point. increase slightly in hope of getting better result
1493 0 : new_freq += 0.1f * freq_incr;
1494 0 : return false;
1495 : }
1496 :
1497 : // test to see if desired phase is bounded with a 0.5 freq_incr delta in freq
1498 93 : float freq_delta = fabsf(prev_test.freq - test.freq);
1499 93 : if (test.phase > desired_phase && prev_test.phase < desired_phase && freq_delta < 0.75f * freq_incr && is_positive(prev_test.freq)) {
1500 22 : est_data.freq = linear_interpolate(prev_test.freq,test.freq,desired_phase,prev_test.phase,test.phase);
1501 22 : est_data.gain = linear_interpolate(prev_test.gain,test.gain,desired_phase,prev_test.phase,test.phase);
1502 22 : est_data.phase = desired_phase;
1503 22 : prev_test = {};
1504 22 : return true;
1505 71 : } else if (test.phase < desired_phase && prev_test.phase > desired_phase && freq_delta < 0.75f * freq_incr && is_positive(prev_test.freq)) {
1506 18 : est_data.freq = linear_interpolate(test.freq,prev_test.freq,desired_phase,test.phase,prev_test.phase);
1507 18 : est_data.gain = linear_interpolate(test.gain,prev_test.gain,desired_phase,test.phase,prev_test.phase);
1508 18 : est_data.phase = desired_phase;
1509 18 : prev_test = {};
1510 18 : return true;
1511 : }
1512 :
1513 53 : if (test.phase < desired_phase - phase_delta) {
1514 0 : new_freq += freq_incr;
1515 53 : } else if (test.phase > desired_phase + phase_delta) {
1516 0 : new_freq -= freq_incr;
1517 53 : } else if (test.phase >= desired_phase - phase_delta && test.phase < desired_phase) {
1518 26 : new_freq += 0.5f * freq_incr;
1519 27 : } else if (test.phase <= desired_phase + phase_delta && test.phase >= desired_phase) {
1520 27 : new_freq -= 0.5f * freq_incr;
1521 : }
1522 53 : prev_test = test;
1523 53 : return false;
1524 : }
1525 :
1526 : #if HAL_LOGGING_ENABLED
1527 : // log autotune summary data
1528 147 : void AC_AutoTune_Heli::Log_AutoTune()
1529 : {
1530 :
1531 147 : switch (axis) {
1532 45 : case AxisType::ROLL:
1533 45 : Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_roll_rff, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max_cdss);
1534 45 : break;
1535 47 : case AxisType::PITCH:
1536 47 : Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_pitch_rff, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max_cdss);
1537 47 : break;
1538 55 : case AxisType::YAW:
1539 : case AxisType::YAW_D:
1540 55 : Log_Write_AutoTune(axis, tune_type, curr_data.freq, curr_data.gain, curr_data.phase, tune_yaw_rff, tune_yaw_rp, tune_yaw_rd, tune_yaw_sp, test_accel_max_cdss);
1541 55 : break;
1542 : }
1543 147 : }
1544 :
1545 : // log autotune time history results for command, angular rate, and attitude
1546 550704 : void AC_AutoTune_Heli::Log_AutoTuneDetails()
1547 : {
1548 550704 : if (tune_type == TuneType::ANGLE_P_UP || tune_type == TuneType::TUNE_CHECK) {
1549 81472 : Log_Write_AutoTuneDetails(command_out, 0.0f, 0.0f, filt_target_rate, rotation_rate);
1550 : } else {
1551 469232 : Log_Write_AutoTuneDetails(command_out, filt_target_rate, rotation_rate, 0.0f, 0.0f);
1552 : }
1553 550704 : }
1554 :
1555 : // log autotune frequency response data
1556 1621 : void AC_AutoTune_Heli::Log_AutoTuneSweep()
1557 : {
1558 1621 : Log_Write_AutoTuneSweep(curr_test_mtr.freq, curr_test_mtr.gain, curr_test_mtr.phase,curr_test_tgt.freq, curr_test_tgt.gain, curr_test_tgt.phase);
1559 1621 : }
1560 :
1561 : // @LoggerMessage: ATNH
1562 : // @Description: Heli AutoTune
1563 : // @Vehicles: Copter
1564 : // @Field: TimeUS: Time since system startup
1565 : // @Field: Axis: which axis is currently being tuned
1566 : // @Field: TuneStep: step in autotune process
1567 : // @Field: Freq: target dwell frequency
1568 : // @Field: Gain: measured gain of dwell
1569 : // @Field: Phase: measured phase of dwell
1570 : // @Field: RFF: new rate gain FF term
1571 : // @Field: RP: new rate gain P term
1572 : // @Field: RD: new rate gain D term
1573 : // @Field: SP: new angle P term
1574 : // @Field: ACC: new max accel term
1575 :
1576 : // Write an Autotune summary data packet
1577 147 : void AC_AutoTune_Heli::Log_Write_AutoTune(AxisType _axis, TuneType tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp, float max_accel_radss)
1578 : {
1579 294 : AP::logger().Write(
1580 : "ATNH",
1581 : "TimeUS,Axis,TuneStep,Freq,Gain,Phase,RFF,RP,RD,SP,ACC",
1582 : "s--E-d----e",
1583 : "F--000----0",
1584 : "QBBffffffff",
1585 : AP_HAL::micros64(),
1586 147 : (uint8_t)axis,
1587 : tune_step,
1588 : dwell_freq,
1589 : meas_gain,
1590 : meas_phase,
1591 : new_gain_rff,
1592 : new_gain_rp,
1593 : new_gain_rd,
1594 : new_gain_sp,
1595 147 : degrees(max_accel_radss));
1596 147 : }
1597 :
1598 : // Write an Autotune detailed data packet
1599 550704 : void AC_AutoTune_Heli::Log_Write_AutoTuneDetails(float motor_cmd, float tgt_rate_rads, float rate_rads, float tgt_ang_rad, float ang_rad)
1600 : {
1601 : // @LoggerMessage: ATDH
1602 : // @Description: Heli AutoTune data packet
1603 : // @Vehicles: Copter
1604 : // @Field: TimeUS: Time since system startup
1605 : // @Field: Cmd: current motor command
1606 : // @Field: TRate: current target angular rate
1607 : // @Field: Rate: current angular rate
1608 : // @Field: TAng: current target angle
1609 : // @Field: Ang: current angle
1610 1101408 : AP::logger().WriteStreaming(
1611 : "ATDH",
1612 : "TimeUS,Cmd,TRate,Rate,TAng,Ang",
1613 : "s-kkdd",
1614 : "F00000",
1615 : "Qfffff",
1616 : AP_HAL::micros64(),
1617 : motor_cmd,
1618 550704 : tgt_rate_rads*57.3,
1619 550704 : rate_rads*57.3f,
1620 550704 : tgt_ang_rad*57.3,
1621 550704 : ang_rad*57.3f);
1622 550704 : }
1623 :
1624 : // Write an Autotune frequency response data packet
1625 1621 : void AC_AutoTune_Heli::Log_Write_AutoTuneSweep(float freq_mtr, float gain_mtr, float phase_mtr, float freq_tgt, float gain_tgt, float phase_tgt)
1626 : {
1627 : // @LoggerMessage: ATSH
1628 : // @Description: Heli AutoTune Sweep packet
1629 : // @Vehicles: Copter
1630 : // @Field: TimeUS: Time since system startup
1631 : // @Field: freq_m: current frequency for motor input to rate
1632 : // @Field: gain_m: current response gain for motor input to rate
1633 : // @Field: phase_m: current response phase for motor input to rate
1634 : // @Field: freq_t: current frequency for target rate to rate
1635 : // @Field: gain_t: current response gain for target rate to rate
1636 : // @Field: phase_t: current response phase for target rate to rate
1637 1621 : AP::logger().WriteStreaming(
1638 : "ATSH",
1639 : "TimeUS,freq_m,gain_m,phase_m,freq_t,gain_t,phase_t",
1640 : "sE-dE-d",
1641 : "F000000",
1642 : "Qffffff",
1643 : AP_HAL::micros64(),
1644 : freq_mtr,
1645 : gain_mtr,
1646 : phase_mtr,
1647 : freq_tgt,
1648 : gain_tgt,
1649 : phase_tgt);
1650 1621 : }
1651 : #endif // HAL_LOGGING_ENABLED
1652 :
1653 : // reset the test variables for each vehicle
1654 0 : void AC_AutoTune_Heli::reset_vehicle_test_variables()
1655 : {
1656 : // reset dwell test variables if sweep was interrupted in order to restart sweep
1657 0 : if (!is_equal(start_freq, stop_freq)) {
1658 0 : start_freq = 0.0f;
1659 0 : stop_freq = 0.0f;
1660 0 : next_test_freq = 0.0f;
1661 0 : sweep_complete = false;
1662 : }
1663 0 : }
1664 :
1665 : // reset the update gain variables for heli
1666 11 : void AC_AutoTune_Heli::reset_update_gain_variables()
1667 : {
1668 : // reset max gain variables
1669 11 : reset_maxgains_update_gain_variables();
1670 :
1671 : // reset rd_up variables
1672 11 : rd_prev_gain = 0.0f;
1673 :
1674 : // reset sp_up variables
1675 11 : phase_max = 0.0f;
1676 11 : freq_max = 0.0f;
1677 11 : sp_prev_gain = 0.0f;
1678 11 : found_max_gain_freq = false;
1679 11 : found_peak = false;
1680 :
1681 11 : }
1682 :
1683 : // reset the max_gains update gain variables
1684 14 : void AC_AutoTune_Heli::reset_maxgains_update_gain_variables()
1685 : {
1686 14 : max_rate_p = {};
1687 14 : max_rate_d = {};
1688 :
1689 14 : found_max_p = false;
1690 14 : found_max_d = false;
1691 :
1692 14 : }
1693 :
1694 : // reset the max_gains update gain variables
1695 12 : void AC_AutoTune_Heli::reset_sweep_variables()
1696 : {
1697 12 : sweep_tgt.ph180 = {};
1698 12 : sweep_tgt.ph270 = {};
1699 12 : sweep_tgt.maxgain = {};
1700 12 : sweep_tgt.progress = 0;
1701 :
1702 12 : sweep_mtr.ph180 = {};
1703 12 : sweep_mtr.ph270 = {};
1704 12 : sweep_mtr.maxgain = {};
1705 :
1706 12 : sweep_mtr.progress = 0;
1707 :
1708 12 : }
1709 :
1710 : // set the tuning test sequence
1711 18 : void AC_AutoTune_Heli::set_tune_sequence()
1712 : {
1713 18 : uint8_t seq_cnt = 0;
1714 :
1715 18 : if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_VFF) {
1716 5 : tune_seq[seq_cnt] = TuneType::RATE_FF_UP;
1717 5 : seq_cnt++;
1718 : }
1719 18 : if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_RATE_D) {
1720 6 : tune_seq[seq_cnt] = TuneType::MAX_GAINS;
1721 6 : seq_cnt++;
1722 6 : tune_seq[seq_cnt] = TuneType::RATE_D_UP;
1723 6 : seq_cnt++;
1724 6 : tune_seq[seq_cnt] = TuneType::RATE_P_UP;
1725 6 : seq_cnt++;
1726 : }
1727 18 : if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_ANGLE_P) {
1728 5 : tune_seq[seq_cnt] = TuneType::ANGLE_P_UP;
1729 5 : seq_cnt++;
1730 : }
1731 18 : if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_MAX_GAIN && !(seq_bitmask & AUTOTUNE_SEQ_BITMASK_RATE_D)) {
1732 0 : tune_seq[seq_cnt] = TuneType::MAX_GAINS;
1733 0 : seq_cnt++;
1734 : }
1735 18 : if (seq_bitmask & AUTOTUNE_SEQ_BITMASK_TUNE_CHECK) {
1736 4 : tune_seq[seq_cnt] = TuneType::TUNE_CHECK;
1737 4 : seq_cnt++;
1738 : }
1739 18 : tune_seq[seq_cnt] = TuneType::TUNE_COMPLETE;
1740 :
1741 18 : }
1742 :
1743 : // get_testing_step_timeout_ms accessor
1744 147 : uint32_t AC_AutoTune_Heli::get_testing_step_timeout_ms() const
1745 : {
1746 147 : return AUTOTUNE_TESTING_STEP_TIMEOUT_MS;
1747 : }
1748 :
1749 : // exceeded_freq_range - ensures tuning remains inside frequency range
1750 220509 : bool AC_AutoTune_Heli::exceeded_freq_range(float frequency)
1751 : {
1752 220509 : bool ret = false;
1753 220509 : if (frequency < min_sweep_freq || frequency > max_sweep_freq) {
1754 0 : ret = true;
1755 : }
1756 220509 : return ret;
1757 : }
1758 :
1759 : #endif // AC_AUTOTUNE_ENABLED
|
