forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathAC_CustomControl_PID.cpp
313 lines (268 loc) · 13.8 KB
/
AC_CustomControl_PID.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
#include "AC_CustomControl_PID.h"
#if CUSTOMCONTROL_PID_ENABLED
// table of user settable parameters
const AP_Param::GroupInfo AC_CustomControl_PID::var_info[] = {
// @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_roll2, "ANG_RLL_", 1, AC_CustomControl_PID, 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_pitch2, "ANG_PIT_", 2, AC_CustomControl_PID, 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_yaw2, "ANG_YAW_", 3, AC_CustomControl_PID, AC_P),
// @Param: RAT_RLL_P
// @DisplayName: Roll axis rate controller P gain
// @Description: Roll axis rate controller P gain. Corrects in proportion to the difference between the desired roll rate vs actual roll rate
// @Range: 0.01 0.5
// @Increment: 0.005
// @User: Standard
// @Param: RAT_RLL_I
// @DisplayName: Roll axis rate controller I gain
// @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate
// @Range: 0.01 2.0
// @Increment: 0.01
// @User: Standard
// @Param: RAT_RLL_IMAX
// @DisplayName: Roll axis rate controller I gain maximum
// @Description: Roll axis rate controller I gain maximum. Constrains the maximum that the I term will output
// @Range: 0 1
// @Increment: 0.01
// @User: Standard
// @Param: RAT_RLL_D
// @DisplayName: Roll axis rate controller D gain
// @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate
// @Range: 0.0 0.05
// @Increment: 0.001
// @User: Standard
// @Param: RAT_RLL_FF
// @DisplayName: Roll axis rate controller feed forward
// @Description: Roll axis rate controller feed forward
// @Range: 0 0.5
// @Increment: 0.001
// @User: Standard
// @Param: RAT_RLL_FLTT
// @DisplayName: Roll axis rate controller target frequency in Hz
// @Description: Roll axis rate controller target frequency in Hz
// @Range: 5 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_RLL_FLTE
// @DisplayName: Roll axis rate controller error frequency in Hz
// @Description: Roll axis rate controller error frequency in Hz
// @Range: 0 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_RLL_FLTD
// @DisplayName: Roll axis rate controller derivative frequency in Hz
// @Description: Roll axis rate controller derivative frequency in Hz
// @Range: 5 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_RLL_SMAX
// @DisplayName: Roll slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
AP_SUBGROUPINFO(_pid_atti_rate_roll, "RAT_RLL_", 4, AC_CustomControl_PID, AC_PID),
// @Param: RAT_PIT_P
// @DisplayName: Pitch axis rate controller P gain
// @Description: Pitch axis rate controller P gain. Corrects in proportion to the difference between the desired pitch rate vs actual pitch rate
// @Range: 0.01 0.50
// @Increment: 0.005
// @User: Standard
// @Param: RAT_PIT_I
// @DisplayName: Pitch axis rate controller I gain
// @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate
// @Range: 0.01 2.0
// @Increment: 0.01
// @User: Standard
// @Param: RAT_PIT_IMAX
// @DisplayName: Pitch axis rate controller I gain maximum
// @Description: Pitch axis rate controller I gain maximum. Constrains the maximum that the I term will output
// @Range: 0 1
// @Increment: 0.01
// @User: Standard
// @Param: RAT_PIT_D
// @DisplayName: Pitch axis rate controller D gain
// @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate
// @Range: 0.0 0.05
// @Increment: 0.001
// @User: Standard
// @Param: RAT_PIT_FF
// @DisplayName: Pitch axis rate controller feed forward
// @Description: Pitch axis rate controller feed forward
// @Range: 0 0.5
// @Increment: 0.001
// @User: Standard
// @Param: RAT_PIT_FLTT
// @DisplayName: Pitch axis rate controller target frequency in Hz
// @Description: Pitch axis rate controller target frequency in Hz
// @Range: 5 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_FLTE
// @DisplayName: Pitch axis rate controller error frequency in Hz
// @Description: Pitch axis rate controller error frequency in Hz
// @Range: 0 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_FLTD
// @DisplayName: Pitch axis rate controller derivative frequency in Hz
// @Description: Pitch axis rate controller derivative frequency in Hz
// @Range: 5 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_PIT_SMAX
// @DisplayName: Pitch slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
AP_SUBGROUPINFO(_pid_atti_rate_pitch, "RAT_PIT_", 5, AC_CustomControl_PID, AC_PID),
// @Param: RAT_YAW_P
// @DisplayName: Yaw axis rate controller P gain
// @Description: Yaw axis rate controller P gain. Corrects in proportion to the difference between the desired yaw rate vs actual yaw rate
// @Range: 0.10 2.50
// @Increment: 0.005
// @User: Standard
// @Param: RAT_YAW_I
// @DisplayName: Yaw axis rate controller I gain
// @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate
// @Range: 0.010 1.0
// @Increment: 0.01
// @User: Standard
// @Param: RAT_YAW_IMAX
// @DisplayName: Yaw axis rate controller I gain maximum
// @Description: Yaw axis rate controller I gain maximum. Constrains the maximum that the I term will output
// @Range: 0 1
// @Increment: 0.01
// @User: Standard
// @Param: RAT_YAW_D
// @DisplayName: Yaw axis rate controller D gain
// @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate
// @Range: 0.000 0.02
// @Increment: 0.001
// @User: Standard
// @Param: RAT_YAW_FF
// @DisplayName: Yaw axis rate controller feed forward
// @Description: Yaw axis rate controller feed forward
// @Range: 0 0.5
// @Increment: 0.001
// @User: Standard
// @Param: RAT_YAW_FLTT
// @DisplayName: Yaw axis rate controller target frequency in Hz
// @Description: Yaw axis rate controller target frequency in Hz
// @Range: 1 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_FLTE
// @DisplayName: Yaw axis rate controller error frequency in Hz
// @Description: Yaw axis rate controller error frequency in Hz
// @Range: 0 20
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_FLTD
// @DisplayName: Yaw axis rate controller derivative frequency in Hz
// @Description: Yaw axis rate controller derivative frequency in Hz
// @Range: 5 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: RAT_YAW_SMAX
// @DisplayName: Yaw slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
AP_SUBGROUPINFO(_pid_atti_rate_yaw, "RAT_YAW_", 6, AC_CustomControl_PID, AC_PID),
AP_GROUPEND
};
AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt) :
AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt),
_p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
_p_angle_pitch2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
_p_angle_yaw2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
_pid_atti_rate_roll(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, dt),
_pid_atti_rate_pitch(AC_ATC_MULTI_RATE_RP_P * 0.90f, AC_ATC_MULTI_RATE_RP_I * 0.90f, AC_ATC_MULTI_RATE_RP_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, dt),
_pid_atti_rate_yaw(AC_ATC_MULTI_RATE_YAW_P * 0.90f, AC_ATC_MULTI_RATE_YAW_I * 0.90f, AC_ATC_MULTI_RATE_YAW_D * 0.90f, 0.0f, AC_ATC_MULTI_RATE_YAW_IMAX * 0.90f, AC_ATC_MULTI_RATE_RP_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f, dt)
{
AP_Param::setup_object_defaults(this, var_info);
}
Vector3f AC_CustomControl_PID::update()
{
// reset controller based on spool state
switch (_motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
case AP_Motors::SpoolState::GROUND_IDLE:
// We are still at the ground. Reset custom controller to avoid
// build up, ex: integrator
reset();
break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// we are off the ground
break;
}
// run custom controller after here
Quaternion attitude_body, attitude_target;
_ahrs->get_quat_body_to_ned(attitude_body);
attitude_target = _att_control->get_attitude_target_quat();
// This vector represents the angular error to rotate the thrust vector using x and y and heading using z
Vector3f attitude_error;
float _thrust_angle, _thrust_error_angle;
_att_control->thrust_heading_rotation_angles(attitude_target, attitude_body, attitude_error, _thrust_angle, _thrust_error_angle);
// recalculate ang vel feedforward from attitude target model
// 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 * _att_control->get_attitude_target_ang_vel();
// run attitude controller
Vector3f target_rate;
target_rate[0] = _p_angle_roll2.kP() * attitude_error.x + ang_vel_body_feedforward[0];
target_rate[1] = _p_angle_pitch2.kP() * attitude_error.y + ang_vel_body_feedforward[1];
target_rate[2] = _p_angle_yaw2.kP() * attitude_error.z + ang_vel_body_feedforward[2];
// run rate controller
Vector3f gyro_latest = _ahrs->get_gyro_latest();
Vector3f motor_out;
motor_out.x = _pid_atti_rate_roll.update_all(target_rate[0], gyro_latest[0], 1);
motor_out.y = _pid_atti_rate_pitch.update_all(target_rate[1], gyro_latest[1], 1);
motor_out.z = _pid_atti_rate_yaw.update_all(target_rate[2], gyro_latest[2], 1);
return motor_out;
}
// This example uses exact same controller architecture as ArduCopter attitude controller without all the safe guard against saturation.
// The gains are scaled 0.9 times to better detect switch over response.
// Note that integrator are not reset correctly as it is done in reset_main_att_controller inside AC_CustomControl.cpp
// This is done intentionally to demonstrate switch over performance of two exact controller with different reset handling.
void AC_CustomControl_PID::reset(void)
{
_pid_atti_rate_roll.reset_I();
_pid_atti_rate_pitch.reset_I();
_pid_atti_rate_yaw.reset_I();
_pid_atti_rate_roll.reset_filter();
_pid_atti_rate_pitch.reset_filter();
_pid_atti_rate_yaw.reset_filter();
}
#endif