Line data    Source code

       1             : #include "AP_BattMonitor_config.h"
       2             : 
       3             : #if AP_BATTERY_SMBUS_GENERIC_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_Generic.h"
      11             : 
      12             : uint8_t smbus_cell_ids[] = { 0x3f,  // cell 1
      13             :                              0x3e,  // cell 2
      14             :                              0x3d,  // cell 3
      15             :                              0x3c,  // cell 4
      16             :                              0x3b,  // cell 5
      17             :                              0x3a,  // cell 6
      18             :                              0x39,  // cell 7
      19             :                              0x38,  // cell 8
      20             :                              0x37,  // cell 9
      21             :                              0x36,  // cell 10
      22             :                              0x35,  // cell 11
      23             :                              0x34,  // cell 12
      24             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
      25             :                              0x33,  // cell 13
      26             :                              0x32   // cell 14
      27             : #endif
      28             : };
      29             : 
      30             : #define SMBUS_CELL_COUNT_CHECK_TIMEOUT       15     // check cell count for up to 15 seconds
      31             : 
      32             : /*
      33             :  * Other potentially useful registers, listed here for future use
      34             :  * #define BATTMONITOR_SMBUS_MAXELL_CHARGE_STATUS         0x0d    // relative state of charge
      35             :  * #define BATTMONITOR_SMBUS_MAXELL_BATTERY_STATUS        0x16    // battery status register including alarms
      36             :  * #define BATTMONITOR_SMBUS_MAXELL_BATTERY_CYCLE_COUNT   0x17    // cycle count
      37             :  * #define BATTMONITOR_SMBUS_MAXELL_DESIGN_VOLTAGE        0x19    // design voltage register
      38             :  * #define BATTMONITOR_SMBUS_MAXELL_MANUFACTURE_DATE      0x1b    // manufacturer date
      39             :  * #define BATTMONITOR_SMBUS_MAXELL_SERIALNUM             0x1c    // serial number register
      40             :  * #define BATTMONITOR_SMBUS_MAXELL_HEALTH_STATUS         0x4f    // state of health
      41             :  * #define BATTMONITOR_SMBUS_MAXELL_SAFETY_ALERT          0x50    // safety alert
      42             :  * #define BATTMONITOR_SMBUS_MAXELL_SAFETY_STATUS         0x51    // safety status
      43             :  * #define BATTMONITOR_SMBUS_MAXELL_PF_ALERT              0x52    // safety status
      44             :  * #define BATTMONITOR_SMBUS_MAXELL_PF_STATUS             0x53    // safety status
      45             : */
      46             : 
      47             : // Constructor
      48           2 : AP_BattMonitor_SMBus_Generic::AP_BattMonitor_SMBus_Generic(AP_BattMonitor &mon,
      49             :                                                    AP_BattMonitor::BattMonitor_State &mon_state,
      50           2 :                                                    AP_BattMonitor_Params &params)
      51           2 :     : AP_BattMonitor_SMBus(mon, mon_state, params, AP_BATTMONITOR_SMBUS_BUS_EXTERNAL)
      52           2 : {}
      53             : 
      54         350 : void AP_BattMonitor_SMBus_Generic::timer()
      55             : {
      56             :         // check if PEC is supported
      57         350 :     if (!check_pec_support()) {
      58           0 :         return;
      59             :     }
      60             : 
      61             :     uint16_t data;
      62         350 :     uint32_t tnow = AP_HAL::micros();
      63             : 
      64             :     // read voltage (V)
      65         350 :     if (read_word(BATTMONITOR_SMBUS_VOLTAGE, data)) {
      66         350 :         _state.voltage = (float)data * 0.001f;
      67         350 :         _state.last_time_micros = tnow;
      68         350 :         _state.healthy = true;
      69             :     }
      70             : 
      71             :     // assert that BATTMONITOR_SMBUS_NUM_CELLS_MAX must be no more than smbus_cell_ids
      72             :     static_assert(BATTMONITOR_SMBUS_NUM_CELLS_MAX <= ARRAY_SIZE(smbus_cell_ids), "BATTMONITOR_SMBUS_NUM_CELLS_MAX must be no more than smbus_cell_ids");
      73             : 
      74             :     // check cell count
      75         350 :     if (!_cell_count_fixed) {
      76         187 :         if (_state.healthy) {
      77             :             // when battery first becomes healthy, start check of cell count
      78         187 :             if (_cell_count_check_start_us == 0) {
      79           2 :                 _cell_count_check_start_us = tnow;
      80             :             }
      81         187 :             if (tnow - _cell_count_check_start_us > (SMBUS_CELL_COUNT_CHECK_TIMEOUT * 1e6)) {
      82             :                 // give up checking cell count after 15sec of continuous healthy battery reads
      83           1 :                 _cell_count_fixed = true;
      84             :             }
      85             :         } else {
      86             :             // if battery becomes unhealthy restart cell count check
      87           0 :             _cell_count_check_start_us = 0;
      88             :         }
      89             :     }
      90             : 
      91             :     // we loop over something limited by
      92             :     // BATTMONITOR_SMBUS_NUM_CELLS_MAX but assign into something
      93             :     // limited by AP_BATT_MONITOR_CELLS_MAX - so make sure we won't
      94             :     // over-write:
      95             :     static_assert(BATTMONITOR_SMBUS_NUM_CELLS_MAX <= ARRAY_SIZE(_state.cell_voltages.cells), "BATTMONITOR_SMBUS_NUM_CELLS_MAX must be <= number of cells in state voltages");
      96             : 
      97             :     // read cell voltages
      98        5250 :     for (uint8_t i = 0; i < (_cell_count_fixed ? _cell_count : BATTMONITOR_SMBUS_NUM_CELLS_MAX); i++) {
      99        4900 :         if (read_word(smbus_cell_ids[i], data) && (data > 0) && (data < UINT16_MAX)) {
     100        4515 :             _has_cell_voltages = true;
     101        4515 :             _state.cell_voltages.cells[i] = data;
     102        4515 :             _last_cell_update_us[i] = tnow;
     103        4515 :             if (!_cell_count_fixed) {
     104        2219 :                 _cell_count = MAX(_cell_count, i + 1);
     105             :             }
     106         385 :         } else if ((tnow - _last_cell_update_us[i]) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) {
     107           0 :             _state.cell_voltages.cells[i] = UINT16_MAX;
     108             :         }
     109             :     }
     110             : 
     111             :     // timeout after 5 seconds
     112         350 :     if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) {
     113           0 :         _state.healthy = false;
     114           0 :         return;
     115             :     }
     116             : 
     117             :     // read current (A)
     118         350 :     if (read_word(BATTMONITOR_SMBUS_CURRENT, data)) {
     119         350 :         _state.current_amps = -(float)((int16_t)data) * 0.001f;
     120         350 :         _state.last_time_micros = tnow;
     121             :     }
     122             : 
     123         350 :     read_full_charge_capacity();
     124             : 
     125             :     // FIXME: Perform current integration if the remaining capacity can't be requested
     126         350 :     read_remaining_capacity();
     127             : 
     128         350 :     read_temp();
     129             : 
     130         350 :     read_serial_number();
     131             : 
     132         350 :     read_cycle_count();
     133             : }
     134             : 
     135             : // check if PEC supported with the version value in SpecificationInfo() function
     136             : // returns true once PEC is confirmed as working or not working
     137         350 : bool AP_BattMonitor_SMBus_Generic::check_pec_support()
     138             : {
     139             :     // exit immediately if we have already confirmed pec support
     140         350 :     if (_pec_confirmed) {
     141         348 :         return true;
     142             :     }
     143             : 
     144             :     // specification info
     145             :     uint16_t data;
     146           2 :     if (!read_word(BATTMONITOR_SMBUS_SPECIFICATION_INFO, data)) {
     147           0 :         return false;
     148             :     }
     149             : 
     150             :     // extract version
     151           2 :     uint8_t version = (data & 0xF0) >> 4;
     152             : 
     153             :     // version less than 0011b (i.e. 3) do not support PEC
     154           2 :     if (version < 3) {
     155           2 :         _pec_supported = false;
     156           2 :         _pec_confirmed = true;
     157           2 :         return true;
     158             :     }
     159             : 
     160             :     // check manufacturer name
     161           0 :     uint8_t buff[AP_BATTMONITOR_SMBUS_READ_BLOCK_MAXIMUM_TRANSFER + 1] {};
     162           0 :     if (read_block(BATTMONITOR_SMBUS_MANUFACTURE_NAME, buff, sizeof(buff))) {
     163             :         // Hitachi maxell batteries do not support PEC
     164           0 :         if (strcmp((char*)buff, "Hitachi maxell") == 0) {
     165           0 :             _pec_supported = false;
     166           0 :             _pec_confirmed = true;
     167           0 :             return true;
     168             :         }
     169             :     }
     170             : 
     171             :     // assume all other batteries support PEC
     172           0 :         _pec_supported = true;
     173           0 :         _pec_confirmed = true;
     174           0 :         return true;
     175             : }
     176             : 
     177             : #endif  // AP_BATTERY_SMBUS_GENERIC_ENABLED