Line data Source code
1 : #include "AC_AutoTune_config.h"
2 :
3 : #if AC_AUTOTUNE_ENABLED
4 :
5 : #include "AC_AutoTune.h"
6 :
7 : #include <AP_Logger/AP_Logger.h>
8 : #include <AP_Scheduler/AP_Scheduler.h>
9 : #include <AP_Notify/AP_Notify.h>
10 : #include <GCS_MAVLink/GCS.h>
11 : #include <AP_Vehicle/AP_Vehicle_Type.h>
12 :
13 : #define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds
14 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
15 : # define AUTOTUNE_LEVEL_ANGLE_CD 500 // angle which qualifies as level (Plane uses more relaxed 5deg)
16 : # define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch (Plane uses more relaxed 10deg/sec)
17 : #else
18 : # define AUTOTUNE_LEVEL_ANGLE_CD 250 // angle which qualifies as level
19 : # define AUTOTUNE_LEVEL_RATE_RP_CD 500 // rate which qualifies as level for roll and pitch
20 : #endif
21 : #define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw
22 : #define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 250 // time we require the aircraft to be level before starting next test
23 : #define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level
24 : #define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000 // level failure warning messages sent at this interval to users
25 :
26 1693 : AC_AutoTune::AC_AutoTune()
27 : {
28 1693 : }
29 :
30 : // autotune_init - should be called when autotune mode is selected
31 13 : bool AC_AutoTune::init_internals(bool _use_poshold,
32 : AC_AttitudeControl *_attitude_control,
33 : AC_PosControl *_pos_control,
34 : AP_AHRS_View *_ahrs_view)
35 : {
36 13 : use_poshold = _use_poshold;
37 13 : attitude_control = _attitude_control;
38 13 : pos_control = _pos_control;
39 13 : ahrs_view = _ahrs_view;
40 13 : motors = AP_Motors::get_singleton();
41 13 : const uint32_t now_ms = AP_HAL::millis();
42 :
43 : // exit immediately if motor are not armed
44 13 : if ((motors == nullptr) || !motors->armed()) {
45 0 : return false;
46 : }
47 :
48 : // initialise position controller
49 13 : init_position_controller();
50 :
51 13 : switch (mode) {
52 12 : case TuneMode::FAILED:
53 : // Fall through to restart the tuning process from scratch
54 : FALLTHROUGH;
55 :
56 : case TuneMode::UNINITIALISED:
57 : // First-time run: store the current gains as the baseline (original gains)
58 12 : backup_gains_and_initialise();
59 : // Set the mode to TUNING to begin the autotune process
60 12 : mode = TuneMode::TUNING;
61 : // Notify GCS that autotune has started
62 12 : update_gcs(AUTOTUNE_MESSAGE_STARTED);
63 12 : break;
64 :
65 0 : case TuneMode::TUNING:
66 : // Resuming from previous tuning session, restart from current axis and tune step
67 0 : reset_vehicle_test_variables();
68 0 : step = Step::WAITING_FOR_LEVEL;
69 0 : step_start_time_ms = now_ms;
70 0 : level_start_time_ms = now_ms;
71 : // Reload gains with low I-term and restart logging
72 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_RESTART);
73 0 : update_gcs(AUTOTUNE_MESSAGE_STARTED);
74 0 : break;
75 :
76 1 : case TuneMode::FINISHED:
77 : case TuneMode::VALIDATING:
78 : // The user is now validating the tuned gains in flight
79 1 : mode = TuneMode::VALIDATING;
80 1 : update_gcs(AUTOTUNE_MESSAGE_TESTING);
81 1 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_PILOT_TESTING);
82 1 : break;
83 : }
84 :
85 13 : have_position = false;
86 :
87 13 : return true;
88 : }
89 :
90 : // stop - should be called when the ch7/ch8 switch is switched OFF
91 11 : void AC_AutoTune::stop()
92 : {
93 : // set gains to their original values
94 11 : load_gains(GainType::ORIGINAL);
95 :
96 11 : update_gcs(AUTOTUNE_MESSAGE_STOPPED);
97 :
98 11 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_OFF);
99 :
100 : // Note: we leave the mode as it was so that we know how the autotune ended
101 : // we expect the caller will change the flight mode back to the flight mode indicated by the flight mode switch
102 11 : }
103 :
104 : // Autotune aux function trigger
105 5 : void AC_AutoTune::do_aux_function(const RC_Channel::AuxSwitchPos ch_flag)
106 : {
107 5 : if (mode != TuneMode::FINISHED) {
108 2 : if (ch_flag == RC_Channel::AuxSwitchPos::HIGH) {
109 1 : gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: must be complete to test gains");
110 : }
111 2 : return;
112 : }
113 :
114 3 : switch(ch_flag) {
115 1 : case RC_Channel::AuxSwitchPos::LOW:
116 : // load original gains
117 1 : load_gains(GainType::ORIGINAL);
118 1 : update_gcs(AUTOTUNE_MESSAGE_TESTING_END);
119 1 : break;
120 0 : case RC_Channel::AuxSwitchPos::MIDDLE:
121 : // Middle position is unused for now_ms
122 0 : break;
123 2 : case RC_Channel::AuxSwitchPos::HIGH:
124 : // Load tuned gains
125 2 : load_gains(GainType::TUNED);
126 2 : update_gcs(AUTOTUNE_MESSAGE_TESTING);
127 2 : break;
128 : }
129 :
130 3 : testing_switch_used = true;
131 : }
132 :
133 : // Possibly save gains, called on disarm
134 943 : void AC_AutoTune::disarmed(const bool in_autotune_mode)
135 : {
136 : // True if pilot is testing tuned gains
137 943 : const bool testing_tuned = testing_switch_used && (loaded_gains == GainType::TUNED);
138 :
139 : // True if in autotune mode and no pilot testing commands have been received
140 943 : const bool tune_complete_no_testing = !testing_switch_used && in_autotune_mode;
141 :
142 943 : if (tune_complete_no_testing || testing_tuned) {
143 9 : save_tuning_gains();
144 : } else {
145 934 : reset();
146 : }
147 943 : }
148 :
149 : // initialise position controller
150 13 : bool AC_AutoTune::init_position_controller(void)
151 : {
152 : // initialize vertical maximum speeds and acceleration
153 13 : init_z_limits();
154 :
155 : // initialise the vertical position controller
156 13 : pos_control->D_init_controller();
157 :
158 13 : return true;
159 : }
160 :
161 898 : void AC_AutoTune::send_step_string()
162 : {
163 898 : if (pilot_override) {
164 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Paused: Pilot Override Active");
165 0 : return;
166 : }
167 898 : switch (step) {
168 208 : case Step::WAITING_FOR_LEVEL:
169 208 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Leveling");
170 208 : return;
171 0 : case Step::UPDATE_GAINS:
172 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Updating Gains");
173 0 : return;
174 0 : case Step::ABORT:
175 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Aborting Test");
176 0 : return;
177 690 : case Step::EXECUTING_TEST:
178 690 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: Testing");
179 690 : return;
180 : }
181 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: unknown step");
182 : }
183 :
184 2282 : const char *AC_AutoTune::get_tune_type_name() const
185 : {
186 2282 : switch (tune_type) {
187 569 : case TuneType::RATE_D_UP:
188 569 : return "Rate D Up";
189 224 : case TuneType::RATE_D_DOWN:
190 224 : return "Rate D Down";
191 518 : case TuneType::RATE_P_UP:
192 518 : return "Rate P Up";
193 428 : case TuneType::RATE_FF_UP:
194 428 : return "Rate FF Up";
195 324 : case TuneType::ANGLE_P_UP:
196 324 : return "Angle P Up";
197 80 : case TuneType::ANGLE_P_DOWN:
198 80 : return "Angle P Down";
199 99 : case TuneType::MAX_GAINS:
200 99 : return "Find Max Gains";
201 40 : case TuneType::TUNE_CHECK:
202 40 : return "Check Tune Frequency Response";
203 0 : case TuneType::TUNE_COMPLETE:
204 0 : return "Tune Complete";
205 : }
206 0 : return "";
207 : // this should never happen
208 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
209 : }
210 :
211 : // return current axis string
212 2282 : const char *AC_AutoTune::get_axis_name() const
213 : {
214 2282 : switch (axis) {
215 410 : case AxisType::ROLL:
216 410 : return "Roll";
217 482 : case AxisType::PITCH:
218 482 : return "Pitch";
219 990 : case AxisType::YAW:
220 990 : return "Yaw(E)";
221 400 : case AxisType::YAW_D:
222 400 : return "Yaw(D)";
223 : }
224 0 : return "";
225 : }
226 :
227 : // Main update loop for Autotune mode. Handles all states: tuning, validating, or idle.
228 : // Should be called at ≥100Hz for consistent performance.
229 1816797 : void AC_AutoTune::run()
230 : {
231 : // Initialise vertical climb rate and acceleration limits
232 1816797 : init_z_limits();
233 :
234 : // Exit early if the vehicle is disarmed or motor interlock is not enabled
235 : // (this condition should not occur if init() is working correctly)
236 1816797 : if (!motors->armed() || !motors->get_interlock()) {
237 23893 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
238 23893 : attitude_control->set_throttle_out(0.0f, true, 0.0f);
239 23893 : pos_control->D_relax_controller(0.0f);
240 23893 : return;
241 : }
242 :
243 : float desired_yaw_rate_rads; // used during manual control
244 1792904 : get_pilot_desired_rp_yrate_rad(desired_roll_rad, desired_pitch_rad, desired_yaw_rate_rads);
245 :
246 : // Get pilot's desired climb rate
247 1792904 : const float target_climb_rate_ms = get_desired_climb_rate_ms();
248 :
249 1792904 : const bool zero_rp_input = is_zero(desired_roll_rad) && is_zero(desired_pitch_rad);
250 1792904 : if (zero_rp_input) {
251 : // Use position hold if enabled
252 1792904 : get_poshold_attitude_rad(desired_roll_rad, desired_pitch_rad, desired_yaw_rad);
253 : }
254 :
255 1792904 : const uint32_t now_ms = AP_HAL::millis();
256 :
257 1792904 : switch (mode) {
258 1779135 : case TuneMode::TUNING:
259 : // Detect pilot override
260 1779135 : if (!zero_rp_input || !is_zero(desired_yaw_rate_rads) || !is_zero(target_climb_rate_ms)) {
261 0 : if (!pilot_override) {
262 0 : pilot_override = true;
263 : // Restore original gains while pilot is in control
264 : }
265 : // Update last override time
266 0 : override_time = now_ms;
267 0 : if (!zero_rp_input) {
268 : // Invalidate position hold if pilot inputs roll/pitch
269 0 : have_position = false;
270 : }
271 1779135 : } else if (pilot_override) {
272 : // Check if pilot has released sticks long enough to resume tuning
273 0 : if (now_ms - override_time > AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS) {
274 0 : pilot_override = false;
275 0 : step = Step::WAITING_FOR_LEVEL;
276 0 : step_start_time_ms = now_ms;
277 0 : level_start_time_ms = now_ms;
278 : // TODO: Consider using our current target.
279 0 : desired_yaw_rad = ahrs_view->get_yaw_rad(); // Reset yaw reference
280 : }
281 : }
282 :
283 1779135 : if (pilot_override) {
284 : // Pilot is actively controlling the vehicle; fly on original gains
285 0 : if (now_ms - last_pilot_override_warning > 1000) {
286 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AutoTune: pilot overrides active");
287 0 : last_pilot_override_warning = now_ms;
288 : }
289 0 : load_gains(GainType::ORIGINAL);
290 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(desired_roll_rad, desired_pitch_rad, desired_yaw_rate_rads);
291 : } else {
292 : // Autotune controls the aircraft
293 1779135 : control_attitude();
294 1779135 : do_gcs_announcements();
295 : }
296 1779135 : break;
297 :
298 0 : case TuneMode::UNINITIALISED:
299 : // Should never reach this state; init() must be called before run()
300 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
301 : FALLTHROUGH;
302 :
303 10636 : case TuneMode::FAILED:
304 : FALLTHROUGH;
305 :
306 : case TuneMode::FINISHED:
307 : // Tuning is complete or failed; fly using original gains
308 10636 : load_gains(GainType::ORIGINAL);
309 10636 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(desired_roll_rad, desired_pitch_rad, desired_yaw_rate_rads);
310 10636 : break;
311 :
312 3133 : case TuneMode::VALIDATING:
313 : // Pilot is evaluating tuned gains
314 3133 : load_gains(GainType::TUNED);
315 3133 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(desired_roll_rad, desired_pitch_rad, desired_yaw_rate_rads);
316 3133 : break;
317 : }
318 :
319 1792904 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
320 :
321 : // Update vertical position controller with pilot climb rate input
322 1792904 : pos_control->D_set_pos_target_from_climb_rate_ms(target_climb_rate_ms);
323 1792904 : pos_control->D_update_controller();
324 : }
325 :
326 : // return true if vehicle is close to level
327 921436 : bool AC_AutoTune::currently_level()
328 : {
329 : // abort AutoTune if we pass 2 * AUTOTUNE_LEVEL_TIMEOUT_MS
330 921436 : const uint32_t now_ms = AP_HAL::millis();
331 921436 : if (fabsf(attitude_control->get_rate_ef_target_rads().z) > 0.5 * attitude_control->get_slew_yaw_max_rads()) {
332 : // reset if the target yaw rate is above half the slew rate
333 2037 : level_start_time_ms = now_ms;
334 : }
335 921436 : if (now_ms - level_start_time_ms > 3 * AUTOTUNE_LEVEL_TIMEOUT_MS) {
336 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AutoTune: Failed to level, please tune manually");
337 0 : mode = TuneMode::FAILED;
338 0 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED);
339 : }
340 :
341 : // slew threshold to ensure sufficient settling time for aircraft unable to obtain small thresholds
342 : // relax threshold if we pass AUTOTUNE_LEVEL_TIMEOUT_MS
343 921436 : const float threshold_mul = constrain_float((float)(now_ms - level_start_time_ms) / (float)AUTOTUNE_LEVEL_TIMEOUT_MS, 0.0, 2.0);
344 :
345 921436 : if (fabsf(ahrs_view->get_roll_rad() - desired_roll_rad) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_ANGLE_CD)) {
346 237284 : return false;
347 : }
348 684152 : if (fabsf(ahrs_view->get_pitch_rad() - desired_pitch_rad) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_ANGLE_CD)) {
349 73774 : return false;
350 : }
351 610378 : if (fabsf(wrap_PI(ahrs_view->get_yaw_rad() - desired_yaw_rad)) > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_ANGLE_CD)) {
352 312592 : return false;
353 : }
354 297786 : if (ahrs_view->get_gyro().x > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_RATE_RP_CD)) {
355 11757 : return false;
356 : }
357 286029 : if (ahrs_view->get_gyro().y > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_RATE_RP_CD)) {
358 11259 : return false;
359 : }
360 274770 : if (ahrs_view->get_gyro().z > threshold_mul * cd_to_rad(AUTOTUNE_LEVEL_RATE_Y_CD)) {
361 45974 : return false;
362 : }
363 228796 : return true;
364 : }
365 :
366 : // Main tuning state machine. Handles all StepTypes: WAITING_FOR_LEVEL, EXECUTING_TEST, UPDATE_GAINS, ABORT.
367 : // Updates attitude controller targets and processes test results to adjust PID gains.
368 1779135 : void AC_AutoTune::control_attitude()
369 : {
370 1779135 : rotation_rate = 0.0f;
371 1779135 : lean_angle = 0.0f;
372 1779135 : const float direction_sign = positive_direction ? 1.0f : -1.0f;
373 1779135 : const uint32_t now_ms = AP_HAL::millis();
374 :
375 1779135 : switch (step) {
376 :
377 921436 : case Step::WAITING_FOR_LEVEL: {
378 : // Use intra-test gains while holding level between tests
379 921436 : load_gains(GainType::INTRA_TEST);
380 :
381 921436 : attitude_control->input_euler_angle_roll_pitch_yaw_rad(desired_roll_rad, desired_pitch_rad, desired_yaw_rad, true);
382 :
383 : // Require a short stable period before executing the next test
384 921436 : if (!currently_level()) {
385 692640 : step_start_time_ms = now_ms;
386 : }
387 :
388 921436 : if (now_ms - step_start_time_ms > AUTOTUNE_REQUIRED_LEVEL_TIME_MS) {
389 : // Begin the test phase
390 2329 : step = Step::EXECUTING_TEST;
391 2329 : step_start_time_ms = now_ms;
392 2329 : step_timeout_ms = get_testing_step_timeout_ms();
393 :
394 : // Record starting angular position and rate
395 2329 : switch (axis) {
396 438 : case AxisType::ROLL:
397 438 : start_rate = degrees(ahrs_view->get_gyro().x) * 100.0f;
398 438 : start_angle = ahrs_view->roll_sensor;
399 438 : break;
400 433 : case AxisType::PITCH:
401 433 : start_rate = degrees(ahrs_view->get_gyro().y) * 100.0f;
402 433 : start_angle = ahrs_view->pitch_sensor;
403 433 : break;
404 1458 : case AxisType::YAW:
405 : case AxisType::YAW_D:
406 1458 : start_rate = degrees(ahrs_view->get_gyro().z) * 100.0f;
407 1458 : start_angle = ahrs_view->yaw_sensor;
408 1458 : break;
409 : }
410 :
411 : // Apply test gains and initialise test-specific variables
412 2329 : load_gains(GainType::TEST);
413 2329 : test_init();
414 : }
415 921436 : break;
416 : }
417 :
418 855370 : case Step::EXECUTING_TEST: {
419 : // Run the test with current trial gains
420 855370 : load_gains(GainType::TEST);
421 855370 : test_run(axis, direction_sign);
422 :
423 : // Detect failure due to reverse response or excessive lean angle
424 1710740 : if (lean_angle <= -angle_lim_neg_rpy_cd() ||
425 855370 : attitude_control->lean_angle_deg() * 100 > angle_lim_max_rp_cd()) {
426 0 : step = Step::ABORT;
427 : }
428 :
429 : #if HAL_LOGGING_ENABLED
430 855370 : Log_AutoTuneDetails();
431 855370 : attitude_control->Write_Rate(*pos_control);
432 855370 : log_pids();
433 : #endif
434 :
435 : // Update yaw target for next test if required
436 855370 : if (axis == AxisType::YAW || axis == AxisType::YAW_D) {
437 526830 : desired_yaw_rad = ahrs_view->get_yaw_rad();
438 : }
439 855370 : break;
440 : }
441 :
442 2244 : case Step::UPDATE_GAINS:
443 :
444 : #if HAL_LOGGING_ENABLED
445 2244 : Log_AutoTune(); // Log gain adjustment results
446 : #endif
447 :
448 : // Announce test results before gains are changed
449 2244 : do_post_test_gcs_announcements();
450 :
451 : // Update gains based on the current tuning strategy
452 2244 : switch (tune_type) {
453 692 : case TuneType::RATE_D_UP:
454 692 : updating_rate_d_up_all(axis);
455 692 : break;
456 431 : case TuneType::RATE_D_DOWN:
457 431 : updating_rate_d_down_all(axis);
458 431 : break;
459 578 : case TuneType::RATE_P_UP:
460 578 : updating_rate_p_up_all(axis);
461 578 : break;
462 93 : case TuneType::ANGLE_P_DOWN:
463 93 : updating_angle_p_down_all(axis);
464 93 : break;
465 391 : case TuneType::ANGLE_P_UP:
466 391 : updating_angle_p_up_all(axis);
467 391 : break;
468 35 : case TuneType::RATE_FF_UP:
469 35 : updating_rate_ff_up_all(axis);
470 35 : break;
471 21 : case TuneType::MAX_GAINS:
472 21 : updating_max_gains_all(axis);
473 21 : break;
474 3 : case TuneType::TUNE_CHECK:
475 3 : success_counter = AUTOTUNE_SUCCESS_COUNT;
476 : FALLTHROUGH;
477 3 : case TuneType::TUNE_COMPLETE:
478 3 : break;
479 : }
480 :
481 : // If tuning step was successful, proceed to the next step
482 2244 : if (success_counter >= AUTOTUNE_SUCCESS_COUNT) {
483 88 : success_counter = 0;
484 88 : step_scaler = 1.0f;
485 88 : set_tuning_gains_with_backoff(axis);
486 88 : next_tune_type(tune_type, false);
487 :
488 88 : if (tune_type == TuneType::TUNE_COMPLETE) {
489 : // Complete tuning for this axis and determine the next one
490 25 : next_tune_type(tune_type, true);
491 25 : report_final_gains(axis);
492 :
493 25 : bool complete = false;
494 25 : switch (axis) {
495 8 : case AxisType::ROLL:
496 8 : axes_completed |= AUTOTUNE_AXIS_BITMASK_ROLL;
497 8 : if (pitch_enabled()) {
498 7 : axis = AxisType::PITCH;
499 1 : } else if (yaw_enabled()) {
500 0 : axis = AxisType::YAW;
501 1 : } else if (yaw_d_enabled()) {
502 0 : axis = AxisType::YAW_D;
503 : } else {
504 1 : complete = true;
505 : }
506 8 : break;
507 8 : case AxisType::PITCH:
508 8 : axes_completed |= AUTOTUNE_AXIS_BITMASK_PITCH;
509 8 : if (yaw_enabled()) {
510 5 : axis = AxisType::YAW;
511 3 : } else if (yaw_d_enabled()) {
512 0 : axis = AxisType::YAW_D;
513 : } else {
514 3 : complete = true;
515 : }
516 8 : break;
517 8 : case AxisType::YAW:
518 8 : axes_completed |= AUTOTUNE_AXIS_BITMASK_YAW;
519 8 : if (yaw_d_enabled()) {
520 1 : axis = AxisType::YAW_D;
521 : } else {
522 7 : complete = true;
523 : }
524 8 : break;
525 1 : case AxisType::YAW_D:
526 1 : axes_completed |= AUTOTUNE_AXIS_BITMASK_YAW_D;
527 1 : complete = true;
528 1 : break;
529 : }
530 :
531 25 : if (complete) {
532 12 : mode = TuneMode::FINISHED;
533 12 : update_gcs(AUTOTUNE_MESSAGE_SUCCESS);
534 12 : LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SUCCESS);
535 12 : AP_Notify::events.autotune_complete = true;
536 12 : load_gains(GainType::ORIGINAL); // Reset for landing
537 : } else {
538 13 : AP_Notify::events.autotune_next_axis = true;
539 13 : reset_update_gain_variables();
540 : }
541 : }
542 : }
543 : FALLTHROUGH;
544 :
545 : case Step::ABORT:
546 : // Recover from failed test or move on after a successful one
547 :
548 2329 : attitude_control->input_euler_angle_roll_pitch_yaw_rad(desired_roll_rad, desired_pitch_rad, desired_yaw_rad, true);
549 :
550 2329 : load_gains(GainType::INTRA_TEST);
551 :
552 2329 : step = Step::WAITING_FOR_LEVEL;
553 2329 : positive_direction = reverse_test_direction();
554 2329 : step_start_time_ms = now_ms;
555 2329 : level_start_time_ms = now_ms;
556 2329 : step_timeout_ms = AUTOTUNE_REQUIRED_LEVEL_TIME_MS;
557 2329 : break;
558 : }
559 1779135 : }
560 :
561 : // backup_gains_and_initialise - store current gains as originals
562 : // called before tuning starts to backup original gains
563 12 : void AC_AutoTune::backup_gains_and_initialise()
564 : {
565 12 : const uint32_t now_ms = AP_HAL::millis();
566 :
567 : // initialise state because this is our first time
568 12 : if (roll_enabled()) {
569 8 : axis = AxisType::ROLL;
570 4 : } else if (pitch_enabled()) {
571 1 : axis = AxisType::PITCH;
572 3 : } else if (yaw_enabled()) {
573 3 : axis = AxisType::YAW;
574 0 : } else if (yaw_d_enabled()) {
575 0 : axis = AxisType::YAW_D;
576 : }
577 : // no axes are complete
578 12 : axes_completed = 0;
579 :
580 : // reset update gain variables for each vehicle
581 12 : reset_update_gain_variables();
582 :
583 : // start at the beginning of tune sequence
584 12 : next_tune_type(tune_type, true);
585 :
586 12 : step = Step::WAITING_FOR_LEVEL;
587 12 : positive_direction = false;
588 12 : step_start_time_ms = now_ms;
589 12 : level_start_time_ms = now_ms;
590 12 : step_scaler = 1.0f;
591 :
592 12 : desired_yaw_rad = ahrs_view->get_yaw_rad();
593 12 : }
594 :
595 : /*
596 : load a specified set of gains
597 : */
598 1795259 : void AC_AutoTune::load_gains(enum GainType gain_type)
599 : {
600 1795259 : if (loaded_gains == gain_type) {
601 : // Loaded gains are already of correct type
602 1790561 : return;
603 : }
604 4698 : loaded_gains = gain_type;
605 :
606 4698 : switch (gain_type) {
607 25 : case GainType::ORIGINAL:
608 25 : load_orig_gains();
609 25 : break;
610 2341 : case GainType::INTRA_TEST:
611 2341 : load_intra_test_gains();
612 2341 : break;
613 2329 : case GainType::TEST:
614 2329 : load_test_gains();
615 2329 : break;
616 3 : case GainType::TUNED:
617 3 : load_tuned_gains();
618 3 : break;
619 : }
620 : }
621 :
622 : // update_gcs - send message to ground station
623 48 : void AC_AutoTune::update_gcs(uint8_t message_id) const
624 : {
625 48 : switch (message_id) {
626 12 : case AUTOTUNE_MESSAGE_STARTED:
627 12 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,"AutoTune: Started");
628 12 : break;
629 11 : case AUTOTUNE_MESSAGE_STOPPED:
630 11 : GCS_SEND_TEXT(MAV_SEVERITY_INFO,"AutoTune: Stopped");
631 11 : break;
632 12 : case AUTOTUNE_MESSAGE_SUCCESS:
633 12 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: Success");
634 12 : break;
635 0 : case AUTOTUNE_MESSAGE_FAILED:
636 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: Failed");
637 0 : break;
638 12 : case AUTOTUNE_MESSAGE_TESTING:
639 : case AUTOTUNE_MESSAGE_SAVED_GAINS:
640 12 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: %s gains for %s%s%s%s",
641 : (message_id == AUTOTUNE_MESSAGE_SAVED_GAINS) ? "Saved" : "Pilot Testing",
642 : (axes_completed&AUTOTUNE_AXIS_BITMASK_ROLL)?"Roll ":"",
643 : (axes_completed&AUTOTUNE_AXIS_BITMASK_PITCH)?"Pitch ":"",
644 : (axes_completed&AUTOTUNE_AXIS_BITMASK_YAW)?"Yaw(E)":"",
645 : (axes_completed&AUTOTUNE_AXIS_BITMASK_YAW_D)?"Yaw(D)":"");
646 12 : break;
647 1 : case AUTOTUNE_MESSAGE_TESTING_END:
648 1 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE,"AutoTune: original gains restored");
649 1 : break;
650 : }
651 48 : }
652 :
653 : // axis helper functions
654 2387 : bool AC_AutoTune::roll_enabled() const
655 : {
656 2387 : return get_axis_bitmask() & AUTOTUNE_AXIS_BITMASK_ROLL;
657 : }
658 :
659 2387 : bool AC_AutoTune::pitch_enabled() const
660 : {
661 2387 : return get_axis_bitmask() & AUTOTUNE_AXIS_BITMASK_PITCH;
662 : }
663 :
664 2397 : bool AC_AutoTune::yaw_enabled() const
665 : {
666 2397 : return get_axis_bitmask() & AUTOTUNE_AXIS_BITMASK_YAW;
667 : }
668 :
669 23 : bool AC_AutoTune::yaw_d_enabled() const
670 : {
671 : #if APM_BUILD_TYPE(APM_BUILD_Heli)
672 : return false;
673 : #else
674 23 : return get_axis_bitmask() & AUTOTUNE_AXIS_BITMASK_YAW_D;
675 : #endif
676 : }
677 :
678 : /*
679 : check if we have a good position estimate
680 : */
681 129524 : bool AC_AutoTune::position_ok(void)
682 : {
683 129524 : if (!AP::ahrs().have_inertial_nav()) {
684 : // do not allow navigation with dcm position
685 0 : return false;
686 : }
687 :
688 : // with EKF use filter status and ekf check
689 129524 : nav_filter_status filt_status {};
690 129524 : AP::ahrs().get_filter_status(filt_status);
691 :
692 : // require a good absolute position and EKF must not be in const_pos_mode
693 129524 : return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
694 : }
695 :
696 : // get attitude for slow position hold in autotune mode
697 1792904 : void AC_AutoTune::get_poshold_attitude_rad(float &roll_out_rad, float &pitch_out_rad, float &yaw_out_rad)
698 : {
699 1792904 : roll_out_rad = pitch_out_rad = 0;
700 :
701 1792904 : if (!use_poshold) {
702 : // we are not trying to hold position
703 1773124 : return;
704 : }
705 :
706 : // do we know where we are? If not then don't do poshold
707 487048 : if (!position_ok()) {
708 0 : return;
709 : }
710 :
711 487048 : if (!have_position) {
712 3 : have_position = true;
713 3 : start_position_ned_m = pos_control->get_pos_estimate_NED_m();
714 : }
715 :
716 : // don't go past 10 degrees, as autotune result would deteriorate too much
717 487048 : const float angle_max_rad = radians(10.0);
718 :
719 : // hit the 10 degree limit at 20 meters position error
720 487048 : const float dist_limit_m = 20.00;
721 :
722 : // we only start adjusting yaw if we are more than 5m from the
723 : // target position. That corresponds to a lean angle of 2.5 degrees
724 487048 : const float yaw_dist_limit_m = 5.0;
725 :
726 487048 : Vector3f pos_error_ned_m = (pos_control->get_pos_estimate_NED_m() - start_position_ned_m).tofloat();
727 487048 : pos_error_ned_m.z = 0;
728 487048 : float dist_m = pos_error_ned_m.length();
729 487048 : if (dist_m < 0.10) {
730 : // don't do anything within 10cm
731 28149 : return;
732 : }
733 :
734 : /*
735 : very simple linear controller
736 : */
737 458899 : float scaling = constrain_float(angle_max_rad * dist_m / dist_limit_m, 0, angle_max_rad);
738 458899 : Vector2f angle_ne(pos_error_ned_m.x, pos_error_ned_m.y);
739 458899 : angle_ne *= scaling / dist_m;
740 :
741 : // rotate into body frame
742 458899 : pitch_out_rad = angle_ne.x * ahrs_view->cos_yaw() + angle_ne.y * ahrs_view->sin_yaw();
743 458899 : roll_out_rad = angle_ne.x * ahrs_view->sin_yaw() - angle_ne.y * ahrs_view->cos_yaw();
744 :
745 458899 : if (dist_m < yaw_dist_limit_m) {
746 : // no yaw adjustment
747 439119 : return;
748 : }
749 :
750 : /*
751 : also point so that test occurs perpendicular to the wind,
752 : if we have drifted more than yaw_dist_limit_m from the desired
753 : position. This ensures that autotune doesn't have to deal with
754 : more than 2.5 degrees of attitude on the axis it is tuning
755 : */
756 19780 : float target_yaw_rad = atan2f(pos_error_ned_m.y, pos_error_ned_m.x);
757 19780 : if (axis == AxisType::PITCH) {
758 : // for roll and yaw tuning we point along the wind, for pitch
759 : // we point across the wind
760 9317 : target_yaw_rad += radians(90);
761 : }
762 : // go to the nearest 180 degree mark, with 5 degree slop to prevent oscillation
763 19780 : if (fabsf(wrap_PI(yaw_out_rad - target_yaw_rad)) > radians(95.0)) {
764 5990 : target_yaw_rad += radians(180.0);
765 : }
766 :
767 19780 : yaw_out_rad = target_yaw_rad;
768 : }
769 :
770 : // get the next tune type
771 125 : void AC_AutoTune::next_tune_type(TuneType &curr_tune_type, bool reset)
772 : {
773 125 : if (reset) {
774 37 : set_tune_sequence();
775 37 : tune_seq_index = 0;
776 88 : } else if (curr_tune_type == TuneType::TUNE_COMPLETE) {
777 : // leave tune_type as TUNE_COMPLETE to initiate next axis or exit autotune
778 0 : return;
779 : } else {
780 88 : tune_seq_index++;
781 : }
782 :
783 125 : curr_tune_type = tune_seq[tune_seq_index];
784 : }
785 :
786 : #endif // AC_AUTOTUNE_ENABLED
|
