Parameters.cpp 28 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733
  1. #include "Sub.h"
  2. /*
  3. This program is free software: you can redistribute it and/or modify
  4. it under the terms of the GNU General Public License as published by
  5. the Free Software Foundation, either version 3 of the License, or
  6. (at your option) any later version.
  7. This program is distributed in the hope that it will be useful,
  8. but WITHOUT ANY WARRANTY; without even the implied warranty of
  9. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  10. GNU General Public License for more details.
  11. You should have received a copy of the GNU General Public License
  12. along with this program. If not, see <http://www.gnu.org/licenses/>.
  13. */
  14. /*
  15. * ArduSub parameter definitions
  16. *
  17. */
  18. #define GSCALAR(v, name, def) { sub.g.v.vtype, name, Parameters::k_param_ ## v, &sub.g.v, {def_value : def} }
  19. #define ASCALAR(v, name, def) { sub.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&sub.aparm.v, {def_value : def} }
  20. #define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &sub.g.v, {group_info : class::var_info} }
  21. #define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&sub.v, {group_info : class::var_info} }
  22. #define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&sub.v, {group_info : class::var_info} }
  23. const AP_Param::Info Sub::var_info[] = {
  24. // @Param: SURFACE_DEPTH
  25. // @DisplayName: Depth reading at surface
  26. // @Description: The depth the external pressure sensor will read when the vehicle is considered at the surface (in centimeters)
  27. // @Units: cm
  28. // @Range: -100 0
  29. // @User: Standard
  30. GSCALAR(surface_depth, "SURFACE_DEPTH", SURFACE_DEPTH_DEFAULT),
  31. // @Param: SYSID_SW_MREV
  32. // @DisplayName: Eeprom format version number
  33. // @Description: This value is incremented when changes are made to the eeprom format
  34. // @User: Advanced
  35. // @ReadOnly: True
  36. GSCALAR(format_version, "SYSID_SW_MREV", 0),
  37. // @Param: SYSID_THISMAV
  38. // @DisplayName: MAVLink system ID of this vehicle
  39. // @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network
  40. // @Range: 1 255
  41. // @User: Advanced
  42. GSCALAR(sysid_this_mav, "SYSID_THISMAV", MAV_SYSTEM_ID),
  43. // @Param: SYSID_MYGCS
  44. // @DisplayName: My ground station number
  45. // @Description: Allows restricting radio overrides to only come from my ground station
  46. // @User: Advanced
  47. GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255),
  48. // @Param: PILOT_THR_FILT
  49. // @DisplayName: Throttle filter cutoff
  50. // @Description: Throttle filter cutoff (Hz) - active whenever altitude control is inactive - 0 to disable
  51. // @User: Advanced
  52. // @Units: Hz
  53. // @Range: 0 10
  54. // @Increment: .5
  55. GSCALAR(throttle_filt, "PILOT_THR_FILT", 0),
  56. // @Group: SERIAL
  57. // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp
  58. GOBJECT(serial_manager, "SERIAL", AP_SerialManager),
  59. // @Param: GCS_PID_MASK
  60. // @DisplayName: GCS PID tuning mask
  61. // @Description: bitmask of PIDs to send MAVLink PID_TUNING messages for
  62. // @User: Advanced
  63. // @Values: 0:None,1:Roll,2:Pitch,4:Yaw
  64. // @Bitmask: 0:Roll,1:Pitch,2:Yaw
  65. GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0),
  66. #if RANGEFINDER_ENABLED == ENABLED
  67. // @Param: RNGFND_GAIN
  68. // @DisplayName: Rangefinder gain
  69. // @Description: Used to adjust the speed with which the target altitude is changed when objects are sensed below the sub
  70. // @Range: 0.01 2.0
  71. // @Increment: 0.01
  72. // @User: Standard
  73. GSCALAR(rangefinder_gain, "RNGFND_GAIN", RANGEFINDER_GAIN_DEFAULT),
  74. #endif
  75. // @Param: FS_GCS_ENABLE
  76. // @DisplayName: Ground Station Failsafe Enable
  77. // @Description: Controls what action to take when GCS heartbeat is lost.
  78. // @Values: 0:Disabled,1:Warn only,2:Disarm,3:Enter depth hold mode,4:Enter surface mode
  79. // @User: Standard
  80. GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISARM),
  81. // @Param: FS_LEAK_ENABLE
  82. // @DisplayName: Leak Failsafe Enable
  83. // @Description: Controls what action to take if a leak is detected.
  84. // @Values: 0:Disabled,1:Warn only,2:Enter surface mode
  85. // @User: Standard
  86. GSCALAR(failsafe_leak, "FS_LEAK_ENABLE", FS_LEAK_WARN_ONLY),
  87. // @Param: FS_PRESS_ENABLE
  88. // @DisplayName: Internal Pressure Failsafe Enable
  89. // @Description: Controls what action to take if internal pressure exceeds FS_PRESS_MAX parameter.
  90. // @Values: 0:Disabled,1:Warn only
  91. // @User: Standard
  92. GSCALAR(failsafe_pressure, "FS_PRESS_ENABLE", FS_PRESS_DISABLED),
  93. // @Param: FS_TEMP_ENABLE
  94. // @DisplayName: Internal Temperature Failsafe Enable
  95. // @Description: Controls what action to take if internal temperature exceeds FS_TEMP_MAX parameter.
  96. // @Values: 0:Disabled,1:Warn only
  97. // @User: Standard
  98. GSCALAR(failsafe_temperature, "FS_TEMP_ENABLE", FS_TEMP_DISABLED),
  99. // @Param: FS_PRESS_MAX
  100. // @DisplayName: Internal Pressure Failsafe Threshold
  101. // @Description: The maximum internal pressure allowed before triggering failsafe. Failsafe action is determined by FS_PRESS_ENABLE parameter
  102. // @Units: Pa
  103. // @User: Standard
  104. GSCALAR(failsafe_pressure_max, "FS_PRESS_MAX", FS_PRESS_MAX_DEFAULT),
  105. // @Param: FS_TEMP_MAX
  106. // @DisplayName: Internal Temperature Failsafe Threshold
  107. // @Description: The maximum internal temperature allowed before triggering failsafe. Failsafe action is determined by FS_TEMP_ENABLE parameter.
  108. // @Units: degC
  109. // @User: Standard
  110. GSCALAR(failsafe_temperature_max, "FS_TEMP_MAX", FS_TEMP_MAX_DEFAULT),
  111. // @Param: FS_TERRAIN_ENAB
  112. // @DisplayName: Terrain Failsafe Enable
  113. // @Description: Controls what action to take if terrain information is lost during AUTO mode
  114. // @Values: 0:Disarm, 1:Hold Position, 2:Surface
  115. // @User: Standard
  116. GSCALAR(failsafe_terrain, "FS_TERRAIN_ENAB", FS_TERRAIN_DISARM),
  117. // @Param: FS_PILOT_INPUT
  118. // @DisplayName: Pilot input failsafe action
  119. // @Description: Controls what action to take if no pilot input has been received after the timeout period specified by the FS_PILOT_TIMEOUT parameter
  120. // @Values: 0:Disabled, 1:Warn Only, 2:Disarm
  121. // @User: Standard
  122. GSCALAR(failsafe_pilot_input, "FS_PILOT_INPUT", FS_PILOT_INPUT_DISARM),
  123. // @Param: FS_PILOT_TIMEOUT
  124. // @DisplayName: Timeout for activation of pilot input failsafe
  125. // @Description: Controls the maximum interval between received pilot inputs before the failsafe action is triggered
  126. // @Units: s
  127. // @Range: 0.1 3.0
  128. // @User: Standard
  129. GSCALAR(failsafe_pilot_input_timeout, "FS_PILOT_TIMEOUT", 3.0f),
  130. // @Param: XTRACK_ANG_LIM
  131. // @DisplayName: Crosstrack correction angle limit
  132. // @Description: Maximum allowed angle (in degrees) between current track and desired heading during waypoint navigation
  133. // @Range: 10 90
  134. // @User: Standard
  135. GSCALAR(xtrack_angle_limit,"XTRACK_ANG_LIM", 45),
  136. // @Param: WP_YAW_BEHAVIOR
  137. // @DisplayName: Yaw behaviour during missions
  138. // @Description: Determines how the autopilot controls the yaw during missions and RTL
  139. // @Values: 0:Never change yaw, 1:Face next waypoint, 2:Face next waypoint except RTL, 3:Face along GPS course, 4:Correct crosstrack error
  140. // @User: Standard
  141. GSCALAR(wp_yaw_behavior, "WP_YAW_BEHAVIOR", WP_YAW_BEHAVIOR_DEFAULT),
  142. // @Param: PILOT_SPEED_UP
  143. // @DisplayName: Pilot maximum vertical ascending speed
  144. // @Description: The maximum vertical ascending velocity the pilot may request in cm/s
  145. // @Units: cm/s
  146. // @Range: 50 500
  147. // @Increment: 10
  148. // @User: Standard
  149. GSCALAR(pilot_speed_up, "PILOT_SPEED_UP", PILOT_VELZ_MAX),
  150. // @Param: PILOT_SPEED_DN
  151. // @DisplayName: Pilot maximum vertical descending speed
  152. // @Description: The maximum vertical descending velocity the pilot may request in cm/s
  153. // @Units: cm/s
  154. // @Range: 50 500
  155. // @Increment: 10
  156. // @User: Standard
  157. GSCALAR(pilot_speed_dn, "PILOT_SPEED_DN", 0),
  158. // @Param: PILOT_ACCEL_Z
  159. // @DisplayName: Pilot vertical acceleration
  160. // @Description: The vertical acceleration used when pilot is controlling the altitude
  161. // @Units: cm/s/s
  162. // @Range: 50 500
  163. // @Increment: 10
  164. // @User: Standard
  165. GSCALAR(pilot_accel_z, "PILOT_ACCEL_Z", PILOT_ACCEL_Z_DEFAULT),
  166. // @Param: THR_DZ
  167. // @DisplayName: Throttle deadzone
  168. // @Description: The PWM deadzone in microseconds above and below mid throttle. Used in AltHold, Loiter, PosHold flight modes
  169. // @User: Standard
  170. // @Range: 0 300
  171. // @Units: PWM
  172. // @Increment: 1
  173. GSCALAR(throttle_deadzone, "THR_DZ", THR_DZ_DEFAULT),
  174. // @Param: LOG_BITMASK
  175. // @DisplayName: Log bitmask
  176. // @Description: 4 byte bitmap of log types to enable
  177. // @Values: 830:Default,894:Default+RCIN,958:Default+IMU,1854:Default+Motors,-6146:NearlyAll-AC315,45054:NearlyAll,131071:All+FastATT,262142:All+MotBatt,393214:All+FastIMU,397310:All+FastIMU+PID,655358:All+FullIMU,0:Disabled
  178. // @Bitmask: 0:ATTITUDE_FAST,1:ATTITUDE_MED,2:GPS,3:PM,4:CTUN,5:NTUN,6:RCIN,7:IMU,8:CMD,9:CURRENT,10:RCOUT,11:OPTFLOW,12:PID,13:COMPASS,14:INAV,15:CAMERA,17:MOTBATT,18:IMU_FAST,19:IMU_RAW
  179. // @User: Standard
  180. GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK),
  181. // @Param: ANGLE_MAX
  182. // @DisplayName: Angle Max
  183. // @Description: Maximum lean angle in all flight modes
  184. // @Units: cdeg
  185. // @Range: 1000 8000
  186. // @User: Advanced
  187. ASCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX),
  188. // @Param: FS_EKF_ACTION
  189. // @DisplayName: EKF Failsafe Action
  190. // @Description: Controls the action that will be taken when an EKF failsafe is invoked
  191. // @Values: 0:Disabled, 1:Warn only, 2:Disarm
  192. // @User: Advanced
  193. GSCALAR(fs_ekf_action, "FS_EKF_ACTION", FS_EKF_ACTION_DEFAULT),
  194. // @Param: FS_EKF_THRESH
  195. // @DisplayName: EKF failsafe variance threshold
  196. // @Description: Allows setting the maximum acceptable compass and velocity variance
  197. // @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed
  198. // @User: Advanced
  199. GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", FS_EKF_THRESHOLD_DEFAULT),
  200. // @Param: FS_CRASH_CHECK
  201. // @DisplayName: Crash check enable
  202. // @Description: This enables automatic crash checking. When enabled the motors will disarm if a crash is detected.
  203. // @Values: 0:Disabled,1:Warn only,2:Disarm
  204. // @User: Advanced
  205. GSCALAR(fs_crash_check, "FS_CRASH_CHECK", FS_CRASH_DISABLED),
  206. // @Param: JS_GAIN_DEFAULT
  207. // @DisplayName: Default gain at boot
  208. // @Description: Default gain at boot, must be in range [JS_GAIN_MIN , JS_GAIN_MAX]
  209. // @User: Standard
  210. // @Range: 0.1 1.0
  211. GSCALAR(gain_default, "JS_GAIN_DEFAULT", 0.5),
  212. // @Param: JS_GAIN_MAX
  213. // @DisplayName: Maximum joystick gain
  214. // @Description: Maximum joystick gain
  215. // @User: Standard
  216. // @Range: 0.2 1.0
  217. GSCALAR(maxGain, "JS_GAIN_MAX", 1.0),
  218. // @Param: JS_GAIN_MIN
  219. // @DisplayName: Minimum joystick gain
  220. // @Description: Minimum joystick gain
  221. // @User: Standard
  222. // @Range: 0.1 0.8
  223. GSCALAR(minGain, "JS_GAIN_MIN", 0.25),
  224. // @Param: JS_GAIN_STEPS
  225. // @DisplayName: Gain steps
  226. // @Description: Controls the number of steps between minimum and maximum joystick gain when the gain is adjusted using buttons. Set to 1 to always use JS_GAIN_DEFAULT.
  227. // @User: Standard
  228. // @Range: 1 10
  229. GSCALAR(numGainSettings, "JS_GAIN_STEPS", 4),
  230. // @Param: JS_LIGHTS_STEPS
  231. // @DisplayName: Lights brightness steps
  232. // @Description: Number of steps in brightness between minimum and maximum brightness
  233. // @User: Standard
  234. // @Range: 1 10
  235. // @Units: PWM
  236. GSCALAR(lights_steps, "JS_LIGHTS_STEPS", 8),
  237. // @Param: JS_THR_GAIN
  238. // @DisplayName: Throttle gain scalar
  239. // @Description: Scalar for gain on the throttle channel
  240. // @User: Standard
  241. // @Range: 0.5 4.0
  242. GSCALAR(throttle_gain, "JS_THR_GAIN", 1.0f),
  243. // @Param: FRAME_CONFIG
  244. // @DisplayName: Frame configuration
  245. // @Description: Set this parameter according to your vehicle/motor configuration
  246. // @User: Standard
  247. // @RebootRequired: True
  248. // @Values: 0:BlueROV1, 1:Vectored, 2:Vectored_6DOF, 3:Vectored_6DOF_90, 4:SimpleROV-3, 5:SimpleROV-4, 6:SimpleROV-5, 7:Custom
  249. GSCALAR(frame_configuration, "FRAME_CONFIG", AP_Motors6DOF::SUB_FRAME_VECTORED),
  250. // @Group: BTN0_
  251. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  252. GGROUP(jbtn_0, "BTN0_", JSButton),
  253. // @Group: BTN1_
  254. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  255. GGROUP(jbtn_1, "BTN1_", JSButton),
  256. // @Group: BTN2_
  257. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  258. GGROUP(jbtn_2, "BTN2_", JSButton),
  259. // @Group: BTN3_
  260. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  261. GGROUP(jbtn_3, "BTN3_", JSButton),
  262. // @Group: BTN4_
  263. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  264. GGROUP(jbtn_4, "BTN4_", JSButton),
  265. // @Group: BTN5_
  266. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  267. GGROUP(jbtn_5, "BTN5_", JSButton),
  268. // @Group: BTN6_
  269. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  270. GGROUP(jbtn_6, "BTN6_", JSButton),
  271. // @Group: BTN7_
  272. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  273. GGROUP(jbtn_7, "BTN7_", JSButton),
  274. // @Group: BTN8_
  275. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  276. GGROUP(jbtn_8, "BTN8_", JSButton),
  277. // @Group: BTN9_
  278. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  279. GGROUP(jbtn_9, "BTN9_", JSButton),
  280. // @Group: BTN10_
  281. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  282. GGROUP(jbtn_10, "BTN10_", JSButton),
  283. // @Group: BTN11_
  284. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  285. GGROUP(jbtn_11, "BTN11_", JSButton),
  286. // @Group: BTN12_
  287. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  288. GGROUP(jbtn_12, "BTN12_", JSButton),
  289. // @Group: BTN13_
  290. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  291. GGROUP(jbtn_13, "BTN13_", JSButton),
  292. // @Group: BTN14_
  293. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  294. GGROUP(jbtn_14, "BTN14_", JSButton),
  295. // @Group: BTN15_
  296. // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp
  297. GGROUP(jbtn_15, "BTN15_", JSButton),
  298. // @Param: RC_SPEED
  299. // @DisplayName: ESC Update Speed
  300. // @Description: This is the speed in Hertz that your ESCs will receive updates
  301. // @Units: Hz
  302. // @Range: 50 490
  303. // @Increment: 1
  304. // @User: Advanced
  305. GSCALAR(rc_speed, "RC_SPEED", RC_SPEED_DEFAULT),
  306. // @Param: ACRO_RP_P
  307. // @DisplayName: Acro Roll and Pitch P gain
  308. // @Description: Converts pilot roll and pitch into a desired rate of rotation in ACRO and SPORT mode. Higher values mean faster rate of rotation.
  309. // @Range: 1 10
  310. // @User: Standard
  311. GSCALAR(acro_rp_p, "ACRO_RP_P", ACRO_RP_P),
  312. // @Param: ACRO_YAW_P
  313. // @DisplayName: Acro Yaw P gain
  314. // @Description: Converts pilot yaw input into a desired rate of rotation. Higher values mean faster rate of rotation.
  315. // @Range: 1 10
  316. // @User: Standard
  317. GSCALAR(acro_yaw_p, "ACRO_YAW_P", ACRO_YAW_P),
  318. // @Param: ACRO_BAL_ROLL
  319. // @DisplayName: Acro Balance Roll
  320. // @Description: rate at which roll angle returns to level in acro mode. A higher value causes the vehicle to return to level faster.
  321. // @Range: 0 3
  322. // @Increment: 0.1
  323. // @User: Advanced
  324. GSCALAR(acro_balance_roll, "ACRO_BAL_ROLL", ACRO_BALANCE_ROLL),
  325. // @Param: ACRO_BAL_PITCH
  326. // @DisplayName: Acro Balance Pitch
  327. // @Description: rate at which pitch angle returns to level in acro mode. A higher value causes the vehicle to return to level faster.
  328. // @Range: 0 3
  329. // @Increment: 0.1
  330. // @User: Advanced
  331. GSCALAR(acro_balance_pitch, "ACRO_BAL_PITCH", ACRO_BALANCE_PITCH),
  332. // @Param: ACRO_TRAINER
  333. // @DisplayName: Acro Trainer
  334. // @Description: Type of trainer used in acro mode
  335. // @Values: 0:Disabled,1:Leveling,2:Leveling and Limited
  336. // @User: Advanced
  337. GSCALAR(acro_trainer, "ACRO_TRAINER", ACRO_TRAINER_LIMITED),
  338. // @Param: ACRO_EXPO
  339. // @DisplayName: Acro Expo
  340. // @Description: Acro roll/pitch Expo to allow faster rotation when stick at edges
  341. // @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
  342. // @User: Advanced
  343. GSCALAR(acro_expo, "ACRO_EXPO", ACRO_EXPO_DEFAULT),
  344. // variables not in the g class which contain EEPROM saved variables
  345. #if CAMERA == ENABLED
  346. // @Group: CAM_
  347. // @Path: ../libraries/AP_Camera/AP_Camera.cpp
  348. GOBJECT(camera, "CAM_", AP_Camera),
  349. #endif
  350. // @Group: RELAY_
  351. // @Path: ../libraries/AP_Relay/AP_Relay.cpp
  352. GOBJECT(relay, "RELAY_", AP_Relay),
  353. // @Group: COMPASS_
  354. // @Path: ../libraries/AP_Compass/AP_Compass.cpp
  355. GOBJECT(compass, "COMPASS_", Compass),
  356. // @Group: INS_
  357. // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
  358. GOBJECT(ins, "INS_", AP_InertialSensor),
  359. // @Group: WPNAV_
  360. // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp
  361. GOBJECT(wp_nav, "WPNAV_", AC_WPNav),
  362. // @Group: LOIT_
  363. // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp
  364. GOBJECT(loiter_nav, "LOIT_", AC_Loiter),
  365. #if CIRCLE_NAV_ENABLED == ENABLED
  366. // @Group: CIRCLE_
  367. // @Path: ../libraries/AC_WPNav/AC_Circle.cpp
  368. GOBJECT(circle_nav, "CIRCLE_", AC_Circle),
  369. #endif
  370. // @Group: ATC_
  371. // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Sub.cpp
  372. GOBJECT(attitude_control, "ATC_", AC_AttitudeControl_Sub),
  373. // @Group: PSC
  374. // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp
  375. GOBJECT(pos_control, "PSC", AC_PosControl),
  376. // @Group: SR0_
  377. // @Path: GCS_Mavlink.cpp
  378. GOBJECTN(_gcs.chan_parameters[0], gcs0, "SR0_", GCS_MAVLINK_Parameters),
  379. // @Group: SR1_
  380. // @Path: GCS_Mavlink.cpp
  381. GOBJECTN(_gcs.chan_parameters[1], gcs1, "SR1_", GCS_MAVLINK_Parameters),
  382. // @Group: SR2_
  383. // @Path: GCS_Mavlink.cpp
  384. GOBJECTN(_gcs.chan_parameters[2], gcs2, "SR2_", GCS_MAVLINK_Parameters),
  385. // @Group: SR3_
  386. // @Path: GCS_Mavlink.cpp
  387. GOBJECTN(_gcs.chan_parameters[3], gcs3, "SR3_", GCS_MAVLINK_Parameters),
  388. // @Group: AHRS_
  389. // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
  390. GOBJECT(ahrs, "AHRS_", AP_AHRS),
  391. #if MOUNT == ENABLED
  392. // @Group: MNT
  393. // @Path: ../libraries/AP_Mount/AP_Mount.cpp
  394. GOBJECT(camera_mount, "MNT", AP_Mount),
  395. #endif
  396. // @Group: LOG
  397. // @Path: ../libraries/AP_Logger/AP_Logger.cpp
  398. GOBJECT(logger, "LOG", AP_Logger),
  399. // @Group: BATT
  400. // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
  401. GOBJECT(battery, "BATT", AP_BattMonitor),
  402. // @Group: ARMING_
  403. // @Path: AP_Arming_Sub.cpp,../libraries/AP_Arming/AP_Arming.cpp
  404. GOBJECT(arming, "ARMING_", AP_Arming_Sub),
  405. // @Group: BRD_
  406. // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig.cpp
  407. GOBJECT(BoardConfig, "BRD_", AP_BoardConfig),
  408. #if HAL_WITH_UAVCAN
  409. // @Group: CAN_
  410. // @Path: ../libraries/AP_BoardConfig/AP_BoardConfig_CAN.cpp
  411. GOBJECT(BoardConfig_CAN, "CAN_", AP_BoardConfig_CAN),
  412. #endif
  413. #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
  414. GOBJECT(sitl, "SIM_", SITL::SITL),
  415. #endif
  416. // @Group: GND_
  417. // @Path: ../libraries/AP_Baro/AP_Baro.cpp
  418. GOBJECT(barometer, "GND_", AP_Baro),
  419. // GPS driver
  420. // @Group: GPS_
  421. // @Path: ../libraries/AP_GPS/AP_GPS.cpp
  422. GOBJECT(gps, "GPS_", AP_GPS),
  423. // Leak detector
  424. // @Group: LEAK
  425. // @Path: ../libraries/AP_LeakDetector/AP_LeakDetector.cpp
  426. GOBJECT(leak_detector, "LEAK", AP_LeakDetector),
  427. // @Group: SCHED_
  428. // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp
  429. GOBJECT(scheduler, "SCHED_", AP_Scheduler),
  430. #if AC_FENCE == ENABLED
  431. // @Group: FENCE_
  432. // @Path: ../libraries/AC_Fence/AC_Fence.cpp
  433. GOBJECT(fence, "FENCE_", AC_Fence),
  434. #endif
  435. #if AVOIDANCE_ENABLED == ENABLED
  436. // @Group: AVOID_
  437. // @Path: ../libraries/AC_Avoidance/AC_Avoid.cpp
  438. GOBJECT(avoid, "AVOID_", AC_Avoid),
  439. #endif
  440. #if AC_RALLY == ENABLED
  441. // @Group: RALLY_
  442. // @Path: ../libraries/AP_Rally/AP_Rally.cpp
  443. GOBJECT(rally, "RALLY_", AP_Rally),
  444. #endif
  445. // @Group: MOT_
  446. // @Path: ../libraries/AP_Motors/AP_Motors6DOF.cpp,../libraries/AP_Motors/AP_MotorsMulticopter.cpp
  447. GOBJECT(motors, "MOT_", AP_Motors6DOF),
  448. #if RCMAP_ENABLED == ENABLED
  449. // @Group: RCMAP_
  450. // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp
  451. GOBJECT(rcmap, "RCMAP_", RCMapper),
  452. #endif
  453. // @Group: EK2_
  454. // @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp
  455. GOBJECTN(EKF2, NavEKF2, "EK2_", NavEKF2),
  456. // @Group: EK3_
  457. // @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp
  458. GOBJECTN(EKF3, NavEKF3, "EK3_", NavEKF3),
  459. // @Group: MIS_
  460. // @Path: ../libraries/AP_Mission/AP_Mission.cpp
  461. GOBJECT(mission, "MIS_", AP_Mission),
  462. #if RANGEFINDER_ENABLED == ENABLED
  463. // @Group: RNGFND
  464. // @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
  465. GOBJECT(rangefinder, "RNGFND", RangeFinder),
  466. #endif
  467. #if AP_TERRAIN_AVAILABLE && AC_TERRAIN
  468. // @Group: TERRAIN_
  469. // @Path: ../libraries/AP_Terrain/AP_Terrain.cpp
  470. GOBJECT(terrain, "TERRAIN_", AP_Terrain),
  471. #endif
  472. #if OPTFLOW == ENABLED
  473. // @Group: FLOW
  474. // @Path: ../libraries/AP_OpticalFlow/OpticalFlow.cpp
  475. GOBJECT(optflow, "FLOW", OpticalFlow),
  476. #endif
  477. #if RPM_ENABLED == ENABLED
  478. // @Group: RPM
  479. // @Path: ../libraries/AP_RPM/AP_RPM.cpp
  480. GOBJECT(rpm_sensor, "RPM", AP_RPM),
  481. #endif
  482. // @Group: NTF_
  483. // @Path: ../libraries/AP_Notify/AP_Notify.cpp
  484. GOBJECT(notify, "NTF_", AP_Notify),
  485. #if AP_TERRAIN_AVAILABLE && AC_TERRAIN
  486. // @Param: TERRAIN_FOLLOW
  487. // @DisplayName: Terrain Following use control
  488. // @Description: This enables terrain following for RTL and SURFACE flight modes. To use this option TERRAIN_ENABLE must be 1 and the GCS must support sending terrain data to the aircraft. In RTL the RTL_ALT will be considered a height above the terrain. In LAND mode the vehicle will slow to LAND_SPEED 10m above terrain (instead of 10m above home). This parameter does not affect AUTO and Guided which use a per-command flag to determine if the height is above-home, absolute or above-terrain.
  489. // @Values: 0:Do Not Use in RTL and SURFACE,1:Use in RTL and SURFACE
  490. // @User: Standard
  491. GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0),
  492. #endif
  493. // @Group:
  494. // @Path: Parameters.cpp
  495. GOBJECT(g2, "", ParametersG2),
  496. AP_VAREND
  497. };
  498. /*
  499. 2nd group of parameters
  500. */
  501. const AP_Param::GroupInfo ParametersG2::var_info[] = {
  502. #if PROXIMITY_ENABLED == ENABLED
  503. // @Group: PRX
  504. // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp
  505. AP_SUBGROUPINFO(proximity, "PRX", 2, ParametersG2, AP_Proximity),
  506. #endif
  507. #if GRIPPER_ENABLED == ENABLED
  508. // @Group: GRIP_
  509. // @Path: ../libraries/AP_Gripper/AP_Gripper.cpp
  510. AP_SUBGROUPINFO(gripper, "GRIP_", 3, ParametersG2, AP_Gripper),
  511. #endif
  512. // @Group: SERVO
  513. // @Path: ../libraries/SRV_Channel/SRV_Channels.cpp
  514. AP_SUBGROUPINFO(servo_channels, "SERVO", 16, ParametersG2, SRV_Channels),
  515. // @Group: RC
  516. // @Path: ../libraries/RC_Channel/RC_Channels_VarInfo.h
  517. AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels),
  518. #ifdef ENABLE_SCRIPTING
  519. // @Group: SCR_
  520. // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp
  521. AP_SUBGROUPINFO(scripting, "SCR_", 18, ParametersG2, AP_Scripting),
  522. #endif
  523. AP_GROUPEND
  524. };
  525. /*
  526. constructor for g2 object
  527. */
  528. ParametersG2::ParametersG2()
  529. #if PROXIMITY_ENABLED == ENABLED
  530. : proximity(sub.serial_manager)
  531. #endif
  532. {
  533. AP_Param::setup_object_defaults(this, var_info);
  534. }
  535. const AP_Param::ConversionInfo conversion_table[] = {
  536. { Parameters::k_param_fs_batt_voltage, 0, AP_PARAM_FLOAT, "BATT_LOW_VOLT" },
  537. { Parameters::k_param_fs_batt_mah, 0, AP_PARAM_FLOAT, "BATT_LOW_MAH" },
  538. { Parameters::k_param_failsafe_battery_enabled, 0, AP_PARAM_INT8, "BATT_FS_LOW_ACT" },
  539. { Parameters::k_param_compass_enabled_deprecated, 0, AP_PARAM_INT8, "COMPASS_ENABLE" },
  540. { Parameters::k_param_arming, 2, AP_PARAM_INT16, "ARMING_CHECK" },
  541. };
  542. void Sub::load_parameters()
  543. {
  544. if (!AP_Param::check_var_info()) {
  545. hal.console->printf("Bad var table\n");
  546. AP_HAL::panic("Bad var table");
  547. }
  548. // disable centrifugal force correction, it will be enabled as part of the arming process
  549. ahrs.set_correct_centrifugal(false);
  550. hal.util->set_soft_armed(false);
  551. if (!g.format_version.load() ||
  552. g.format_version != Parameters::k_format_version) {
  553. // erase all parameters
  554. hal.console->printf("Firmware change: erasing EEPROM...\n");
  555. StorageManager::erase();
  556. AP_Param::erase_all();
  557. // save the current format version
  558. g.format_version.set_and_save(Parameters::k_format_version);
  559. hal.console->println("done.");
  560. }
  561. uint32_t before = AP_HAL::micros();
  562. // Load all auto-loaded EEPROM variables
  563. AP_Param::load_all();
  564. hal.console->printf("load_all took %uus\n", (unsigned)(AP_HAL::micros() - before));
  565. AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table));
  566. AP_Param::set_frame_type_flags(AP_PARAM_FRAME_SUB);
  567. convert_old_parameters();
  568. AP_Param::set_default_by_name("BRD_SAFETYENABLE", 0);
  569. AP_Param::set_default_by_name("ARMING_CHECK",
  570. AP_Arming::ARMING_CHECK_RC |
  571. AP_Arming::ARMING_CHECK_VOLTAGE |
  572. AP_Arming::ARMING_CHECK_BATTERY);
  573. AP_Param::set_default_by_name("CIRCLE_RATE", 2.0f);
  574. AP_Param::set_default_by_name("ATC_ACCEL_Y_MAX", 110000.0f);
  575. AP_Param::set_default_by_name("RC3_TRIM", 1100);
  576. AP_Param::set_default_by_name("COMPASS_OFFS_MAX", 1000);
  577. AP_Param::set_default_by_name("INS_GYR_CAL", 0);
  578. AP_Param::set_default_by_name("MNT_DEFLT_MODE", MAV_MOUNT_MODE_RC_TARGETING);
  579. AP_Param::set_default_by_name("MNT_JSTICK_SPD", 100);
  580. AP_Param::set_by_name("MNT_RC_IN_PAN", 7);
  581. AP_Param::set_by_name("MNT_RC_IN_TILT", 8);
  582. }
  583. void Sub::convert_old_parameters()
  584. {
  585. // attitude control filter parameter changes from _FILT to FLTE or FLTD
  586. const AP_Param::ConversionInfo filt_conversion_info[] = {
  587. // move ATC_RAT_RLL/PIT_FILT to FLTD, move ATC_RAT_YAW_FILT to FLTE
  588. { Parameters::k_param_attitude_control, 385, AP_PARAM_FLOAT, "ATC_RAT_RLL_FLTE" },
  589. { Parameters::k_param_attitude_control, 386, AP_PARAM_FLOAT, "ATC_RAT_PIT_FLTE" },
  590. { Parameters::k_param_attitude_control, 387, AP_PARAM_FLOAT, "ATC_RAT_YAW_FLTE" },
  591. };
  592. uint8_t filt_table_size = ARRAY_SIZE(filt_conversion_info);
  593. for (uint8_t i=0; i<filt_table_size; i++) {
  594. AP_Param::convert_old_parameters(&filt_conversion_info[i], 1.0f);
  595. }
  596. const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old, Parameters::k_param_rc_2_old,
  597. Parameters::k_param_rc_3_old, Parameters::k_param_rc_4_old,
  598. Parameters::k_param_rc_5_old, Parameters::k_param_rc_6_old,
  599. Parameters::k_param_rc_7_old, Parameters::k_param_rc_8_old,
  600. Parameters::k_param_rc_9_old, Parameters::k_param_rc_10_old,
  601. Parameters::k_param_rc_11_old, Parameters::k_param_rc_12_old,
  602. Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old
  603. };
  604. const uint16_t old_aux_chan_mask = 0x3FF0;
  605. // note that we don't pass in rcmap as we don't want output channel functions changed based on rcmap
  606. SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, nullptr);
  607. }