forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathAC_AttitudeControl.cpp
1242 lines (1057 loc) · 60.8 KB
/
AC_AttitudeControl.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_AttitudeControl.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_Scheduler/AP_Scheduler.h>
extern const AP_HAL::HAL& hal;
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// default gains for Plane
# define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.2f // Soft
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 5.0 // Min lean angle so that vehicle can maintain limited control
#else
// default gains for Copter and Sub
# define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.15f // Medium
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 10.0 // Min lean angle so that vehicle can maintain limited control
#endif
AC_AttitudeControl *AC_AttitudeControl::_singleton;
// table of user settable parameters
const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
// 0, 1 were RATE_RP_MAX, RATE_Y_MAX
// @Param: SLEW_YAW
// @DisplayName: Yaw target slew rate
// @Description: Maximum rate the yaw target can be updated in RTL and Auto flight modes
// @Units: cdeg/s
// @Range: 500 18000
// @Increment: 100
// @User: Advanced
AP_GROUPINFO("SLEW_YAW", 2, AC_AttitudeControl, _slew_yaw, AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS),
// 3 was for ACCEL_RP_MAX
// @Param: ACCEL_Y_MAX
// @DisplayName: Acceleration Max for Yaw
// @Description: Maximum acceleration in yaw axis
// @Units: cdeg/s/s
// @Range: 0 72000
// @Values: 0:Disabled, 9000:VerySlow, 18000:Slow, 36000:Medium, 54000:Fast
// @Increment: 1000
// @User: Advanced
AP_GROUPINFO("ACCEL_Y_MAX", 4, AC_AttitudeControl, _accel_yaw_max, AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS),
// @Param: RATE_FF_ENAB
// @DisplayName: Rate Feedforward Enable
// @Description: Controls whether body-frame rate feedforward is enabled or disabled
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO("RATE_FF_ENAB", 5, AC_AttitudeControl, _rate_bf_ff_enabled, AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT),
// @Param: ACCEL_R_MAX
// @DisplayName: Acceleration Max for Roll
// @Description: Maximum acceleration in roll axis
// @Units: cdeg/s/s
// @Range: 0 180000
// @Increment: 1000
// @Values: 0:Disabled, 30000:VerySlow, 72000:Slow, 108000:Medium, 162000:Fast
// @User: Advanced
AP_GROUPINFO("ACCEL_R_MAX", 6, AC_AttitudeControl, _accel_roll_max, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS),
// @Param: ACCEL_P_MAX
// @DisplayName: Acceleration Max for Pitch
// @Description: Maximum acceleration in pitch axis
// @Units: cdeg/s/s
// @Range: 0 180000
// @Increment: 1000
// @Values: 0:Disabled, 30000:VerySlow, 72000:Slow, 108000:Medium, 162000:Fast
// @User: Advanced
AP_GROUPINFO("ACCEL_P_MAX", 7, AC_AttitudeControl, _accel_pitch_max, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS),
// IDs 8,9,10,11 RESERVED (in use on Solo)
// @Param: ANGLE_BOOST
// @DisplayName: Angle Boost
// @Description: Angle Boost increases output throttle as the vehicle leans to reduce loss of altitude
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO("ANGLE_BOOST", 12, AC_AttitudeControl, _angle_boost_enabled, 1),
// @Param: ANG_RLL_P
// @DisplayName: Roll axis angle controller P gain
// @Description: Roll axis angle controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate
// @Range: 3.000 12.000
// @Range{Sub}: 0.0 12.000
// @User: Standard
AP_SUBGROUPINFO(_p_angle_roll, "ANG_RLL_", 13, AC_AttitudeControl, AC_P),
// @Param: ANG_PIT_P
// @DisplayName: Pitch axis angle controller P gain
// @Description: Pitch axis angle controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate
// @Range: 3.000 12.000
// @Range{Sub}: 0.0 12.000
// @User: Standard
AP_SUBGROUPINFO(_p_angle_pitch, "ANG_PIT_", 14, AC_AttitudeControl, AC_P),
// @Param: ANG_YAW_P
// @DisplayName: Yaw axis angle controller P gain
// @Description: Yaw axis angle controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate
// @Range: 3.000 12.000
// @Range{Sub}: 0.0 6.000
// @User: Standard
AP_SUBGROUPINFO(_p_angle_yaw, "ANG_YAW_", 15, AC_AttitudeControl, AC_P),
// @Param: ANG_LIM_TC
// @DisplayName: Angle Limit (to maintain altitude) Time Constant
// @Description: Angle Limit (to maintain altitude) Time Constant
// @Range: 0.5 10.0
// @User: Advanced
AP_GROUPINFO("ANG_LIM_TC", 16, AC_AttitudeControl, _angle_limit_tc, AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT),
// @Param: RATE_R_MAX
// @DisplayName: Angular Velocity Max for Roll
// @Description: Maximum angular velocity in roll axis
// @Units: deg/s
// @Range: 0 1080
// @Increment: 1
// @Values: 0:Disabled, 60:Slow, 180:Medium, 360:Fast
// @User: Advanced
AP_GROUPINFO("RATE_R_MAX", 17, AC_AttitudeControl, _ang_vel_roll_max, 0.0f),
// @Param: RATE_P_MAX
// @DisplayName: Angular Velocity Max for Pitch
// @Description: Maximum angular velocity in pitch axis
// @Units: deg/s
// @Range: 0 1080
// @Increment: 1
// @Values: 0:Disabled, 60:Slow, 180:Medium, 360:Fast
// @User: Advanced
AP_GROUPINFO("RATE_P_MAX", 18, AC_AttitudeControl, _ang_vel_pitch_max, 0.0f),
// @Param: RATE_Y_MAX
// @DisplayName: Angular Velocity Max for Yaw
// @Description: Maximum angular velocity in yaw axis
// @Units: deg/s
// @Range: 0 1080
// @Increment: 1
// @Values: 0:Disabled, 60:Slow, 180:Medium, 360:Fast
// @User: Advanced
AP_GROUPINFO("RATE_Y_MAX", 19, AC_AttitudeControl, _ang_vel_yaw_max, 0.0f),
// @Param: INPUT_TC
// @DisplayName: Attitude control input time constant
// @Description: Attitude control input time constant. Low numbers lead to sharper response, higher numbers to softer response
// @Units: s
// @Range: 0 1
// @Increment: 0.01
// @Values: 0.5:Very Soft, 0.2:Soft, 0.15:Medium, 0.1:Crisp, 0.05:Very Crisp
// @User: Standard
AP_GROUPINFO("INPUT_TC", 20, AC_AttitudeControl, _input_tc, AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT),
// @Param: LAND_R_MULT
// @DisplayName: Landed roll gain multiplier
// @Description: Roll gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the roll axis.
// @Range: 0.25 1.0
// @User: Advanced
AP_GROUPINFO("LAND_R_MULT", 21, AC_AttitudeControl, _land_roll_mult, 1.0),
// @Param: LAND_P_MULT
// @DisplayName: Landed pitch gain multiplier
// @Description: Pitch gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the pitch axis.
// @Range: 0.25 1.0
// @User: Advanced
AP_GROUPINFO("LAND_P_MULT", 22, AC_AttitudeControl, _land_pitch_mult, 1.0),
// @Param: LAND_Y_MULT
// @DisplayName: Landed yaw gain multiplier
// @Description: Yaw gain multiplier active when landed. A factor of 1.0 means no reduction in gain while landed. Reduce this factor to reduce ground oscitation in the yaw axis.
// @Range: 0.25 1.0
// @User: Advanced
AP_GROUPINFO("LAND_Y_MULT", 23, AC_AttitudeControl, _land_yaw_mult, 1.0),
AP_GROUPEND
};
constexpr Vector3f AC_AttitudeControl::VECTORF_111;
// get the slew yaw rate limit in deg/s
float AC_AttitudeControl::get_slew_yaw_max_degs() const
{
if (!is_positive(_ang_vel_yaw_max)) {
return _slew_yaw * 0.01;
}
return MIN(_ang_vel_yaw_max, _slew_yaw * 0.01);
}
// Ensure attitude controller have zero errors to relax rate controller output
void AC_AttitudeControl::relax_attitude_controllers()
{
// Initialize the attitude variables to the current attitude
_ahrs.get_quat_body_to_ned(_attitude_target);
_attitude_target.to_euler(_euler_angle_target);
_attitude_ang_error.initialise();
// Initialize the angular rate variables to the current rate
_ang_vel_target = _ahrs.get_gyro();
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
_ang_vel_body = _ahrs.get_gyro();
// Initialize remaining variables
_thrust_error_angle = 0.0f;
// Reset the PID filters
get_rate_roll_pid().reset_filter();
get_rate_pitch_pid().reset_filter();
get_rate_yaw_pid().reset_filter();
// Reset the I terms
reset_rate_controller_I_terms();
}
void AC_AttitudeControl::reset_rate_controller_I_terms()
{
get_rate_roll_pid().reset_I();
get_rate_pitch_pid().reset_I();
get_rate_yaw_pid().reset_I();
}
// reset rate controller I terms smoothly to zero in 0.5 seconds
void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly()
{
get_rate_roll_pid().relax_integrator(0.0, _dt, AC_ATTITUDE_RATE_RELAX_TC);
get_rate_pitch_pid().relax_integrator(0.0, _dt, AC_ATTITUDE_RATE_RELAX_TC);
get_rate_yaw_pid().relax_integrator(0.0, _dt, AC_ATTITUDE_RATE_RELAX_TC);
}
// Reduce attitude control gains while landed to stop ground resonance
void AC_AttitudeControl::landed_gain_reduction(bool landed)
{
if (is_positive(_input_tc)) {
// use 2.0 x tc to match the response time to 86% commanded
const float spool_step = _dt / (2.0 * _input_tc);
if (landed) {
_landed_gain_ratio = MIN(1.0, _landed_gain_ratio + spool_step);
} else {
_landed_gain_ratio = MAX(0.0, _landed_gain_ratio - spool_step);
}
} else {
_landed_gain_ratio = landed ? 1.0 : 0.0;
}
Vector3f scale_mult = VECTORF_111 * (1.0 - _landed_gain_ratio) + Vector3f(_land_roll_mult, _land_pitch_mult, _land_yaw_mult) * _landed_gain_ratio;
set_PD_scale_mult(scale_mult);
set_angle_P_scale_mult(scale_mult);
}
// The attitude controller works around the concept of the desired attitude, target attitude
// and measured attitude. The desired attitude is the attitude input into the attitude controller
// that expresses where the higher level code would like the aircraft to move to. The target attitude is moved
// to the desired attitude with jerk, acceleration, and velocity limits. The target angular velocities are fed
// directly into the rate controllers. The angular error between the measured attitude and the target attitude is
// fed into the angle controller and the output of the angle controller summed at the input of the rate controllers.
// By feeding the target angular velocity directly into the rate controllers the measured and target attitudes
// remain very close together.
//
// All input functions below follow the same procedure
// 1. define the desired attitude the aircraft should attempt to achieve using the input variables
// 2. using the desired attitude and input variables, define the target angular velocity so that it should
// move the target attitude towards the desired attitude
// 3. if _rate_bf_ff_enabled is not being used then make the target attitude
// and target angular velocities equal to the desired attitude and desired angular velocities.
// 4. ensure _attitude_target, _euler_angle_target, _euler_rate_target and
// _ang_vel_target have been defined. This ensures input modes can be changed without discontinuity.
// 5. attitude_controller_run_quat is then run to pass the target angular velocities to the rate controllers and
// integrate them into the target attitude. Any errors between the target attitude and the measured attitude are
// corrected by first correcting the thrust vector until the angle between the target thrust vector measured
// trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE. At this point the heading is also corrected.
// Command a Quaternion attitude with feedforward and smoothing
// attitude_desired_quat: is updated on each time_step by the integral of the body frame angular velocity
void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body)
{
Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat;
Vector3f attitude_error_angle;
attitude_error_quat.to_axis_angle(attitude_error_angle);
// Limit the angular velocity
ang_vel_limit(ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
Vector3f ang_vel_target = attitude_desired_quat * ang_vel_body;
if (_rate_bf_ff_enabled) {
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by _input_tc at the end.
_ang_vel_target.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), _input_tc, get_accel_roll_max_radss(), _ang_vel_target.x, ang_vel_target.x, radians(_ang_vel_roll_max), _dt);
_ang_vel_target.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), _input_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, ang_vel_target.y, radians(_ang_vel_pitch_max), _dt);
_ang_vel_target.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), _input_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, ang_vel_target.z, radians(_ang_vel_yaw_max), _dt);
} else {
_attitude_target = attitude_desired_quat;
_ang_vel_target = ang_vel_target;
}
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
// rotate target and normalize
Quaternion attitude_desired_update;
attitude_desired_update.from_axis_angle(ang_vel_target * _dt);
attitude_desired_quat = attitude_desired_quat * attitude_desired_update;
attitude_desired_quat.normalize();
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle += get_roll_trim_rad();
if (_rate_bf_ff_enabled) {
// translate the roll pitch and yaw acceleration limits to the euler axis
const Vector3f euler_accel = euler_accel_limit(_attitude_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()});
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by smoothing_gain at the end.
_euler_rate_target.x = input_shaping_angle(wrap_PI(euler_roll_angle - _euler_angle_target.x), _input_tc, euler_accel.x, _euler_rate_target.x, _dt);
_euler_rate_target.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _euler_angle_target.y), _input_tc, euler_accel.y, _euler_rate_target.y, _dt);
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
_euler_rate_target.z = input_shaping_ang_vel(_euler_rate_target.z, euler_yaw_rate, euler_accel.z, _dt, _rate_y_tc);
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target, _euler_rate_target, _ang_vel_target);
// Limit the angular velocity
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
} else {
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
_euler_angle_target.x = euler_roll_angle;
_euler_angle_target.y = euler_pitch_angle;
_euler_angle_target.z += euler_yaw_rate * _dt;
// Compute quaternion target attitude
_attitude_target.from_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
// Set rate feedforward requests to zero
_euler_rate_target.zero();
_ang_vel_target.zero();
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
{
// Convert from centidegrees on public interface to radians
float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
float euler_yaw_angle = radians(euler_yaw_angle_cd * 0.01f);
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
// Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
euler_roll_angle += get_roll_trim_rad();
const float slew_yaw_max_rads = get_slew_yaw_max_rads();
if (_rate_bf_ff_enabled) {
// translate the roll pitch and yaw acceleration limits to the euler axis
const Vector3f euler_accel = euler_accel_limit(_attitude_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()});
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by _input_tc at the end.
_euler_rate_target.x = input_shaping_angle(wrap_PI(euler_roll_angle - _euler_angle_target.x), _input_tc, euler_accel.x, _euler_rate_target.x, _dt);
_euler_rate_target.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _euler_angle_target.y), _input_tc, euler_accel.y, _euler_rate_target.y, _dt);
_euler_rate_target.z = input_shaping_angle(wrap_PI(euler_yaw_angle - _euler_angle_target.z), _input_tc, euler_accel.z, _euler_rate_target.z, _dt);
if (slew_yaw) {
_euler_rate_target.z = constrain_float(_euler_rate_target.z, -slew_yaw_max_rads, slew_yaw_max_rads);
}
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target, _euler_rate_target, _ang_vel_target);
// Limit the angular velocity
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
} else {
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
_euler_angle_target.x = euler_roll_angle;
_euler_angle_target.y = euler_pitch_angle;
if (slew_yaw) {
// Compute constrained angle error
float angle_error = constrain_float(wrap_PI(euler_yaw_angle - _euler_angle_target.z), -slew_yaw_max_rads * _dt, slew_yaw_max_rads * _dt);
// Update attitude target from constrained angle error
_euler_angle_target.z = wrap_PI(angle_error + _euler_angle_target.z);
} else {
_euler_angle_target.z = euler_yaw_angle;
}
// Compute quaternion target attitude
_attitude_target.from_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
// Set rate feedforward requests to zero
_euler_rate_target.zero();
_ang_vel_target.zero();
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds)
{
// Convert from centidegrees on public interface to radians
float euler_roll_rate = radians(euler_roll_rate_cds * 0.01f);
float euler_pitch_rate = radians(euler_pitch_rate_cds * 0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
if (_rate_bf_ff_enabled) {
// translate the roll pitch and yaw acceleration limits to the euler axis
const Vector3f euler_accel = euler_accel_limit(_attitude_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()});
// When acceleration limiting is enabled, the input shaper constrains angular acceleration, slewing
// the output rate towards the input rate.
_euler_rate_target.x = input_shaping_ang_vel(_euler_rate_target.x, euler_roll_rate, euler_accel.x, _dt, _rate_rp_tc);
_euler_rate_target.y = input_shaping_ang_vel(_euler_rate_target.y, euler_pitch_rate, euler_accel.y, _dt, _rate_rp_tc);
_euler_rate_target.z = input_shaping_ang_vel(_euler_rate_target.z, euler_yaw_rate, euler_accel.z, _dt, _rate_y_tc);
// Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
euler_rate_to_ang_vel(_attitude_target, _euler_rate_target, _ang_vel_target);
} else {
// When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
// Pitch angle is restricted to +- 85.0 degrees to avoid gimbal lock discontinuities.
_euler_angle_target.x = wrap_PI(_euler_angle_target.x + euler_roll_rate * _dt);
_euler_angle_target.y = constrain_float(_euler_angle_target.y + euler_pitch_rate * _dt, radians(-85.0f), radians(85.0f));
_euler_angle_target.z = wrap_2PI(_euler_angle_target.z + euler_yaw_rate * _dt);
// Set rate feedforward requests to zero
_euler_rate_target.zero();
_ang_vel_target.zero();
// Compute quaternion target attitude
_attitude_target.from_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an angular velocity with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
// Convert from centidegrees on public interface to radians
float roll_rate_rads = radians(roll_rate_bf_cds * 0.01f);
float pitch_rate_rads = radians(pitch_rate_bf_cds * 0.01f);
float yaw_rate_rads = radians(yaw_rate_bf_cds * 0.01f);
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
if (_rate_bf_ff_enabled) {
// Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
_ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt, _rate_rp_tc);
_ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt, _rate_rp_tc);
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt, _rate_y_tc);
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
} else {
// When feedforward is not enabled, the quaternion is calculated and is input into the target and the feedforward rate is zeroed.
Quaternion attitude_target_update;
attitude_target_update.from_axis_angle(Vector3f{roll_rate_rads * _dt, pitch_rate_rads * _dt, yaw_rate_rads * _dt});
_attitude_target = _attitude_target * attitude_target_update;
_attitude_target.normalize();
// Set rate feedforward requests to zero
_euler_rate_target.zero();
_ang_vel_target.zero();
}
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command an angular velocity with angular velocity smoothing using rate loops only with no attitude loop stabilization
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
// Convert from centidegrees on public interface to radians
float roll_rate_rads = radians(roll_rate_bf_cds * 0.01f);
float pitch_rate_rads = radians(pitch_rate_bf_cds * 0.01f);
float yaw_rate_rads = radians(yaw_rate_bf_cds * 0.01f);
// Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
_ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt, _rate_rp_tc);
_ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt, _rate_rp_tc);
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt, _rate_y_tc);
// Update the unused targets attitude based on current attitude to condition mode change
_ahrs.get_quat_body_to_ned(_attitude_target);
_attitude_target.to_euler(_euler_angle_target);
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
_ang_vel_body = _ang_vel_target;
}
// Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
// Convert from centidegrees on public interface to radians
float roll_rate_rads = radians(roll_rate_bf_cds * 0.01f);
float pitch_rate_rads = radians(pitch_rate_bf_cds * 0.01f);
float yaw_rate_rads = radians(yaw_rate_bf_cds * 0.01f);
// Update attitude error
Vector3f attitude_error;
_attitude_ang_error.to_axis_angle(attitude_error);
Quaternion attitude_ang_error_update_quat;
// limit the integrated error angle
float err_mag = attitude_error.length();
if (err_mag > AC_ATTITUDE_THRUST_ERROR_ANGLE) {
attitude_error *= AC_ATTITUDE_THRUST_ERROR_ANGLE / err_mag;
_attitude_ang_error.from_axis_angle(attitude_error);
}
Vector3f gyro_latest = _ahrs.get_gyro_latest();
attitude_ang_error_update_quat.from_axis_angle(Vector3f{(_ang_vel_target.x-gyro_latest.x) * _dt, (_ang_vel_target.y-gyro_latest.y) * _dt, (_ang_vel_target.z-gyro_latest.z) * _dt});
_attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error;
// Compute acceleration-limited body frame rates
// When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing
// the output rate towards the input rate.
_ang_vel_target.x = input_shaping_ang_vel(_ang_vel_target.x, roll_rate_rads, get_accel_roll_max_radss(), _dt, _rate_rp_tc);
_ang_vel_target.y = input_shaping_ang_vel(_ang_vel_target.y, pitch_rate_rads, get_accel_pitch_max_radss(), _dt, _rate_rp_tc);
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, yaw_rate_rads, get_accel_yaw_max_radss(), _dt, _rate_y_tc);
// Retrieve quaternion body attitude
Quaternion attitude_body;
_ahrs.get_quat_body_to_ned(attitude_body);
// Update the unused targets attitude based on current attitude to condition mode change
_attitude_target = attitude_body * _attitude_ang_error;
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
// Compute the angular velocity target from the integrated rate error
_attitude_ang_error.to_axis_angle(attitude_error);
_ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);
_ang_vel_body += _ang_vel_target;
// ensure Quaternions stay normalized
_attitude_ang_error.normalize();
}
// Command an angular step (i.e change) in body frame angle
// Used to command a step in angle without exciting the orthogonal axis during autotune
void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd)
{
// Convert from centidegrees on public interface to radians
float roll_step_rads = radians(roll_angle_step_bf_cd * 0.01f);
float pitch_step_rads = radians(pitch_angle_step_bf_cd * 0.01f);
float yaw_step_rads = radians(yaw_angle_step_bf_cd * 0.01f);
// rotate attitude target by desired step
Quaternion attitude_target_update;
attitude_target_update.from_axis_angle(Vector3f{roll_step_rads, pitch_step_rads, yaw_step_rads});
_attitude_target = _attitude_target * attitude_target_update;
_attitude_target.normalize();
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
// Set rate feedforward requests to zero
_euler_rate_target.zero();
_ang_vel_target.zero();
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command a thrust vector and heading rate
void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw)
{
// Convert from centidegrees on public interface to radians
float heading_rate = radians(heading_rate_cds * 0.01f);
if (slew_yaw) {
// a zero _angle_vel_yaw_max means that setting is disabled
const float slew_yaw_max_rads = get_slew_yaw_max_rads();
heading_rate = constrain_float(heading_rate, -slew_yaw_max_rads, slew_yaw_max_rads);
}
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
// convert thrust vector to a quaternion attitude
Quaternion thrust_vec_quat = attitude_from_thrust_vector(thrust_vector, 0.0f);
// calculate the angle error in x and y.
float thrust_vector_diff_angle;
Quaternion thrust_vec_correction_quat;
Vector3f attitude_error;
float returned_thrust_vector_angle;
thrust_vector_rotation_angles(thrust_vec_quat, _attitude_target, thrust_vec_correction_quat, attitude_error, returned_thrust_vector_angle, thrust_vector_diff_angle);
if (_rate_bf_ff_enabled) {
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
_ang_vel_target.x = input_shaping_angle(attitude_error.x, _input_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
_ang_vel_target.y = input_shaping_angle(attitude_error.y, _input_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
_ang_vel_target.z = input_shaping_ang_vel(_ang_vel_target.z, heading_rate, get_accel_yaw_max_radss(), _dt, _rate_y_tc);
// Limit the angular velocity
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
} else {
Quaternion yaw_quat;
yaw_quat.from_axis_angle(Vector3f{0.0f, 0.0f, heading_rate * _dt});
_attitude_target = _attitude_target * thrust_vec_correction_quat * yaw_quat;
// Set rate feedforward requests to zero
_euler_rate_target.zero();
_ang_vel_target.zero();
}
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command a thrust vector, heading and heading rate
void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds)
{
// a zero _angle_vel_yaw_max means that setting is disabled
const float slew_yaw_max_rads = get_slew_yaw_max_rads();
// Convert from centidegrees on public interface to radians
float heading_rate = constrain_float(radians(heading_rate_cds * 0.01f), -slew_yaw_max_rads, slew_yaw_max_rads);
float heading_angle = radians(heading_angle_cd * 0.01f);
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target);
// convert thrust vector and heading to a quaternion attitude
const Quaternion desired_attitude_quat = attitude_from_thrust_vector(thrust_vector, heading_angle);
if (_rate_bf_ff_enabled) {
// calculate the angle error in x and y.
Vector3f attitude_error;
float thrust_vector_diff_angle;
Quaternion thrust_vec_correction_quat;
float returned_thrust_vector_angle;
thrust_vector_rotation_angles(desired_attitude_quat, _attitude_target, thrust_vec_correction_quat, attitude_error, returned_thrust_vector_angle, thrust_vector_diff_angle);
// When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
// the output rate towards the input rate.
_ang_vel_target.x = input_shaping_angle(attitude_error.x, _input_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
_ang_vel_target.y = input_shaping_angle(attitude_error.y, _input_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
_ang_vel_target.z = input_shaping_angle(attitude_error.z, _input_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, heading_rate, slew_yaw_max_rads, _dt);
// Limit the angular velocity
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), slew_yaw_max_rads);
} else {
// set persisted quaternion target attitude
_attitude_target = desired_attitude_quat;
// Set rate feedforward requests to zero
_euler_rate_target.zero();
_ang_vel_target.zero();
}
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
// Call quaternion attitude controller
attitude_controller_run_quat();
}
// Command a thrust vector and heading rate
void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vector, HeadingCommand heading)
{
switch (heading.heading_mode) {
case HeadingMode::Rate_Only:
input_thrust_vector_rate_heading(thrust_vector, heading.yaw_rate_cds);
break;
case HeadingMode::Angle_Only:
input_thrust_vector_heading(thrust_vector, heading.yaw_angle_cd, 0.0);
break;
case HeadingMode::Angle_And_Rate:
input_thrust_vector_heading(thrust_vector, heading.yaw_angle_cd, heading.yaw_rate_cds);
break;
}
}
Quaternion AC_AttitudeControl::attitude_from_thrust_vector(Vector3f thrust_vector, float heading_angle) const
{
const Vector3f thrust_vector_up{0.0f, 0.0f, -1.0f};
if (is_zero(thrust_vector.length_squared())) {
thrust_vector = thrust_vector_up;
} else {
thrust_vector.normalize();
}
// the cross product of the desired and target thrust vector defines the rotation vector
Vector3f thrust_vec_cross = thrust_vector_up % thrust_vector;
// the dot product is used to calculate the angle between the target and desired thrust vectors
const float thrust_vector_angle = acosf(constrain_float(thrust_vector_up * thrust_vector, -1.0f, 1.0f));
// Normalize the thrust rotation vector
const float thrust_vector_length = thrust_vec_cross.length();
if (is_zero(thrust_vector_length) || is_zero(thrust_vector_angle)) {
thrust_vec_cross = thrust_vector_up;
} else {
thrust_vec_cross /= thrust_vector_length;
}
Quaternion thrust_vec_quat;
thrust_vec_quat.from_axis_angle(thrust_vec_cross, thrust_vector_angle);
Quaternion yaw_quat;
yaw_quat.from_axis_angle(Vector3f{0.0f, 0.0f, 1.0f}, heading_angle);
return thrust_vec_quat*yaw_quat;
}
// Calculates the body frame angular velocities to follow the target attitude
void AC_AttitudeControl::attitude_controller_run_quat()
{
// This represents a quaternion rotation in NED frame to the body
Quaternion attitude_body;
_ahrs.get_quat_body_to_ned(attitude_body);
// This vector represents the angular error to rotate the thrust vector using x and y and heading using z
Vector3f attitude_error;
thrust_heading_rotation_angles(_attitude_target, attitude_body, attitude_error, _thrust_angle, _thrust_error_angle);
// Compute the angular velocity corrections in the body frame from the attitude error
_ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);
// ensure angular velocity does not go over configured limits
ang_vel_limit(_ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
// rotation from the target frame to the body frame
Quaternion rotation_target_to_body = attitude_body.inverse() * _attitude_target;
// target angle velocity vector in the body frame
Vector3f ang_vel_body_feedforward = rotation_target_to_body * _ang_vel_target;
// Correct the thrust vector and smoothly add feedforward and yaw input
_feedforward_scalar = 1.0f;
if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f) {
_ang_vel_body.z = _ahrs.get_gyro().z;
} else if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE) {
_feedforward_scalar = (1.0f - (_thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE) / AC_ATTITUDE_THRUST_ERROR_ANGLE);
_ang_vel_body.x += ang_vel_body_feedforward.x * _feedforward_scalar;
_ang_vel_body.y += ang_vel_body_feedforward.y * _feedforward_scalar;
_ang_vel_body.z += ang_vel_body_feedforward.z;
_ang_vel_body.z = _ahrs.get_gyro().z * (1.0 - _feedforward_scalar) + _ang_vel_body.z * _feedforward_scalar;
} else {
_ang_vel_body += ang_vel_body_feedforward;
}
if (_rate_bf_ff_enabled) {
// rotate target and normalize
Quaternion attitude_target_update;
attitude_target_update.from_axis_angle(Vector3f{_ang_vel_target.x * _dt, _ang_vel_target.y * _dt, _ang_vel_target.z * _dt});
_attitude_target = _attitude_target * attitude_target_update;
}
// ensure Quaternion stay normalised
_attitude_target.normalize();
// Record error to handle EKF resets
_attitude_ang_error = attitude_body.inverse() * _attitude_target;
}
// thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
// The maximum error in the yaw axis is limited based on static output saturation.
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const
{
Quaternion thrust_vector_correction;
thrust_vector_rotation_angles(attitude_target, attitude_body, thrust_vector_correction, attitude_error, thrust_angle, thrust_error_angle);
// Todo: Limit roll an pitch error based on output saturation and maximum error.
// Limit Yaw Error based to the maximum that would saturate the output when yaw rate is zero.
Quaternion heading_vec_correction_quat;
float heading_accel_max = constrain_float(get_accel_yaw_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS);
if (!is_zero(get_rate_yaw_pid().kP())) {
float heading_error_max = MIN(inv_sqrt_controller(1.0 / get_rate_yaw_pid().kP(), _p_angle_yaw.kP(), heading_accel_max), AC_ATTITUDE_YAW_MAX_ERROR_ANGLE);
if (!is_zero(_p_angle_yaw.kP()) && fabsf(attitude_error.z) > heading_error_max) {
attitude_error.z = constrain_float(wrap_PI(attitude_error.z), -heading_error_max, heading_error_max);
heading_vec_correction_quat.from_axis_angle(Vector3f{0.0f, 0.0f, attitude_error.z});
attitude_target = attitude_body * thrust_vector_correction * heading_vec_correction_quat;
}
}
}
// thrust_vector_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void AC_AttitudeControl::thrust_vector_rotation_angles(const Quaternion& attitude_target, const Quaternion& attitude_body, Quaternion& thrust_vector_correction, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const
{
// The direction of thrust is [0,0,-1] is any body-fixed frame, inc. body frame and target frame.
const Vector3f thrust_vector_up{0.0f, 0.0f, -1.0f};
// attitude_target and attitude_body are passive rotations from target / body frames to the NED frame
// Rotating [0,0,-1] by attitude_target expresses (gets a view of) the target thrust vector in the inertial frame
Vector3f att_target_thrust_vec = attitude_target * thrust_vector_up; // target thrust vector
// Rotating [0,0,-1] by attitude_target expresses (gets a view of) the current thrust vector in the inertial frame
Vector3f att_body_thrust_vec = attitude_body * thrust_vector_up; // current thrust vector
// the dot product is used to calculate the current lean angle for use of external functions
thrust_angle = acosf(constrain_float(thrust_vector_up * att_body_thrust_vec,-1.0f,1.0f));
// the cross product of the desired and target thrust vector defines the rotation vector
Vector3f thrust_vec_cross = att_body_thrust_vec % att_target_thrust_vec;
// the dot product is used to calculate the angle between the target and desired thrust vectors
thrust_error_angle = acosf(constrain_float(att_body_thrust_vec * att_target_thrust_vec, -1.0f, 1.0f));
// Normalize the thrust rotation vector
float thrust_vector_length = thrust_vec_cross.length();
if (is_zero(thrust_vector_length) || is_zero(thrust_error_angle)) {
thrust_vec_cross = thrust_vector_up;
} else {
thrust_vec_cross /= thrust_vector_length;
}
// thrust_vector_correction is defined relative to the body frame but its axis `thrust_vec_cross` was computed in
// the inertial frame. First rotate it by the inverse of attitude_body to express it back in the body frame
thrust_vec_cross = attitude_body.inverse() * thrust_vec_cross;
thrust_vector_correction.from_axis_angle(thrust_vec_cross, thrust_error_angle);
// calculate the angle error in x and y.
Vector3f rotation;
thrust_vector_correction.to_axis_angle(rotation);
attitude_error.x = rotation.x;
attitude_error.y = rotation.y;
// calculate the remaining rotation required after thrust vector is rotated transformed to the body frame
// heading_vector_correction
Quaternion heading_vec_correction_quat = thrust_vector_correction.inverse() * attitude_body.inverse() * attitude_target;
// calculate the angle error in z (x and y should be zero here).
heading_vec_correction_quat.to_axis_angle(rotation);
attitude_error.z = rotation.z;
}
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using _input_tc
float AC_AttitudeControl::input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float desired_ang_vel, float max_ang_vel, float dt)
{
// Calculate the velocity as error approaches zero with acceleration limited by accel_max_radss
desired_ang_vel += sqrt_controller(error_angle, 1.0f / MAX(input_tc, 0.01f), accel_max, dt);
if (is_positive(max_ang_vel)) {
desired_ang_vel = constrain_float(desired_ang_vel, -max_ang_vel, max_ang_vel);
}
// Acceleration is limited directly to smooth the beginning of the curve.
return input_shaping_ang_vel(target_ang_vel, desired_ang_vel, accel_max, dt, 0.0f);
}
// Shapes the velocity request based on a rate time constant. The angular acceleration and deceleration is limited.
float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt, float input_tc)
{
if (is_positive(input_tc)) {
// Calculate the acceleration to smoothly achieve rate. Jerk is not limited.
float error_rate = desired_ang_vel - target_ang_vel;
float desired_ang_accel = sqrt_controller(error_rate, 1.0f / MAX(input_tc, 0.01f), 0.0f, dt);
desired_ang_vel = target_ang_vel + desired_ang_accel * dt;
}
// Acceleration is limited directly to smooth the beginning of the curve.
if (is_positive(accel_max)) {
float delta_ang_vel = accel_max * dt;
return constrain_float(desired_ang_vel, target_ang_vel - delta_ang_vel, target_ang_vel + delta_ang_vel);
} else {
return desired_ang_vel;
}
}
// calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings.
// This function can be used to predict the delay associated with angle requests.
void AC_AttitudeControl::input_shaping_rate_predictor(const Vector2f &error_angle, Vector2f& target_ang_vel, float dt) const
{
if (_rate_bf_ff_enabled) {
// translate the roll pitch and yaw acceleration limits to the euler axis
target_ang_vel.x = input_shaping_angle(wrap_PI(error_angle.x), _input_tc, get_accel_roll_max_radss(), target_ang_vel.x, dt);
target_ang_vel.y = input_shaping_angle(wrap_PI(error_angle.y), _input_tc, get_accel_pitch_max_radss(), target_ang_vel.y, dt);
} else {
const float angleP_roll = _p_angle_roll.kP() * _angle_P_scale.x;
const float angleP_pitch = _p_angle_pitch.kP() * _angle_P_scale.y;
target_ang_vel.x = angleP_roll * wrap_PI(error_angle.x);
target_ang_vel.y = angleP_pitch * wrap_PI(error_angle.y);
}
// Limit the angular velocity correction
Vector3f ang_vel(target_ang_vel.x, target_ang_vel.y, 0.0f);
ang_vel_limit(ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), 0.0f);
target_ang_vel.x = ang_vel.x;
target_ang_vel.y = ang_vel.y;
}
// limits angular velocity
void AC_AttitudeControl::ang_vel_limit(Vector3f& euler_rad, float ang_vel_roll_max, float ang_vel_pitch_max, float ang_vel_yaw_max) const
{
if (is_zero(ang_vel_roll_max) || is_zero(ang_vel_pitch_max)) {
if (!is_zero(ang_vel_roll_max)) {
euler_rad.x = constrain_float(euler_rad.x, -ang_vel_roll_max, ang_vel_roll_max);
}
if (!is_zero(ang_vel_pitch_max)) {
euler_rad.y = constrain_float(euler_rad.y, -ang_vel_pitch_max, ang_vel_pitch_max);
}
} else {
Vector2f thrust_vector_ang_vel(euler_rad.x / ang_vel_roll_max, euler_rad.y / ang_vel_pitch_max);
float thrust_vector_length = thrust_vector_ang_vel.length();
if (thrust_vector_length > 1.0f) {
euler_rad.x = thrust_vector_ang_vel.x * ang_vel_roll_max / thrust_vector_length;
euler_rad.y = thrust_vector_ang_vel.y * ang_vel_pitch_max / thrust_vector_length;
}
}
if (!is_zero(ang_vel_yaw_max)) {
euler_rad.z = constrain_float(euler_rad.z, -ang_vel_yaw_max, ang_vel_yaw_max);
}
}
// translates body frame acceleration limits to the euler axis
Vector3f AC_AttitudeControl::euler_accel_limit(const Quaternion &att, const Vector3f &euler_accel)
{
if (!is_positive(euler_accel.x) || !is_positive(euler_accel.y) || !is_positive(euler_accel.z)) {
return Vector3f { euler_accel };
}
const float phi = att.get_euler_roll();
const float theta = att.get_euler_pitch();
const float sin_phi = constrain_float(fabsf(sinf(phi)), 0.1f, 1.0f);
const float cos_phi = constrain_float(fabsf(cosf(phi)), 0.1f, 1.0f);
const float sin_theta = constrain_float(fabsf(sinf(theta)), 0.1f, 1.0f);
const float cos_theta = constrain_float(fabsf(cosf(theta)), 0.1f, 1.0f);
return Vector3f {
euler_accel.x,
MIN(euler_accel.y / cos_phi, euler_accel.z / sin_phi),
MIN(MIN(euler_accel.x / sin_theta, euler_accel.y / (sin_phi * cos_theta)), euler_accel.z / (cos_phi * cos_theta))
};
}
// Sets attitude target to vehicle attitude and sets all rates to zero
// If reset_rate is false rates are not reset to allow the rate controllers to run
void AC_AttitudeControl::reset_target_and_rate(bool reset_rate)
{
// move attitude target to current attitude
_ahrs.get_quat_body_to_ned(_attitude_target);
_attitude_target.to_euler(_euler_angle_target);
if (reset_rate) {
_ang_vel_target.zero();
_euler_rate_target.zero();
}
}
// Sets yaw target to vehicle heading and sets yaw rate to zero
// If reset_rate is false rates are not reset to allow the rate controllers to run
void AC_AttitudeControl::reset_yaw_target_and_rate(bool reset_rate)
{
// move attitude target to current heading
float yaw_shift = _ahrs.yaw - _euler_angle_target.z;
Quaternion _attitude_target_update;
_attitude_target_update.from_axis_angle(Vector3f{0.0f, 0.0f, yaw_shift});
_attitude_target = _attitude_target_update * _attitude_target;
if (reset_rate) {
// set yaw rate to zero
_euler_rate_target.z = 0.0f;