diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index b2d5c508ba..2cc0ff529f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -57,7 +57,16 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { AP_GROUPINFO("_CAPACITY", 6, AP_BattMonitor, _pack_capacity[0], AP_BATT_CAPACITY_DEFAULT), // 7 & 8 were used for VOLT2_PIN and VOLT2_MULT - // 9..10 left for future expansion + + // @Param: _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. + // @Units: Watts + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("_WATT_MAX", 9, AP_BattMonitor, _watt_max[0], AP_BATT_MAX_WATT_DEFAULT), + + // 10 is left for future expansion #if AP_BATT_MONITOR_MAX_INSTANCES > 1 // @Param: 2_MONITOR @@ -109,6 +118,15 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @User: Standard AP_GROUPINFO("2_CAPACITY", 17, AP_BattMonitor, _pack_capacity[1], AP_BATT_CAPACITY_DEFAULT), + + // @Param: 2_WATT_MAX + // @DisplayName: Maximum allowed current + // @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. + // @Units: Amps + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("2_WATT_MAX", 18, AP_BattMonitor, _watt_max[1], AP_BATT_MAX_WATT_DEFAULT), + #endif // AP_BATT_MONITOR_MAX_INSTANCES > 1 AP_GROUPEND @@ -303,3 +321,22 @@ bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_ca // if we've gotten this far battery is ok return false; } + +// return true if any battery is pushing too much power +bool AP_BattMonitor::overpower_detected() const +{ + bool result = false; + for (int instance = 0; instance < AP_BATT_MONITOR_MAX_INSTANCES; instance++) { + result |= overpower_detected(instance); + } + return result; +} + +bool AP_BattMonitor::overpower_detected(uint8_t instance) const +{ + if (instance < AP_BATT_MONITOR_MAX_INSTANCES && _watt_max[instance] > 0) { + float power = _BattMonitor_STATE(instance).current_amps * _BattMonitor_STATE(instance).voltage; + return _BattMonitor_STATE(instance).healthy && (power > _watt_max[instance]); + } + return false; +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 29ac4a2fe0..1750a7e9ac 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -13,6 +13,7 @@ #define AP_BATT_CAPACITY_DEFAULT 3300 #define AP_BATT_LOW_VOLT_TIMEOUT_MS 10000 // low voltage of 10 seconds will cause battery_exhausted to return true +#define AP_BATT_MAX_WATT_DEFAULT 0 // declare backend class class AP_BattMonitor_Backend; @@ -107,6 +108,13 @@ public: /// set_monitoring - sets the monitor type (used for example sketch only) void set_monitoring(uint8_t instance, uint8_t mon) { _monitoring[instance].set(mon); } + bool get_watt_max() { return get_watt_max(AP_BATT_PRIMARY_INSTANCE); } + bool get_watt_max(uint8_t instance) { return _watt_max[instance]; } + + /// true when (voltage * current) > watt_max + bool overpower_detected() const; + bool overpower_detected(uint8_t instance) const; + static const struct AP_Param::GroupInfo var_info[]; protected: @@ -119,6 +127,7 @@ protected: AP_Float _curr_amp_per_volt[AP_BATT_MONITOR_MAX_INSTANCES]; /// voltage on current pin multiplied by this to calculate current in amps AP_Float _curr_amp_offset[AP_BATT_MONITOR_MAX_INSTANCES]; /// offset voltage that is subtracted from current pin before conversion to amps AP_Int32 _pack_capacity[AP_BATT_MONITOR_MAX_INSTANCES]; /// battery pack capacity less reserve in mAh + AP_Int16 _watt_max[AP_BATT_MONITOR_MAX_INSTANCES]; /// max battery power allowed. Reduce max throttle to reduce current to satisfy this limit private: BattMonitor_State state[AP_BATT_MONITOR_MAX_INSTANCES];