Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Path: blob/master/Blimp/Parameters.cpp
Views: 1798
#include "Blimp.h"12/*3This program is free software: you can redistribute it and/or modify4it under the terms of the GNU General Public License as published by5the Free Software Foundation, either version 3 of the License, or6(at your option) any later version.78This program is distributed in the hope that it will be useful,9but WITHOUT ANY WARRANTY; without even the implied warranty of10MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the11GNU General Public License for more details.1213You should have received a copy of the GNU General Public License14along with this program. If not, see <http://www.gnu.org/licenses/>.15*/1617/*18* Blimp parameter definitions19*20*/2122#define DEFAULT_FRAME_CLASS 02324const AP_Param::Info Blimp::var_info[] = {25// @Param: FORMAT_VERSION26// @DisplayName: Eeprom format version number27// @Description: This value is incremented when changes are made to the eeprom format28// @User: Advanced29GSCALAR(format_version, "FORMAT_VERSION", 0),3031// @Param: SYSID_THISMAV32// @DisplayName: MAVLink system ID of this vehicle33// @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network34// @Range: 1 25535// @User: Advanced36GSCALAR(sysid_this_mav, "SYSID_THISMAV", MAV_SYSTEM_ID),3738// @Param: SYSID_MYGCS39// @DisplayName: My ground station number40// @Description: Allows restricting radio overrides to only come from my ground station41// @Range: 1 25542// @Increment: 143// @User: Advanced44GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255),4546// @Param: PILOT_THR_FILT47// @DisplayName: Throttle filter cutoff48// @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable49// @User: Advanced50// @Units: Hz51// @Range: 0 1052// @Increment: 0.553GSCALAR(throttle_filt, "PILOT_THR_FILT", 0),5455// @Param: PILOT_THR_BHV56// @DisplayName: Throttle stick behavior57// @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.58// @User: Standard59// @Bitmask: 0:Feedback from mid stick,1:High throttle cancels landing,2:Disarm on land detection60GSCALAR(throttle_behavior, "PILOT_THR_BHV", 0),6162// SerialManager was here6364// @Param: TELEM_DELAY65// @DisplayName: Telemetry startup delay66// @Description: The amount of time (in seconds) to delay radio telemetry to prevent an Xbee bricking on power up67// @User: Advanced68// @Units: s69// @Range: 0 3070// @Increment: 171GSCALAR(telem_delay, "TELEM_DELAY", 0),7273// @Param: GCS_PID_MASK74// @DisplayName: GCS PID tuning mask75// @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for76// @User: Advanced77// @Bitmask: 0:VELX,1:VELY,2:VELZ,3:VELYAW,4:POSX,5:POSY,6:POZ,7:POSYAW78GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0),7980// @Param: FS_GCS_ENABLE81// @DisplayName: Ground Station Failsafe Enable82// @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.83// @Values: 0:Disabled/NoAction,5:Land84// @User: Standard85GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED),8687// @Param: GPS_HDOP_GOOD88// @DisplayName: GPS Hdop Good89// @Description: GPS Hdop value at or below this value represent a good position. Used for pre-arm checks90// @Range: 100 90091// @User: Advanced92GSCALAR(gps_hdop_good, "GPS_HDOP_GOOD", GPS_HDOP_GOOD_DEFAULT),9394// @Param: FS_THR_ENABLE95// @DisplayName: Throttle Failsafe Enable96// @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel97// @Values: 0:Disabled,3:Enabled always Land98// @User: Standard99GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL),100101// @Param: FS_THR_VALUE102// @DisplayName: Throttle Failsafe Value103// @Description: The PWM level in microseconds on channel 3 below which throttle failsafe triggers104// @Range: 910 1100105// @Units: PWM106// @Increment: 1107// @User: Standard108GSCALAR(failsafe_throttle_value, "FS_THR_VALUE", FS_THR_VALUE_DEFAULT),109110// @Param: THR_DZ111// @DisplayName: Throttle deadzone112// @Description: The deadzone above and below mid throttle in PWM microseconds. Used in AltHold, Loiter, PosHold flight modes113// @User: Standard114// @Range: 0 300115// @Units: PWM116// @Increment: 1117GSCALAR(throttle_deadzone, "THR_DZ", THR_DZ_DEFAULT),118119// @Param: FLTMODE1120// @DisplayName: Flight Mode 1121// @Description: Flight mode when Channel 5 pwm is <= 1230122// @Values: 0:LAND,1:MANUAL,2:VELOCITY,3:LOITER123// @User: Standard124GSCALAR(flight_mode1, "FLTMODE1", (uint8_t)FLIGHT_MODE_1),125126// @Param: FLTMODE2127// @CopyFieldsFrom: FLTMODE1128// @DisplayName: Flight Mode 2129// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360130GSCALAR(flight_mode2, "FLTMODE2", (uint8_t)FLIGHT_MODE_2),131132// @Param: FLTMODE3133// @CopyFieldsFrom: FLTMODE1134// @DisplayName: Flight Mode 3135// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490136GSCALAR(flight_mode3, "FLTMODE3", (uint8_t)FLIGHT_MODE_3),137138// @Param: FLTMODE4139// @CopyFieldsFrom: FLTMODE1140// @DisplayName: Flight Mode 4141// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620142GSCALAR(flight_mode4, "FLTMODE4", (uint8_t)FLIGHT_MODE_4),143144// @Param: FLTMODE5145// @CopyFieldsFrom: FLTMODE1146// @DisplayName: Flight Mode 5147// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749148GSCALAR(flight_mode5, "FLTMODE5", (uint8_t)FLIGHT_MODE_5),149150// @Param: FLTMODE6151// @CopyFieldsFrom: FLTMODE1152// @DisplayName: Flight Mode 6153// @Description: Flight mode when Channel 5 pwm is >=1750154GSCALAR(flight_mode6, "FLTMODE6", (uint8_t)FLIGHT_MODE_6),155156// @Param: FLTMODE_CH157// @DisplayName: Flightmode channel158// @Description: RC Channel to use for flight mode control159// @Values: 0:Disabled,5:Channel5,6:Channel6,7:Channel7,8:Channel8160// @User: Advanced161GSCALAR(flight_mode_chan, "FLTMODE_CH", CH_MODE_DEFAULT),162163// @Param: INITIAL_MODE164// @DisplayName: Initial flight mode165// @Description: This selects the mode to start in on boot.166// @CopyValuesFrom: FLTMODE1167// @User: Advanced168GSCALAR(initial_mode, "INITIAL_MODE", (uint8_t)Mode::Number::MANUAL),169170// @Param: LOG_BITMASK171// @DisplayName: Log bitmask172// @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.173// @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:Compass174// @User: Standard175GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK),176177// @Group: ARMING_178// @Path: ../libraries/AP_Arming/AP_Arming.cpp179GOBJECT(arming, "ARMING_", AP_Arming_Blimp),180181// @Param: DISARM_DELAY182// @DisplayName: Disarm delay183// @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm.184// @Units: s185// @Range: 0 127186// @User: Advanced187GSCALAR(disarm_delay, "DISARM_DELAY", AUTO_DISARMING_DELAY),188189// @Param: FS_EKF_ACTION190// @DisplayName: EKF Failsafe Action191// @Description: Controls the action that will be taken when an EKF failsafe is invoked192// @Values: 1:Land, 3:Land even in MANUAL193// @User: Advanced194GSCALAR(fs_ekf_action, "FS_EKF_ACTION", FS_EKF_ACTION_DEFAULT),195196// @Param: FS_EKF_THRESH197// @DisplayName: EKF failsafe variance threshold198// @Description: Allows setting the maximum acceptable compass and velocity variance199// @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed200// @User: Advanced201GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", FS_EKF_THRESHOLD_DEFAULT),202203// @Param: FS_CRASH_CHECK204// @DisplayName: Crash check enable205// @Description: This enables automatic crash checking. When enabled the motors will disarm if a crash is detected.206// @Values: 0:Disabled, 1:Enabled207// @User: Advanced208GSCALAR(fs_crash_check, "FS_CRASH_CHECK", 1),209210// @Param: MAX_VEL_XY211// @DisplayName: Max XY Velocity212// @Description: Sets the maximum XY velocity, in m/s213// @Range: 0.2 5214// @User: Standard215GSCALAR(max_vel_xy, "MAX_VEL_XY", 0.5),216217// @Param: MAX_VEL_Z218// @DisplayName: Max Z Velocity219// @Description: Sets the maximum Z velocity, in m/s220// @Range: 0.2 5221// @User: Standard222GSCALAR(max_vel_z, "MAX_VEL_Z", 0.4),223224// @Param: MAX_VEL_YAW225// @DisplayName: Max yaw Velocity226// @Description: Sets the maximum yaw velocity, in rad/s227// @Range: 0.2 5228// @User: Standard229GSCALAR(max_vel_yaw, "MAX_VEL_YAW", 0.5),230231// @Param: MAX_POS_XY232// @DisplayName: Max XY Position change233// @Description: Sets the maximum XY position change, in m/s234// @Range: 0.1 5235// @User: Standard236GSCALAR(max_pos_xy, "MAX_POS_XY", 0.2),237238// @Param: MAX_POS_Z239// @DisplayName: Max Z Position change240// @Description: Sets the maximum Z position change, in m/s241// @Range: 0.1 5242// @User: Standard243GSCALAR(max_pos_z, "MAX_POS_Z", 0.15),244245// @Param: MAX_POS_YAW246// @DisplayName: Max Yaw Position change247// @Description: Sets the maximum Yaw position change, in rad/s248// @Range: 0.1 5249// @User: Standard250GSCALAR(max_pos_yaw, "MAX_POS_YAW", 0.3),251252// @Param: SIMPLE_MODE253// @DisplayName: Simple mode254// @Description: Simple mode for Position control - "forward" moves blimp in +ve X direction world-frame255// @Values: 0:Disabled, 1:Enabled256// @User: Standard257GSCALAR(simple_mode, "SIMPLE_MODE", 0),258259// @Param: DIS_MASK260// @DisplayName: Disable output mask261// @Description: Mask for disabling (setting to zero) one or more of the 4 output axis in mode Velocity or Loiter262// @Bitmask: 0:Right,1:Front,2:Down,3:Yaw263// @User: Standard264GSCALAR(dis_mask, "DIS_MASK", 0),265266// @Param: PID_DZ267// @DisplayName: Deadzone for the position PIDs268// @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.269// @Units: m270// @Range: 0.1 1271// @User: Standard272GSCALAR(pid_dz, "PID_DZ", 0),273274// @Param: RC_SPEED275// @DisplayName: ESC Update Speed276// @Description: This is the speed in Hertz that your ESCs will receive updates277// @Units: Hz278// @Range: 50 490279// @Increment: 1280// @User: Advanced281GSCALAR(rc_speed, "RC_SPEED", RC_FAST_SPEED),282283// variables not in the g class which contain EEPROM saved variables284285// @Group: COMPASS_286// @Path: ../libraries/AP_Compass/AP_Compass.cpp287GOBJECT(compass, "COMPASS_", Compass),288289// @Group: INS290// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp291GOBJECT(ins, "INS", AP_InertialSensor),292293// @Group: SR0_294// @Path: GCS_MAVLink_Blimp.cpp295GOBJECTN(_gcs.chan_parameters[0], gcs0, "SR0_", GCS_MAVLINK_Parameters),296297#if MAVLINK_COMM_NUM_BUFFERS >= 2298// @Group: SR1_299// @Path: GCS_MAVLink_Blimp.cpp300GOBJECTN(_gcs.chan_parameters[1], gcs1, "SR1_", GCS_MAVLINK_Parameters),301#endif302303#if MAVLINK_COMM_NUM_BUFFERS >= 3304// @Group: SR2_305// @Path: GCS_MAVLink_Blimp.cpp306GOBJECTN(_gcs.chan_parameters[2], gcs2, "SR2_", GCS_MAVLINK_Parameters),307#endif308309#if MAVLINK_COMM_NUM_BUFFERS >= 4310// @Group: SR3_311// @Path: GCS_MAVLink_Blimp.cpp312GOBJECTN(_gcs.chan_parameters[3], gcs3, "SR3_", GCS_MAVLINK_Parameters),313#endif314315#if MAVLINK_COMM_NUM_BUFFERS >= 5316// @Group: SR4_317// @Path: GCS_MAVLink_Blimp.cpp318GOBJECTN(_gcs.chan_parameters[4], gcs4, "SR4_", GCS_MAVLINK_Parameters),319#endif320321#if MAVLINK_COMM_NUM_BUFFERS >= 6322// @Group: SR5_323// @Path: GCS_MAVLink_Blimp.cpp324GOBJECTN(_gcs.chan_parameters[5], gcs5, "SR5_", GCS_MAVLINK_Parameters),325#endif326327#if MAVLINK_COMM_NUM_BUFFERS >= 7328// @Group: SR6_329// @Path: GCS_MAVLink_Blimp.cpp330GOBJECTN(_gcs.chan_parameters[6], gcs6, "SR6_", GCS_MAVLINK_Parameters),331#endif332333// @Group: AHRS_334// @Path: ../libraries/AP_AHRS/AP_AHRS.cpp335GOBJECT(ahrs, "AHRS_", AP_AHRS),336337// @Group: BATT338// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp339GOBJECT(battery, "BATT", AP_BattMonitor),340341// @Group: BRD_342// @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp343GOBJECT(BoardConfig, "BRD_", AP_BoardConfig),344345#if HAL_MAX_CAN_PROTOCOL_DRIVERS346// @Group: CAN_347// @Path: ../libraries/AP_CANManager/AP_CANManager.cpp348GOBJECT(can_mgr, "CAN_", AP_CANManager),349#endif350351#if AP_SIM_ENABLED352GOBJECT(sitl, "SIM_", SITL::SIM),353#endif354355// @Group: BARO356// @Path: ../libraries/AP_Baro/AP_Baro.cpp357GOBJECT(barometer, "BARO", AP_Baro),358359// GPS driver360// @Group: GPS361// @Path: ../libraries/AP_GPS/AP_GPS.cpp362GOBJECT(gps, "GPS", AP_GPS),363364// @Group: SCHED_365// @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp366GOBJECT(scheduler, "SCHED_", AP_Scheduler),367368// @Group: RCMAP_369// @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp370GOBJECT(rcmap, "RCMAP_", RCMapper),371372#if HAL_NAVEKF2_AVAILABLE373// @Group: EK2_374// @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp375GOBJECTN(ahrs.EKF2, NavEKF2, "EK2_", NavEKF2),376#endif377378#if HAL_NAVEKF3_AVAILABLE379// @Group: EK3_380// @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp381GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3),382#endif383384#if AP_RSSI_ENABLED385// @Group: RSSI_386// @Path: ../libraries/AP_RSSI/AP_RSSI.cpp387GOBJECT(rssi, "RSSI_", AP_RSSI),388#endif389390// @Group: NTF_391// @Path: ../libraries/AP_Notify/AP_Notify.cpp392GOBJECT(notify, "NTF_", AP_Notify),393394// @Group:395// @Path: Parameters.cpp396GOBJECT(g2, "", ParametersG2),397398// @Group: FINS_399// @Path: Fins.cpp400GOBJECTPTR(motors, "FINS_", Fins),401402// @Param: VELXY_P403// @DisplayName: Velocity (horizontal) P gain404// @Description: Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration405// @Range: 0.1 6.0406// @Increment: 0.1407// @User: Advanced408409// @Param: VELXY_I410// @DisplayName: Velocity (horizontal) I gain411// @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration412// @Range: 0.02 1.00413// @Increment: 0.01414// @User: Advanced415416// @Param: VELXY_D417// @DisplayName: Velocity (horizontal) D gain418// @Description: Velocity (horizontal) D gain. Corrects short-term changes in velocity419// @Range: 0.00 1.00420// @Increment: 0.001421// @User: Advanced422423// @Param: VELXY_IMAX424// @DisplayName: Velocity (horizontal) integrator maximum425// @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output426// @Range: 0 4500427// @Increment: 10428// @Units: cm/s/s429// @User: Advanced430431// @Param: VELXY_FLTE432// @DisplayName: Velocity (horizontal) input filter433// @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms434// @Range: 0 100435// @Units: Hz436// @User: Advanced437438// @Param: VELXY_FLTD439// @DisplayName: Velocity (horizontal) input filter440// @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for D term441// @Range: 0 100442// @Units: Hz443// @User: Advanced444445// @Param: VELXY_FF446// @DisplayName: Velocity (horizontal) feed forward gain447// @Description: Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration448// @Range: 0 6449// @Increment: 0.01450// @User: Advanced451GOBJECT(pid_vel_xy, "VELXY_", AC_PID_2D),452453// @Param: VELZ_P454// @DisplayName: Velocity (vertical) P gain455// @Description: Velocity (vertical) P gain. Converts the difference between desired and actual velocity to a target acceleration456// @Range: 0.1 6.0457// @Increment: 0.1458// @User: Advanced459460// @Param: VELZ_I461// @DisplayName: Velocity (vertical) I gain462// @Description: Velocity (vertical) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration463// @Range: 0.02 1.00464// @Increment: 0.01465// @User: Advanced466467// @Param: VELZ_D468// @DisplayName: Velocity (vertical) D gain469// @Description: Velocity (vertical) D gain. Corrects short-term changes in velocity470// @Range: 0.00 1.00471// @Increment: 0.001472// @User: Advanced473474// @Param: VELZ_IMAX475// @DisplayName: Velocity (vertical) integrator maximum476// @Description: Velocity (vertical) integrator maximum. Constrains the target acceleration that the I gain will output477// @Range: 0 4500478// @Increment: 10479// @Units: cm/s/s480// @User: Advanced481482// @Param: VELZ_FLTE483// @DisplayName: Velocity (vertical) input filter484// @Description: Velocity (vertical) input filter. This filter (in Hz) is applied to the input for P and I terms485// @Range: 0 100486// @Units: Hz487// @User: Advanced488489// @Param: VELZ_FLTD490// @DisplayName: Velocity (vertical) input filter491// @Description: Velocity (vertical) input filter. This filter (in Hz) is applied to the input for D term492// @Range: 0 100493// @Units: Hz494// @User: Advanced495496// @Param: VELZ_FF497// @DisplayName: Velocity (vertical) feed forward gain498// @Description: Velocity (vertical) feed forward gain. Converts the difference between desired velocity to a target acceleration499// @Range: 0 6500// @Increment: 0.01501// @User: Advanced502GOBJECT(pid_vel_z, "VELZ_", AC_PID_Basic),503504// @Param: VELYAW_P505// @DisplayName: Velocity (yaw) P gain506// @Description: Velocity (yaw) P gain. Converts the difference between desired and actual velocity to a target acceleration507// @Range: 0.1 6.0508// @Increment: 0.1509// @User: Advanced510511// @Param: VELYAW_I512// @DisplayName: Velocity (yaw) I gain513// @Description: Velocity (yaw) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration514// @Range: 0.02 1.00515// @Increment: 0.01516// @User: Advanced517518// @Param: VELYAW_D519// @DisplayName: Velocity (yaw) D gain520// @Description: Velocity (yaw) D gain. Corrects short-term changes in velocity521// @Range: 0.00 1.00522// @Increment: 0.001523// @User: Advanced524525// @Param: VELYAW_IMAX526// @DisplayName: Velocity (yaw) integrator maximum527// @Description: Velocity (yaw) integrator maximum. Constrains the target acceleration that the I gain will output528// @Range: 0 4500529// @Increment: 10530// @Units: cm/s/s531// @User: Advanced532533// @Param: VELYAW_FLTE534// @DisplayName: Velocity (yaw) input filter535// @Description: Velocity (yaw) input filter. This filter (in Hz) is applied to the input for P and I terms536// @Range: 0 100537// @Units: Hz538// @User: Advanced539540// @Param: VELYAW_FF541// @DisplayName: Velocity (yaw) feed forward gain542// @Description: Velocity (yaw) feed forward gain. Converts the difference between desired velocity to a target acceleration543// @Range: 0 6544// @Increment: 0.01545// @User: Advanced546GOBJECT(pid_vel_yaw, "VELYAW_", AC_PID_Basic),547548// @Param: POSXY_P549// @DisplayName: Position (horizontal) P gain550// @Description: Position (horizontal) P gain. Converts the difference between desired and actual position to a target velocity551// @Range: 0.1 6.0552// @Increment: 0.1553// @User: Advanced554555// @Param: POSXY_I556// @DisplayName: Position (horizontal) I gain557// @Description: Position (horizontal) I gain. Corrects long-term difference between desired and actual position to a target velocity558// @Range: 0.02 1.00559// @Increment: 0.01560// @User: Advanced561562// @Param: POSXY_D563// @DisplayName: Position (horizontal) D gain564// @Description: Position (horizontal) D gain. Corrects short-term changes in position565// @Range: 0.00 1.00566// @Increment: 0.001567// @User: Advanced568569// @Param: POSXY_IMAX570// @DisplayName: Position (horizontal) integrator maximum571// @Description: Position (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output572// @Range: 0 4500573// @Increment: 10574// @Units: cm/s/s575// @User: Advanced576577// @Param: POSXY_FLTE578// @DisplayName: Position (horizontal) input filter579// @Description: Position (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms580// @Range: 0 100581// @Units: Hz582// @User: Advanced583584// @Param: POSXY_FLTD585// @DisplayName: Position (horizontal) input filter586// @Description: Position (horizontal) input filter. This filter (in Hz) is applied to the input for D term587// @Range: 0 100588// @Units: Hz589// @User: Advanced590591// @Param: POSXY_FF592// @DisplayName: Position (horizontal) feed forward gain593// @Description: Position (horizontal) feed forward gain. Converts the difference between desired position to a target velocity594// @Range: 0 6595// @Increment: 0.01596// @User: Advanced597GOBJECT(pid_pos_xy, "POSXY_", AC_PID_2D),598599// @Param: POSZ_P600// @DisplayName: Position (vertical) P gain601// @Description: Position (vertical) P gain. Converts the difference between desired and actual position to a target velocity602// @Range: 0.1 6.0603// @Increment: 0.1604// @User: Advanced605606// @Param: POSZ_I607// @DisplayName: Position (vertical) I gain608// @Description: Position (vertical) I gain. Corrects long-term difference between desired and actual position to a target velocity609// @Range: 0.02 1.00610// @Increment: 0.01611// @User: Advanced612613// @Param: POSZ_D614// @DisplayName: Position (vertical) D gain615// @Description: Position (vertical) D gain. Corrects short-term changes in position616// @Range: 0.00 1.00617// @Increment: 0.001618// @User: Advanced619620// @Param: POSZ_IMAX621// @DisplayName: Position (vertical) integrator maximum622// @Description: Position (vertical) integrator maximum. Constrains the target acceleration that the I gain will output623// @Range: 0 4500624// @Increment: 10625// @Units: cm/s/s626// @User: Advanced627628// @Param: POSZ_FLTE629// @DisplayName: Position (vertical) input filter630// @Description: Position (vertical) input filter. This filter (in Hz) is applied to the input for P and I terms631// @Range: 0 100632// @Units: Hz633// @User: Advanced634635// @Param: POSZ_FLTD636// @DisplayName: Position (vertical) input filter637// @Description: Position (vertical) input filter. This filter (in Hz) is applied to the input for D term638// @Range: 0 100639// @Units: Hz640// @User: Advanced641642// @Param: POSZ_FF643// @DisplayName: Position (vertical) feed forward gain644// @Description: Position (vertical) feed forward gain. Converts the difference between desired position to a target velocity645// @Range: 0 6646// @Increment: 0.01647// @User: Advanced648GOBJECT(pid_pos_z, "POSZ_", AC_PID_Basic),649650// @Param: POSYAW_P651// @DisplayName: Position (yaw) axis controller P gain652// @Description: Position (yaw) axis controller P gain.653// @Range: 0.0 3.0654// @Increment: 0.01655// @User: Standard656657// @Param: POSYAW_I658// @DisplayName: Position (yaw) axis controller I gain659// @Description: Position (yaw) axis controller I gain.660// @Range: 0.0 3.0661// @Increment: 0.01662// @User: Standard663664// @Param: POSYAW_IMAX665// @DisplayName: Position (yaw) axis controller I gain maximum666// @Description: Position (yaw) axis controller I gain maximum.667// @Range: 0 4000668// @Increment: 10669// @Units: d%670// @User: Standard671672// @Param: POSYAW_D673// @DisplayName: Position (yaw) axis controller D gain674// @Description: Position (yaw) axis controller D gain.675// @Range: 0.001 0.1676// @Increment: 0.001677// @User: Standard678679// @Param: POSYAW_FF680// @DisplayName: Position (yaw) axis controller feed forward681// @Description: Position (yaw) axis controller feed forward682// @Range: 0 0.5683// @Increment: 0.001684// @User: Standard685686// @Param: POSYAW_FLTT687// @DisplayName: Position (yaw) target frequency filter in Hz688// @Description: Position (yaw) target frequency filter in Hz689// @Range: 1 50690// @Increment: 1691// @Units: Hz692// @User: Standard693694// @Param: POSYAW_FLTE695// @DisplayName: Position (yaw) error frequency filter in Hz696// @Description: Position (yaw) error frequency filter in Hz697// @Range: 1 100698// @Increment: 1699// @Units: Hz700// @User: Standard701702// @Param: POSYAW_FLTD703// @DisplayName: Position (yaw) derivative input filter in Hz704// @Description: Position (yaw) derivative input filter in Hz705// @Range: 1 100706// @Increment: 1707// @Units: Hz708// @User: Standard709710// @Param: POSYAW_SMAX711// @DisplayName: Yaw slew rate limit712// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains.713// @Range: 0 200714// @Increment: 0.5715// @User: Advanced716717// @Param: POSYAW_PDMX718// @DisplayName: Position (yaw) axis controller PD sum maximum719// @Description: Position (yaw) axis controller PD sum maximum. The maximum/minimum value that the sum of the P and D term can output720// @Range: 0 4000721// @Increment: 10722// @Units: d%723// @User: Advanced724725// @Param: POSYAW_D_FF726// @DisplayName: Position (yaw) Derivative FeedForward Gain727// @Description: FF D Gain which produces an output that is proportional to the rate of change of the target728// @Range: 0 0.1729// @Increment: 0.001730// @User: Advanced731732// @Param: POSYAW_NTF733// @DisplayName: Position (yaw) Target notch filter index734// @Description: Position (yaw) Target notch filter index735// @Range: 1 8736// @User: Advanced737738// @Param: POSYAW_NEF739// @DisplayName: Position (yaw) Error notch filter index740// @Description: Position (yaw) Error notch filter index741// @Range: 1 8742// @User: Advanced743744GOBJECT(pid_pos_yaw, "POSYAW_", AC_PID),745746// @Group:747// @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp748PARAM_VEHICLE_INFO,749750AP_VAREND751};752753/*7542nd group of parameters755*/756const AP_Param::GroupInfo ParametersG2::var_info[] = {757758// @Param: DEV_OPTIONS759// @DisplayName: Development options760// @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 meaning761// @Bitmask: 0:Unknown762// @User: Advanced763AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0),764765// @Param: SYSID_ENFORCE766// @DisplayName: GCS sysid enforcement767// @Description: This controls whether packets from other than the expected GCS system ID will be accepted768// @Values: 0:NotEnforced,1:Enforced769// @User: Advanced770AP_GROUPINFO("SYSID_ENFORCE", 11, ParametersG2, sysid_enforce, 0),771772// 12 was AP_Stats773774// @Param: FRAME_CLASS775// @DisplayName: Frame Class776// @Description: Controls major frame class for blimp.777// @Values: 0:Finnedblimp778// @User: Standard779// @RebootRequired: True780AP_GROUPINFO("FRAME_CLASS", 15, ParametersG2, frame_class, DEFAULT_FRAME_CLASS),781782// @Group: SERVO783// @Path: ../libraries/SRV_Channel/SRV_Channels.cpp784AP_SUBGROUPINFO(servo_channels, "SERVO", 16, ParametersG2, SRV_Channels),785786// @Group: RC787// @Path: ../libraries/RC_Channel/RC_Channels_VarInfo.h788AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels_Blimp),789790// @Param: PILOT_SPEED_DN791// @DisplayName: Pilot maximum vertical speed descending792// @Description: The maximum vertical descending velocity the pilot may request in cm/s793// @Units: cm/s794// @Range: 50 500795// @Increment: 10796// @User: Standard797AP_GROUPINFO("PILOT_SPEED_DN", 24, ParametersG2, pilot_speed_dn, 0),798799// 30 was AP_Scripting800801// @Param: FS_VIBE_ENABLE802// @DisplayName: Vibration Failsafe enable803// @Description: This enables the vibration failsafe which will use modified altitude estimation and control during high vibrations804// @Values: 0:Disabled, 1:Enabled805// @User: Standard806AP_GROUPINFO("FS_VIBE_ENABLE", 35, ParametersG2, fs_vibe_enabled, 1),807808// @Param: FS_OPTIONS809// @DisplayName: Failsafe options bitmask810// @Description: Bitmask of additional options for battery, radio, & GCS failsafes. 0 (default) disables all options.811// @Bitmask: 4:Continue if in pilot controlled modes on GCS failsafe812// @User: Advanced813AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, (float)Blimp::FailsafeOption::GCS_CONTINUE_IF_PILOT_CONTROL),814815// @Param: FS_GCS_TIMEOUT816// @DisplayName: GCS failsafe timeout817// @Description: Timeout before triggering the GCS failsafe818// @Units: s819// @Range: 2 120820// @Increment: 1821// @User: Standard822AP_GROUPINFO("FS_GCS_TIMEOUT", 42, ParametersG2, fs_gcs_timeout, 5),823824AP_GROUPEND825};826827/*828constructor for g2 object829*/830ParametersG2::ParametersG2(void)831{832AP_Param::setup_object_defaults(this, var_info);833}834835void Blimp::load_parameters(void)836{837AP_Vehicle::load_parameters(g.format_version, Parameters::k_format_version);838839static const AP_Param::G2ObjectConversion g2_conversions[] {840#if AP_STATS_ENABLED841// PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6842{ &stats, stats.var_info, 12 },843#endif844#if AP_SCRIPTING_ENABLED845// PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6846{ &scripting, scripting.var_info, 30 },847#endif848};849AP_Param::convert_g2_objects(&g2, g2_conversions, ARRAY_SIZE(g2_conversions));850851// PARAMETER_CONVERSION - Added: Feb-2024852#if HAL_LOGGING_ENABLED853AP_Param::convert_class(g.k_param_logger, &logger, logger.var_info, 0, true);854#endif855856static const AP_Param::TopLevelObjectConversion toplevel_conversions[] {857#if AP_SERIALMANAGER_ENABLED858// PARAMETER_CONVERSION - Added: Feb-2024859{ &serial_manager, serial_manager.var_info, Parameters::k_param_serial_manager_old },860#endif861};862863AP_Param::convert_toplevel_objects(toplevel_conversions, ARRAY_SIZE(toplevel_conversions));864865// setup AP_Param frame type flags866AP_Param::set_frame_type_flags(AP_PARAM_FRAME_BLIMP);867}868869870