forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 18
/
AC_AutoTune.cpp
1779 lines (1624 loc) · 74.9 KB
/
AC_AutoTune.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#include "AC_AutoTune.h"
#include <GCS_MAVLink/GCS.h>
#include <AP_Scheduler/AP_Scheduler.h>
/*
* autotune support for multicopters
*
* Instructions:
* 1) Set up one flight mode switch position to be AltHold.
* 2) Set the Ch7 Opt or Ch8 Opt to AutoTune to allow you to turn the auto tuning on/off with the ch7 or ch8 switch.
* 3) Ensure the ch7 or ch8 switch is in the LOW position.
* 4) Wait for a calm day and go to a large open area.
* 5) Take off and put the vehicle into AltHold mode at a comfortable altitude.
* 6) Set the ch7/ch8 switch to the HIGH position to engage auto tuning:
* a) You will see it twitch about 20 degrees left and right for a few minutes, then it will repeat forward and back.
* b) Use the roll and pitch stick at any time to reposition the copter if it drifts away (it will use the original PID gains during repositioning and between tests).
* When you release the sticks it will continue auto tuning where it left off.
* c) Move the ch7/ch8 switch into the LOW position at any time to abandon the autotuning and return to the origin PIDs.
* d) Make sure that you do not have any trim set on your transmitter or the autotune may not get the signal that the sticks are centered.
* 7) When the tune completes the vehicle will change back to the original PID gains.
* 8) Put the ch7/ch8 switch into the LOW position then back to the HIGH position to test the tuned PID gains.
* 9) Put the ch7/ch8 switch into the LOW position to fly using the original PID gains.
* 10) If you are happy with the autotuned PID gains, leave the ch7/ch8 switch in the HIGH position, land and disarm to save the PIDs permanently.
* If you DO NOT like the new PIDS, switch ch7/ch8 LOW to return to the original PIDs. The gains will not be saved when you disarm
*
* What it's doing during each "twitch":
* a) invokes 90 deg/sec rate request
* b) records maximum "forward" roll rate and bounce back rate
* c) when copter reaches 20 degrees or 1 second has passed, it commands level
* d) tries to keep max rotation rate between 80% ~ 100% of requested rate (90deg/sec) by adjusting rate P
* e) increases rate D until the bounce back becomes greater than 10% of requested rate (90deg/sec)
* f) decreases rate D until the bounce back becomes less than 10% of requested rate (90deg/sec)
* g) increases rate P until the max rotate rate becomes greater than the request rate (90deg/sec)
* h) invokes a 20deg angle request on roll or pitch
* i) increases stab P until the maximum angle becomes greater than 110% of the requested angle (20deg)
* j) decreases stab P by 25%
*
*/
#define AUTOTUNE_AXIS_BITMASK_ROLL 1
#define AUTOTUNE_AXIS_BITMASK_PITCH 2
#define AUTOTUNE_AXIS_BITMASK_YAW 4
#define AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS 500 // restart tuning if pilot has left sticks in middle for 2 seconds
#define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 1000U // timeout for tuning mode's testing step
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
# define AUTOTUNE_LEVEL_ANGLE_CD 500 // angle which qualifies as level (Plane uses more relaxed 5deg)
# define AUTOTUNE_LEVEL_RATE_RP_CD 1000 // rate which qualifies as level for roll and pitch (Plane uses more relaxed 10deg/sec)
#else
# define AUTOTUNE_LEVEL_ANGLE_CD 250 // angle which qualifies as level
# define AUTOTUNE_LEVEL_RATE_RP_CD 500 // rate which qualifies as level for roll and pitch
#endif
#define AUTOTUNE_LEVEL_RATE_Y_CD 750 // rate which qualifies as level for yaw
#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the aircraft to be level
#define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level (relaxes criteria)
#define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000 // level failure warning messages sent at this interval to users
#define AUTOTUNE_RD_STEP 0.05f // minimum increment when increasing/decreasing Rate D term
#define AUTOTUNE_RP_STEP 0.05f // minimum increment when increasing/decreasing Rate P term
#define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term
#define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing
#define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing
#define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing
#define AUTOTUNE_RD_MAX 0.200f // maximum Rate D value
#define AUTOTUNE_RLPF_MIN 1.0f // minimum Rate Yaw filter value
#define AUTOTUNE_RLPF_MAX 5.0f // maximum Rate Yaw filter value
#define AUTOTUNE_RP_MIN 0.01f // minimum Rate P value
#define AUTOTUNE_RP_MAX 2.0f // maximum Rate P value
#define AUTOTUNE_SP_MAX 20.0f // maximum Stab P value
#define AUTOTUNE_SP_MIN 0.5f // maximum Stab P value
#define AUTOTUNE_RP_ACCEL_MIN 4000.0f // Minimum acceleration for Roll and Pitch
#define AUTOTUNE_Y_ACCEL_MIN 1000.0f // Minimum acceleration for Yaw
#define AUTOTUNE_Y_FILT_FREQ 10.0f // Autotune filter frequency when testing Yaw
#define AUTOTUNE_SUCCESS_COUNT 4 // The number of successful iterations we need to freeze at current gains
#define AUTOTUNE_D_UP_DOWN_MARGIN 0.2f // The margin below the target that we tune D in
#define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning
#define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning
#define AUTOTUNE_SP_BACKOFF 0.9f // Stab P gains are reduced to 90% of their maximum value discovered during tuning
#define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration
#define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration
// roll and pitch axes
#define AUTOTUNE_TARGET_ANGLE_RLLPIT_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_RATE_RLLPIT_CDS 18000 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step
#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step
// yaw axis
#define AUTOTUNE_TARGET_ANGLE_YAW_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step
#define AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD 500 // minimum target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step
// Auto Tune message ids for ground station
#define AUTOTUNE_MESSAGE_STARTED 0
#define AUTOTUNE_MESSAGE_STOPPED 1
#define AUTOTUNE_MESSAGE_SUCCESS 2
#define AUTOTUNE_MESSAGE_FAILED 3
#define AUTOTUNE_MESSAGE_SAVED_GAINS 4
#define AUTOTUNE_MESSAGE_TESTING 5
#define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000
// second table of user settable parameters for quadplanes, this
// allows us to go beyond the 64 parameter limit
const AP_Param::GroupInfo AC_AutoTune::var_info[] = {
// @Param: AXES
// @DisplayName: Autotune axis bitmask
// @Description: 1-byte bitmap of axes to autotune
// @Values: 7:All,1:Roll Only,2:Pitch Only,4:Yaw Only,3:Roll and Pitch,5:Roll and Yaw,6:Pitch and Yaw
// @Bitmask: 0:Roll,1:Pitch,2:Yaw
// @User: Standard
AP_GROUPINFO("AXES", 1, AC_AutoTune, axis_bitmask, 7), // AUTOTUNE_AXIS_BITMASK_DEFAULT
// @Param: AGGR
// @DisplayName: Autotune aggressiveness
// @Description: Autotune aggressiveness. Defines the bounce back used to detect size of the D term.
// @Range: 0.05 0.10
// @User: Standard
AP_GROUPINFO("AGGR", 2, AC_AutoTune, aggressiveness, 0.1f),
// @Param: MIN_D
// @DisplayName: AutoTune minimum D
// @Description: Defines the minimum D gain
// @Range: 0.001 0.006
// @User: Standard
AP_GROUPINFO("MIN_D", 3, AC_AutoTune, min_d, 0.001f),
AP_GROUPEND
};
AC_AutoTune::AC_AutoTune()
{
AP_Param::setup_object_defaults(this, var_info);
}
// autotune_init - should be called when autotune mode is selected
bool AC_AutoTune::init_internals(bool _use_poshold,
AC_AttitudeControl_Multi *_attitude_control,
AC_PosControl *_pos_control,
AP_AHRS_View *_ahrs_view,
AP_InertialNav *_inertial_nav)
{
bool success = true;
use_poshold = _use_poshold;
attitude_control = _attitude_control;
pos_control = _pos_control;
ahrs_view = _ahrs_view;
inertial_nav = _inertial_nav;
motors = AP_Motors::get_singleton();
switch (mode) {
case FAILED:
// autotune has been run but failed so reset state to uninitialized
mode = UNINITIALISED;
// fall through to restart the tuning
FALLTHROUGH;
case UNINITIALISED:
// autotune has never been run
success = start();
if (success) {
// so store current gains as original gains
backup_gains_and_initialise();
// advance mode to tuning
mode = TUNING;
// send message to ground station that we've started tuning
update_gcs(AUTOTUNE_MESSAGE_STARTED);
}
break;
case TUNING:
// we are restarting tuning after the user must have switched ch7/ch8 off so we restart tuning where we left off
success = start();
if (success) {
// reset gains to tuning-start gains (i.e. low I term)
load_gains(GAIN_INTRA_TEST);
AP::logger().Write_Event(LogEvent::AUTOTUNE_RESTART);
update_gcs(AUTOTUNE_MESSAGE_STARTED);
}
break;
case SUCCESS:
// we have completed a tune and the pilot wishes to test the new gains in the current flight mode
// so simply apply tuning gains (i.e. do not change flight mode)
load_gains(GAIN_TUNED);
update_gcs(AUTOTUNE_MESSAGE_TESTING);
AP::logger().Write_Event(LogEvent::AUTOTUNE_PILOT_TESTING);
break;
}
have_position = false;
return success;
}
// stop - should be called when the ch7/ch8 switch is switched OFF
void AC_AutoTune::stop()
{
// set gains to their original values
load_gains(GAIN_ORIGINAL);
// re-enable angle-to-rate request limits
attitude_control->use_sqrt_controller(true);
update_gcs(AUTOTUNE_MESSAGE_STOPPED);
AP::logger().Write_Event(LogEvent::AUTOTUNE_OFF);
// Note: we leave the mode as it was so that we know how the autotune ended
// we expect the caller will change the flight mode back to the flight mode indicated by the flight mode switch
}
// start - Initialize autotune flight mode
bool AC_AutoTune::start(void)
{
if (!motors->armed()) {
return false;
}
// initialize vertical speeds and leash lengths
init_z_limits();
// initialise position and desired velocity
if (!pos_control->is_active_z()) {
pos_control->set_alt_target_to_current_alt();
pos_control->set_desired_velocity_z(inertial_nav->get_velocity_z());
}
return true;
}
const char *AC_AutoTune::level_issue_string() const
{
switch (level_problem.issue) {
case LevelIssue::NONE:
return "None";
case LevelIssue::ANGLE_ROLL:
return "Angle(R)";
case LevelIssue::ANGLE_PITCH:
return "Angle(P)";
case LevelIssue::ANGLE_YAW:
return "Angle(Y)";
case LevelIssue::RATE_ROLL:
return "Rate(R)";
case LevelIssue::RATE_PITCH:
return "Rate(P)";
case LevelIssue::RATE_YAW:
return "Rate(Y)";
}
return "Bug";
}
void AC_AutoTune::send_step_string()
{
if (pilot_override) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Paused: Pilot Override Active");
return;
}
switch (step) {
case WAITING_FOR_LEVEL:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Leveling (%s %4.1f > %4.1f)", level_issue_string(), (double)(level_problem.current*0.01f), (double)(level_problem.maximum*0.01f));
return;
case UPDATE_GAINS:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Updating Gains");
return;
case TWITCHING:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Twitching");
return;
}
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: unknown step");
}
const char *AC_AutoTune::type_string() const
{
switch (tune_type) {
case RD_UP:
return "Rate D Up";
case RD_DOWN:
return "Rate D Down";
case RP_UP:
return "Rate P Up";
case SP_DOWN:
return "Angle P Down";
case SP_UP:
return "Angle P Up";
}
return "Bug";
}
void AC_AutoTune::do_gcs_announcements()
{
const uint32_t now = AP_HAL::millis();
if (now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS) {
return;
}
float tune_rp = 0.0f;
float tune_rd = 0.0f;
float tune_sp = 0.0f;
float tune_accel = 0.0f;
char axis_char = '?';
switch (axis) {
case ROLL:
tune_rp = tune_roll_rp;
tune_rd = tune_roll_rd;
tune_sp = tune_roll_sp;
tune_accel = tune_roll_accel;
axis_char = 'R';
break;
case PITCH:
tune_rp = tune_pitch_rp;
tune_rd = tune_pitch_rd;
tune_sp = tune_pitch_sp;
tune_accel = tune_pitch_accel;
axis_char = 'P';
break;
case YAW:
tune_rp = tune_yaw_rp;
tune_rd = tune_yaw_rLPF;
tune_sp = tune_yaw_sp;
tune_accel = tune_yaw_accel;
axis_char = 'Y';
break;
}
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: (%c) %s", axis_char, type_string());
send_step_string();
if (!is_zero(lean_angle)) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: lean=%f target=%f", (double)lean_angle, (double)target_angle);
}
if (!is_zero(rotation_rate)) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: rotation=%f target=%f", (double)(rotation_rate*0.01f), (double)(target_rate*0.01f));
}
switch (tune_type) {
case RD_UP:
case RD_DOWN:
case RP_UP:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f d=%f", (double)tune_rp, (double)tune_rd);
break;
case SP_DOWN:
case SP_UP:
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: p=%f accel=%f", (double)tune_sp, (double)tune_accel);
break;
}
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: success %u/%u", counter, AUTOTUNE_SUCCESS_COUNT);
announce_time = now;
}
// run - runs the autotune flight mode
// should be called at 100hz or more
void AC_AutoTune::run()
{
// initialize vertical speeds and acceleration
init_z_limits();
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
// this should not actually be possible because of the init() checks
if (!motors->armed() || !motors->get_interlock()) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control->set_throttle_out(0.0f, true, 0.0f);
pos_control->relax_alt_hold_controllers(0.0f);
return;
}
float target_roll_cd, target_pitch_cd, target_yaw_rate_cds;
get_pilot_desired_rp_yrate_cd(target_roll_cd, target_pitch_cd, target_yaw_rate_cds);
// get pilot desired climb rate
const float target_climb_rate_cms = get_pilot_desired_climb_rate_cms();
const bool zero_rp_input = is_zero(target_roll_cd) && is_zero(target_pitch_cd);
const uint32_t now = AP_HAL::millis();
if (!zero_rp_input || !is_zero(target_yaw_rate_cds) || !is_zero(target_climb_rate_cms)) {
if (!pilot_override) {
pilot_override = true;
// set gains to their original values
load_gains(GAIN_ORIGINAL);
attitude_control->use_sqrt_controller(true);
}
// reset pilot override time
override_time = now;
if (!zero_rp_input) {
// only reset position on roll or pitch input
have_position = false;
}
} else if (pilot_override) {
// check if we should resume tuning after pilot's override
if (now - override_time > AUTOTUNE_PILOT_OVERRIDE_TIMEOUT_MS) {
pilot_override = false; // turn off pilot override
// set gains to their intra-test values (which are very close to the original gains)
// load_gains(GAIN_INTRA_TEST); //I think we should be keeping the originals here to let the I term settle quickly
step = WAITING_FOR_LEVEL; // set tuning step back from beginning
step_start_time_ms = now;
level_start_time_ms = now;
desired_yaw_cd = ahrs_view->yaw_sensor;
}
}
if (pilot_override) {
if (now - last_pilot_override_warning > 1000) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: pilot overrides active");
last_pilot_override_warning = now;
}
}
if (zero_rp_input) {
// pilot input on throttle and yaw will still use position hold if enabled
get_poshold_attitude(target_roll_cd, target_pitch_cd, desired_yaw_cd);
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// if pilot override call attitude controller
if (pilot_override || mode != TUNING) {
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll_cd, target_pitch_cd, target_yaw_rate_cds);
} else {
// somehow get attitude requests from autotuning
control_attitude();
// tell the user what's going on
do_gcs_announcements();
}
// call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate_cms, AP::scheduler().get_last_loop_time_s(), false);
pos_control->update_z_controller();
}
bool AC_AutoTune::check_level(const LevelIssue issue, const float current, const float maximum)
{
if (current > maximum) {
level_problem.current = current;
level_problem.maximum = maximum;
level_problem.issue = issue;
return false;
}
return true;
}
bool AC_AutoTune::currently_level()
{
float threshold_mul = 1.0;
uint32_t now_ms = AP_HAL::millis();
if (now_ms - level_start_time_ms > AUTOTUNE_LEVEL_TIMEOUT_MS) {
// after a long wait we use looser threshold, to allow tuning
// with poor initial gains
threshold_mul *= 2;
}
// display warning if vehicle fails to level
if ((now_ms - level_start_time_ms > AUTOTUNE_LEVEL_WARNING_INTERVAL_MS) &&
(now_ms - level_fail_warning_time_ms > AUTOTUNE_LEVEL_WARNING_INTERVAL_MS)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "AutoTune: failing to level, manual tune may be required");
level_fail_warning_time_ms = now_ms;
}
if (!check_level(LevelIssue::ANGLE_ROLL,
fabsf(ahrs_view->roll_sensor - roll_cd),
threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD)) {
return false;
}
if (!check_level(LevelIssue::ANGLE_PITCH,
fabsf(ahrs_view->pitch_sensor - pitch_cd),
threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD)) {
return false;
}
if (!check_level(LevelIssue::ANGLE_YAW,
fabsf(wrap_180_cd(ahrs_view->yaw_sensor - desired_yaw_cd)),
threshold_mul*AUTOTUNE_LEVEL_ANGLE_CD)) {
return false;
}
if (!check_level(LevelIssue::RATE_ROLL,
(ToDeg(ahrs_view->get_gyro().x) * 100.0f),
threshold_mul*AUTOTUNE_LEVEL_RATE_RP_CD)) {
return false;
}
if (!check_level(LevelIssue::RATE_PITCH,
(ToDeg(ahrs_view->get_gyro().y) * 100.0f),
threshold_mul*AUTOTUNE_LEVEL_RATE_RP_CD)) {
return false;
}
if (!check_level(LevelIssue::RATE_YAW,
(ToDeg(ahrs_view->get_gyro().z) * 100.0f),
threshold_mul*AUTOTUNE_LEVEL_RATE_Y_CD)) {
return false;
}
return true;
}
// attitude_controller - sets attitude control targets during tuning
void AC_AutoTune::control_attitude()
{
rotation_rate = 0.0f; // rotation rate in radians/second
lean_angle = 0.0f;
const float direction_sign = positive_direction ? 1.0f : -1.0f;
const uint32_t now = AP_HAL::millis();
// check tuning step
switch (step) {
case WAITING_FOR_LEVEL: {
// Note: we should be using intra-test gains (which are very close to the original gains but have lower I)
// re-enable rate limits
attitude_control->use_sqrt_controller(true);
get_poshold_attitude(roll_cd, pitch_cd, desired_yaw_cd);
// hold level attitude
attitude_control->input_euler_angle_roll_pitch_yaw(roll_cd, pitch_cd, desired_yaw_cd, true);
// hold the copter level for 0.5 seconds before we begin a twitch
// reset counter if we are no longer level
if (!currently_level()) {
step_start_time_ms = now;
}
// if we have been level for a sufficient amount of time (0.5 seconds) move onto tuning step
if (now - step_start_time_ms > AUTOTUNE_REQUIRED_LEVEL_TIME_MS) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Twitch");
// initiate variables for next step
step = TWITCHING;
step_start_time_ms = now;
step_time_limit_ms = AUTOTUNE_TESTING_STEP_TIMEOUT_MS;
twitch_first_iter = true;
test_rate_max = 0.0f;
test_rate_min = 0.0f;
test_angle_max = 0.0f;
test_angle_min = 0.0f;
rotation_rate_filt.reset(0.0f);
rate_max = 0.0f;
// set gains to their to-be-tested values
load_gains(GAIN_TWITCH);
} else {
// when waiting for level we use the intra-test gains
load_gains(GAIN_INTRA_TEST);
}
float target_max_rate;
switch (axis) {
case ROLL:
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
start_rate = ToDeg(ahrs_view->get_gyro().x) * 100.0f;
start_angle = ahrs_view->roll_sensor;
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz()*2.0f);
break;
case PITCH:
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
start_rate = ToDeg(ahrs_view->get_gyro().y) * 100.0f;
start_angle = ahrs_view->pitch_sensor;
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz()*2.0f);
break;
case YAW:
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS);
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD);
abort_angle = AUTOTUNE_TARGET_ANGLE_YAW_CD;
start_rate = ToDeg(ahrs_view->get_gyro().z) * 100.0f;
start_angle = ahrs_view->yaw_sensor;
rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ);
break;
}
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
rotation_rate_filt.reset(start_rate);
} else {
rotation_rate_filt.reset(0);
}
break;
}
case TWITCHING: {
// Run the twitching step
load_gains(GAIN_TWITCH);
// disable rate limits
attitude_control->use_sqrt_controller(false);
// hold current attitude
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
// step angle targets on first iteration
if (twitch_first_iter) {
twitch_first_iter = false;
// Testing increasing stabilize P gain so will set lean angle target
switch (axis) {
case ROLL:
// request roll to 20deg
attitude_control->input_angle_step_bf_roll_pitch_yaw(direction_sign * target_angle, 0.0f, 0.0f);
break;
case PITCH:
// request pitch to 20deg
attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, direction_sign * target_angle, 0.0f);
break;
case YAW:
// request pitch to 20deg
attitude_control->input_angle_step_bf_roll_pitch_yaw(0.0f, 0.0f, direction_sign * target_angle);
break;
}
}
} else {
// Testing rate P and D gains so will set body-frame rate targets.
// Rate controller will use existing body-frame rates and convert to motor outputs
// for all axes except the one we override here.
switch (axis) {
case ROLL:
// override body-frame roll rate
attitude_control->rate_bf_roll_target(direction_sign * target_rate + start_rate);
break;
case PITCH:
// override body-frame pitch rate
attitude_control->rate_bf_pitch_target(direction_sign * target_rate + start_rate);
break;
case YAW:
// override body-frame yaw rate
attitude_control->rate_bf_yaw_target(direction_sign * target_rate + start_rate);
break;
}
}
// capture this iterations rotation rate and lean angle
float gyro_reading = 0;
switch (axis) {
case ROLL:
gyro_reading = ahrs_view->get_gyro().x;
lean_angle = direction_sign * (ahrs_view->roll_sensor - (int32_t)start_angle);
break;
case PITCH:
gyro_reading = ahrs_view->get_gyro().y;
lean_angle = direction_sign * (ahrs_view->pitch_sensor - (int32_t)start_angle);
break;
case YAW:
gyro_reading = ahrs_view->get_gyro().z;
lean_angle = direction_sign * wrap_180_cd(ahrs_view->yaw_sensor-(int32_t)start_angle);
break;
}
// Add filter to measurements
float filter_value;
switch (tune_type) {
case SP_DOWN:
case SP_UP:
filter_value = direction_sign * (ToDeg(gyro_reading) * 100.0f);
break;
default:
filter_value = direction_sign * (ToDeg(gyro_reading) * 100.0f - start_rate);
break;
}
rotation_rate = rotation_rate_filt.apply(filter_value,
AP::scheduler().get_loop_period_s());
switch (tune_type) {
case RD_UP:
case RD_DOWN:
twitching_test_rate(rotation_rate, target_rate, test_rate_min, test_rate_max);
twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max);
twitching_abort_rate(lean_angle, rotation_rate, abort_angle, test_rate_min);
break;
case RP_UP:
twitching_test_rate(rotation_rate, target_rate*(1+0.5f*aggressiveness), test_rate_min, test_rate_max);
twitching_measure_acceleration(test_accel_max, rotation_rate, rate_max);
twitching_abort_rate(lean_angle, rotation_rate, abort_angle, test_rate_min);
break;
case SP_DOWN:
case SP_UP:
twitching_test_angle(lean_angle, rotation_rate, target_angle*(1+0.5f*aggressiveness), test_angle_min, test_angle_max, test_rate_min, test_rate_max);
twitching_measure_acceleration(test_accel_max, rotation_rate - direction_sign * start_rate, rate_max);
break;
}
// Check for failure causing reverse response
if (lean_angle <= -AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD) {
step = WAITING_FOR_LEVEL;
}
// log this iterations lean angle and rotation rate
Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control);
log_pids();
break;
}
case UPDATE_GAINS:
// re-enable rate limits
attitude_control->use_sqrt_controller(true);
// log the latest gains
if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) {
switch (axis) {
case ROLL:
Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max);
break;
case PITCH:
Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max);
break;
case YAW:
Log_Write_AutoTune(axis, tune_type, target_angle, test_angle_min, test_angle_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max);
break;
}
} else {
switch (axis) {
case ROLL:
Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_roll_rp, tune_roll_rd, tune_roll_sp, test_accel_max);
break;
case PITCH:
Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_pitch_rp, tune_pitch_rd, tune_pitch_sp, test_accel_max);
break;
case YAW:
Log_Write_AutoTune(axis, tune_type, target_rate, test_rate_min, test_rate_max, tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, test_accel_max);
break;
}
}
// Check results after mini-step to increase rate D gain
switch (tune_type) {
case RD_UP:
switch (axis) {
case ROLL:
updating_rate_d_up(tune_roll_rd, min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case PITCH:
updating_rate_d_up(tune_pitch_rd, min_d, AUTOTUNE_RD_MAX, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case YAW:
updating_rate_d_up(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RLPF_MAX, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
}
break;
// Check results after mini-step to decrease rate D gain
case RD_DOWN:
switch (axis) {
case ROLL:
updating_rate_d_down(tune_roll_rd, min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case PITCH:
updating_rate_d_down(tune_pitch_rd, min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case YAW:
updating_rate_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
}
break;
// Check results after mini-step to increase rate P gain
case RP_UP:
switch (axis) {
case ROLL:
updating_rate_p_up_d_down(tune_roll_rd, min_d, AUTOTUNE_RD_STEP, tune_roll_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case PITCH:
updating_rate_p_up_d_down(tune_pitch_rd, min_d, AUTOTUNE_RD_STEP, tune_pitch_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
case YAW:
updating_rate_p_up_d_down(tune_yaw_rLPF, AUTOTUNE_RLPF_MIN, AUTOTUNE_RD_STEP, tune_yaw_rp, AUTOTUNE_RP_MIN, AUTOTUNE_RP_MAX, AUTOTUNE_RP_STEP, target_rate, test_rate_min, test_rate_max);
break;
}
break;
// Check results after mini-step to increase stabilize P gain
case SP_DOWN:
switch (axis) {
case ROLL:
updating_angle_p_down(tune_roll_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
case PITCH:
updating_angle_p_down(tune_pitch_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
case YAW:
updating_angle_p_down(tune_yaw_sp, AUTOTUNE_SP_MIN, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
}
break;
// Check results after mini-step to increase stabilize P gain
case SP_UP:
switch (axis) {
case ROLL:
updating_angle_p_up(tune_roll_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
case PITCH:
updating_angle_p_up(tune_pitch_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
case YAW:
updating_angle_p_up(tune_yaw_sp, AUTOTUNE_SP_MAX, AUTOTUNE_SP_STEP, target_angle, test_angle_max, test_rate_min, test_rate_max);
break;
}
break;
}
// we've complete this step, finalize pids and move to next step
if (counter >= AUTOTUNE_SUCCESS_COUNT) {
// reset counter
counter = 0;
// reset scaling factor
step_scaler = 1.0f;
// move to the next tuning type
switch (tune_type) {
case RD_UP:
tune_type = TuneType(tune_type + 1);
break;
case RD_DOWN:
tune_type = TuneType(tune_type + 1);
switch (axis) {
case ROLL:
tune_roll_rd = MAX(min_d, tune_roll_rd * AUTOTUNE_RD_BACKOFF);
tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RD_BACKOFF);
break;
case PITCH:
tune_pitch_rd = MAX(min_d, tune_pitch_rd * AUTOTUNE_RD_BACKOFF);
tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RD_BACKOFF);
break;
case YAW:
tune_yaw_rLPF = MAX(AUTOTUNE_RLPF_MIN, tune_yaw_rLPF * AUTOTUNE_RD_BACKOFF);
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RD_BACKOFF);
break;
}
break;
case RP_UP:
tune_type = TuneType(tune_type + 1);
switch (axis) {
case ROLL:
tune_roll_rp = MAX(AUTOTUNE_RP_MIN, tune_roll_rp * AUTOTUNE_RP_BACKOFF);
break;
case PITCH:
tune_pitch_rp = MAX(AUTOTUNE_RP_MIN, tune_pitch_rp * AUTOTUNE_RP_BACKOFF);
break;
case YAW:
tune_yaw_rp = MAX(AUTOTUNE_RP_MIN, tune_yaw_rp * AUTOTUNE_RP_BACKOFF);
break;
}
break;
case SP_DOWN:
tune_type = TuneType(tune_type + 1);
break;
case SP_UP:
// we've reached the end of a D-up-down PI-up-down tune type cycle
tune_type = RD_UP;
// advance to the next axis
bool complete = false;
switch (axis) {
case ROLL:
axes_completed |= AUTOTUNE_AXIS_BITMASK_ROLL;
tune_roll_sp = MAX(AUTOTUNE_SP_MIN, tune_roll_sp * AUTOTUNE_SP_BACKOFF);
tune_roll_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
if (pitch_enabled()) {
axis = PITCH;
} else if (yaw_enabled()) {
axis = YAW;
} else {
complete = true;
}
break;
case PITCH:
axes_completed |= AUTOTUNE_AXIS_BITMASK_PITCH;
tune_pitch_sp = MAX(AUTOTUNE_SP_MIN, tune_pitch_sp * AUTOTUNE_SP_BACKOFF);
tune_pitch_accel = MAX(AUTOTUNE_RP_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_RP_BACKOFF);
if (yaw_enabled()) {
axis = YAW;
} else {
complete = true;
}
break;
case YAW:
axes_completed |= AUTOTUNE_AXIS_BITMASK_YAW;
tune_yaw_sp = MAX(AUTOTUNE_SP_MIN, tune_yaw_sp * AUTOTUNE_SP_BACKOFF);
tune_yaw_accel = MAX(AUTOTUNE_Y_ACCEL_MIN, test_accel_max * AUTOTUNE_ACCEL_Y_BACKOFF);
complete = true;
break;
}
// if we've just completed all axes we have successfully completed the autotune
// change to TESTING mode to allow user to fly with new gains
if (complete) {
mode = SUCCESS;
update_gcs(AUTOTUNE_MESSAGE_SUCCESS);
AP::logger().Write_Event(LogEvent::AUTOTUNE_SUCCESS);
AP_Notify::events.autotune_complete = true;
} else {
AP_Notify::events.autotune_next_axis = true;
}
break;
}
}
// reverse direction
positive_direction = !positive_direction;
if (axis == YAW) {
attitude_control->input_euler_angle_roll_pitch_yaw(0.0f, 0.0f, ahrs_view->yaw_sensor, false);
}
// set gains to their intra-test values (which are very close to the original gains)
load_gains(GAIN_INTRA_TEST);
// reset testing step
step = WAITING_FOR_LEVEL;
step_start_time_ms = now;
level_start_time_ms = step_start_time_ms;
step_time_limit_ms = AUTOTUNE_REQUIRED_LEVEL_TIME_MS;
break;
}
}
// backup_gains_and_initialise - store current gains as originals
// called before tuning starts to backup original gains
void AC_AutoTune::backup_gains_and_initialise()
{
// initialise state because this is our first time
if (roll_enabled()) {
axis = ROLL;
} else if (pitch_enabled()) {
axis = PITCH;
} else if (yaw_enabled()) {
axis = YAW;
}
// no axes are complete
axes_completed = 0;
positive_direction = false;
step = WAITING_FOR_LEVEL;
step_start_time_ms = AP_HAL::millis();
level_start_time_ms = step_start_time_ms;
tune_type = RD_UP;
step_scaler = 1.0f;
desired_yaw_cd = ahrs_view->yaw_sensor;
aggressiveness = constrain_float(aggressiveness, 0.05f, 0.2f);
orig_bf_feedforward = attitude_control->get_bf_feedforward();
// backup original pids and initialise tuned pid values
orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
orig_roll_fltt = attitude_control->get_rate_roll_pid().filt_T_hz();
orig_roll_sp = attitude_control->get_angle_roll_p().kP();
orig_roll_accel = attitude_control->get_accel_roll_max();
tune_roll_rp = attitude_control->get_rate_roll_pid().kP();
tune_roll_rd = attitude_control->get_rate_roll_pid().kD();
tune_roll_sp = attitude_control->get_angle_roll_p().kP();
tune_roll_accel = attitude_control->get_accel_roll_max();
orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
orig_pitch_fltt = attitude_control->get_rate_pitch_pid().filt_T_hz();
orig_pitch_sp = attitude_control->get_angle_pitch_p().kP();
orig_pitch_accel = attitude_control->get_accel_pitch_max();
tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
tune_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
tune_pitch_sp = attitude_control->get_angle_pitch_p().kP();
tune_pitch_accel = attitude_control->get_accel_pitch_max();
orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
orig_yaw_fltt = attitude_control->get_rate_yaw_pid().filt_T_hz();
orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
orig_yaw_accel = attitude_control->get_accel_yaw_max();
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
tune_yaw_sp = attitude_control->get_angle_yaw_p().kP();
tune_yaw_accel = attitude_control->get_accel_yaw_max();
AP::logger().Write_Event(LogEvent::AUTOTUNE_INITIALISED);
}
// load_orig_gains - set gains to their original values
// called by stop and failed functions
void AC_AutoTune::load_orig_gains()
{
attitude_control->bf_feedforward(orig_bf_feedforward);
if (roll_enabled()) {
if (!is_zero(orig_roll_rp)) {
attitude_control->get_rate_roll_pid().kP(orig_roll_rp);
attitude_control->get_rate_roll_pid().kI(orig_roll_ri);
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
attitude_control->get_rate_roll_pid().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
attitude_control->set_accel_roll_max(orig_roll_accel);
}
}
if (pitch_enabled()) {
if (!is_zero(orig_pitch_rp)) {
attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp);
attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri);
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);