Line data    Source code

       1             : #include "AP_BattMonitor_config.h"
       2             : 
       3             : #if AP_BATTERY_SMBUS_SUI_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : #include <AP_Common/AP_Common.h>
       7             : #include <AP_Math/AP_Math.h>
       8             : #include "AP_BattMonitor.h"
       9             : 
      10             : #include "AP_BattMonitor_SMBus_SUI.h"
      11             : 
      12             : extern const AP_HAL::HAL& hal;
      13             : 
      14             : #define REG_CELL_VOLTAGE        0x28
      15             : #define REG_CURRENT             0x2a
      16             : 
      17             : // maximum number of cells that we can read data for
      18             : #define SUI_MAX_CELL_READ       4
      19             : 
      20             : // Constructor
      21           0 : AP_BattMonitor_SMBus_SUI::AP_BattMonitor_SMBus_SUI(AP_BattMonitor &mon,
      22             :         AP_BattMonitor::BattMonitor_State &mon_state,
      23             :         AP_BattMonitor_Params &params,
      24           0 :         uint8_t _cell_count)
      25             :     : AP_BattMonitor_SMBus(mon, mon_state, params, AP_BATTMONITOR_SMBUS_BUS_INTERNAL),
      26           0 :       cell_count(_cell_count)
      27             : {
      28           0 :     _pec_supported = false;
      29           0 : }
      30             : 
      31           0 : void AP_BattMonitor_SMBus_SUI::init(void)
      32             : {
      33           0 :     AP_BattMonitor_SMBus::init();
      34           0 :     if (_dev) {
      35           0 :         _dev->set_retries(2);
      36             :     }
      37           0 :     if (_dev && timer_handle) {
      38             :         // run twice as fast for two phases
      39           0 :         _dev->adjust_periodic_callback(timer_handle, 50000);
      40             :     }
      41           0 : }
      42             : 
      43           0 : void AP_BattMonitor_SMBus_SUI::timer()
      44             : {
      45           0 :     uint32_t tnow = AP_HAL::micros();
      46             : 
      47             :     // we read in two phases as the device can stall if you read
      48             :     // current too rapidly after voltages
      49           0 :     phase_voltages = !phase_voltages;
      50             : 
      51           0 :     if (phase_voltages) {
      52           0 :         read_cell_voltages();
      53           0 :         update_health();
      54           0 :         return;
      55             :     }
      56             : 
      57             :     // read current
      58             :     int32_t current_ma;
      59           0 :     if (read_block_bare(REG_CURRENT, (uint8_t *)&current_ma, sizeof(current_ma))) {
      60           0 :         _state.current_amps = current_ma * -0.001;
      61           0 :         _state.last_time_micros = tnow;
      62             :     }
      63             : 
      64           0 :     read_full_charge_capacity();
      65             : 
      66           0 :     read_temp();
      67           0 :     read_serial_number();
      68           0 :     read_remaining_capacity();
      69           0 :     update_health();
      70             : }
      71             : 
      72             : // read_bare_block - returns true if successful
      73           0 : bool AP_BattMonitor_SMBus_SUI::read_block_bare(uint8_t reg, uint8_t* data, uint8_t len) const
      74             : {
      75             :     // read bytes
      76           0 :     if (!_dev->read_registers(reg, data, len)) {
      77           0 :         return false;
      78             :     }
      79             : 
      80             :     // return success
      81           0 :     return true;
      82             : }
      83             : 
      84           0 : void AP_BattMonitor_SMBus_SUI::read_cell_voltages()
      85             : {
      86             :     // read cell voltages
      87             :     uint16_t voltbuff[SUI_MAX_CELL_READ];
      88             : 
      89           0 :     if (!read_block(REG_CELL_VOLTAGE, (uint8_t *)voltbuff, sizeof(voltbuff))) {
      90           0 :         return;
      91             :     }
      92           0 :     float pack_voltage_mv = 0.0f;
      93             :         
      94           0 :     for (uint8_t i = 0; i < MIN(SUI_MAX_CELL_READ, cell_count); i++) {
      95           0 :         const uint16_t cell = voltbuff[i];
      96           0 :         _state.cell_voltages.cells[i] = cell;
      97           0 :         pack_voltage_mv += (float)cell;
      98             :     }
      99             : 
     100           0 :     if (cell_count >= SUI_MAX_CELL_READ) {
     101             :         // we can't read voltage of all cells. get overall pack voltage to work out
     102             :         // an average for remaining cells
     103             :         uint16_t total_mv;
     104           0 :         if (read_word(BATTMONITOR_SMBUS_VOLTAGE, total_mv)) {
     105             :             // if total voltage is below pack_voltage_mv then we will
     106             :             // read zero volts for the extra cells.
     107           0 :             total_mv = MAX(total_mv, pack_voltage_mv);
     108           0 :             const uint16_t cell_mv = (total_mv - pack_voltage_mv) / (cell_count - SUI_MAX_CELL_READ);
     109           0 :             for (uint8_t i = SUI_MAX_CELL_READ; i < cell_count; i++) {
     110           0 :                 _state.cell_voltages.cells[i] = cell_mv;
     111             :             }
     112           0 :             pack_voltage_mv = total_mv;
     113             :         } else {
     114             :             // we can't get total pack voltage. Use average of cells we have so far
     115           0 :             const uint16_t cell_mv = pack_voltage_mv / SUI_MAX_CELL_READ;
     116           0 :             for (uint8_t i = SUI_MAX_CELL_READ; i < cell_count; i++) {
     117           0 :                 _state.cell_voltages.cells[i] = cell_mv;
     118             :             }
     119           0 :             pack_voltage_mv += cell_mv * (cell_count - SUI_MAX_CELL_READ);
     120             :         }
     121             :     }
     122             : 
     123           0 :     _has_cell_voltages = true;
     124             : 
     125             :     // accumulate the pack voltage out of the total of the cells
     126           0 :     _state.voltage = pack_voltage_mv * 0.001;
     127           0 :     last_volt_read_us = AP_HAL::micros();
     128             : }
     129             : 
     130             : /*
     131             :   update healthy flag
     132             :  */
     133           0 : void AP_BattMonitor_SMBus_SUI::update_health()
     134             : {
     135           0 :     uint32_t now = AP_HAL::micros();
     136           0 :     _state.healthy = (now - last_volt_read_us < AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) &&
     137           0 :         (now - _state.last_time_micros < AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS);
     138           0 : }
     139             : 
     140             : #endif  // AP_BATTERY_SMBUS_SUI_ENABLED