Line data    Source code

       1             : #include "AP_BattMonitor_config.h"
       2             : 
       3             : #if AP_BATTERY_SMBUS_NEODESIGN_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : #include <AP_Common/AP_Common.h>
       7             : #include "AP_BattMonitor.h"
       8             : 
       9             : #include "AP_BattMonitor_SMBus_NeoDesign.h"
      10             : 
      11             : #define BATTMONITOR_ND_CELL_COUNT                0x5C    // cell-count register
      12             : #define BATTMONITOR_ND_CELL_START                0x30    // first cell register
      13             : 
      14             : // Constructor
      15           0 : AP_BattMonitor_SMBus_NeoDesign::AP_BattMonitor_SMBus_NeoDesign(AP_BattMonitor &mon,
      16             :                                                    AP_BattMonitor::BattMonitor_State &mon_state,
      17           0 :                                                    AP_BattMonitor_Params &params)
      18           0 :     : AP_BattMonitor_SMBus(mon, mon_state, params, AP_BATTMONITOR_SMBUS_BUS_INTERNAL)
      19             : {
      20           0 :     _pec_supported = true;
      21           0 : }
      22             : 
      23           0 : void AP_BattMonitor_SMBus_NeoDesign::timer()
      24             : {
      25             :     uint16_t data;
      26             :     // Get the cell count once, it's not likely to change in flight
      27           0 :     if (_cell_count == 0) {
      28           0 :         if (!read_word(BATTMONITOR_ND_CELL_COUNT, data)) {
      29           0 :             return; // something wrong, don't try anything else
      30             :         }
      31             :         // constrain maximum cellcount in case of i2c corruption
      32           0 :         if (data > max_cell_count) {
      33           0 :             _cell_count = max_cell_count;
      34             :         } else {
      35           0 :             _cell_count = data;
      36             :         }
      37             :     }
      38             : 
      39           0 :     bool read_all_cells = true;
      40           0 :     for(uint8_t i = 0; i < _cell_count; ++i) {
      41           0 :         if(read_word(BATTMONITOR_ND_CELL_START + i, data)) {
      42           0 :             _state.cell_voltages.cells[i] = data;
      43           0 :             _has_cell_voltages = true;
      44             :         } else {
      45           0 :             read_all_cells = false;
      46             :         }
      47             :     }
      48             : 
      49           0 :     const uint32_t tnow = AP_HAL::micros();
      50             : 
      51           0 :     if (read_all_cells && (_cell_count > 0)) {
      52           0 :         uint32_t summed = 0;
      53           0 :         for (int i = 0; i < _cell_count; i++) {
      54           0 :             summed += _state.cell_voltages.cells[i];
      55             :         }
      56           0 :         _state.voltage = (float)summed * 1e-3f;
      57           0 :         _state.last_time_micros = tnow;
      58           0 :         _state.healthy = true;
      59           0 :     } else if (read_word(BATTMONITOR_SMBUS_VOLTAGE, data)) {
      60             :             // fallback to the voltage register if we didn't manage to poll the cells
      61           0 :             _state.voltage = (float)data * 1e-3f;
      62           0 :             _state.last_time_micros = tnow;
      63           0 :             _state.healthy = true;
      64             :     }
      65             : 
      66             :     // timeout after 5 seconds
      67           0 :     if ((tnow - _state.last_time_micros) > AP_BATTMONITOR_SMBUS_TIMEOUT_MICROS) {
      68           0 :         _state.healthy = false;
      69             :         // do not attempt to read any more data from battery
      70           0 :         return;
      71             :     }
      72             : 
      73             :     // read current (A)
      74           0 :     if (read_word(BATTMONITOR_SMBUS_CURRENT, data)) {
      75           0 :         _state.current_amps = -(float)((int16_t)data) * 1e-3f;
      76           0 :         _state.last_time_micros = tnow;
      77             :     }
      78             : 
      79           0 :     read_full_charge_capacity();
      80           0 :     read_remaining_capacity();
      81           0 :     read_temp();
      82             : }
      83             : 
      84             : #endif  // AP_BATTERY_SMBUS_NEODESIGN_ENABLED