From a444004a8a31c8ad8554102907451445e9edbefc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 9 Mar 2024 13:04:00 +1100 Subject: [PATCH] AP_BattMonitor: added MAX_AMPS and SHUNT parameters for INA239 allows for a wider range of current --- .../AP_BattMonitor/AP_BattMonitor_INA239.cpp | 27 +++++++++++++++++-- .../AP_BattMonitor/AP_BattMonitor_INA239.h | 5 ++++ 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp index 94676c02c1..d59a5f2dbb 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp @@ -30,11 +30,34 @@ extern const AP_HAL::HAL& hal; #define HAL_BATTMON_INA239_MAX_CURRENT 90 #endif +const AP_Param::GroupInfo AP_BattMonitor_INA239::var_info[] = { + + // @Param: MAX_AMPS + // @DisplayName: Battery monitor max current + // @Description: This controls the maximum current the INA239 sensor will work with. + // @Range: 1 400 + // @Units: A + // @User: Advanced + AP_GROUPINFO("MAX_AMPS", 27, AP_BattMonitor_INA239, max_amps, HAL_BATTMON_INA239_MAX_CURRENT), + + // @Param: SHUNT + // @DisplayName: Battery monitor shunt resistor + // @Description: This sets the shunt resistor used in the device + // @Range: 0.0001 0.01 + // @Units: Ohm + // @User: Advanced + AP_GROUPINFO("SHUNT", 28, AP_BattMonitor_INA239, rShunt, HAL_BATTMON_INA239_SHUNT_RESISTANCE), + + AP_GROUPEND +}; + AP_BattMonitor_INA239::AP_BattMonitor_INA239(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms) : AP_BattMonitor_Backend(mon, mon_state, params) { + AP_Param::setup_object_defaults(this, var_info); + _state.var_info = var_info; } void AP_BattMonitor_INA239::init(void) @@ -61,8 +84,8 @@ void AP_BattMonitor_INA239::configure(void) } voltage_LSB = 3.125e-3; - current_LSB = float(HAL_BATTMON_INA239_MAX_CURRENT) / 0x8000; - const int16_t shunt_cal = 819.2 * 1e6 * current_LSB * HAL_BATTMON_INA239_SHUNT_RESISTANCE; + current_LSB = max_amps.get() / 0x8000; + const int16_t shunt_cal = 819.2 * 1e6 * current_LSB * rShunt.get(); int16_t shunt_cal2 = 0; if (!write_word(REG_SHUNT_CAL, shunt_cal) || diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA239.h b/libraries/AP_BattMonitor/AP_BattMonitor_INA239.h index 3f5f87ee21..0875709019 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA239.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA239.h @@ -23,6 +23,8 @@ public: void init(void) override; void read() override; + static const struct AP_Param::GroupInfo var_info[]; + protected: AP_HAL::OwnPtr dev; @@ -44,6 +46,9 @@ protected: } accumulate; float current_LSB; float voltage_LSB; + + AP_Float max_amps; + AP_Float rShunt; }; #endif // AP_BATTERY_INA239_ENABLED