Book a Demo!
CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutPoliciesSign UpSign In
Ardupilot
GitHub Repository: Ardupilot/ardupilot
Path: blob/master/Blimp/Parameters.cpp
9374 views
1
#include "Blimp.h"
2
3
/*
4
This program is free software: you can redistribute it and/or modify
5
it under the terms of the GNU General Public License as published by
6
the Free Software Foundation, either version 3 of the License, or
7
(at your option) any later version.
8
9
This program is distributed in the hope that it will be useful,
10
but WITHOUT ANY WARRANTY; without even the implied warranty of
11
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12
GNU General Public License for more details.
13
14
You should have received a copy of the GNU General Public License
15
along with this program. If not, see <http://www.gnu.org/licenses/>.
16
*/
17
18
/*
19
* Blimp parameter definitions
20
*
21
*/
22
23
#define DEFAULT_FRAME_CLASS 0
24
25
const AP_Param::Info Blimp::var_info[] = {
26
// @Param: FORMAT_VERSION
27
// @DisplayName: Eeprom format version number
28
// @Description: This value is incremented when changes are made to the eeprom format
29
// @User: Advanced
30
GSCALAR(format_version, "FORMAT_VERSION", 0),
31
32
// SYSID_THISMAV was here
33
34
// SYSID_MYGCS was here
35
36
// @Param: PILOT_THR_FILT
37
// @DisplayName: Throttle filter cutoff
38
// @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable
39
// @User: Advanced
40
// @Units: Hz
41
// @Range: 0 10
42
// @Increment: 0.5
43
GSCALAR(throttle_filt, "PILOT_THR_FILT", 0),
44
45
// @Param: PILOT_THR_BHV
46
// @DisplayName: Throttle stick behavior
47
// @Description: Bitmask containing various throttle stick options. TX with sprung throttle can set PILOT_THR_BHV to "1" so motor feedback when landed starts from mid-stick instead of bottom of stick.
48
// @User: Standard
49
// @Bitmask: 0:Feedback from mid stick,1:High throttle cancels landing,2:Disarm on land detection
50
GSCALAR(throttle_behavior, "PILOT_THR_BHV", 0),
51
52
// SerialManager was here
53
54
// @Param: GCS_PID_MASK
55
// @DisplayName: GCS PID tuning mask
56
// @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for
57
// @User: Advanced
58
// @Bitmask: 0:VELX,1:VELY,2:VELZ,3:VELYAW,4:POSX,5:POSY,6:POZ,7:POSYAW
59
GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0),
60
61
// @Param: FS_GCS_ENABLE
62
// @DisplayName: Ground Station Failsafe Enable
63
// @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. See FS_OPTIONS param for additional actions, or for cases allowing Mission continuation, when GCS failsafe is enabled.
64
// @Values: 0:Disabled/NoAction,5:Land
65
// @User: Standard
66
GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED),
67
68
// @Param: GPS_HDOP_GOOD
69
// @DisplayName: GPS Hdop Good
70
// @Description: GPS Hdop value at or below this value represent a good position. Used for pre-arm checks
71
// @Range: 100 900
72
// @User: Advanced
73
GSCALAR(gps_hdop_good, "GPS_HDOP_GOOD", GPS_HDOP_GOOD_DEFAULT),
74
75
// @Param: FS_THR_ENABLE
76
// @DisplayName: Throttle Failsafe Enable
77
// @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel
78
// @Values: 0:Disabled,3:Enabled always Land
79
// @User: Standard
80
GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL),
81
82
// @Param: FS_THR_VALUE
83
// @DisplayName: Throttle Failsafe Value
84
// @Description: The PWM level in microseconds on channel 3 below which throttle failsafe triggers
85
// @Range: 910 1100
86
// @Units: PWM
87
// @Increment: 1
88
// @User: Standard
89
GSCALAR(failsafe_throttle_value, "FS_THR_VALUE", FS_THR_VALUE_DEFAULT),
90
91
// @Param: THR_DZ
92
// @DisplayName: Throttle deadzone
93
// @Description: The deadzone above and below mid throttle in PWM microseconds. Used in AltHold, Loiter, PosHold flight modes
94
// @User: Standard
95
// @Range: 0 300
96
// @Units: PWM
97
// @Increment: 1
98
GSCALAR(throttle_deadzone, "THR_DZ", THR_DZ_DEFAULT),
99
100
// @Param: FLTMODE1
101
// @DisplayName: Flight Mode 1
102
// @Description: Flight mode when Channel 5 pwm is <= 1230
103
// @Values: 0:LAND,1:MANUAL,2:VELOCITY,3:LOITER
104
// @User: Standard
105
GSCALAR(flight_mode1, "FLTMODE1", (uint8_t)FLIGHT_MODE_1),
106
107
// @Param: FLTMODE2
108
// @CopyFieldsFrom: FLTMODE1
109
// @DisplayName: Flight Mode 2
110
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
111
GSCALAR(flight_mode2, "FLTMODE2", (uint8_t)FLIGHT_MODE_2),
112
113
// @Param: FLTMODE3
114
// @CopyFieldsFrom: FLTMODE1
115
// @DisplayName: Flight Mode 3
116
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
117
GSCALAR(flight_mode3, "FLTMODE3", (uint8_t)FLIGHT_MODE_3),
118
119
// @Param: FLTMODE4
120
// @CopyFieldsFrom: FLTMODE1
121
// @DisplayName: Flight Mode 4
122
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
123
GSCALAR(flight_mode4, "FLTMODE4", (uint8_t)FLIGHT_MODE_4),
124
125
// @Param: FLTMODE5
126
// @CopyFieldsFrom: FLTMODE1
127
// @DisplayName: Flight Mode 5
128
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
129
GSCALAR(flight_mode5, "FLTMODE5", (uint8_t)FLIGHT_MODE_5),
130
131
// @Param: FLTMODE6
132
// @CopyFieldsFrom: FLTMODE1
133
// @DisplayName: Flight Mode 6
134
// @Description: Flight mode when Channel 5 pwm is >=1750
135
GSCALAR(flight_mode6, "FLTMODE6", (uint8_t)FLIGHT_MODE_6),
136
137
// @Param: FLTMODE_CH
138
// @DisplayName: Flightmode channel
139
// @Description: RC Channel to use for flight mode control
140
// @Values: 0:Disabled,5:Channel5,6:Channel6,7:Channel7,8:Channel8
141
// @User: Advanced
142
GSCALAR(flight_mode_chan, "FLTMODE_CH", CH_MODE_DEFAULT),
143
144
// @Param: INITIAL_MODE
145
// @DisplayName: Initial flight mode
146
// @Description: This selects the mode to start in on boot.
147
// @CopyValuesFrom: FLTMODE1
148
// @User: Advanced
149
GSCALAR(initial_mode, "INITIAL_MODE", (uint8_t)Mode::Number::MANUAL),
150
151
// @Param: LOG_BITMASK
152
// @DisplayName: Log bitmask
153
// @Description: Bitmap of what log types to enable in on-board logger. This value is made up of the sum of each of the log types you want to be saved. On boards supporting microSD cards or other large block-storage devices it is usually best just to enable all basic log types by setting this to 65535.
154
// @Bitmask: 0:Fast Attitude,1:Medium Attitude,2:GPS,3:System Performance,4:Control Tuning,6:RC Input,7:IMU,9:Battery Monitor,10:RC Output,12:PID,13:Compass
155
// @User: Standard
156
GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK),
157
158
// @Group: ARMING_
159
// @Path: ../libraries/AP_Arming/AP_Arming.cpp
160
GOBJECT(arming, "ARMING_", AP_Arming_Blimp),
161
162
// @Param: DISARM_DELAY
163
// @DisplayName: Disarm delay
164
// @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm.
165
// @Units: s
166
// @Range: 0 127
167
// @User: Advanced
168
GSCALAR(disarm_delay, "DISARM_DELAY", AUTO_DISARMING_DELAY),
169
170
// @Param: FS_EKF_ACTION
171
// @DisplayName: EKF Failsafe Action
172
// @Description: Controls the action that will be taken when an EKF failsafe is invoked
173
// @Values: 1:Land, 3:Land even in MANUAL
174
// @User: Advanced
175
GSCALAR(fs_ekf_action, "FS_EKF_ACTION", FS_EKF_ACTION_DEFAULT),
176
177
// @Param: FS_EKF_THRESH
178
// @DisplayName: EKF failsafe variance threshold
179
// @Description: Allows setting the maximum acceptable compass and velocity variance
180
// @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed
181
// @Range: 0.6 1.0
182
// @User: Advanced
183
GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", FS_EKF_THRESHOLD_DEFAULT),
184
185
// @Param: FS_CRASH_CHECK
186
// @DisplayName: Crash check enable
187
// @Description: This enables automatic crash checking. When enabled the motors will disarm if a crash is detected.
188
// @Values: 0:Disabled, 1:Enabled
189
// @User: Advanced
190
GSCALAR(fs_crash_check, "FS_CRASH_CHECK", 1),
191
192
// @Param: MAX_VEL_XY
193
// @DisplayName: Max XY Velocity
194
// @Description: Sets the maximum XY velocity, in m/s
195
// @Range: 0.2 5
196
// @User: Standard
197
GSCALAR(max_vel_xy, "MAX_VEL_XY", 0.5),
198
199
// @Param: MAX_VEL_Z
200
// @DisplayName: Max Z Velocity
201
// @Description: Sets the maximum Z velocity, in m/s
202
// @Range: 0.2 5
203
// @User: Standard
204
GSCALAR(max_vel_z, "MAX_VEL_Z", 0.4),
205
206
// @Param: MAX_VEL_YAW
207
// @DisplayName: Max yaw Velocity
208
// @Description: Sets the maximum yaw velocity, in rad/s
209
// @Range: 0.2 5
210
// @User: Standard
211
GSCALAR(max_vel_yaw, "MAX_VEL_YAW", 0.5),
212
213
// @Param: MAX_POS_XY
214
// @DisplayName: Max XY Position change
215
// @Description: Sets the maximum XY position change, in m/s
216
// @Range: 0.1 5
217
// @User: Standard
218
GSCALAR(max_pos_xy, "MAX_POS_XY", 0.2),
219
220
// @Param: MAX_POS_Z
221
// @DisplayName: Max Z Position change
222
// @Description: Sets the maximum Z position change, in m/s
223
// @Range: 0.1 5
224
// @User: Standard
225
GSCALAR(max_pos_z, "MAX_POS_Z", 0.15),
226
227
// @Param: MAX_POS_YAW
228
// @DisplayName: Max Yaw Position change
229
// @Description: Sets the maximum Yaw position change, in rad/s
230
// @Range: 0.1 5
231
// @User: Standard
232
GSCALAR(max_pos_yaw, "MAX_POS_YAW", 0.3),
233
234
// @Param: SIMPLE_MODE
235
// @DisplayName: Simple mode
236
// @Description: Simple mode for Position control - "forward" moves blimp in +ve X direction world-frame
237
// @Values: 0:Disabled, 1:Enabled
238
// @User: Standard
239
GSCALAR(simple_mode, "SIMPLE_MODE", 0),
240
241
// @Param: DIS_MASK
242
// @DisplayName: Disable output mask
243
// @Description: Mask for disabling (setting to zero) one or more of the 4 output axis in mode Velocity or Loiter
244
// @Bitmask: 0:Right,1:Front,2:Down,3:Yaw
245
// @User: Standard
246
GSCALAR(dis_mask, "DIS_MASK", 0),
247
248
// @Param: PID_DZ
249
// @DisplayName: Deadzone for the position PIDs
250
// @Description: Output 0 thrust signal when blimp is within this distance (in meters) of the target position. Warning: If this param is greater than MAX_POS_XY param then the blimp won't move at all in the XY plane in Loiter mode as it does not allow more than a second's lag. Same for the other axes.
251
// @Units: m
252
// @Range: 0.1 1
253
// @User: Standard
254
GSCALAR(pid_dz, "PID_DZ", 0),
255
256
// @Param: RC_SPEED
257
// @DisplayName: ESC Update Speed
258
// @Description: This is the speed in Hertz that your ESCs will receive updates
259
// @Units: Hz
260
// @Range: 50 490
261
// @Increment: 1
262
// @User: Advanced
263
GSCALAR(rc_speed, "RC_SPEED", RC_FAST_SPEED),
264
265
// variables not in the g class which contain EEPROM saved variables
266
267
// @Group: COMPASS_
268
// @Path: ../libraries/AP_Compass/AP_Compass.cpp
269
GOBJECT(compass, "COMPASS_", Compass),
270
271
// @Group: INS
272
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
273
GOBJECT(ins, "INS", AP_InertialSensor),
274
275
// SR0 through SR6 was here
276
277
// @Group: AHRS_
278
// @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
279
GOBJECT(ahrs, "AHRS_", AP_AHRS),
280
281
// @Group: BATT
282
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
283
GOBJECT(battery, "BATT", AP_BattMonitor),
284
285
// @Group: BRD_
286
// @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp
287
GOBJECT(BoardConfig, "BRD_", AP_BoardConfig),
288
289
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
290
// @Group: CAN_
291
// @Path: ../libraries/AP_CANManager/AP_CANManager.cpp
292
GOBJECT(can_mgr, "CAN_", AP_CANManager),
293
#endif
294
295
#if AP_SIM_ENABLED
296
GOBJECT(sitl, "SIM_", SITL::SIM),
297
#endif
298
299
// @Group: BARO
300
// @Path: ../libraries/AP_Baro/AP_Baro.cpp
301
GOBJECT(barometer, "BARO", AP_Baro),
302
303
// GPS driver
304
// @Group: GPS
305
// @Path: ../libraries/AP_GPS/AP_GPS.cpp
306
GOBJECT(gps, "GPS", AP_GPS),
307
308
// @Group: SCHED_
309
// @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
310
GOBJECT(scheduler, "SCHED_", AP_Scheduler),
311
312
// @Group: RCMAP_
313
// @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp
314
GOBJECT(rcmap, "RCMAP_", RCMapper),
315
316
#if HAL_NAVEKF2_AVAILABLE
317
// @Group: EK2_
318
// @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp
319
GOBJECTN(ahrs.EKF2, NavEKF2, "EK2_", NavEKF2),
320
#endif
321
322
#if HAL_NAVEKF3_AVAILABLE
323
// @Group: EK3_
324
// @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp
325
GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3),
326
#endif
327
328
#if AP_RSSI_ENABLED
329
// @Group: RSSI_
330
// @Path: ../libraries/AP_RSSI/AP_RSSI.cpp
331
GOBJECT(rssi, "RSSI_", AP_RSSI),
332
#endif
333
334
// @Group: NTF_
335
// @Path: ../libraries/AP_Notify/AP_Notify.cpp
336
GOBJECT(notify, "NTF_", AP_Notify),
337
338
// @Group:
339
// @Path: Parameters.cpp
340
GOBJECT(g2, "", ParametersG2),
341
342
// @Group: FINS_
343
// @Path: Fins.cpp
344
GOBJECTPTR(motors, "FINS_", Fins),
345
346
// @Param: VELXY_P
347
// @DisplayName: Velocity (horizontal) P gain
348
// @Description: Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration
349
// @Range: 0.1 6.0
350
// @Increment: 0.1
351
// @User: Advanced
352
353
// @Param: VELXY_I
354
// @DisplayName: Velocity (horizontal) I gain
355
// @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration
356
// @Range: 0.02 1.00
357
// @Increment: 0.01
358
// @User: Advanced
359
360
// @Param: VELXY_D
361
// @DisplayName: Velocity (horizontal) D gain
362
// @Description: Velocity (horizontal) D gain. Corrects short-term changes in velocity
363
// @Range: 0.00 1.00
364
// @Increment: 0.001
365
// @User: Advanced
366
367
// @Param: VELXY_IMAX
368
// @DisplayName: Velocity (horizontal) integrator maximum
369
// @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output
370
// @Range: 0 4500
371
// @Increment: 10
372
// @Units: cm/s/s
373
// @User: Advanced
374
375
// @Param: VELXY_FLTE
376
// @DisplayName: Velocity (horizontal) input filter
377
// @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms
378
// @Range: 0 100
379
// @Units: Hz
380
// @User: Advanced
381
382
// @Param: VELXY_FLTD
383
// @DisplayName: Velocity (horizontal) input filter
384
// @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for D term
385
// @Range: 0 100
386
// @Units: Hz
387
// @User: Advanced
388
389
// @Param: VELXY_FF
390
// @DisplayName: Velocity (horizontal) feed forward gain
391
// @Description: Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration
392
// @Range: 0 6
393
// @Increment: 0.01
394
// @User: Advanced
395
GOBJECT(pid_vel_xy, "VELXY_", AC_PID_2D),
396
397
// @Param: VELZ_P
398
// @DisplayName: Velocity (vertical) P gain
399
// @Description: Velocity (vertical) P gain. Converts the difference between desired and actual velocity to a target acceleration
400
// @Range: 0.1 6.0
401
// @Increment: 0.1
402
// @User: Advanced
403
404
// @Param: VELZ_I
405
// @DisplayName: Velocity (vertical) I gain
406
// @Description: Velocity (vertical) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration
407
// @Range: 0.02 1.00
408
// @Increment: 0.01
409
// @User: Advanced
410
411
// @Param: VELZ_D
412
// @DisplayName: Velocity (vertical) D gain
413
// @Description: Velocity (vertical) D gain. Corrects short-term changes in velocity
414
// @Range: 0.00 1.00
415
// @Increment: 0.001
416
// @User: Advanced
417
418
// @Param: VELZ_IMAX
419
// @DisplayName: Velocity (vertical) integrator maximum
420
// @Description: Velocity (vertical) integrator maximum. Constrains the target acceleration that the I gain will output
421
// @Range: 0 4500
422
// @Increment: 10
423
// @Units: cm/s/s
424
// @User: Advanced
425
426
// @Param: VELZ_FLTE
427
// @DisplayName: Velocity (vertical) input filter
428
// @Description: Velocity (vertical) input filter. This filter (in Hz) is applied to the input for P and I terms
429
// @Range: 0 100
430
// @Units: Hz
431
// @User: Advanced
432
433
// @Param: VELZ_FLTD
434
// @DisplayName: Velocity (vertical) input filter
435
// @Description: Velocity (vertical) input filter. This filter (in Hz) is applied to the input for D term
436
// @Range: 0 100
437
// @Units: Hz
438
// @User: Advanced
439
440
// @Param: VELZ_FF
441
// @DisplayName: Velocity (vertical) feed forward gain
442
// @Description: Velocity (vertical) feed forward gain. Converts the difference between desired velocity to a target acceleration
443
// @Range: 0 6
444
// @Increment: 0.01
445
// @User: Advanced
446
GOBJECT(pid_vel_z, "VELZ_", AC_PID_Basic),
447
448
// @Param: VELYAW_P
449
// @DisplayName: Velocity (yaw) P gain
450
// @Description: Velocity (yaw) P gain. Converts the difference between desired and actual velocity to a target acceleration
451
// @Range: 0.1 6.0
452
// @Increment: 0.1
453
// @User: Advanced
454
455
// @Param: VELYAW_I
456
// @DisplayName: Velocity (yaw) I gain
457
// @Description: Velocity (yaw) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration
458
// @Range: 0.02 1.00
459
// @Increment: 0.01
460
// @User: Advanced
461
462
// @Param: VELYAW_D
463
// @DisplayName: Velocity (yaw) D gain
464
// @Description: Velocity (yaw) D gain. Corrects short-term changes in velocity
465
// @Range: 0.00 1.00
466
// @Increment: 0.001
467
// @User: Advanced
468
469
// @Param: VELYAW_IMAX
470
// @DisplayName: Velocity (yaw) integrator maximum
471
// @Description: Velocity (yaw) integrator maximum. Constrains the target acceleration that the I gain will output
472
// @Range: 0 4500
473
// @Increment: 10
474
// @Units: cm/s/s
475
// @User: Advanced
476
477
// @Param: VELYAW_FLTE
478
// @DisplayName: Velocity (yaw) input filter
479
// @Description: Velocity (yaw) input filter. This filter (in Hz) is applied to the input for P and I terms
480
// @Range: 0 100
481
// @Units: Hz
482
// @User: Advanced
483
484
// @Param: VELYAW_FF
485
// @DisplayName: Velocity (yaw) feed forward gain
486
// @Description: Velocity (yaw) feed forward gain. Converts the difference between desired velocity to a target acceleration
487
// @Range: 0 6
488
// @Increment: 0.01
489
// @User: Advanced
490
GOBJECT(pid_vel_yaw, "VELYAW_", AC_PID_Basic),
491
492
// @Param: POSXY_P
493
// @DisplayName: Position (horizontal) P gain
494
// @Description: Position (horizontal) P gain. Converts the difference between desired and actual position to a target velocity
495
// @Range: 0.1 6.0
496
// @Increment: 0.1
497
// @User: Advanced
498
499
// @Param: POSXY_I
500
// @DisplayName: Position (horizontal) I gain
501
// @Description: Position (horizontal) I gain. Corrects long-term difference between desired and actual position to a target velocity
502
// @Range: 0.02 1.00
503
// @Increment: 0.01
504
// @User: Advanced
505
506
// @Param: POSXY_D
507
// @DisplayName: Position (horizontal) D gain
508
// @Description: Position (horizontal) D gain. Corrects short-term changes in position
509
// @Range: 0.00 1.00
510
// @Increment: 0.001
511
// @User: Advanced
512
513
// @Param: POSXY_IMAX
514
// @DisplayName: Position (horizontal) integrator maximum
515
// @Description: Position (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output
516
// @Range: 0 4500
517
// @Increment: 10
518
// @Units: cm/s/s
519
// @User: Advanced
520
521
// @Param: POSXY_FLTE
522
// @DisplayName: Position (horizontal) input filter
523
// @Description: Position (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms
524
// @Range: 0 100
525
// @Units: Hz
526
// @User: Advanced
527
528
// @Param: POSXY_FLTD
529
// @DisplayName: Position (horizontal) input filter
530
// @Description: Position (horizontal) input filter. This filter (in Hz) is applied to the input for D term
531
// @Range: 0 100
532
// @Units: Hz
533
// @User: Advanced
534
535
// @Param: POSXY_FF
536
// @DisplayName: Position (horizontal) feed forward gain
537
// @Description: Position (horizontal) feed forward gain. Converts the difference between desired position to a target velocity
538
// @Range: 0 6
539
// @Increment: 0.01
540
// @User: Advanced
541
GOBJECT(pid_pos_xy, "POSXY_", AC_PID_2D),
542
543
// @Param: POSZ_P
544
// @DisplayName: Position (vertical) P gain
545
// @Description: Position (vertical) P gain. Converts the difference between desired and actual position to a target velocity
546
// @Range: 0.1 6.0
547
// @Increment: 0.1
548
// @User: Advanced
549
550
// @Param: POSZ_I
551
// @DisplayName: Position (vertical) I gain
552
// @Description: Position (vertical) I gain. Corrects long-term difference between desired and actual position to a target velocity
553
// @Range: 0.02 1.00
554
// @Increment: 0.01
555
// @User: Advanced
556
557
// @Param: POSZ_D
558
// @DisplayName: Position (vertical) D gain
559
// @Description: Position (vertical) D gain. Corrects short-term changes in position
560
// @Range: 0.00 1.00
561
// @Increment: 0.001
562
// @User: Advanced
563
564
// @Param: POSZ_IMAX
565
// @DisplayName: Position (vertical) integrator maximum
566
// @Description: Position (vertical) integrator maximum. Constrains the target acceleration that the I gain will output
567
// @Range: 0 4500
568
// @Increment: 10
569
// @Units: cm/s/s
570
// @User: Advanced
571
572
// @Param: POSZ_FLTE
573
// @DisplayName: Position (vertical) input filter
574
// @Description: Position (vertical) input filter. This filter (in Hz) is applied to the input for P and I terms
575
// @Range: 0 100
576
// @Units: Hz
577
// @User: Advanced
578
579
// @Param: POSZ_FLTD
580
// @DisplayName: Position (vertical) input filter
581
// @Description: Position (vertical) input filter. This filter (in Hz) is applied to the input for D term
582
// @Range: 0 100
583
// @Units: Hz
584
// @User: Advanced
585
586
// @Param: POSZ_FF
587
// @DisplayName: Position (vertical) feed forward gain
588
// @Description: Position (vertical) feed forward gain. Converts the difference between desired position to a target velocity
589
// @Range: 0 6
590
// @Increment: 0.01
591
// @User: Advanced
592
GOBJECT(pid_pos_z, "POSZ_", AC_PID_Basic),
593
594
// @Param: POSYAW_P
595
// @DisplayName: Position (yaw) axis controller P gain
596
// @Description: Position (yaw) axis controller P gain.
597
// @Range: 0.0 3.0
598
// @Increment: 0.01
599
// @User: Standard
600
601
// @Param: POSYAW_I
602
// @DisplayName: Position (yaw) axis controller I gain
603
// @Description: Position (yaw) axis controller I gain.
604
// @Range: 0.0 3.0
605
// @Increment: 0.01
606
// @User: Standard
607
608
// @Param: POSYAW_IMAX
609
// @DisplayName: Position (yaw) axis controller I gain maximum
610
// @Description: Position (yaw) axis controller I gain maximum.
611
// @Range: 0 4000
612
// @Increment: 10
613
// @Units: d%
614
// @User: Standard
615
616
// @Param: POSYAW_D
617
// @DisplayName: Position (yaw) axis controller D gain
618
// @Description: Position (yaw) axis controller D gain.
619
// @Range: 0.001 0.1
620
// @Increment: 0.001
621
// @User: Standard
622
623
// @Param: POSYAW_FF
624
// @DisplayName: Position (yaw) axis controller feed forward
625
// @Description: Position (yaw) axis controller feed forward
626
// @Range: 0 0.5
627
// @Increment: 0.001
628
// @User: Standard
629
630
// @Param: POSYAW_FLTT
631
// @DisplayName: Position (yaw) target frequency filter in Hz
632
// @Description: Position (yaw) target frequency filter in Hz
633
// @Range: 1 50
634
// @Increment: 1
635
// @Units: Hz
636
// @User: Standard
637
638
// @Param: POSYAW_FLTE
639
// @DisplayName: Position (yaw) error frequency filter in Hz
640
// @Description: Position (yaw) error frequency filter in Hz
641
// @Range: 1 100
642
// @Increment: 1
643
// @Units: Hz
644
// @User: Standard
645
646
// @Param: POSYAW_FLTD
647
// @DisplayName: Position (yaw) derivative input filter in Hz
648
// @Description: Position (yaw) derivative input filter in Hz
649
// @Range: 1 100
650
// @Increment: 1
651
// @Units: Hz
652
// @User: Standard
653
654
// @Param: POSYAW_SMAX
655
// @DisplayName: Yaw slew rate limit
656
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains.
657
// @Range: 0 200
658
// @Increment: 0.5
659
// @User: Advanced
660
661
// @Param: POSYAW_PDMX
662
// @DisplayName: Position (yaw) axis controller PD sum maximum
663
// @Description: Position (yaw) axis controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output
664
// @Range: 0 4000
665
// @Increment: 10
666
// @Units: d%
667
// @User: Advanced
668
669
// @Param: POSYAW_D_FF
670
// @DisplayName: Position (yaw) Derivative FeedForward Gain
671
// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
672
// @Range: 0 0.1
673
// @Increment: 0.001
674
// @User: Advanced
675
676
// @Param: POSYAW_NTF
677
// @DisplayName: Position (yaw) Target notch filter index
678
// @Description: Position (yaw) Target notch filter index
679
// @Range: 1 8
680
// @User: Advanced
681
682
// @Param: POSYAW_NEF
683
// @DisplayName: Position (yaw) Error notch filter index
684
// @Description: Position (yaw) Error notch filter index
685
// @Range: 1 8
686
// @User: Advanced
687
688
GOBJECT(pid_pos_yaw, "POSYAW_", AC_PID),
689
690
// @Group:
691
// @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp
692
PARAM_VEHICLE_INFO,
693
694
#if HAL_GCS_ENABLED
695
// @Group: MAV
696
// @Path: ../libraries/GCS_MAVLink/GCS.cpp
697
GOBJECT(_gcs, "MAV", GCS),
698
#endif
699
700
AP_VAREND
701
};
702
703
/*
704
2nd group of parameters
705
*/
706
const AP_Param::GroupInfo ParametersG2::var_info[] = {
707
708
// @Param: DEV_OPTIONS
709
// @DisplayName: Development options
710
// @Description: Bitmask of developer options. The meanings of the bit fields in this parameter may vary at any time. Developers should check the source code for current meaning
711
// @Bitmask: 0:Unknown
712
// @User: Advanced
713
AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0),
714
715
// 11 was SYSID_ENFORCE
716
717
// 12 was AP_Stats
718
719
// @Param: FRAME_CLASS
720
// @DisplayName: Frame Class
721
// @Description: Controls major frame class for blimp.
722
// @Values: 0:Finnedblimp
723
// @User: Standard
724
// @RebootRequired: True
725
AP_GROUPINFO("FRAME_CLASS", 15, ParametersG2, frame_class, DEFAULT_FRAME_CLASS),
726
727
// @Group: SERVO
728
// @Path: ../libraries/SRV_Channel/SRV_Channels.cpp
729
AP_SUBGROUPINFO(servo_channels, "SERVO", 16, ParametersG2, SRV_Channels),
730
731
// @Group: RC
732
// @Path: ../libraries/RC_Channel/RC_Channels_VarInfo.h
733
AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels_Blimp),
734
735
// @Param: PILOT_SPEED_DN
736
// @DisplayName: Pilot maximum vertical speed descending
737
// @Description: The maximum vertical descending velocity the pilot may request in cm/s
738
// @Units: cm/s
739
// @Range: 50 500
740
// @Increment: 10
741
// @User: Standard
742
AP_GROUPINFO("PILOT_SPEED_DN", 24, ParametersG2, pilot_speed_dn, 0),
743
744
// 30 was AP_Scripting
745
746
// @Param: FS_VIBE_ENABLE
747
// @DisplayName: Vibration Failsafe enable
748
// @Description: This enables the vibration failsafe which will use modified altitude estimation and control during high vibrations
749
// @Values: 0:Disabled, 1:Enabled
750
// @User: Standard
751
AP_GROUPINFO("FS_VIBE_ENABLE", 35, ParametersG2, fs_vibe_enabled, 1),
752
753
// @Param: FS_OPTIONS
754
// @DisplayName: Failsafe options bitmask
755
// @Description: Bitmask of additional options for battery, radio, & GCS failsafes. 0 (default) disables all options.
756
// @Bitmask: 4:Continue if in pilot controlled modes on GCS failsafe
757
// @User: Advanced
758
AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, (float)Blimp::FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL),
759
760
// @Param: FS_GCS_TIMEOUT
761
// @DisplayName: GCS failsafe timeout
762
// @Description: Timeout before triggering the GCS failsafe
763
// @Units: s
764
// @Range: 2 120
765
// @Increment: 1
766
// @User: Standard
767
AP_GROUPINFO("FS_GCS_TIMEOUT", 42, ParametersG2, fs_gcs_timeout, 5),
768
769
AP_GROUPEND
770
};
771
772
/*
773
constructor for g2 object
774
*/
775
ParametersG2::ParametersG2(void)
776
{
777
AP_Param::setup_object_defaults(this, var_info);
778
}
779
780
void Blimp::load_parameters(void)
781
{
782
AP_Vehicle::load_parameters(g.format_version, Parameters::k_format_version);
783
784
static const AP_Param::G2ObjectConversion g2_conversions[] {
785
#if AP_STATS_ENABLED
786
// PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6
787
{ &stats, stats.var_info, 12 },
788
#endif
789
#if AP_SCRIPTING_ENABLED
790
// PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6
791
{ &scripting, scripting.var_info, 30 },
792
#endif
793
};
794
AP_Param::convert_g2_objects(&g2, g2_conversions, ARRAY_SIZE(g2_conversions));
795
796
// PARAMETER_CONVERSION - Added: Feb-2024
797
#if HAL_LOGGING_ENABLED
798
AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, true);
799
#endif
800
801
static const AP_Param::TopLevelObjectConversion toplevel_conversions[] {
802
#if AP_SERIALMANAGER_ENABLED
803
// PARAMETER_CONVERSION - Added: Feb-2024
804
{ &serial_manager, serial_manager.var_info, Parameters::k_param_serial_manager_old },
805
#endif
806
};
807
808
AP_Param::convert_toplevel_objects(toplevel_conversions, ARRAY_SIZE(toplevel_conversions));
809
810
// setup AP_Param frame type flags
811
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_BLIMP);
812
813
#if HAL_GCS_ENABLED
814
// Move parameters into new MAV_ parameter namespace
815
// PARAMETER_CONVERSION - Added: Mar-2025 for ArduPilot-4.7
816
{
817
static const AP_Param::ConversionInfo gcs_conversion_info[] {
818
{ Parameters::k_param_sysid_this_mav_old, 0, AP_PARAM_INT16, "MAV_SYSID" },
819
{ Parameters::k_param_sysid_my_gcs_old, 0, AP_PARAM_INT16, "MAV_GCS_SYSID" },
820
{ Parameters::k_param_g2, 11, AP_PARAM_INT8, "MAV_OPTIONS" },
821
{ Parameters::k_param_telem_delay_old, 0, AP_PARAM_INT8, "MAV_TELEM_DELAY" },
822
};
823
AP_Param::convert_old_parameters(&gcs_conversion_info[0], ARRAY_SIZE(gcs_conversion_info));
824
}
825
#endif // HAL_GCS_ENABLED
826
827
}
828
829