CoCalc -- AC_CustomControl

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AC_CustomControl/AC_CustomControl_PID.cpp
Views: ¹⁷⁹⁸
1
#include "AC_CustomControl_config.h"
2

3
#if AP_CUSTOMCONTROL_PID_ENABLED
4

5
#include "AC_CustomControl_PID.h"
6
#include "AC_AttitudeControl/AC_AttitudeControl_Multi.h"
7

8
// table of user settable parameters
9
const AP_Param::GroupInfo AC_CustomControl_PID::var_info[] = {
10
    // @Param: ANG_RLL_P
11
    // @DisplayName: Roll axis angle controller P gain
12
    // @Description: Roll axis angle controller P gain.  Converts the error between the desired roll angle and actual angle to a desired roll rate
13
    // @Range: 3.000 12.000
14
    // @Range{Sub}: 0.0 12.000
15
    // @User: Standard
16
    AP_SUBGROUPINFO(_p_angle_roll2, "ANG_RLL_", 1, AC_CustomControl_PID, AC_P),
17

18
    // @Param: ANG_PIT_P
19
    // @DisplayName: Pitch axis angle controller P gain
20
    // @Description: Pitch axis angle controller P gain.  Converts the error between the desired pitch angle and actual angle to a desired pitch rate
21
    // @Range: 3.000 12.000
22
    // @Range{Sub}: 0.0 12.000
23
    // @User: Standard
24
    AP_SUBGROUPINFO(_p_angle_pitch2, "ANG_PIT_", 2, AC_CustomControl_PID, AC_P),
25

26
    // @Param: ANG_YAW_P
27
    // @DisplayName: Yaw axis angle controller P gain
28
    // @Description: Yaw axis angle controller P gain.  Converts the error between the desired yaw angle and actual angle to a desired yaw rate
29
    // @Range: 3.000 12.000
30
    // @Range{Sub}: 0.0 6.000
31
    // @User: Standard    
32
    AP_SUBGROUPINFO(_p_angle_yaw2, "ANG_YAW_", 3, AC_CustomControl_PID, AC_P),
33

34

35
    // @Param: RAT_RLL_P
36
    // @DisplayName: Roll axis rate controller P gain
37
    // @Description: Roll axis rate controller P gain. Corrects in proportion to the difference between the desired roll rate vs actual roll rate
38
    // @Range: 0.01 0.5
39
    // @Increment: 0.005
40
    // @User: Standard
41

42
    // @Param: RAT_RLL_I
43
    // @DisplayName: Roll axis rate controller I gain
44
    // @Description: Roll axis rate controller I gain.  Corrects long-term difference in desired roll rate vs actual roll rate
45
    // @Range: 0.01 2.0
46
    // @Increment: 0.01
47
    // @User: Standard
48

49
    // @Param: RAT_RLL_IMAX
50
    // @DisplayName: Roll axis rate controller I gain maximum
51
    // @Description: Roll axis rate controller I gain maximum.  Constrains the maximum that the I term will output
52
    // @Range: 0 1
53
    // @Increment: 0.01
54
    // @User: Standard
55

56
    // @Param: RAT_RLL_D
57
    // @DisplayName: Roll axis rate controller D gain
58
    // @Description: Roll axis rate controller D gain.  Compensates for short-term change in desired roll rate vs actual roll rate
59
    // @Range: 0.0 0.05
60
    // @Increment: 0.001
61
    // @User: Standard
62

63
    // @Param: RAT_RLL_FF
64
    // @DisplayName: Roll axis rate controller feed forward
65
    // @Description: Roll axis rate controller feed forward
66
    // @Range: 0 0.5
67
    // @Increment: 0.001
68
    // @User: Standard
69

70
    // @Param: RAT_RLL_FLTT
71
    // @DisplayName: Roll axis rate controller target frequency in Hz
72
    // @Description: Roll axis rate controller target frequency in Hz
73
    // @Range: 5 100
74
    // @Increment: 1
75
    // @Units: Hz
76
    // @User: Standard
77

78
    // @Param: RAT_RLL_FLTE
79
    // @DisplayName: Roll axis rate controller error frequency in Hz
80
    // @Description: Roll axis rate controller error frequency in Hz
81
    // @Range: 0 100
82
    // @Increment: 1
83
    // @Units: Hz
84
    // @User: Standard
85

86
    // @Param: RAT_RLL_FLTD
87
    // @DisplayName: Roll axis rate controller derivative frequency in Hz
88
    // @Description: Roll axis rate controller derivative frequency in Hz
89
    // @Range: 5 100
90
    // @Increment: 1
91
    // @Units: Hz
92
    // @User: Standard
93

94
    // @Param: RAT_RLL_SMAX
95
    // @DisplayName: Roll slew rate limit
96
    // @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.
97
    // @Range: 0 200
98
    // @Increment: 0.5
99
    // @User: Advanced
100

101
    // @Param: RAT_RLL_PDMX
102
    // @DisplayName: Roll axis rate controller PD sum maximum
103
    // @Description: Roll axis rate controller PD sum maximum.  The maximum/minimum value that the sum of the P and D term can output
104
    // @Range: 0 1
105
    // @Increment: 0.01
106
    // @User: Advanced
107

108
    // @Param: RAT_RLL_D_FF
109
    // @DisplayName: Roll Derivative FeedForward Gain
110
    // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
111
    // @Range: 0 0.02
112
    // @Increment: 0.0001
113
    // @User: Advanced
114

115
    // @Param: RAT_RLL_NTF
116
    // @DisplayName: Roll Target notch filter index
117
    // @Description: Roll Target notch filter index
118
    // @Range: 1 8
119
    // @User: Advanced
120

121
    // @Param: RAT_RLL_NEF
122
    // @DisplayName: Roll Error notch filter index
123
    // @Description: Roll Error notch filter index
124
    // @Range: 1 8
125
    // @User: Advanced
126

127
    AP_SUBGROUPINFO(_pid_atti_rate_roll, "RAT_RLL_", 4, AC_CustomControl_PID, AC_PID),
128

129
    // @Param: RAT_PIT_P
130
    // @DisplayName: Pitch axis rate controller P gain
131
    // @Description: Pitch axis rate controller P gain.  Corrects in proportion to the difference between the desired pitch rate vs actual pitch rate
132
    // @Range: 0.01 0.50
133
    // @Increment: 0.005
134
    // @User: Standard
135

136
    // @Param: RAT_PIT_I
137
    // @DisplayName: Pitch axis rate controller I gain
138
    // @Description: Pitch axis rate controller I gain.  Corrects long-term difference in desired pitch rate vs actual pitch rate
139
    // @Range: 0.01 2.0
140
    // @Increment: 0.01
141
    // @User: Standard
142

143
    // @Param: RAT_PIT_IMAX
144
    // @DisplayName: Pitch axis rate controller I gain maximum
145
    // @Description: Pitch axis rate controller I gain maximum.  Constrains the maximum that the I term will output
146
    // @Range: 0 1
147
    // @Increment: 0.01
148
    // @User: Standard
149

150
    // @Param: RAT_PIT_D
151
    // @DisplayName: Pitch axis rate controller D gain
152
    // @Description: Pitch axis rate controller D gain.  Compensates for short-term change in desired pitch rate vs actual pitch rate
153
    // @Range: 0.0 0.05
154
    // @Increment: 0.001
155
    // @User: Standard
156

157
    // @Param: RAT_PIT_FF
158
    // @DisplayName: Pitch axis rate controller feed forward
159
    // @Description: Pitch axis rate controller feed forward
160
    // @Range: 0 0.5
161
    // @Increment: 0.001
162
    // @User: Standard
163

164
    // @Param: RAT_PIT_FLTT
165
    // @DisplayName: Pitch axis rate controller target frequency in Hz
166
    // @Description: Pitch axis rate controller target frequency in Hz
167
    // @Range: 5 100
168
    // @Increment: 1
169
    // @Units: Hz
170
    // @User: Standard
171

172
    // @Param: RAT_PIT_FLTE
173
    // @DisplayName: Pitch axis rate controller error frequency in Hz
174
    // @Description: Pitch axis rate controller error frequency in Hz
175
    // @Range: 0 100
176
    // @Increment: 1
177
    // @Units: Hz
178
    // @User: Standard
179

180
    // @Param: RAT_PIT_FLTD
181
    // @DisplayName: Pitch axis rate controller derivative frequency in Hz
182
    // @Description: Pitch axis rate controller derivative frequency in Hz
183
    // @Range: 5 100
184
    // @Increment: 1
185
    // @Units: Hz
186
    // @User: Standard
187

188
    // @Param: RAT_PIT_SMAX
189
    // @DisplayName: Pitch slew rate limit
190
    // @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.
191
    // @Range: 0 200
192
    // @Increment: 0.5
193
    // @User: Advanced
194

195
    // @Param: RAT_PIT_PDMX
196
    // @DisplayName: Pitch axis rate controller PD sum maximum
197
    // @Description: Pitch axis rate controller PD sum maximum.  The maximum/minimum value that the sum of the P and D term can output
198
    // @Range: 0 1
199
    // @Increment: 0.01
200
    // @User: Advanced
201

202
    // @Param: RAT_PIT_D_FF
203
    // @DisplayName: Pitch Derivative FeedForward Gain
204
    // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
205
    // @Range: 0 0.02
206
    // @Increment: 0.0001
207
    // @User: Advanced
208

209
    // @Param: RAT_PIT_NTF
210
    // @DisplayName: Pitch Target notch filter index
211
    // @Description: Pitch Target notch filter index
212
    // @Range: 1 8
213
    // @User: Advanced
214

215
    // @Param: RAT_PIT_NEF
216
    // @DisplayName: Pitch Error notch filter index
217
    // @Description: Pitch Error notch filter index
218
    // @Range: 1 8
219
    // @User: Advanced
220

221
    AP_SUBGROUPINFO(_pid_atti_rate_pitch, "RAT_PIT_", 5, AC_CustomControl_PID, AC_PID),
222

223

224
    // @Param: RAT_YAW_P
225
    // @DisplayName: Yaw axis rate controller P gain
226
    // @Description: Yaw axis rate controller P gain.  Corrects in proportion to the difference between the desired yaw rate vs actual yaw rate
227
    // @Range: 0.10 2.50
228
    // @Increment: 0.005
229
    // @User: Standard
230

231
    // @Param: RAT_YAW_I
232
    // @DisplayName: Yaw axis rate controller I gain
233
    // @Description: Yaw axis rate controller I gain.  Corrects long-term difference in desired yaw rate vs actual yaw rate
234
    // @Range: 0.010 1.0
235
    // @Increment: 0.01
236
    // @User: Standard
237

238
    // @Param: RAT_YAW_IMAX
239
    // @DisplayName: Yaw axis rate controller I gain maximum
240
    // @Description: Yaw axis rate controller I gain maximum.  Constrains the maximum that the I term will output
241
    // @Range: 0 1
242
    // @Increment: 0.01
243
    // @User: Standard
244

245
    // @Param: RAT_YAW_D
246
    // @DisplayName: Yaw axis rate controller D gain
247
    // @Description: Yaw axis rate controller D gain.  Compensates for short-term change in desired yaw rate vs actual yaw rate
248
    // @Range: 0.000 0.02
249
    // @Increment: 0.001
250
    // @User: Standard
251

252
    // @Param: RAT_YAW_FF
253
    // @DisplayName: Yaw axis rate controller feed forward
254
    // @Description: Yaw axis rate controller feed forward
255
    // @Range: 0 0.5
256
    // @Increment: 0.001
257
    // @User: Standard
258

259
    // @Param: RAT_YAW_FLTT
260
    // @DisplayName: Yaw axis rate controller target frequency in Hz
261
    // @Description: Yaw axis rate controller target frequency in Hz
262
    // @Range: 1 50
263
    // @Increment: 1
264
    // @Units: Hz
265
    // @User: Standard
266

267
    // @Param: RAT_YAW_FLTE
268
    // @DisplayName: Yaw axis rate controller error frequency in Hz
269
    // @Description: Yaw axis rate controller error frequency in Hz
270
    // @Range: 0 20
271
    // @Increment: 1
272
    // @Units: Hz
273
    // @User: Standard
274

275
    // @Param: RAT_YAW_FLTD
276
    // @DisplayName: Yaw axis rate controller derivative frequency in Hz
277
    // @Description: Yaw axis rate controller derivative frequency in Hz
278
    // @Range: 5 50
279
    // @Increment: 1
280
    // @Units: Hz
281
    // @User: Standard
282

283
    // @Param: RAT_YAW_SMAX
284
    // @DisplayName: Yaw slew rate limit
285
    // @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.
286
    // @Range: 0 200
287
    // @Increment: 0.5
288
    // @User: Advanced
289

290
    // @Param: RAT_YAW_PDMX
291
    // @DisplayName: Yaw axis rate controller PD sum maximum
292
    // @Description: Yaw axis rate controller PD sum maximum.  The maximum/minimum value that the sum of the P and D term can output
293
    // @Range: 0 1
294
    // @Increment: 0.01
295
    // @User: Advanced
296

297
    // @Param: RAT_YAW_D_FF
298
    // @DisplayName: Yaw Derivative FeedForward Gain
299
    // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
300
    // @Range: 0 0.02
301
    // @Increment: 0.0001
302
    // @User: Advanced
303

304
    // @Param: RAT_YAW_NTF
305
    // @DisplayName: Yaw Target notch filter index
306
    // @Description: Yaw Target notch filter index
307
    // @Range: 1 8
308
    // @User: Advanced
309

310
    // @Param: RAT_YAW_NEF
311
    // @DisplayName: Yaw Error notch filter index
312
    // @Description: Yaw Error notch filter index
313
    // @Range: 1 8
314
    // @User: Advanced
315

316
    AP_SUBGROUPINFO(_pid_atti_rate_yaw, "RAT_YAW_", 6, AC_CustomControl_PID, AC_PID),
317

318
    AP_GROUPEND
319
};
320

