diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp index cab13b302f..94be30abc7 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp @@ -31,8 +31,10 @@ extern const AP_HAL::HAL& hal; #define REG_228_CURRENT 0x07 #define REG_228_MANUFACT_ID 0x3e #define REG_228_DEVICE_ID 0x3f +#define REG_228_DIETEMP 0x06 +#define INA_228_TEMP_C_LSB 7.8125e-3 -// INA238 specific registers +// INA237/INA238 specific registers #define REG_238_CONFIG 0x00 #define REG_238_CONFIG_RESET 0x8000 #define REG_238_ADC_CONFIG 0x01 @@ -41,6 +43,8 @@ extern const AP_HAL::HAL& hal; #define REG_238_CURRENT 0x07 #define REG_238_MANUFACT_ID 0x3e #define REG_238_DEVICE_ID 0x3f +#define REG_238_DIETEMP 0x06 +#define INA_238_TEMP_C_LSB 7.8125e-3 // need to mask bottom 4 bits #ifndef DEFAULT_BATTMON_INA2XX_MAX_AMPS #define DEFAULT_BATTMON_INA2XX_MAX_AMPS 90.0 @@ -262,10 +266,12 @@ bool AP_BattMonitor_INA2XX::detect_device(void) if (read_word16(REG_228_MANUFACT_ID, id) && id == 0x5449 && read_word16(REG_228_DEVICE_ID, id) && (id&0xFFF0) == 0x2280) { + has_temp = true; return configure(DevType::INA228); } if (read_word16(REG_238_MANUFACT_ID, id) && id == 0x5449 && read_word16(REG_238_DEVICE_ID, id) && (id&0xFFF0) == 0x2380) { + has_temp = true; return configure(DevType::INA238); } if (read_word16(REG_226_MANUFACT_ID, id) && id == 0x5449 && @@ -311,8 +317,10 @@ void AP_BattMonitor_INA2XX::timer(void) case DevType::INA228: { int32_t bus_voltage24, current24; + int16_t temp16; if (!read_word24(REG_228_VBUS, bus_voltage24) || - !read_word24(REG_228_CURRENT, current24)) { + !read_word24(REG_228_CURRENT, current24) || + !read_word16(REG_228_DIETEMP, temp16)) { failed_reads++; if (failed_reads > 10) { // device has disconnected, we need to reconfigure it @@ -322,13 +330,15 @@ void AP_BattMonitor_INA2XX::timer(void) } voltage = (bus_voltage24>>4) * voltage_LSB; current = (current24>>4) * current_LSB; + temperature = temp16 * INA_228_TEMP_C_LSB; break; } case DevType::INA238: { - int16_t bus_voltage16, current16; + int16_t bus_voltage16, current16, temp16; if (!read_word16(REG_238_VBUS, bus_voltage16) || - !read_word16(REG_238_CURRENT, current16)) { + !read_word16(REG_238_CURRENT, current16) || + !read_word16(REG_238_DIETEMP, temp16)) { failed_reads++; if (failed_reads > 10) { // device has disconnected, we need to reconfigure it @@ -338,6 +348,7 @@ void AP_BattMonitor_INA2XX::timer(void) } voltage = bus_voltage16 * voltage_LSB; current = current16 * current_LSB; + temperature = (temp16&0xFFF0) * INA_238_TEMP_C_LSB; break; } } @@ -350,4 +361,13 @@ void AP_BattMonitor_INA2XX::timer(void) accumulate.count++; } +/* + get last temperature + */ +bool AP_BattMonitor_INA2XX::get_temperature(float &temp) const +{ + temp = temperature; + return has_temp; +} + #endif // AP_BATTERY_INA2XX_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h index d49e7c8274..b23b4420ea 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h @@ -17,9 +17,10 @@ public: AP_BattMonitor_Params ¶ms); bool has_cell_voltages() const override { return false; } - bool has_temperature() const override { return false; } + bool has_temperature() const override { return has_temp; } bool has_current() const override { return true; } bool get_cycle_count(uint16_t &cycles) const override { return false; } + bool get_temperature(float &temperature) const override; void init(void) override; void read() override; @@ -63,6 +64,10 @@ private: } accumulate; float current_LSB; float voltage_LSB; + + float temperature; + + bool has_temp; }; #endif // AP_BATTERY_INA2XX_ENABLED