CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

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