321
AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) :
322
    AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt),
323
    _p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
324
    _p_angle_pitch2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
325
    _p_angle_yaw2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f),
326
    _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_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f),
327
    _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_RPY_FILT_HZ * 0.90f, 0.0f, AC_ATC_MULTI_RATE_RPY_FILT_HZ * 0.90f),
328
    _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_RPY_FILT_HZ * 0.90f, AC_ATC_MULTI_RATE_YAW_FILT_HZ * 0.90f, 0.0f)
329
{
330
    _dt = dt;
331
    AP_Param::setup_object_defaults(this, var_info);
332
}
333

334
Vector3f AC_CustomControl_PID::update()
335
{
336
      // reset controller based on spool state
337
    switch (_motors->get_spool_state()) {
338
        case AP_Motors::SpoolState::SHUT_DOWN:
339
        case AP_Motors::SpoolState::GROUND_IDLE:
340
            // We are still at the ground. Reset custom controller to avoid
341
            // build up, ex: integrator
342
            reset();
343
            break;
344

345
        case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
346
        case AP_Motors::SpoolState::SPOOLING_UP:
347
        case AP_Motors::SpoolState::SPOOLING_DOWN:
348
            // we are off the ground
349
            break;
350
    }
351

352
    // run custom controller after here
353
     Quaternion attitude_body, attitude_target;
354
    _ahrs->get_quat_body_to_ned(attitude_body);
355

356
    attitude_target = _att_control->get_attitude_target_quat();
357
    // This vector represents the angular error to rotate the thrust vector using x and y and heading using z
358
    Vector3f attitude_error;
359
    float _thrust_angle, _thrust_error_angle;
360
    _att_control->thrust_heading_rotation_angles(attitude_target, attitude_body, attitude_error, _thrust_angle, _thrust_error_angle);
361

362
    // recalculate ang vel feedforward from attitude target model
363
    // rotation from the target frame to the body frame
364
    Quaternion rotation_target_to_body = attitude_body.inverse() * attitude_target;
365
    // target angle velocity vector in the body frame
366
    Vector3f ang_vel_body_feedforward = rotation_target_to_body * _att_control->get_attitude_target_ang_vel();
367

368
    // run attitude controller
369
    Vector3f target_rate;
370
    target_rate[0] = _p_angle_roll2.kP() * attitude_error.x + ang_vel_body_feedforward[0];
371
    target_rate[1] = _p_angle_pitch2.kP() * attitude_error.y + ang_vel_body_feedforward[1];
372
    target_rate[2] = _p_angle_yaw2.kP() * attitude_error.z + ang_vel_body_feedforward[2];
373

374
    // run rate controller
375
    Vector3f gyro_latest = _ahrs->get_gyro_latest();
376
    Vector3f motor_out;
377
    motor_out.x = _pid_atti_rate_roll.update_all(target_rate[0], gyro_latest[0], _dt, false);
378
    motor_out.y = _pid_atti_rate_pitch.update_all(target_rate[1], gyro_latest[1], _dt, false);
379
    motor_out.z = _pid_atti_rate_yaw.update_all(target_rate[2], gyro_latest[2], _dt, false);
380

381
    return motor_out;
382
}
383

