Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include <AP_AHRS/AP_AHRS.h>
17 : #include <AP_Math/AP_Math.h>
18 : #include <AP_HAL/AP_HAL.h>
19 : #include "AR_AttitudeControl.h"
20 : #include <AP_GPS/AP_GPS.h>
21 :
22 : // attitude control default definition
23 : #define AR_ATTCONTROL_STEER_ANG_P 2.00f
24 : #define AR_ATTCONTROL_STEER_RATE_FF 0.20f
25 : #define AR_ATTCONTROL_STEER_RATE_P 0.20f
26 : #define AR_ATTCONTROL_STEER_RATE_I 0.20f
27 : #define AR_ATTCONTROL_STEER_RATE_IMAX 1.00f
28 : #define AR_ATTCONTROL_STEER_RATE_D 0.00f
29 : #define AR_ATTCONTROL_STEER_RATE_FILT 10.00f
30 : #define AR_ATTCONTROL_STEER_RATE_MAX 120.0f
31 : #define AR_ATTCONTROL_STEER_ACCEL_MAX 120.0f
32 : #define AR_ATTCONTROL_STEER_DECEL_MAX 0.0f
33 : #define AR_ATTCONTROL_THR_SPEED_P 0.20f
34 : #define AR_ATTCONTROL_THR_SPEED_I 0.20f
35 : #define AR_ATTCONTROL_THR_SPEED_IMAX 1.00f
36 : #define AR_ATTCONTROL_THR_SPEED_D 0.00f
37 : #define AR_ATTCONTROL_THR_SPEED_FILT 10.00f
38 : #define AR_ATTCONTROL_PITCH_THR_P 1.80f
39 : #define AR_ATTCONTROL_PITCH_THR_I 1.50f
40 : #define AR_ATTCONTROL_PITCH_THR_D 0.03f
41 : #define AR_ATTCONTROL_PITCH_THR_IMAX 1.0f
42 : #define AR_ATTCONTROL_PITCH_THR_FILT 10.0f
43 : #define AR_ATTCONTROL_BAL_PITCH_FF 0.4f
44 : #define AR_ATTCONTROL_PITCH_LIM_TC 0.5f // pitch limit default time constant
45 : #define AR_ATTCONTROL_PITCH_RELAX_RATIO 0.5f // pitch limit relaxed 2x slower than it is limited
46 : #define AR_ATTCONTROL_PITCH_LIM_THR_THRESH 0.60 // pitch limiting starts if throttle exceeds 60%
47 : #define AR_ATTCONTROL_DT 0.02f
48 : #define AR_ATTCONTROL_TIMEOUT_MS 200
49 : #define AR_ATTCONTROL_HEEL_SAIL_P 1.0f
50 : #define AR_ATTCONTROL_HEEL_SAIL_I 0.1f
51 : #define AR_ATTCONTROL_HEEL_SAIL_D 0.0f
52 : #define AR_ATTCONTROL_HEEL_SAIL_IMAX 1.0f
53 : #define AR_ATTCONTROL_HEEL_SAIL_FILT 10.0f
54 : #define AR_ATTCONTROL_DT 0.02f
55 :
56 : // throttle/speed control maximum acceleration/deceleration (in m/s) (_ACCEL_MAX parameter default)
57 : #define AR_ATTCONTROL_THR_ACCEL_MAX 1.00f
58 :
59 : // minimum speed in m/s
60 : #define AR_ATTCONTROL_STEER_SPEED_MIN 1.0f
61 :
62 : // speed (in m/s) at or below which vehicle is considered stopped (_STOP_SPEED parameter default)
63 : #define AR_ATTCONTROL_STOP_SPEED_DEFAULT 0.1f
64 :
65 : extern const AP_HAL::HAL& hal;
66 :
67 : AR_AttitudeControl *AR_AttitudeControl::_singleton;
68 :
69 : const AP_Param::GroupInfo AR_AttitudeControl::var_info[] = {
70 :
71 : // @Param: _STR_RAT_P
72 : // @DisplayName: Steering control rate P gain
73 : // @Description: Steering control rate P gain. Converts the turn rate error (in radians/sec) to a steering control output (in the range -1 to +1)
74 : // @Range: 0.000 2.000
75 : // @Increment: 0.001
76 : // @User: Standard
77 :
78 : // @Param: _STR_RAT_I
79 : // @DisplayName: Steering control I gain
80 : // @Description: Steering control I gain. Corrects long term error between the desired turn rate (in rad/s) and actual
81 : // @Range: 0.000 2.000
82 : // @Increment: 0.001
83 : // @User: Standard
84 :
85 : // @Param: _STR_RAT_IMAX
86 : // @DisplayName: Steering control I gain maximum
87 : // @Description: Steering control I gain maximum. Constrains the steering output (range -1 to +1) that the I term will generate
88 : // @Range: 0.000 1.000
89 : // @Increment: 0.01
90 : // @User: Standard
91 :
92 : // @Param: _STR_RAT_D
93 : // @DisplayName: Steering control D gain
94 : // @Description: Steering control D gain. Compensates for short-term change in desired turn rate vs actual
95 : // @Range: 0.000 0.400
96 : // @Increment: 0.001
97 : // @User: Standard
98 :
99 : // @Param: _STR_RAT_FF
100 : // @DisplayName: Steering control feed forward
101 : // @Description: Steering control feed forward
102 : // @Range: 0.000 3.000
103 : // @Increment: 0.001
104 : // @User: Standard
105 :
106 : // @Param: _STR_RAT_FILT
107 : // @DisplayName: Steering control filter frequency
108 : // @Description: Steering control input filter. Lower values reduce noise but add delay.
109 : // @Range: 0.000 100.000
110 : // @Increment: 0.1
111 : // @Units: Hz
112 : // @User: Standard
113 :
114 : // @Param: _STR_RAT_FLTT
115 : // @DisplayName: Steering control Target filter frequency in Hz
116 : // @Description: Target filter frequency in Hz
117 : // @Range: 0.000 100.000
118 : // @Increment: 0.1
119 : // @Units: Hz
120 : // @User: Standard
121 :
122 : // @Param: _STR_RAT_FLTE
123 : // @DisplayName: Steering control Error filter frequency in Hz
124 : // @Description: Error filter frequency in Hz
125 : // @Range: 0.000 100.000
126 : // @Increment: 0.1
127 : // @Units: Hz
128 : // @User: Standard
129 :
130 : // @Param: _STR_RAT_FLTD
131 : // @DisplayName: Steering control Derivative term filter frequency in Hz
132 : // @Description: Derivative filter frequency in Hz
133 : // @Range: 0.000 100.000
134 : // @Increment: 0.1
135 : // @Units: Hz
136 : // @User: Standard
137 :
138 : // @Param: _STR_RAT_SMAX
139 : // @DisplayName: Steering slew rate limit
140 : // @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.
141 : // @Range: 0 200
142 : // @Increment: 0.5
143 : // @User: Advanced
144 :
145 : // @Param: _STR_RAT_PDMX
146 : // @DisplayName: Steering control PD sum maximum
147 : // @Description: Steering control PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
148 : // @Range: 0.000 1.000
149 : // @Increment: 0.01
150 :
151 : // @Param: _STR_RAT_D_FF
152 : // @DisplayName: Steering control Derivative FeedForward Gain
153 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
154 : // @Range: 0 0.03
155 : // @Increment: 0.001
156 : // @User: Advanced
157 :
158 : // @Param: _STR_RAT_NTF
159 : // @DisplayName: Steering control Target notch filter index
160 : // @Description: Steering control Target notch filter index, zero disables
161 : // @Range: 0 8
162 : // @User: Advanced
163 :
164 : // @Param: _STR_RAT_NEF
165 : // @DisplayName: Steering control Error notch filter index
166 : // @Description: Steering control Error notch filter index, zero disables
167 : // @Range: 0 8
168 : // @User: Advanced
169 :
170 : AP_SUBGROUPINFO(_steer_rate_pid, "_STR_RAT_", 1, AR_AttitudeControl, AC_PID),
171 :
172 : // @Param: _SPEED_P
173 : // @DisplayName: Speed control P gain
174 : // @Description: Speed control P gain. Converts the error between the desired speed (in m/s) and actual speed to a motor output (in the range -1 to +1)
175 : // @Range: 0.010 2.000
176 : // @Increment: 0.01
177 : // @User: Standard
178 :
179 : // @Param: _SPEED_I
180 : // @DisplayName: Speed control I gain
181 : // @Description: Speed control I gain. Corrects long term error between the desired speed (in m/s) and actual speed
182 : // @Range: 0.000 2.000
183 : // @Increment: 0.01
184 : // @User: Standard
185 :
186 : // @Param: _SPEED_IMAX
187 : // @DisplayName: Speed control I gain maximum
188 : // @Description: Speed control I gain maximum. Constrains the maximum motor output (range -1 to +1) that the I term will generate
189 : // @Range: 0.000 1.000
190 : // @Increment: 0.01
191 : // @User: Standard
192 :
193 : // @Param: _SPEED_D
194 : // @DisplayName: Speed control D gain
195 : // @Description: Speed control D gain. Compensates for short-term change in desired speed vs actual
196 : // @Range: 0.000 0.400
197 : // @Increment: 0.001
198 : // @User: Standard
199 :
200 : // @Param: _SPEED_FF
201 : // @DisplayName: Speed control feed forward
202 : // @Description: Speed control feed forward
203 : // @Range: 0.000 0.500
204 : // @Increment: 0.001
205 : // @User: Standard
206 :
207 : // @Param: _SPEED_FILT
208 : // @DisplayName: Speed control filter frequency
209 : // @Description: Speed control input filter. Lower values reduce noise but add delay.
210 : // @Range: 0.000 100.000
211 : // @Increment: 0.1
212 : // @Units: Hz
213 : // @User: Standard
214 :
215 : // @Param: _SPEED_FLTT
216 : // @DisplayName: Speed control Target filter frequency in Hz
217 : // @Description: Target filter frequency in Hz
218 : // @Range: 0.000 100.000
219 : // @Increment: 0.1
220 : // @Units: Hz
221 : // @User: Standard
222 :
223 : // @Param: _SPEED_FLTE
224 : // @DisplayName: Speed control Error filter frequency in Hz
225 : // @Description: Error filter frequency in Hz
226 : // @Range: 0.000 100.000
227 : // @Increment: 0.1
228 : // @Units: Hz
229 : // @User: Standard
230 :
231 : // @Param: _SPEED_FLTD
232 : // @DisplayName: Speed control Derivative term filter frequency in Hz
233 : // @Description: Derivative filter frequency in Hz
234 : // @Range: 0.000 100.000
235 : // @Increment: 0.1
236 : // @Units: Hz
237 : // @User: Standard
238 :
239 : // @Param: _SPEED_SMAX
240 : // @DisplayName: Speed control slew rate limit
241 : // @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.
242 : // @Range: 0 200
243 : // @Increment: 0.5
244 : // @User: Advanced
245 :
246 : // @Param: _SPEED_PDMX
247 : // @DisplayName: Speed control PD sum maximum
248 : // @Description: Speed control PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
249 : // @Range: 0.000 1.000
250 : // @Increment: 0.01
251 :
252 : // @Param: _SPEED_D_FF
253 : // @DisplayName: Speed control Derivative FeedForward Gain
254 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
255 : // @Range: 0 0.03
256 : // @Increment: 0.001
257 : // @User: Advanced
258 :
259 : // @Param: _SPEED_NTF
260 : // @DisplayName: Speed control Target notch filter index
261 : // @Description: Speed control Target notch filter index, zero disables
262 : // @Range: 0 8
263 : // @User: Advanced
264 :
265 : // @Param: _SPEED_NEF
266 : // @DisplayName: Speed control Error notch filter index
267 : // @Description: Speed control Error notch filter index, zero disables
268 : // @Range: 0 8
269 : // @User: Advanced
270 :
271 : AP_SUBGROUPINFO(_throttle_speed_pid, "_SPEED_", 2, AR_AttitudeControl, AC_PID),
272 :
273 : // @Param: _ACCEL_MAX
274 : // @DisplayName: Speed control acceleration (and deceleration) maximum in m/s/s
275 : // @Description: Speed control acceleration (and deceleration) maximum in m/s/s. 0 to disable acceleration limiting
276 : // @Range: 0.0 10.0
277 : // @Increment: 0.1
278 : // @Units: m/s/s
279 : // @User: Standard
280 : AP_GROUPINFO("_ACCEL_MAX", 3, AR_AttitudeControl, _throttle_accel_max, AR_ATTCONTROL_THR_ACCEL_MAX),
281 :
282 : // @Param: _BRAKE
283 : // @DisplayName: Speed control brake enable/disable
284 : // @Description: Speed control brake enable/disable. Allows sending a reversed output to the motors to slow the vehicle.
285 : // @Values: 0:Disable,1:Enable
286 : // @User: Standard
287 : AP_GROUPINFO("_BRAKE", 4, AR_AttitudeControl, _brake_enable, 1),
288 :
289 : // @Param: _STOP_SPEED
290 : // @DisplayName: Speed control stop speed
291 : // @Description: Speed control stop speed. Motor outputs to zero once vehicle speed falls below this value
292 : // @Range: 0.00 0.50
293 : // @Increment: 0.01
294 : // @Units: m/s
295 : // @User: Standard
296 : AP_GROUPINFO("_STOP_SPEED", 5, AR_AttitudeControl, _stop_speed, AR_ATTCONTROL_STOP_SPEED_DEFAULT),
297 :
298 : // @Param: _STR_ANG_P
299 : // @DisplayName: Steering control angle P gain
300 : // @Description: Steering control angle P gain. Converts the error between the desired heading/yaw (in radians) and actual heading/yaw to a desired turn rate (in rad/sec)
301 : // @Range: 1.000 10.000
302 : // @Increment: 0.1
303 : // @User: Standard
304 : AP_SUBGROUPINFO(_steer_angle_p, "_STR_ANG_", 6, AR_AttitudeControl, AC_P),
305 :
306 : // @Param: _STR_ACC_MAX
307 : // @DisplayName: Steering control angular acceleration maximum
308 : // @Description: Steering control angular acceleration maximum (in deg/s/s). 0 to disable acceleration limiting
309 : // @Range: 0 1000
310 : // @Increment: 0.1
311 : // @Units: deg/s/s
312 : // @User: Standard
313 : AP_GROUPINFO("_STR_ACC_MAX", 7, AR_AttitudeControl, _steer_accel_max, AR_ATTCONTROL_STEER_ACCEL_MAX),
314 :
315 : // @Param: _STR_RAT_MAX
316 : // @DisplayName: Steering control rotation rate maximum
317 : // @Description: Steering control rotation rate maximum in deg/s. 0 to remove rate limiting
318 : // @Range: 0 1000
319 : // @Increment: 0.1
320 : // @Units: deg/s
321 : // @User: Standard
322 : AP_GROUPINFO("_STR_RAT_MAX", 8, AR_AttitudeControl, _steer_rate_max, AR_ATTCONTROL_STEER_RATE_MAX),
323 :
324 : // @Param: _DECEL_MAX
325 : // @DisplayName: Speed control deceleration maximum in m/s/s
326 : // @Description: Speed control and deceleration maximum in m/s/s. 0 to use ATC_ACCEL_MAX for deceleration
327 : // @Range: 0.0 10.0
328 : // @Increment: 0.1
329 : // @Units: m/s/s
330 : // @User: Standard
331 : AP_GROUPINFO("_DECEL_MAX", 9, AR_AttitudeControl, _throttle_decel_max, 0.00f),
332 :
333 : // @Param: _BAL_P
334 : // @DisplayName: Pitch control P gain
335 : // @Description: Pitch control P gain for BalanceBots. Converts the error between the desired pitch (in radians) and actual pitch to a motor output (in the range -1 to +1)
336 : // @Range: 0.000 2.000
337 : // @Increment: 0.01
338 : // @User: Standard
339 :
340 : // @Param: _BAL_I
341 : // @DisplayName: Pitch control I gain
342 : // @Description: Pitch control I gain for BalanceBots. Corrects long term error between the desired pitch (in radians) and actual pitch
343 : // @Range: 0.000 2.000
344 : // @Increment: 0.01
345 : // @User: Standard
346 :
347 : // @Param: _BAL_IMAX
348 : // @DisplayName: Pitch control I gain maximum
349 : // @Description: Pitch control I gain maximum. Constrains the maximum motor output (range -1 to +1) that the I term will generate
350 : // @Range: 0.000 1.000
351 : // @Increment: 0.01
352 : // @User: Standard
353 :
354 : // @Param: _BAL_D
355 : // @DisplayName: Pitch control D gain
356 : // @Description: Pitch control D gain. Compensates for short-term change in desired pitch vs actual
357 : // @Range: 0.000 0.100
358 : // @Increment: 0.001
359 : // @User: Standard
360 :
361 : // @Param: _BAL_FF
362 : // @DisplayName: Pitch control feed forward
363 : // @Description: Pitch control feed forward
364 : // @Range: 0.000 0.500
365 : // @Increment: 0.001
366 : // @User: Standard
367 :
368 : // @Param: _BAL_FILT
369 : // @DisplayName: Pitch control filter frequency
370 : // @Description: Pitch control input filter. Lower values reduce noise but add delay.
371 : // @Range: 0.000 100.000
372 : // @Increment: 0.1
373 : // @Units: Hz
374 : // @User: Standard
375 :
376 : // @Param: _BAL_FLTT
377 : // @DisplayName: Pitch control Target filter frequency in Hz
378 : // @Description: Pitch control Target filter frequency in Hz
379 : // @Range: 0.000 100.000
380 : // @Increment: 0.1
381 : // @Units: Hz
382 : // @User: Standard
383 :
384 : // @Param: _BAL_FLTE
385 : // @DisplayName: Pitch control Error filter frequency in Hz
386 : // @Description: Pitch control Error filter frequency in Hz
387 : // @Range: 0.000 100.000
388 : // @Increment: 0.1
389 : // @Units: Hz
390 : // @User: Standard
391 :
392 : // @Param: _BAL_FLTD
393 : // @DisplayName: Pitch control Derivative term filter frequency in Hz
394 : // @Description: Pitch control Derivative filter frequency in Hz
395 : // @Range: 0.000 100.000
396 : // @Increment: 0.1
397 : // @Units: Hz
398 : // @User: Standard
399 :
400 : // @Param: _BAL_SMAX
401 : // @DisplayName: Pitch control slew rate limit
402 : // @Description: Pitch control 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.
403 : // @Range: 0 200
404 : // @Increment: 0.5
405 : // @User: Advanced
406 :
407 : // @Param: _BAL_PDMX
408 : // @DisplayName: Pitch control PD sum maximum
409 : // @Description: Pitch control PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
410 : // @Range: 0.000 1.000
411 : // @Increment: 0.01
412 :
413 : // @Param: _BAL_D_FF
414 : // @DisplayName: Pitch control Derivative FeedForward Gain
415 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
416 : // @Range: 0 0.03
417 : // @Increment: 0.001
418 : // @User: Advanced
419 :
420 : // @Param: _BAL_NTF
421 : // @DisplayName: Pitch control Target notch filter index
422 : // @Description: Pitch control Target notch filter index, zero disables
423 : // @Range: 0 8
424 : // @User: Advanced
425 :
426 : // @Param: _BAL_NEF
427 : // @DisplayName: Pitch control Error notch filter index
428 : // @Description: Pitch control Error notch filter index, zero disables
429 : // @Range: 0 8
430 : // @User: Advanced
431 :
432 : AP_SUBGROUPINFO(_pitch_to_throttle_pid, "_BAL_", 10, AR_AttitudeControl, AC_PID),
433 :
434 : // @Param: _BAL_PIT_FF
435 : // @DisplayName: Pitch control feed forward from current pitch angle
436 : // @Description: Pitch control feed forward from current pitch angle
437 : // @Range: 0.0 1.0
438 : // @Increment: 0.01
439 : // @User: Standard
440 : AP_GROUPINFO("_BAL_PIT_FF", 11, AR_AttitudeControl, _pitch_to_throttle_ff, AR_ATTCONTROL_BAL_PITCH_FF),
441 :
442 : // @Param: _SAIL_P
443 : // @DisplayName: Sail Heel control P gain
444 : // @Description: Sail Heel control P gain for sailboats. Converts the error between the desired heel angle (in radians) and actual heel to a main sail output (in the range -1 to +1)
445 : // @Range: 0.000 2.000
446 : // @Increment: 0.01
447 : // @User: Standard
448 :
449 : // @Param: _SAIL_I
450 : // @DisplayName: Sail Heel control I gain
451 : // @Description: Sail Heel control I gain for sailboats. Corrects long term error between the desired heel angle (in radians) and actual
452 : // @Range: 0.000 2.000
453 : // @Increment: 0.01
454 : // @User: Standard
455 :
456 : // @Param: _SAIL_IMAX
457 : // @DisplayName: Sail Heel control I gain maximum
458 : // @Description: Sail Heel control I gain maximum. Constrains the maximum I term contribution to the main sail output (range -1 to +1)
459 : // @Range: 0.000 1.000
460 : // @Increment: 0.01
461 : // @User: Standard
462 :
463 : // @Param: _SAIL_D
464 : // @DisplayName: Sail Heel control D gain
465 : // @Description: Sail Heel control D gain. Compensates for short-term change in desired heel angle vs actual
466 : // @Range: 0.000 0.100
467 : // @Increment: 0.001
468 : // @User: Standard
469 :
470 : // @Param: _SAIL_FF
471 : // @DisplayName: Sail Heel control feed forward
472 : // @Description: Sail Heel control feed forward
473 : // @Range: 0.000 0.500
474 : // @Increment: 0.001
475 : // @User: Standard
476 :
477 : // @Param: _SAIL_FILT
478 : // @DisplayName: Sail Heel control filter frequency
479 : // @Description: Sail Heel control input filter. Lower values reduce noise but add delay.
480 : // @Range: 0.000 100.000
481 : // @Increment: 0.1
482 : // @Units: Hz
483 : // @User: Standard
484 :
485 : // @Param: _SAIL_FLTT
486 : // @DisplayName: Sail Heel Target filter frequency in Hz
487 : // @Description: Target filter frequency in Hz
488 : // @Range: 0.000 100.000
489 : // @Increment: 0.1
490 : // @Units: Hz
491 : // @User: Standard
492 :
493 : // @Param: _SAIL_FLTE
494 : // @DisplayName: Sail Heel Error filter frequency in Hz
495 : // @Description: Error filter frequency in Hz
496 : // @Range: 0.000 100.000
497 : // @Increment: 0.1
498 : // @Units: Hz
499 : // @User: Standard
500 :
501 : // @Param: _SAIL_FLTD
502 : // @DisplayName: Sail Heel Derivative term filter frequency in Hz
503 : // @Description: Derivative filter frequency in Hz
504 : // @Range: 0.000 100.000
505 : // @Increment: 0.1
506 : // @Units: Hz
507 : // @User: Standard
508 :
509 : // @Param: _SAIL_SMAX
510 : // @DisplayName: Sail heel slew rate limit
511 : // @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.
512 : // @Range: 0 200
513 : // @Increment: 0.5
514 : // @User: Advanced
515 :
516 : // @Param: _SAIL_PDMX
517 : // @DisplayName: Sail Heel control PD sum maximum
518 : // @Description: Sail Heel control PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
519 : // @Range: 0.000 1.000
520 : // @Increment: 0.01
521 :
522 : // @Param: _SAIL_D_FF
523 : // @DisplayName: Sail Heel Derivative FeedForward Gain
524 : // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
525 : // @Range: 0 0.03
526 : // @Increment: 0.001
527 : // @User: Advanced
528 :
529 : // @Param: _SAIL_NTF
530 : // @DisplayName: Sail Heel Target notch filter index
531 : // @Description: Sail Heel Target notch filter index, zero disables
532 : // @Range: 0 8
533 : // @User: Advanced
534 :
535 : // @Param: _SAIL_NEF
536 : // @DisplayName: Sail Heel Error notch filter index
537 : // @Description: Sail Heel Error notch filter index, zero disables
538 : // @Range: 0 8
539 : // @User: Advanced
540 :
541 : AP_SUBGROUPINFO(_sailboat_heel_pid, "_SAIL_", 12, AR_AttitudeControl, AC_PID),
542 :
543 : // @Param: _TURN_MAX_G
544 : // @DisplayName: Turning maximum G force
545 : // @Description: The maximum turning acceleration (in units of gravities) that the rover can handle while remaining stable. The navigation code will keep the lateral acceleration below this level to avoid rolling over or slipping the wheels in turns
546 : // @Units: gravities
547 : // @Range: 0.1 10
548 : // @Increment: 0.01
549 : // @User: Standard
550 : AP_GROUPINFO("_TURN_MAX_G", 13, AR_AttitudeControl, _turn_lateral_G_max, 0.6f),
551 :
552 : // @Param: _BAL_LIM_TC
553 : // @DisplayName: Pitch control limit time constant
554 : // @Description: Pitch control limit time constant to protect against falling. Lower values limit pitch more quickly, higher values limit more slowly. Set to 0 to disable
555 : // @Range: 0.0 5.0
556 : // @Increment: 0.01
557 : // @User: Standard
558 : AP_GROUPINFO("_BAL_LIM_TC", 14, AR_AttitudeControl, _pitch_limit_tc, AR_ATTCONTROL_PITCH_LIM_TC),
559 :
560 : // @Param: _BAL_LIM_THR
561 : // @DisplayName: Pitch control limit throttle threshold
562 : // @Description: Pitch control limit throttle threshold. Pitch angle will be limited if throttle crosses this threshold (from 0 to 1)
563 : // @Range: 0.0 1.0
564 : // @Increment: 0.01
565 : // @User: Standard
566 : AP_GROUPINFO("_BAL_LIM_THR", 15, AR_AttitudeControl, _pitch_limit_throttle_thresh, AR_ATTCONTROL_PITCH_LIM_THR_THRESH),
567 :
568 : // @Param: _STR_DEC_MAX
569 : // @DisplayName: Steering control angular deceleration maximum
570 : // @Description: Steering control angular deceleration maximum (in deg/s/s). 0 to disable deceleration limiting
571 : // @Range: 0 1000
572 : // @Increment: 0.1
573 : // @Units: deg/s/s
574 : // @User: Standard
575 : AP_GROUPINFO("_STR_DEC_MAX", 16, AR_AttitudeControl, _steer_decel_max, AR_ATTCONTROL_STEER_DECEL_MAX),
576 :
577 : AP_GROUPEND
578 : };
579 :
580 285 : AR_AttitudeControl::AR_AttitudeControl() :
581 285 : _steer_angle_p(AR_ATTCONTROL_STEER_ANG_P),
582 285 : _steer_rate_pid(AR_ATTCONTROL_STEER_RATE_P, AR_ATTCONTROL_STEER_RATE_I, AR_ATTCONTROL_STEER_RATE_D, AR_ATTCONTROL_STEER_RATE_FF, AR_ATTCONTROL_STEER_RATE_IMAX, 0.0f, AR_ATTCONTROL_STEER_RATE_FILT, 0.0f),
583 285 : _throttle_speed_pid(AR_ATTCONTROL_THR_SPEED_P, AR_ATTCONTROL_THR_SPEED_I, AR_ATTCONTROL_THR_SPEED_D, 0.0f, AR_ATTCONTROL_THR_SPEED_IMAX, 0.0f, AR_ATTCONTROL_THR_SPEED_FILT, 0.0f),
584 285 : _pitch_to_throttle_pid(AR_ATTCONTROL_PITCH_THR_P, AR_ATTCONTROL_PITCH_THR_I, AR_ATTCONTROL_PITCH_THR_D, 0.0f, AR_ATTCONTROL_PITCH_THR_IMAX, 0.0f, AR_ATTCONTROL_PITCH_THR_FILT, 0.0f),
585 285 : _sailboat_heel_pid(AR_ATTCONTROL_HEEL_SAIL_P, AR_ATTCONTROL_HEEL_SAIL_I, AR_ATTCONTROL_HEEL_SAIL_D, 0.0f, AR_ATTCONTROL_HEEL_SAIL_IMAX, 0.0f, AR_ATTCONTROL_HEEL_SAIL_FILT, 0.0f)
586 : {
587 285 : _singleton = this;
588 285 : AP_Param::setup_object_defaults(this, var_info);
589 285 : }
590 :
591 : // return a steering servo output from -1.0 to +1.0 given a desired lateral acceleration rate in m/s/s.
592 : // positive lateral acceleration is to the right.
593 3598 : float AR_AttitudeControl::get_steering_out_lat_accel(float desired_accel, bool motor_limit_left, bool motor_limit_right, float dt)
594 : {
595 : // record desired accel for reporting purposes
596 3598 : _steer_lat_accel_last_ms = AP_HAL::millis();
597 3598 : _desired_lat_accel = desired_accel;
598 :
599 : // get speed forward
600 : float speed;
601 3598 : if (!get_forward_speed(speed)) {
602 : // we expect caller will not try to control heading using rate control without a valid speed estimate
603 : // on failure to get speed we do not attempt to steer
604 0 : return 0.0f;
605 : }
606 :
607 3598 : const float desired_rate = get_turn_rate_from_lat_accel(desired_accel, speed);
608 :
609 3598 : return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt);
610 : }
611 :
612 : // return a steering servo output from -1 to +1 given a heading in radians
613 : // set rate_max_rads to a non-zero number to apply a limit on the desired turn rate
614 : // return value is normally in range -1.0 to +1.0 but can be higher or lower
615 23820 : float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate_max_rads, bool motor_limit_left, bool motor_limit_right, float dt)
616 : {
617 : // calculate the desired turn rate (in radians) from the angle error (also in radians)
618 23820 : float desired_rate = get_turn_rate_from_heading(heading_rad, rate_max_rads);
619 :
620 23820 : return get_steering_out_rate(desired_rate, motor_limit_left, motor_limit_right, dt);
621 : }
622 :
623 : // return a desired turn-rate given a desired heading in radians
624 23820 : float AR_AttitudeControl::get_turn_rate_from_heading(float heading_rad, float rate_max_rads) const
625 : {
626 23820 : const float yaw_error = wrap_PI(heading_rad - AP::ahrs().get_yaw_rad());
627 :
628 : // Calculate the desired turn rate (in radians) from the angle error (also in radians)
629 23820 : float desired_rate = _steer_angle_p.get_p(yaw_error);
630 :
631 : // limit desired_rate if a custom pivot turn rate is selected, otherwise use ATC_STR_RAT_MAX
632 23820 : if (is_positive(rate_max_rads)) {
633 786 : desired_rate = constrain_float(desired_rate, -rate_max_rads, rate_max_rads);
634 : }
635 :
636 : // if deceleration limit is provided, ensure rate can be slowed to zero in time to stop at heading_rad (i.e. avoid overshoot)
637 23820 : if (is_positive(_steer_decel_max)) {
638 0 : const float steer_decel_rate_max_rads = safe_sqrt(2.0 * fabsf(yaw_error) * radians(_steer_decel_max));
639 0 : desired_rate = constrain_float(desired_rate, -steer_decel_rate_max_rads, steer_decel_rate_max_rads);
640 23820 : } else if (is_positive(_steer_accel_max)) {
641 : // if no deceleration limit, use acceleration limit
642 23820 : const float steer_accel_rate_max_rads = safe_sqrt(2.0 * fabsf(yaw_error) * radians(_steer_accel_max));
643 23820 : desired_rate = constrain_float(desired_rate, -steer_accel_rate_max_rads, steer_accel_rate_max_rads);
644 : }
645 :
646 23820 : return desired_rate;
647 : }
648 :
649 : // return a steering servo output given a desired yaw rate in radians/sec.
650 : // positive yaw is to the right
651 : // return value is normally in range -1.0 to +1.0 but can be higher or lower
652 : // also sets steering_limit_left and steering_limit_right flags
653 220275 : float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool motor_limit_left, bool motor_limit_right, float dt)
654 : {
655 : // sanity check dt
656 220275 : dt = constrain_float(dt, 0.0f, 1.0f);
657 :
658 : // update steering limit flags used by higher level controllers (e.g. position controller)
659 220275 : _steering_limit_left = motor_limit_left;
660 220275 : _steering_limit_right = motor_limit_right;
661 :
662 : // if not called recently, reset input filter and desired turn rate to actual turn rate (used for accel limiting)
663 220275 : const uint32_t now = AP_HAL::millis();
664 220275 : if ((_steer_turn_last_ms == 0) || ((now - _steer_turn_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
665 88 : _steer_rate_pid.reset_filter();
666 88 : _steer_rate_pid.reset_I();
667 88 : _desired_turn_rate = AP::ahrs().get_yaw_rate_earth();
668 : }
669 220275 : _steer_turn_last_ms = now;
670 :
671 : // acceleration limit desired turn rate
672 220275 : if (is_positive(_steer_accel_max)) {
673 220275 : const float change_max = radians(_steer_accel_max) * dt;
674 220275 : if (desired_rate <= _desired_turn_rate - change_max) {
675 2445 : _steering_limit_left = true;
676 : }
677 220275 : if (desired_rate >= _desired_turn_rate + change_max) {
678 4849 : _steering_limit_right = true;
679 : }
680 220275 : desired_rate = constrain_float(desired_rate, _desired_turn_rate - change_max, _desired_turn_rate + change_max);
681 : }
682 220275 : _desired_turn_rate = desired_rate;
683 :
684 : // rate limit desired turn rate
685 220275 : if (is_positive(_steer_rate_max)) {
686 220275 : const float steer_rate_max_rad = radians(_steer_rate_max);
687 220275 : if (_desired_turn_rate <= -steer_rate_max_rad) {
688 465 : _steering_limit_left = true;
689 : }
690 220275 : if (_desired_turn_rate >= steer_rate_max_rad) {
691 2504 : _steering_limit_right = true;
692 : }
693 220275 : _desired_turn_rate = constrain_float(_desired_turn_rate, -steer_rate_max_rad, steer_rate_max_rad);
694 : }
695 :
696 : // G limit based on speed
697 : float speed;
698 220275 : if (get_forward_speed(speed)) {
699 : // do not limit to less than 1 deg/s
700 220090 : const float turn_rate_max = MAX(get_turn_rate_from_lat_accel(get_turn_lat_accel_max(), fabsf(speed)), radians(1.0f));
701 220090 : if (_desired_turn_rate <= -turn_rate_max) {
702 16 : _steering_limit_left = true;
703 : }
704 220090 : if (_desired_turn_rate >= turn_rate_max) {
705 263 : _steering_limit_right = true;
706 : }
707 220090 : _desired_turn_rate = constrain_float(_desired_turn_rate, -turn_rate_max, turn_rate_max);
708 : }
709 :
710 : // update pid to calculate output to motors
711 220275 : float output = _steer_rate_pid.update_all(_desired_turn_rate, AP::ahrs().get_yaw_rate_earth(), dt, (motor_limit_left || motor_limit_right));
712 220275 : output += _steer_rate_pid.get_ff();
713 : // constrain and return final output
714 220275 : return output;
715 : }
716 :
717 : // get latest desired turn rate in rad/sec (recorded during calls to get_steering_out_rate)
718 119840 : float AR_AttitudeControl::get_desired_turn_rate() const
719 : {
720 : // return zero if no recent calls to turn rate controller
721 119840 : if ((_steer_turn_last_ms == 0) || ((AP_HAL::millis() - _steer_turn_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
722 75630 : return 0.0f;
723 : }
724 44210 : return _desired_turn_rate;
725 : }
726 :
727 : // get latest desired lateral acceleration in m/s/s (recorded during calls to get_steering_out_lat_accel)
728 0 : float AR_AttitudeControl::get_desired_lat_accel() const
729 : {
730 : // return zero if no recent calls to lateral acceleration controller
731 0 : if ((_steer_lat_accel_last_ms == 0) || ((AP_HAL::millis() - _steer_lat_accel_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
732 0 : return 0.0f;
733 : }
734 0 : return _desired_lat_accel;
735 : }
736 :
737 : // get actual lateral acceleration in m/s/s. returns true on success
738 119840 : bool AR_AttitudeControl::get_lat_accel(float &lat_accel) const
739 : {
740 : float speed;
741 119840 : if (!get_forward_speed(speed)) {
742 7235 : return false;
743 : }
744 112605 : lat_accel = speed * AP::ahrs().get_yaw_rate_earth();
745 112605 : return true;
746 : }
747 :
748 : // calculate the turn rate in rad/sec given a lateral acceleration (in m/s/s) and speed (in m/s)
749 402146 : float AR_AttitudeControl::get_turn_rate_from_lat_accel(float lat_accel, float speed) const
750 : {
751 : // enforce minimum speed to stop oscillations when first starting to move
752 402146 : if (fabsf(speed) < AR_ATTCONTROL_STEER_SPEED_MIN) {
753 143509 : if (is_negative(speed)) {
754 1478 : speed = -AR_ATTCONTROL_STEER_SPEED_MIN;
755 : } else {
756 142031 : speed = AR_ATTCONTROL_STEER_SPEED_MIN;
757 : }
758 : }
759 :
760 402146 : return lat_accel / speed;
761 : }
762 :
763 : // return a throttle output from -1 to +1 given a desired speed in m/s (use negative speeds to travel backwards)
764 : // motor_limit should be true if motors have hit their upper or lower limits
765 : // cruise speed should be in m/s, cruise throttle should be a number from -1 to +1
766 180652 : float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt)
767 : {
768 : // sanity check dt
769 180652 : dt = constrain_float(dt, 0.0f, 1.0f);
770 :
771 : // get speed forward
772 : float speed;
773 180652 : if (!get_forward_speed(speed)) {
774 : // we expect caller will not try to control heading using rate control without a valid speed estimate
775 : // on failure to get speed we do not attempt to steer
776 0 : return 0.0f;
777 : }
778 :
779 : // if not called recently, reset input filter and desired speed to actual speed (used for accel limiting)
780 180652 : if (!speed_control_active()) {
781 37 : _throttle_speed_pid.reset_filter();
782 37 : _throttle_speed_pid.reset_I();
783 37 : _desired_speed = speed;
784 : }
785 180652 : _speed_last_ms = AP_HAL::millis();
786 :
787 : // acceleration limit desired speed
788 180652 : _desired_speed = get_desired_speed_accel_limited(desired_speed, dt);
789 :
790 : // calculate base throttle (protect against divide by zero)
791 180652 : float throttle_base = 0.0f;
792 180652 : if (is_positive(cruise_speed) && is_positive(cruise_throttle)) {
793 158417 : throttle_base = _desired_speed * (cruise_throttle / cruise_speed);
794 : }
795 :
796 : // calculate final output
797 180652 : float throttle_out = _throttle_speed_pid.update_all(_desired_speed, speed, dt, (motor_limit_low || motor_limit_high || _throttle_limit_low || _throttle_limit_high));
798 180652 : throttle_out += _throttle_speed_pid.get_ff();
799 180652 : throttle_out += throttle_base;
800 :
801 : // update PID info for reporting purposes
802 180652 : _throttle_speed_pid_info = _throttle_speed_pid.get_pid_info();
803 180652 : _throttle_speed_pid_info.FF += throttle_base;
804 :
805 : // clear local limit flags used to stop i-term build-up as we stop reversed outputs going to motors
806 180652 : _throttle_limit_low = false;
807 180652 : _throttle_limit_high = false;
808 :
809 : // protect against reverse output being sent to the motors unless braking has been enabled
810 180652 : if (!_brake_enable) {
811 : // if both desired speed and actual speed are positive, do not allow negative values
812 0 : if ((_desired_speed >= 0.0f) && (throttle_out <= 0.0f)) {
813 0 : throttle_out = 0.0f;
814 0 : _throttle_limit_low = true;
815 0 : } else if ((_desired_speed <= 0.0f) && (throttle_out >= 0.0f)) {
816 0 : throttle_out = 0.0f;
817 0 : _throttle_limit_high = true;
818 : }
819 : }
820 :
821 : // final output throttle in range -1 to 1
822 180652 : return throttle_out;
823 : }
824 :
825 : // return a throttle output from -1 to +1 to perform a controlled stop. returns true once the vehicle has stopped
826 67282 : float AR_AttitudeControl::get_throttle_out_stop(bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle, float dt, bool &stopped)
827 : {
828 : // get current system time
829 67282 : const uint32_t now = AP_HAL::millis();
830 :
831 : // if we were stopped in the last 300ms, assume we are still stopped
832 67282 : bool _stopped = (_stop_last_ms != 0) && (now - _stop_last_ms) < 300;
833 :
834 : // get deceleration limited speed
835 67282 : float desired_speed_limited = get_desired_speed_accel_limited(0.0f, dt);
836 :
837 : // get speed forward
838 : float speed;
839 67282 : if (!get_forward_speed(speed)) {
840 : // could not get speed so assume stopped
841 678 : _stopped = true;
842 : } else {
843 : // if desired speed is zero and vehicle drops below _stop_speed consider it stopped
844 66604 : if (is_zero(desired_speed_limited) && fabsf(speed) <= fabsf(_stop_speed)) {
845 63069 : _stopped = true;
846 : }
847 : }
848 :
849 : // set stopped status for caller
850 67282 : stopped = _stopped;
851 :
852 : // if stopped return zero
853 67282 : if (stopped) {
854 : // update last time we thought we were stopped
855 64277 : _stop_last_ms = now;
856 : // set last time speed controller was run so accelerations are limited
857 64277 : _speed_last_ms = now;
858 : // reset filters and I-term
859 64277 : _throttle_speed_pid.reset_filter();
860 64277 : _throttle_speed_pid.reset_I();
861 : // ensure desired speed is zero
862 64277 : _desired_speed = 0.0f;
863 64277 : return 0.0f;
864 : }
865 :
866 : // clear stopped system time
867 3005 : _stop_last_ms = 0;
868 : // run speed controller to bring vehicle to stop
869 3005 : return get_throttle_out_speed(desired_speed_limited, motor_limit_low, motor_limit_high, cruise_speed, cruise_throttle, dt);
870 : }
871 :
872 : // balancebot pitch to throttle controller
873 : // returns a throttle output from -1 to +1 given a desired pitch angle (in radians)
874 : // pitch_max should be the user defined max pitch angle (in radians)
875 : // motor_limit should be true if the motors have hit their upper or lower limit
876 54262 : float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float pitch_max, bool motor_limit, float dt)
877 : {
878 : // sanity check dt
879 54262 : dt = constrain_float(dt, 0.0f, 1.0f);
880 :
881 : // if not called recently, reset input filter
882 54262 : const uint32_t now = AP_HAL::millis();
883 54262 : if ((_balance_last_ms == 0) || ((now - _balance_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
884 31 : _pitch_to_throttle_pid.reset_filter();
885 31 : _pitch_to_throttle_pid.reset_I();
886 31 : _pitch_limit_low = -pitch_max;
887 31 : _pitch_limit_high = pitch_max;
888 : }
889 54262 : _balance_last_ms = now;
890 :
891 : // limit desired pitch to protect against falling
892 54262 : const bool pitch_limit_active = (_pitch_limit_tc >= 0.01) && (_pitch_limit_throttle_thresh > 0);
893 54262 : if (pitch_limit_active) {
894 54262 : desired_pitch = constrain_float(desired_pitch, _pitch_limit_low, _pitch_limit_high);
895 54262 : _pitch_limited = (desired_pitch <= _pitch_limit_low || desired_pitch >= _pitch_limit_high);
896 : } else {
897 0 : _pitch_limited = false;
898 : }
899 :
900 : // initialise output to feed forward from current pitch angle
901 54262 : const float pitch_rad = AP::ahrs().get_pitch_rad();
902 54262 : float output = sinf(pitch_rad) * _pitch_to_throttle_ff;
903 :
904 : // add regular PID control
905 54262 : output += _pitch_to_throttle_pid.update_all(desired_pitch, pitch_rad, dt, motor_limit);
906 54262 : output += _pitch_to_throttle_pid.get_ff();
907 :
908 : // update pitch limits for next iteration
909 : // note: pitch is positive when leaning backwards, negative when leaning forward
910 54262 : if (pitch_limit_active) {
911 54262 : const float pitch_limit_incr = 1.0/_pitch_limit_tc * dt * pitch_max;
912 54262 : const float pitch_relax_incr = pitch_limit_incr * AR_ATTCONTROL_PITCH_RELAX_RATIO;
913 54262 : if (output <= -_pitch_limit_throttle_thresh) {
914 : // very low negative throttle output means we must lower pitch_high (e.g. reduce leaning backwards)
915 0 : _pitch_limit_high = MAX(_pitch_limit_high - pitch_limit_incr, 0);
916 : } else {
917 54262 : _pitch_limit_high = MIN(_pitch_limit_high + pitch_relax_incr, pitch_max);
918 : }
919 54262 : if (output >= _pitch_limit_throttle_thresh) {
920 : // very high positive throttle output means we must raise pitch_low (reduce leaning forwards)
921 0 : _pitch_limit_low = MIN(_pitch_limit_low + pitch_limit_incr, 0);
922 : } else {
923 54262 : _pitch_limit_low = MAX(_pitch_limit_low - pitch_relax_incr, -pitch_max);
924 : }
925 : }
926 :
927 : // constrain and return final output
928 54262 : return output;
929 : }
930 :
931 : // get latest desired pitch in radians for reporting purposes
932 651273 : float AR_AttitudeControl::get_desired_pitch() const
933 : {
934 : // if not called recently, return 0
935 651273 : if ((_balance_last_ms == 0) || ((AP_HAL::millis() - _balance_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
936 597021 : return 0.0f;
937 : }
938 :
939 54252 : return _pitch_to_throttle_pid.get_pid_info().target;
940 : }
941 :
942 : // Sailboat heel(roll) angle controller releases sail to keep at maximum heel angle
943 : // but does not attempt to reach maximum heel angle, ie only lets sails out, does not pull them in
944 17468 : float AR_AttitudeControl::get_sail_out_from_heel(float desired_heel, float dt)
945 : {
946 : // sanity check dt
947 17468 : dt = constrain_float(dt, 0.0f, 1.0f);
948 :
949 : // if not called recently, reset input filter
950 17468 : const uint32_t now = AP_HAL::millis();
951 17468 : if ((_heel_controller_last_ms == 0) || ((now - _heel_controller_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
952 4 : _sailboat_heel_pid.reset_filter();
953 4 : _sailboat_heel_pid.reset_I();
954 : }
955 17468 : _heel_controller_last_ms = now;
956 :
957 17468 : _sailboat_heel_pid.update_all(desired_heel, fabsf(AP::ahrs().get_roll_rad()), dt);
958 :
959 : // get feed-forward
960 17468 : const float ff = _sailboat_heel_pid.get_ff();
961 :
962 : // get p, constrain to be zero or negative
963 17468 : float p = _sailboat_heel_pid.get_p();
964 17468 : if (is_positive(p)) {
965 17468 : p = 0.0f;
966 : }
967 :
968 : // get i, constrain to be zero or negative
969 17468 : float i = _sailboat_heel_pid.get_i();
970 17468 : if (is_positive(i)) {
971 17468 : i = 0.0f;
972 17468 : _sailboat_heel_pid.reset_I();
973 : }
974 :
975 : // get d
976 17468 : const float d = _sailboat_heel_pid.get_d();
977 :
978 : // constrain and return final output
979 17468 : return (ff + p + i + d) * -1.0f;
980 : }
981 :
982 : // get the slew rate value for speed and steering for oscillation detection in lua scripts
983 0 : void AR_AttitudeControl::get_srate(float &steering_srate, float &speed_srate)
984 : {
985 0 : steering_srate = get_steering_rate_pid().get_pid_info().slew_rate;
986 0 : speed_srate = _throttle_speed_pid_info.slew_rate;
987 0 : }
988 :
989 : // get forward speed in m/s (earth-frame horizontal velocity but only along vehicle x-axis). returns true on success
990 1709461 : bool AR_AttitudeControl::get_forward_speed(float &speed) const
991 : {
992 1709461 : Vector3f velocity;
993 1709461 : const AP_AHRS &_ahrs = AP::ahrs();
994 1709461 : if (!_ahrs.get_velocity_NED(velocity)) {
995 : // use less accurate GPS, assuming entire length is along forward/back axis of vehicle
996 55460 : if (AP::gps().status() >= AP_GPS_FixType::FIX_3D) {
997 2 : if (abs(wrap_180_cd(_ahrs.yaw_sensor - AP::gps().ground_course_cd())) <= 9000) {
998 0 : speed = AP::gps().ground_speed();
999 : } else {
1000 2 : speed = -AP::gps().ground_speed();
1001 : }
1002 2 : return true;
1003 : } else {
1004 55458 : return false;
1005 : }
1006 : }
1007 : // calculate forward speed velocity into body frame
1008 1654001 : speed = velocity.x*_ahrs.cos_yaw() + velocity.y*_ahrs.sin_yaw();
1009 1654001 : return true;
1010 : }
1011 :
1012 37929 : float AR_AttitudeControl::get_decel_max() const
1013 : {
1014 37929 : if (is_positive(_throttle_decel_max)) {
1015 0 : return _throttle_decel_max;
1016 : } else {
1017 37929 : return _throttle_accel_max;
1018 : }
1019 : }
1020 :
1021 : // check if speed controller active
1022 2420341 : bool AR_AttitudeControl::speed_control_active() const
1023 : {
1024 : // active if there have been recent calls to speed controller
1025 2420341 : if ((_speed_last_ms == 0) || ((AP_HAL::millis() - _speed_last_ms) > AR_ATTCONTROL_TIMEOUT_MS)) {
1026 459415 : return false;
1027 : }
1028 1960926 : return true;
1029 : }
1030 :
1031 : // get latest desired speed recorded during call to get_throttle_out_speed. For reporting purposes only
1032 345065 : float AR_AttitudeControl::get_desired_speed() const
1033 : {
1034 : // return zero if no recent calls to speed controller
1035 345065 : if (!speed_control_active()) {
1036 70722 : return 0.0f;
1037 : }
1038 274343 : return _desired_speed;
1039 : }
1040 :
1041 : // get acceleration limited desired speed
1042 675362 : float AR_AttitudeControl::get_desired_speed_accel_limited(float desired_speed, float dt) const
1043 : {
1044 : // return input value if no recent calls to speed controller
1045 : // apply no limiting when ATC_ACCEL_MAX is set to zero
1046 675362 : if (!speed_control_active() || !is_positive(_throttle_accel_max)) {
1047 34837 : return desired_speed;
1048 : }
1049 :
1050 : // sanity check dt
1051 640525 : dt = constrain_float(dt, 0.0f, 1.0f);
1052 :
1053 : // use previous desired speed as basis for accel limiting
1054 640525 : float speed_prev = _desired_speed;
1055 :
1056 : // if no recent calls to speed controller limit based on current speed
1057 640525 : if (!speed_control_active()) {
1058 0 : get_forward_speed(speed_prev);
1059 : }
1060 :
1061 : // acceleration limit desired speed
1062 : float speed_change_max;
1063 640525 : if (fabsf(desired_speed) < fabsf(_desired_speed) && is_positive(_throttle_decel_max)) {
1064 0 : speed_change_max = _throttle_decel_max * dt;
1065 : } else {
1066 640525 : speed_change_max = _throttle_accel_max * dt;
1067 : }
1068 640525 : return constrain_float(desired_speed, speed_prev - speed_change_max, speed_prev + speed_change_max);
1069 : }
1070 :
1071 : // get minimum stopping distance (in meters) given a speed (in m/s)
1072 273 : float AR_AttitudeControl::get_stopping_distance(float speed) const
1073 : {
1074 : // get maximum vehicle deceleration
1075 273 : const float decel_max = get_decel_max();
1076 :
1077 273 : if ((decel_max <= 0.0f) || is_zero(speed)) {
1078 0 : return 0.0f;
1079 : }
1080 :
1081 : // assume linear deceleration
1082 273 : return 0.5f * sq(speed) / decel_max;
1083 : }
1084 :
1085 : // relax I terms of throttle and steering controllers
1086 417 : void AR_AttitudeControl::relax_I()
1087 : {
1088 417 : _steer_rate_pid.reset_I();
1089 417 : _throttle_speed_pid.reset_I();
1090 417 : _pitch_to_throttle_pid.reset_I();
1091 417 : }
1092 :
1093 0 : void AR_AttitudeControl::set_notch_sample_rate(float sample_rate)
1094 : {
1095 : #if AP_FILTER_ENABLED
1096 0 : _steer_rate_pid.set_notch_sample_rate(sample_rate);
1097 0 : _throttle_speed_pid.set_notch_sample_rate(sample_rate);
1098 0 : _pitch_to_throttle_pid.set_notch_sample_rate(sample_rate);
1099 : #endif
1100 0 : }
|
