From a96772e46e35b150080da3c77e8fb725b567800e Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Thu, 13 Apr 2017 18:09:46 -0700 Subject: [PATCH] AP_BattMonitor: get Solo battery voltage from sum of cell voltages Minimizes the number of transactions on the bus, and reduces the amount of noise we have to consider --- .../AP_BattMonitor_SMBus_Solo.cpp | 26 +++++++++++-------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp index e0d43bbbb9..0b2dba6c2c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp @@ -11,7 +11,6 @@ extern const AP_HAL::HAL& hal; #include #define BATTMONITOR_SMBUS_SOLO_TEMP 0x08 // temperature register -#define BATTMONITOR_SMBUS_SOLO_VOLTAGE 0x09 // voltage register #define BATTMONITOR_SMBUS_SOLO_REMAINING_CAPACITY 0x0f // predicted remaining battery capacity in milliamps #define BATTMONITOR_SMBUS_SOLO_FULL_CHARGE_CAPACITY 0x10 // full capacity register #define BATTMONITOR_SMBUS_SOLO_MANUFACTURE_DATA 0x23 /// manufacturer data @@ -23,6 +22,7 @@ extern const AP_HAL::HAL& hal; /* * Other potentially useful registers, listed here for future use + * #define BATTMONITOR_SMBUS_SOLO_VOLTAGE 0x09 // voltage register * #define BATTMONITOR_SMBUS_SOLO_BATTERY_STATUS 0x16 // battery status register including alarms * #define BATTMONITOR_SMBUS_SOLO_DESIGN_CAPACITY 0x18 // design capacity register * #define BATTMONITOR_SMBUS_SOLO_DESIGN_VOLTAGE 0x19 // design voltage register @@ -55,9 +55,20 @@ void AP_BattMonitor_SMBus_Solo::timer() uint8_t buff[8]; uint32_t tnow = AP_HAL::micros(); - // read voltage - if (read_word(BATTMONITOR_SMBUS_SOLO_VOLTAGE, data)) { - _state.voltage = (float)data / 1000.0f; + + // read cell voltages + if (read_block(BATTMONITOR_SMBUS_SOLO_CELL_VOLTAGE, buff, 8, false)) { + float pack_voltage_mv = 0.0f; + for (uint8_t i = 0; i < BATTMONITOR_SMBUS_SOLO_NUM_CELLS; i++) { + uint16_t cell = buff[(i * 2) + 1] << 8 | buff[i * 2]; + _state.cell_voltages.cells[i] = cell; + pack_voltage_mv += (float)cell; + } + // accumulate the pack voltage out of the total of the cells + // because the Solo's I2C bus is so noisy, it's worth not spending the + // time and bus bandwidth to request the pack voltage as a seperate + // transaction + _state.voltage = pack_voltage_mv * 1e-3; _state.last_time_micros = tnow; _state.healthy = true; } @@ -116,13 +127,6 @@ void AP_BattMonitor_SMBus_Solo::timer() _state.temperature_time = AP_HAL::millis(); _state.temperature = ((float)(data - 2731)) * 0.1f; } - - // read cell voltages - if (read_block(BATTMONITOR_SMBUS_SOLO_CELL_VOLTAGE, buff, 8, false)) { - for (uint8_t i = 0; i < BATTMONITOR_SMBUS_SOLO_NUM_CELLS; i++) { - _state.cell_voltages.cells[i] = buff[(i * 2) + 1] << 8 | buff[i * 2]; - } - } } // read word from register