Line data Source code
1 : /*
2 : C++ implementation of quicktune based on original lua script
3 : */
4 :
5 : // quicktune is not performance sensitive, save flash
6 : #pragma GCC optimize("Os")
7 :
8 : #include "AP_Quicktune.h"
9 :
10 : #if AP_QUICKTUNE_ENABLED
11 :
12 : #define UPDATE_RATE_HZ 40
13 : #define STAGE_DELAY 4000
14 : #define PILOT_INPUT_DELAY 4000
15 : #define YAW_FLTE_MAX 2.0
16 : #define FLTD_MUL 0.5
17 : #define FLTT_MUL 0.5
18 : #define DEFAULT_SMAX 50.0
19 : #define OPTIONS_TWO_POSITION (1<<0)
20 :
21 : #include <AP_Common/AP_Common.h>
22 : #include <AP_Vehicle/AP_Vehicle.h>
23 : #include <AP_RCMapper/AP_RCMapper.h>
24 : #include <AP_Logger/AP_Logger.h>
25 : #include <AP_Arming/AP_Arming.h>
26 : #include <GCS_MAVLink/GCS.h>
27 :
28 : const AP_Param::GroupInfo AP_Quicktune::var_info[] = {
29 : // @Param: ENABLE
30 : // @DisplayName: Quicktune enable
31 : // @Description: Enable quicktune system
32 : // @Values: 0:Disabled,1:Enabled
33 : // @User: Standard
34 : AP_GROUPINFO_FLAGS("ENABLE", 1, AP_Quicktune, enable, 0, AP_PARAM_FLAG_ENABLE),
35 :
36 : // @Param: AXES
37 : // @DisplayName: Quicktune axes
38 : // @Description: Axes to tune
39 : // @Bitmask: 0:Roll,1:Pitch,2:Yaw
40 : // @User: Standard
41 : AP_GROUPINFO("AXES", 2, AP_Quicktune, axes_enabled, 7),
42 :
43 : // @Param: DOUBLE_TIME
44 : // @DisplayName: Quicktune doubling time
45 : // @Description: Time to double a tuning parameter. Raise this for a slower tune.
46 : // @Range: 5 20
47 : // @Units: s
48 : // @User: Standard
49 : AP_GROUPINFO("DOUBLE_TIME", 3, AP_Quicktune, double_time, 10),
50 :
51 : // @Param: GAIN_MARGIN
52 : // @DisplayName: Quicktune gain margin
53 : // @Description: Reduction in gain after oscillation detected. Raise this number to get a more conservative tune
54 : // @Range: 20 80
55 : // @Units: %
56 : // @User: Standard
57 : AP_GROUPINFO("GAIN_MARGIN", 4, AP_Quicktune, gain_margin, 60),
58 :
59 : // @Param: OSC_SMAX
60 : // @DisplayName: Quicktune oscillation rate threshold
61 : // @Description: Threshold for oscillation detection. A lower value will lead to a more conservative tune.
62 : // @Range: 1 10
63 : // @User: Standard
64 : AP_GROUPINFO("OSC_SMAX", 5, AP_Quicktune, osc_smax, 4),
65 :
66 : // @Param: YAW_P_MAX
67 : // @DisplayName: Quicktune Yaw P max
68 : // @Description: Maximum value for yaw P gain
69 : // @Range: 0.1 3
70 : // @User: Standard
71 : AP_GROUPINFO("YAW_P_MAX", 6, AP_Quicktune, yaw_p_max, 0.5),
72 :
73 : // @Param: YAW_D_MAX
74 : // @DisplayName: Quicktune Yaw D max
75 : // @Description: Maximum value for yaw D gain
76 : // @Range: 0.001 1
77 : // @User: Standard
78 : AP_GROUPINFO("YAW_D_MAX", 7, AP_Quicktune, yaw_d_max, 0.01),
79 :
80 : // @Param: RP_PI_RATIO
81 : // @DisplayName: Quicktune roll/pitch PI ratio
82 : // @Description: Ratio between P and I gains for roll and pitch. Raise this to get a lower I gain
83 : // @Range: 1.0 2.0
84 : // @User: Standard
85 : AP_GROUPINFO("RP_PI_RATIO", 8, AP_Quicktune, rp_pi_ratio, 1.0),
86 :
87 : // @Param: Y_PI_RATIO
88 : // @DisplayName: Quicktune Yaw PI ratio
89 : // @Description: Ratio between P and I gains for yaw. Raise this to get a lower I gain
90 : // @Range: 1.0 20
91 : // @User: Standard
92 : AP_GROUPINFO("Y_PI_RATIO", 9, AP_Quicktune, y_pi_ratio, 10),
93 :
94 : // @Param: AUTO_FILTER
95 : // @DisplayName: Quicktune auto filter enable
96 : // @Description: When enabled the PID filter settings are automatically set based on INS_GYRO_FILTER
97 : // @Values: 0:Disabled,1:Enabled
98 : // @User: Standard
99 : AP_GROUPINFO("AUTO_FILTER", 10, AP_Quicktune, auto_filter, 1),
100 :
101 : // @Param: AUTO_SAVE
102 : // @DisplayName: Quicktune auto save
103 : // @Description: Number of seconds after completion of tune to auto-save. This is useful when using a 2 position switch for quicktune. Zero (the default value) disables automatic saving, and you will need to have a 3 position switch to save or use GCS auxilliary functions.
104 : // @Units: s
105 : // @User: Standard
106 : AP_GROUPINFO("AUTO_SAVE", 11, AP_Quicktune, auto_save, 0),
107 :
108 : // @Param: REDUCE_MAX
109 : // @DisplayName: Quicktune maximum gain reduction
110 : // @Description: This controls how much quicktune is allowed to lower gains from the original gains. If the vehicle already has a reasonable tune and is not oscillating then you can set this to zero to prevent gain reductions. The default of 20% is reasonable for most vehicles. Using a maximum gain reduction lowers the chance of an angle P oscillation happening if quicktune gets a false positive oscillation at a low gain, which can result in very low rate gains and a dangerous angle P oscillation.
111 : // @Units: %
112 : // @Range: 0 100
113 : // @User: Standard
114 : AP_GROUPINFO("REDUCE_MAX", 12, AP_Quicktune, reduce_max, 20),
115 :
116 : // @Param: OPTIONS
117 : // @DisplayName: Quicktune options
118 : // @Description: Additional options. When the Two Position Switch option is enabled then a high switch position will start the tune, low will disable the tune. you should also set a QUIK_AUTO_SAVE time so that you will be able to save the tune.
119 : // @Bitmask: 0:UseTwoPositionSwitch
120 : // @User: Standard
121 : AP_GROUPINFO("OPTIONS", 13, AP_Quicktune, options, 0),
122 :
123 : // @Param: ANGLE_MAX
124 : // @DisplayName: maximum angle error for tune abort
125 : // @Description: If while tuning the angle error goes over this limit then the tune will aborts to prevent a bad oscillation in the case of the tuning algorithm failing. If you get an error "Quicktune: attitude error ABORTING" and you think it is a false positive then you can either raise this parameter or you can try increasing the QWIK_DOUBLE_TIME to do the tune more slowly.
126 : // @Units: deg
127 : // @User: Standard
128 : AP_GROUPINFO("ANGLE_MAX", 14, AP_Quicktune, angle_max, 10),
129 :
130 : AP_GROUPEND
131 : };
132 :
133 : // Call at loop rate
134 2566790 : void AP_Quicktune::update(bool mode_supports_quicktune)
135 : {
136 2566790 : if (enable < 1) {
137 2558096 : if (need_restore) {
138 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "QuickTune disabled");
139 0 : abort_tune();
140 : }
141 2558096 : return;
142 : }
143 8694 : const uint32_t now = AP_HAL::millis();
144 :
145 8694 : if (!mode_supports_quicktune) {
146 : /*
147 : user has switched to a non-quicktune mode. If we have
148 : pending parameter changes then revert
149 : */
150 1971 : if (need_restore) {
151 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "QuickTune aborted");
152 : }
153 1971 : abort_tune();
154 1971 : return;
155 : }
156 :
157 6723 : if (need_restore) {
158 5899 : const float att_error = AC_AttitudeControl::get_singleton()->get_att_error_angle_deg();
159 5899 : if (angle_max > 0 && att_error > angle_max) {
160 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Quicktune: attitude error %.1fdeg - ABORTING", att_error);
161 0 : abort_tune();
162 0 : return;
163 : }
164 : }
165 :
166 6723 : if (have_pilot_input()) {
167 0 : last_pilot_input_ms = now;
168 : }
169 :
170 : SwitchPos sw_pos_tune = SwitchPos::MID;
171 : SwitchPos sw_pos_save = SwitchPos::HIGH;
172 6723 : if ((options & OPTIONS_TWO_POSITION) != 0) {
173 : sw_pos_tune = SwitchPos::HIGH;
174 : sw_pos_save = SwitchPos::NONE;
175 : }
176 :
177 6723 : const auto &vehicle = *AP::vehicle();
178 :
179 6723 : if (sw_pos == sw_pos_tune &&
180 5899 : (!hal.util->get_soft_armed() || !vehicle.get_likely_flying())) {
181 0 : if (now - last_warning_ms > 5000) {
182 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Quicktune: Must be flying to tune");
183 0 : last_warning_ms = now;
184 : }
185 0 : return;
186 : }
187 6723 : if (sw_pos == SwitchPos::LOW || !hal.util->get_soft_armed() || !vehicle.get_likely_flying()) {
188 : // Abort, revert parameters
189 816 : if (need_restore) {
190 0 : need_restore = false;
191 0 : restore_all_params();
192 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Quicktune: Reverted");
193 0 : tune_done_time_ms = 0;
194 : }
195 816 : reset_axes_done();
196 816 : return;
197 : }
198 5907 : if (sw_pos == sw_pos_save) {
199 : // Save all params
200 8 : if (need_restore) {
201 1 : need_restore = false;
202 1 : save_all_params();
203 : }
204 : }
205 5907 : if (sw_pos != sw_pos_tune) {
206 : return;
207 : }
208 :
209 5899 : if (now - last_stage_change_ms < STAGE_DELAY) {
210 : // Update slew gain
211 1026 : if (slew_parm != Param::END) {
212 120 : const float P = get_param_value(slew_parm);
213 120 : const AxisName axis = get_axis(slew_parm);
214 120 : adjust_gain(slew_parm, P+slew_delta);
215 120 : slew_steps = slew_steps - 1;
216 120 : Write_QWIK(get_slew_rate(axis), P, slew_parm);
217 120 : if (slew_steps == 0) {
218 6 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %.4f", get_param_name(slew_parm), P);
219 6 : slew_parm = Param::END;
220 6 : if (get_current_axis() == AxisName::DONE) {
221 1 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: DONE");
222 1 : tune_done_time_ms = now;
223 : }
224 : }
225 : }
226 1026 : return;
227 : }
228 :
229 4873 : const AxisName axis = get_current_axis();
230 :
231 4873 : if (axis == AxisName::DONE) {
232 : // Nothing left to do, check autosave time
233 344 : if (tune_done_time_ms != 0 && auto_save > 0) {
234 0 : if (now - tune_done_time_ms > (auto_save*1000)) {
235 0 : need_restore = false;
236 0 : save_all_params();
237 0 : tune_done_time_ms = 0;
238 : }
239 : }
240 344 : return;
241 : }
242 :
243 4529 : if (!need_restore) {
244 1 : need_restore = true;
245 : // We are just starting tuning, get current values
246 1 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: Starting tune");
247 : // Get all params
248 25 : for (uint8_t p = 0; p < uint8_t(Param::END); p++) {
249 24 : param_saved[p] = get_param_value(Param(p));
250 : }
251 : // Set up SMAX
252 1 : const Param is[3] { Param::RLL_SMAX, Param::PIT_SMAX, Param::YAW_SMAX };
253 4 : for (const auto p : is) {
254 3 : const float smax = get_param_value(p);
255 3 : if (smax <= 0) {
256 0 : adjust_gain(p, DEFAULT_SMAX);
257 : }
258 : }
259 : }
260 :
261 4529 : if (now - last_pilot_input_ms < PILOT_INPUT_DELAY) {
262 : return;
263 : }
264 :
265 4529 : if (!BIT_IS_SET(filters_done, uint8_t(axis))) {
266 3 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Starting %s tune", get_axis_name(axis));
267 3 : setup_filters(axis);
268 : }
269 :
270 4529 : const Param pname = get_pname(axis, current_stage);
271 4529 : const float pval = get_param_value(pname);
272 4529 : const float limit = gain_limit(pname);
273 4529 : const bool limited = (limit > 0.0 && pval >= limit);
274 4529 : const float srate = get_slew_rate(axis);
275 4529 : const bool oscillating = srate > osc_smax;
276 :
277 : // Check if reached limit
278 4529 : if (limited || oscillating) {
279 6 : float reduction = (100.0-gain_margin)*0.01;
280 6 : if (!oscillating) {
281 : reduction = 1.0;
282 : }
283 6 : float new_gain = pval * reduction;
284 6 : if (limit > 0.0 && new_gain > limit) {
285 : new_gain = limit;
286 : }
287 6 : float old_gain = param_saved[uint8_t(pname)];
288 6 : if (new_gain < old_gain && (pname == Param::PIT_D || pname == Param::RLL_D)) {
289 : // We are lowering a D gain from the original gain. Also
290 : // lower the P gain by the same amount so that we don't
291 : // trigger P oscillation. We don't drop P by more than a
292 : // factor of 2
293 0 : const float ratio = fmaxf(new_gain / old_gain, 0.5);
294 : const uint8_t P_TO_D_OFS = uint8_t(Param::RLL_D) - uint8_t(Param::RLL_P);
295 0 : const Param P_name = Param(uint8_t(pname)-P_TO_D_OFS); //from D to P
296 0 : const float old_pval = get_param_value(P_name);;
297 0 : const float new_pval = old_pval * ratio;
298 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Adjusting %s %.3f -> %.3f", get_param_name(P_name), old_pval, new_pval);
299 0 : adjust_gain_limited(P_name, new_pval);
300 : }
301 : // Set up slew gain
302 6 : slew_parm = pname;
303 6 : const float slew_target = limit_gain(pname, new_gain);
304 6 : slew_steps = UPDATE_RATE_HZ/2;
305 6 : slew_delta = (slew_target - get_param_value(pname)) / slew_steps;
306 :
307 6 : Write_QWIK(srate, pval, pname);
308 6 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Quicktune: %s done", get_param_name(pname));
309 6 : advance_stage(axis);
310 6 : last_stage_change_ms = now;
311 : } else {
312 4523 : float new_gain = pval*get_gain_mul();
313 : // cope with the gain starting at zero (some users with have a zero D gain)
314 4523 : new_gain = MAX(new_gain, 0.0001);
315 4523 : adjust_gain_limited(pname, new_gain);
316 4523 : Write_QWIK(srate, pval, pname);
317 4523 : if (now - last_gain_report_ms > 3000U) {
318 37 : last_gain_report_ms = now;
319 37 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s %.4f sr:%.2f", get_param_name(pname), new_gain, srate);
320 : }
321 : }
322 : }
323 :
324 : /*
325 : abort the tune if it has started
326 : */
327 1971 : void AP_Quicktune::abort_tune()
328 : {
329 1971 : if (need_restore) {
330 0 : need_restore = false;
331 0 : restore_all_params();
332 : }
333 1971 : tune_done_time_ms = 0;
334 1971 : reset_axes_done();
335 1971 : sw_pos = SwitchPos::LOW;
336 1971 : }
337 :
338 5 : void AP_Quicktune::update_switch_pos(const RC_Channel::AuxSwitchPos ch_flag)
339 : {
340 5 : sw_pos = SwitchPos(ch_flag);
341 5 : }
342 :
343 2787 : void AP_Quicktune::reset_axes_done()
344 : {
345 2787 : axes_done = 0;
346 2787 : filters_done = 0;
347 2787 : current_stage = Stage::D;
348 2787 : }
349 :
350 3 : void AP_Quicktune::setup_filters(AP_Quicktune::AxisName axis)
351 : {
352 3 : if (auto_filter <= 0) {
353 0 : BIT_SET(filters_done, uint8_t(axis));
354 0 : return;
355 : }
356 3 : AP_InertialSensor *imu = AP_InertialSensor::get_singleton();
357 3 : if (imu == nullptr) {
358 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
359 0 : return;
360 : }
361 3 : const float gyro_filter = imu->get_gyro_filter_hz();
362 3 : adjust_gain(get_pname(axis, Stage::FLTT), gyro_filter * FLTT_MUL);
363 3 : adjust_gain(get_pname(axis, Stage::FLTD), gyro_filter * FLTD_MUL);
364 :
365 3 : if (axis == AxisName::YAW) {
366 1 : const float FLTE = get_param_value(Param::YAW_FLTE);
367 1 : if (FLTE < 0.0 || FLTE > YAW_FLTE_MAX) {
368 1 : adjust_gain(Param::YAW_FLTE, YAW_FLTE_MAX);
369 : }
370 : }
371 3 : BIT_SET(filters_done, uint8_t(axis));
372 : }
373 :
374 : // Check for pilot input to pause tune
375 6723 : bool AP_Quicktune::have_pilot_input() const
376 : {
377 6723 : auto &RC = rc();
378 6723 : const float roll = RC.get_roll_channel().norm_input_dz();
379 6723 : const float pitch = RC.get_pitch_channel().norm_input_dz();
380 6723 : const float yaw = RC.get_yaw_channel().norm_input_dz();
381 :
382 6723 : if (fabsf(roll) > 0 || fabsf(pitch) > 0 || fabsf(yaw) > 0) {
383 0 : return true;
384 : }
385 : return false;
386 : }
387 :
388 : // Get the axis name we are working on, or DONE for all done
389 4879 : AP_Quicktune::AxisName AP_Quicktune::get_current_axis() const
390 : {
391 10883 : for (int8_t i = 0; i < int8_t(AxisName::DONE); i++) {
392 10538 : if (BIT_IS_SET(axes_enabled, i) && !BIT_IS_SET(axes_done, i)) {
393 4534 : return AxisName(i);
394 : }
395 : }
396 : return AxisName::DONE;
397 : }
398 :
399 : // get the AC_PID object for an axis
400 16982 : AC_PID *AP_Quicktune::get_axis_pid(AP_Quicktune::AxisName axis) const
401 : {
402 16982 : auto &attitude_control = *AC_AttitudeControl::get_singleton();
403 16982 : switch(axis) {
404 5224 : case AxisName::RLL:
405 5224 : return &attitude_control.get_rate_roll_pid();
406 5240 : case AxisName::PIT:
407 5240 : return &attitude_control.get_rate_pitch_pid();
408 6518 : case AxisName::YAW:
409 6518 : return &attitude_control.get_rate_yaw_pid();
410 0 : default:
411 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
412 : break;
413 : }
414 0 : return nullptr;
415 : }
416 :
417 : // get slew rate parameter for an axis
418 4649 : float AP_Quicktune::get_slew_rate(AP_Quicktune::AxisName axis) const
419 : {
420 4649 : auto *pid = get_axis_pid(axis);
421 4649 : if (pid == nullptr) {
422 : return 0;
423 : }
424 4649 : return pid->get_pid_info().slew_rate;
425 : }
426 :
427 : // Move to next stage of tune
428 6 : void AP_Quicktune::advance_stage(AP_Quicktune::AxisName axis)
429 : {
430 6 : if (current_stage == Stage::D) {
431 3 : current_stage = Stage::P;
432 : } else {
433 3 : BIT_SET(axes_done, uint8_t(axis));
434 3 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: %s done", get_axis_name(axis));
435 3 : current_stage = Stage::D;
436 : }
437 6 : }
438 :
439 4650 : void AP_Quicktune::adjust_gain(AP_Quicktune::Param param, float value)
440 : {
441 4650 : need_restore = true;
442 4650 : BIT_SET(param_changed, uint8_t(param));
443 4650 : set_param_value(param, value);
444 :
445 4650 : if (get_stage(param) == Stage::P) {
446 : // Also change I gain
447 : const uint8_t P_TO_I_OFS = uint8_t(Param::RLL_I) - uint8_t(Param::RLL_P);
448 : const uint8_t P_TO_FF_OFS = uint8_t(Param::RLL_FF) - uint8_t(Param::RLL_P);
449 1464 : const Param iname = Param(uint8_t(param)+P_TO_I_OFS);
450 1464 : const Param ffname = Param(uint8_t(param)+P_TO_FF_OFS);
451 1464 : float FF = get_param_value(ffname);
452 1464 : if (FF > 0) {
453 : // If we have any FF on an axis then we don't couple I to P,
454 : // usually we want I = FF for a one second time constant for trim
455 : return;
456 : }
457 1464 : BIT_SET(param_changed, uint8_t(iname));
458 :
459 : // Work out ratio of P to I that we want
460 1464 : float pi_ratio = rp_pi_ratio;
461 1464 : if (get_axis(param) == AxisName::YAW) {
462 485 : pi_ratio = y_pi_ratio;
463 : }
464 1464 : if (pi_ratio >= 1) {
465 1464 : set_param_value(iname, value/pi_ratio);
466 : }
467 : }
468 :
469 : }
470 :
471 4523 : void AP_Quicktune::adjust_gain_limited(AP_Quicktune::Param param, float value)
472 : {
473 4523 : adjust_gain(param, limit_gain(param, value));
474 4523 : }
475 :
476 4529 : float AP_Quicktune::limit_gain(AP_Quicktune::Param param, float value)
477 : {
478 4529 : const float saved_value = param_saved[uint8_t(param)];
479 4529 : if (reduce_max >= 0 && reduce_max < 100 && saved_value > 0) {
480 : // Check if we exceeded gain reduction
481 3198 : const float reduction_pct = 100.0 * (saved_value - value) / saved_value;
482 3198 : if (reduction_pct > reduce_max) {
483 0 : const float new_value = saved_value * (100 - reduce_max) * 0.01;
484 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Limiting %s %.3f -> %.3f", get_param_name(param), value, new_value);
485 : value = new_value;
486 : }
487 : }
488 4529 : return value;
489 : }
490 :
491 4698 : const char* AP_Quicktune::get_param_name(AP_Quicktune::Param param) const
492 : {
493 4698 : switch (param)
494 : {
495 : case Param::RLL_P:
496 : return "Roll P";
497 0 : case Param::RLL_I:
498 0 : return "Roll I";
499 921 : case Param::RLL_D:
500 921 : return "Roll D";
501 496 : case Param::PIT_P:
502 496 : return "Pitch P";
503 0 : case Param::PIT_I:
504 0 : return "Pitch I";
505 928 : case Param::PIT_D:
506 928 : return "Pitch D";
507 492 : case Param::YAW_P:
508 492 : return "Yaw P";
509 0 : case Param::YAW_I:
510 0 : return "Yaw I";
511 1364 : case Param::YAW_D:
512 1364 : return "Yaw D";
513 0 : default:
514 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
515 0 : return "UNK";
516 : }
517 : }
518 :
519 4523 : float AP_Quicktune::get_gain_mul() const
520 : {
521 4523 : return expf(logf(2.0)/(UPDATE_RATE_HZ*MAX(1,double_time)));
522 : }
523 :
524 0 : void AP_Quicktune::restore_all_params()
525 : {
526 0 : for (uint8_t p = 0; p < uint8_t(Param::END); p++) {
527 : const auto param = Param(p);
528 0 : if (BIT_IS_SET(param_changed, p)) {
529 0 : set_param_value(param, param_saved[p]);
530 0 : BIT_CLEAR(param_changed, p);
531 : }
532 : }
533 0 : }
534 :
535 1 : void AP_Quicktune::save_all_params()
536 : {
537 25 : for (uint8_t p = 0; p < uint8_t(Param::END); p++) {
538 : const auto param = Param(p);
539 24 : set_and_save_param_value(param, get_param_value(param));
540 24 : param_saved[p] = get_param_value(param);
541 24 : BIT_CLEAR(param_changed, p);
542 : }
543 1 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Quicktune: Saved");
544 1 : }
545 :
546 4535 : AP_Quicktune::Param AP_Quicktune::get_pname(AP_Quicktune::AxisName axis, AP_Quicktune::Stage stage) const
547 : {
548 4535 : const uint8_t axis_offset = uint8_t(axis) * param_per_axis;
549 4535 : switch (stage)
550 : {
551 : case Stage::P:
552 : return Param(uint8_t(Param::RLL_P) + axis_offset);
553 3122 : case Stage::D:
554 3122 : return Param(uint8_t(Param::RLL_D) + axis_offset);
555 3 : case Stage::FLTT:
556 3 : return Param(uint8_t(Param::RLL_FLTT) + axis_offset);
557 3 : case Stage::FLTD:
558 3 : return Param(uint8_t(Param::RLL_FLTD) + axis_offset);
559 0 : default:
560 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
561 0 : return Param::END;
562 : }
563 : }
564 :
565 4650 : AP_Quicktune::Stage AP_Quicktune::get_stage(AP_Quicktune::Param param) const
566 : {
567 4650 : if (param == Param::RLL_P || param == Param::PIT_P || param == Param::YAW_P) {
568 : return Stage::P;
569 : } else {
570 3186 : return Stage::END; //Means "anything but P gain"
571 : }
572 : }
573 :
574 12333 : AP_Float *AP_Quicktune::get_param_pointer(AP_Quicktune::Param param) const
575 : {
576 12333 : const AxisName axis = get_axis(param);
577 12333 : auto *pid = get_axis_pid(axis);
578 12333 : if (pid == nullptr) {
579 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
580 0 : return nullptr;
581 : }
582 12333 : const Param roll_param = Param(uint8_t(param) % param_per_axis);
583 12333 : switch (roll_param)
584 : {
585 2946 : case Param::RLL_P:
586 2946 : return &pid->kP();
587 1476 : case Param::RLL_I:
588 1476 : return &pid->kI();
589 6376 : case Param::RLL_D:
590 6376 : return &pid->kD();
591 15 : case Param::RLL_SMAX:
592 15 : return &pid->slew_limit();
593 15 : case Param::RLL_FLTT:
594 15 : return &pid->filt_T_hz();
595 15 : case Param::RLL_FLTD:
596 15 : return &pid->filt_D_hz();
597 14 : case Param::RLL_FLTE:
598 14 : return &pid->filt_E_hz();
599 1476 : case Param::RLL_FF:
600 1476 : return &pid->ff();
601 : default:
602 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
603 : return nullptr;
604 : }
605 : }
606 :
607 6195 : float AP_Quicktune::get_param_value(AP_Quicktune::Param param) const
608 : {
609 6195 : AP_Float *ptr = get_param_pointer(param);
610 6195 : if (ptr != nullptr) {
611 6195 : return ptr->get();
612 : }
613 : return 0.0;
614 : }
615 :
616 6114 : void AP_Quicktune::set_param_value(AP_Quicktune::Param param, float value)
617 : {
618 6114 : AP_Float *ptr = get_param_pointer(param);
619 6114 : if (ptr != nullptr) {
620 6114 : ptr->set(value);
621 : }
622 6114 : }
623 :
624 24 : void AP_Quicktune::set_and_save_param_value(AP_Quicktune::Param param, float value)
625 : {
626 24 : AP_Float *ptr = get_param_pointer(param);
627 24 : if (ptr != nullptr) {
628 24 : ptr->set_and_save(value);
629 : }
630 24 : }
631 :
632 18446 : AP_Quicktune::AxisName AP_Quicktune::get_axis(AP_Quicktune::Param param) const
633 : {
634 18446 : if (param >= Param::END) {
635 : return AxisName::END;
636 : }
637 18446 : return AxisName(uint8_t(param) / param_per_axis);
638 : }
639 :
640 6 : const char* AP_Quicktune::get_axis_name(AP_Quicktune::AxisName axis) const
641 : {
642 6 : switch (axis)
643 : {
644 : case AxisName::RLL:
645 : return "Roll";
646 2 : case AxisName::PIT:
647 2 : return "Pitch";
648 2 : case AxisName::YAW:
649 2 : return "Yaw";
650 0 : default:
651 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
652 0 : return "UNK";
653 : }
654 : }
655 :
656 4529 : float AP_Quicktune::gain_limit(AP_Quicktune::Param param) const
657 : {
658 4529 : if (get_axis(param) == AxisName::YAW) {
659 1797 : if (param == Param::YAW_P) {
660 466 : return yaw_p_max;
661 : }
662 1331 : if (param == Param::YAW_D) {
663 1331 : return yaw_d_max;
664 : }
665 : }
666 : return 0.0;
667 : }
668 :
669 :
670 : // @LoggerMessage: QWIK
671 : // @Description: Quicktune
672 : // @Field: TimeUS: Time since system startup
673 : // @Field: ParamNo: number of parameter being tuned
674 : // @Field: SRate: slew rate
675 : // @Field: Gain: test gain for current axis and PID element
676 : // @Field: Param: name of parameter being being tuned
677 4649 : void AP_Quicktune::Write_QWIK(float srate, float gain, AP_Quicktune::Param param)
678 : {
679 : #if HAL_LOGGING_ENABLED
680 4649 : AP::logger().WriteStreaming("QWIK","TimeUS,ParamNo,SRate,Gain,Param", "s#---", "-----", "QBffN",
681 : AP_HAL::micros64(),
682 : unsigned(param),
683 : srate,
684 : gain,
685 : get_param_name(param));
686 : #endif
687 4649 : }
688 :
689 : #endif //AP_QUICKTUNE_ENABLED
|