384
// This example uses exact same controller architecture as ArduCopter attitude controller without all the safe guard against saturation.
385
// The gains are scaled 0.9 times to better detect switch over response. 
386
// Note that integrator are not reset correctly as it is done in reset_main_att_controller inside AC_CustomControl.cpp
387
// This is done intentionally to demonstrate switch over performance of two exact controller with different reset handling.
388
void AC_CustomControl_PID::reset(void)
389
{
390
    _pid_atti_rate_roll.reset_I();
391
    _pid_atti_rate_pitch.reset_I();
392
    _pid_atti_rate_yaw.reset_I();
393
    _pid_atti_rate_roll.reset_filter();
394
    _pid_atti_rate_pitch.reset_filter();
395
    _pid_atti_rate_yaw.reset_filter();
396
}
397

398

399
void AC_CustomControl_PID::set_notch_sample_rate(float sample_rate)
400
{
401
#if AP_FILTER_ENABLED
402
    _pid_atti_rate_roll.set_notch_sample_rate(sample_rate);
403
    _pid_atti_rate_pitch.set_notch_sample_rate(sample_rate);
404
    _pid_atti_rate_yaw.set_notch_sample_rate(sample_rate);
405
#endif
406
}
407

408
#endif  // AP_CUSTOMCONTROL_PID_ENABLED
409

410
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

Product

Resources

Company

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more, all in one place. Commercial Alternative to JupyterHub.

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
