AP_BattMonitor_Params.cpp 9.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171
  1. #include <AP_Common/AP_Common.h>
  2. #include <AP_Vehicle/AP_Vehicle_Type.h>
  3. #include "AP_BattMonitor_Params.h"
  4. #include "AP_BattMonitor_Analog.h"
  5. #if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
  6. #define DEFAULT_LOW_BATTERY_VOLTAGE 10.5f
  7. #else
  8. #define DEFAULT_LOW_BATTERY_VOLTAGE 0.0f
  9. #endif // APM_BUILD_TYPE(APM_BUILD_ArduCopter)
  10. const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
  11. // @Param: MONITOR
  12. // @DisplayName: Battery monitoring
  13. // @Description: Controls enabling monitoring of the battery's voltage and current
  14. // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell,8:UAVCAN-BatteryInfo,9:BLHeli ESC,10:SumOfFollowing,11:FuelFlow,12:FuelLevelPWM
  15. // @User: Standard
  16. // @RebootRequired: True
  17. AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, BattMonitor_TYPE_NONE, AP_PARAM_FLAG_ENABLE),
  18. // @Param: VOLT_PIN
  19. // @DisplayName: Battery Voltage sensing pin
  20. // @Description: Sets the analog input pin that should be used for voltage monitoring.
  21. // @Values: -1:Disabled, 2:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 13:Pixhawk2_PM2, 14:CubeOrange, 13:CubeOrange_PM2, 100:PX4-v1
  22. // @User: Standard
  23. // @RebootRequired: True
  24. AP_GROUPINFO("VOLT_PIN", 2, AP_BattMonitor_Params, _volt_pin, AP_BATT_VOLT_PIN),
  25. // @Param: CURR_PIN
  26. // @DisplayName: Battery Current sensing pin
  27. // @Description: Sets the analog input pin that should be used for current monitoring.
  28. // @Values: -1:Disabled, 3:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 14:Pixhawk2_PM2, 15:CubeOrange, 4:CubeOrange_PM2, 101:PX4-v1
  29. // @User: Standard
  30. // @RebootRequired: True
  31. AP_GROUPINFO("CURR_PIN", 3, AP_BattMonitor_Params, _curr_pin, AP_BATT_CURR_PIN),
  32. // @Param: VOLT_MULT
  33. // @DisplayName: Voltage Multiplier
  34. // @Description: Used to convert the voltage of the voltage sensing pin (@PREFIX@VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT). For the 3DR Power brick with a Pixhawk, this should be set to 10.1. For the Pixhawk with the 3DR 4in1 ESC this should be 12.02. For the PX using the PX4IO power supply this should be set to 1.
  35. // @User: Advanced
  36. AP_GROUPINFO("VOLT_MULT", 4, AP_BattMonitor_Params, _volt_multiplier, AP_BATT_VOLTDIVIDER_DEFAULT),
  37. // @Param: AMP_PERVLT
  38. // @DisplayName: Amps per volt
  39. // @Description: Number of amps that a 1V reading on the current sensor corresponds to. With a Pixhawk using the 3DR Power brick this should be set to 17. For the Pixhawk with the 3DR 4in1 ESC this should be 17.
  40. // @Units: A/V
  41. // @User: Standard
  42. AP_GROUPINFO("AMP_PERVLT", 5, AP_BattMonitor_Params, _curr_amp_per_volt, AP_BATT_CURR_AMP_PERVOLT_DEFAULT),
  43. // @Param: AMP_OFFSET
  44. // @DisplayName: AMP offset
  45. // @Description: Voltage offset at zero current on current sensor
  46. // @Units: V
  47. // @User: Standard
  48. AP_GROUPINFO("AMP_OFFSET", 6, AP_BattMonitor_Params, _curr_amp_offset, 0),
  49. // @Param: CAPACITY
  50. // @DisplayName: Battery capacity
  51. // @Description: Capacity of the battery in mAh when full
  52. // @Units: mAh
  53. // @Increment: 50
  54. // @User: Standard
  55. AP_GROUPINFO("CAPACITY", 7, AP_BattMonitor_Params, _pack_capacity, 3300),
  56. // @Param: WATT_MAX
  57. // @DisplayName: Maximum allowed power (Watts)
  58. // @Description: If battery wattage (voltage * current) exceeds this value then the system will reduce max throttle (THR_MAX, TKOFF_THR_MAX and THR_MIN for reverse thrust) to satisfy this limit. This helps limit high current to low C rated batteries regardless of battery voltage. The max throttle will slowly grow back to THR_MAX (or TKOFF_THR_MAX ) and THR_MIN if demanding the current max and under the watt max. Use 0 to disable.
  59. // @Units: W
  60. // @Increment: 1
  61. // @User: Advanced
  62. AP_GROUPINFO_FRAME("WATT_MAX", 8, AP_BattMonitor_Params, _watt_max, 0, AP_PARAM_FRAME_PLANE),
  63. // @Param: SERIAL_NUM
  64. // @DisplayName: Battery serial number
  65. // @Description: Battery serial number, automatically filled in for SMBus batteries, otherwise will be -1. With UAVCAN it is the battery_id.
  66. // @User: Advanced
  67. AP_GROUPINFO("SERIAL_NUM", 9, AP_BattMonitor_Params, _serial_number, AP_BATT_SERIAL_NUMBER_DEFAULT),
  68. // @Param: LOW_TIMER
  69. // @DisplayName: Low voltage timeout
  70. // @Description: This is the timeout in seconds before a low voltage event will be triggered. For aircraft with low C batteries it may be necessary to raise this in order to cope with low voltage on long takeoffs. A value of zero disables low voltage errors.
  71. // @Units: s
  72. // @Increment: 1
  73. // @Range: 0 120
  74. // @User: Advanced
  75. AP_GROUPINFO("LOW_TIMER", 10, AP_BattMonitor_Params, _low_voltage_timeout, 10),
  76. // @Param: FS_VOLTSRC
  77. // @DisplayName: Failsafe voltage source
  78. // @Description: Voltage type used for detection of low voltage event
  79. // @Values: 0:Raw Voltage, 1:Sag Compensated Voltage
  80. // @User: Advanced
  81. AP_GROUPINFO("FS_VOLTSRC", 11, AP_BattMonitor_Params, _failsafe_voltage_source, BattMonitor_LowVoltageSource_Raw),
  82. // @Param: LOW_VOLT
  83. // @DisplayName: Low battery voltage
  84. // @Description: Battery voltage that triggers a low battery failsafe. Set to 0 to disable. If the battery voltage drops below this voltage continuously for more then the period specified by the @PREFIX@LOW_TIMER parameter then the vehicle will perform the failsafe specified by the @PREFIX@FS_LOW_ACT parameter.
  85. // @Units: V
  86. // @Increment: 0.1
  87. // @User: Standard
  88. AP_GROUPINFO("LOW_VOLT", 12, AP_BattMonitor_Params, _low_voltage, DEFAULT_LOW_BATTERY_VOLTAGE),
  89. // @Param: LOW_MAH
  90. // @DisplayName: Low battery capacity
  91. // @Description: Battery capacity at which the low battery failsafe is triggered. Set to 0 to disable battery remaining failsafe. If the battery capacity drops below this level the vehicle will perform the failsafe specified by the @PREFIX@FS_LOW_ACT parameter.
  92. // @Units: mAh
  93. // @Increment: 50
  94. // @User: Standard
  95. AP_GROUPINFO("LOW_MAH", 13, AP_BattMonitor_Params, _low_capacity, 0),
  96. // @Param: CRT_VOLT
  97. // @DisplayName: Critical battery voltage
  98. // @Description: Battery voltage that triggers a critical battery failsafe. Set to 0 to disable. If the battery voltage drops below this voltage continuously for more then the period specified by the @PREFIX@LOW_TIMER parameter then the vehicle will perform the failsafe specified by the @PREFIX@FS_CRT_ACT parameter.
  99. // @Units: V
  100. // @Increment: 0.1
  101. // @User: Standard
  102. AP_GROUPINFO("CRT_VOLT", 14, AP_BattMonitor_Params, _critical_voltage, 0),
  103. // @Param: CRT_MAH
  104. // @DisplayName: Battery critical capacity
  105. // @Description: Battery capacity at which the critical battery failsafe is triggered. Set to 0 to disable battery remaining failsafe. If the battery capacity drops below this level the vehicle will perform the failsafe specified by the @PREFIX@_FS_CRT_ACT parameter.
  106. // @Units: mAh
  107. // @Increment: 50
  108. // @User: Standard
  109. AP_GROUPINFO("CRT_MAH", 15, AP_BattMonitor_Params, _critical_capacity, 0),
  110. // @Param: FS_LOW_ACT
  111. // @DisplayName: Low battery failsafe action
  112. // @Description: What action the vehicle should perform if it hits a low battery failsafe
  113. // @Values{Plane}: 0:None,1:RTL,2:Land,3:Terminate,4:QLand
  114. // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate
  115. // @Values{Sub}: 0:None,2:Disarm,3:Enter surface mode
  116. // @Values{Rover}: 0:None,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold,5:Terminate
  117. // @Values{Tracker}: 0:None
  118. // @User: Standard
  119. AP_GROUPINFO("FS_LOW_ACT", 16, AP_BattMonitor_Params, _failsafe_low_action, 0),
  120. // @Param: FS_CRT_ACT
  121. // @DisplayName: Critical battery failsafe action
  122. // @Description: What action the vehicle should perform if it hits a critical battery failsafe
  123. // @Values{Plane}: 0:None,1:RTL,2:Land,3:Terminate,4:QLand,5:Parachute
  124. // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate
  125. // @Values{Sub}: 0:None,2:Disarm,3:Enter surface mode
  126. // @Values{Rover}: 0:None,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold,5:Terminate
  127. // @Values{Tracker}: 0:None
  128. // @User: Standard
  129. AP_GROUPINFO("FS_CRT_ACT", 17, AP_BattMonitor_Params, _failsafe_critical_action, 0),
  130. // @Param: ARM_VOLT
  131. // @DisplayName: Required arming voltage
  132. // @Description: Battery voltage level which is required to arm the aircraft. Set to 0 to allow arming at any voltage.
  133. // @Units: V
  134. // @Increment: 0.1
  135. // @User: Advanced
  136. AP_GROUPINFO("ARM_VOLT", 18, AP_BattMonitor_Params, _arming_minimum_voltage, 0),
  137. // @Param: ARM_MAH
  138. // @DisplayName: Required arming remaining capacity
  139. // @Description: Battery capacity remaining which is required to arm the aircraft. Set to 0 to allow arming at any capacity. Note that execept for smart batteries rebooting the vehicle will always reset the remaining capacity estimate, which can lead to this check not providing sufficent protection, it is recommended to always use this in conjunction with the @PREFIX@_ARM_VOLT parameter.
  140. // @Units: mAh
  141. // @Increment: 50
  142. // @User: Advanced
  143. AP_GROUPINFO("ARM_MAH", 19, AP_BattMonitor_Params, _arming_minimum_capacity, 0),
  144. AP_GROUPEND
  145. };
  146. AP_BattMonitor_Params::AP_BattMonitor_Params(void) {
  147. AP_Param::setup_object_defaults(this, var_info);
  148. }