mirror of https://github.com/ArduPilot/ardupilot
AP_BattMon: add AP_BATTERY_WATT_MAX_ENABLED
This commit is contained in:
parent
b468af81ac
commit
6dcaf94c3d
|
@ -841,24 +841,25 @@ void AP_BattMonitor::check_failsafes(void)
|
|||
// return true if any battery is pushing too much power
|
||||
bool AP_BattMonitor::overpower_detected() const
|
||||
{
|
||||
bool result = false;
|
||||
#if AP_BATTERY_WATT_MAX_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
||||
for (uint8_t instance = 0; instance < _num_instances; instance++) {
|
||||
result |= overpower_detected(instance);
|
||||
if (overpower_detected(instance)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
||||
bool AP_BattMonitor::overpower_detected(uint8_t instance) const
|
||||
{
|
||||
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
||||
#if AP_BATTERY_WATT_MAX_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
||||
if (instance < _num_instances && _params[instance]._watt_max > 0) {
|
||||
float power = state[instance].current_amps * state[instance].voltage;
|
||||
const float power = state[instance].current_amps * state[instance].voltage;
|
||||
return state[instance].healthy && (power > _params[instance]._watt_max);
|
||||
}
|
||||
return false;
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
||||
bool AP_BattMonitor::has_cell_voltages(const uint8_t instance) const
|
||||
|
|
|
@ -40,7 +40,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
|
|||
// @User: Standard
|
||||
AP_GROUPINFO("CAPACITY", 7, AP_BattMonitor_Params, _pack_capacity, AP_BATT_MONITOR_BATTERY_CAPACITY),
|
||||
|
||||
#ifndef HAL_BUILD_AP_PERIPH
|
||||
#if AP_BATTERY_WATT_MAX_ENABLED
|
||||
// @Param{Plane}: WATT_MAX
|
||||
// @DisplayName: Maximum allowed power (Watts)
|
||||
// @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.
|
||||
|
@ -48,7 +48,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
|
|||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO_FRAME("WATT_MAX", 8, AP_BattMonitor_Params, _watt_max, 0, AP_PARAM_FRAME_PLANE),
|
||||
#endif // HAL_BUILD_AP_PERIPH
|
||||
#endif
|
||||
|
||||
// @Param: SERIAL_NUM
|
||||
// @DisplayName: Battery serial number
|
||||
|
|
|
@ -39,7 +39,9 @@ public:
|
|||
AP_Int32 _arming_minimum_capacity; /// capacity level required to arm
|
||||
AP_Float _arming_minimum_voltage; /// voltage level required to arm
|
||||
AP_Int32 _options; /// Options
|
||||
#if AP_BATTERY_WATT_MAX_ENABLED
|
||||
AP_Int16 _watt_max; /// max battery power allowed. Reduce max throttle to reduce current to satisfy t his limit
|
||||
#endif
|
||||
AP_Int8 _type; /// 0=disabled, 3=voltage only, 4=voltage and current
|
||||
AP_Int8 _low_voltage_timeout; /// timeout in seconds before a low voltage event will be triggered
|
||||
AP_Int8 _failsafe_voltage_source; /// voltage type used for detection of low voltage event
|
||||
|
|
|
@ -34,6 +34,10 @@
|
|||
#define AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED 0
|
||||
#endif
|
||||
|
||||
#ifndef AP_BATTERY_WATT_MAX_ENABLED
|
||||
#define AP_BATTERY_WATT_MAX_ENABLED AP_BATTERY_ENABLED
|
||||
#endif
|
||||
|
||||
#ifndef AP_BATTERY_SMBUS_ENABLED
|
||||
#define AP_BATTERY_SMBUS_ENABLED AP_BATTERY_BACKEND_DEFAULT_ENABLED
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue