Line data    Source code

       1             : #include "SIM_BattMonitor_SMBus.h"
       2             : 
       3        6435 : SITL::SIM_BattMonitor_SMBus::SIM_BattMonitor_SMBus() :
       4        6435 :     SMBusDevice()
       5             : {
       6        6435 :     add_register("Temperature", SMBusBattDevReg::TEMP, SITL::I2CRegisters::RegMode::RDONLY);
       7        6435 :     add_register("Voltage", SMBusBattDevReg::VOLTAGE, SITL::I2CRegisters::RegMode::RDONLY);
       8        6435 :     add_register("Current", SMBusBattDevReg::CURRENT, SITL::I2CRegisters::RegMode::RDONLY);
       9        6435 :     add_register("Remaining Capacity", SMBusBattDevReg::REMAINING_CAPACITY, SITL::I2CRegisters::RegMode::RDONLY);
      10        6435 :     add_register("Full Charge Capacity", SMBusBattDevReg::FULL_CHARGE_CAPACITY, SITL::I2CRegisters::RegMode::RDONLY);
      11        6435 :     add_register("Cycle_Count", SMBusBattDevReg::CYCLE_COUNT, SITL::I2CRegisters::RegMode::RDONLY);
      12        6435 :     add_register("Design Charge Capacity", SMBusBattDevReg::DESIGN_CAPACITY, SITL::I2CRegisters::RegMode::RDONLY);
      13        6435 :     add_register("Design Maximum Voltage", SMBusBattDevReg::DESIGN_VOLTAGE, SITL::I2CRegisters::RegMode::RDONLY);
      14        6435 :     add_register("Specification Info", SMBusBattDevReg::SPECIFICATION_INFO, SITL::I2CRegisters::RegMode::RDONLY);
      15        6435 :     add_register("Manufacture Date", SMBusBattDevReg::MANUFACTURE_DATE, SITL::I2CRegisters::RegMode::RDONLY);
      16        6435 :     add_register("Serial", SMBusBattDevReg::SERIAL, SITL::I2CRegisters::RegMode::RDONLY);
      17        6435 :     add_block("Manufacture Name", SMBusBattDevReg::MANUFACTURE_NAME, SITL::I2CRegisters::RegMode::RDONLY);
      18        6435 :     add_block("Device Name", SMBusBattDevReg::DEVICE_NAME, SITL::I2CRegisters::RegMode::RDONLY);
      19        6435 :     add_block("Device Chemistry", SMBusBattDevReg::DEVICE_CHEMISTRY, SITL::I2CRegisters::RegMode::RDONLY);
      20        6435 :     add_register("Manufacture Data", SMBusBattDevReg::MANUFACTURE_DATA, SITL::I2CRegisters::RegMode::RDONLY);
      21             : 
      22        6435 :     set_register(SMBusBattDevReg::TEMP, (int16_t)((C_TO_KELVIN(15))*10));
      23             :     // see update for voltage
      24             :     // see update for current
      25             : 
      26        6435 :     set_register(SMBusBattDevReg::DESIGN_VOLTAGE, (uint16_t(50400U)));          // (mV) Design maximum voltage
      27             : 
      28             :     // TODO: remaining capacity connect to sim capacity add to update method below?
      29             :     // TODO: Battery mode bit set to mAh vs 10 mWh
      30        6435 :     set_register(SMBusBattDevReg::REMAINING_CAPACITY, (uint16_t(42042U)));      // (mAh) Remaining Capacity
      31             : 
      32             :     // TODO: full capacity fill via SIM parameter
      33        6435 :     set_register(SMBusBattDevReg::FULL_CHARGE_CAPACITY, (uint16_t(45000U)));    // (mAh) Full charge capacity
      34        6435 :     set_register(SMBusBattDevReg::DESIGN_CAPACITY, (uint16_t(52500U)));         // (mAh) Design capacity
      35             : 
      36        6435 :     set_register(SMBusBattDevReg::CYCLE_COUNT, (uint16_t(42U)));
      37             : 
      38             :     // Set SPECIFICATION_INFO
      39             :     union {
      40             :         struct {
      41             :             uint8_t revision : 4;
      42             :             uint8_t version: 4;
      43             :             uint8_t vscale: 4;
      44             :             uint8_t ipscale: 4;
      45             :         } fields;
      46             :         uint16_t word;
      47             :     } specinfo;
      48             : 
      49             :     // TODO: compute PEC for relevant monitors
      50        6435 :     specinfo.fields.revision = 0b0001;  // version 1
      51             : //    specinfo.fields.version = 0b0011;  // 1.1 with PEC; TODO!
      52        6435 :     specinfo.fields.version = 0b0001;  // 1.0
      53        6435 :     specinfo.fields.vscale = 0b0000;  // unknown...
      54        6435 :     specinfo.fields.ipscale = 0b0000;  // unknown...
      55        6435 :     set_register(SMBusBattDevReg::SPECIFICATION_INFO, specinfo.word);
      56             : 
      57        6435 :     set_register(SMBusBattDevReg::SERIAL, (uint16_t)12345);
      58             : 
      59        6435 :     const char *manufacturer_name = "ArduPilot";
      60        6435 :     set_block(SMBusBattDevReg::MANUFACTURE_NAME, manufacturer_name);
      61             : 
      62        6435 :     const char *device_name = "SITLBatMon_V0.99";
      63        6435 :     set_block(SMBusBattDevReg::DEVICE_NAME, device_name);
      64             : 
      65        6435 :     const char *device_chemistry = "LION";
      66        6435 :     set_block(SMBusBattDevReg::DEVICE_CHEMISTRY, device_chemistry);
      67             : 
      68             :     // Set Manufacture date to 2021 APR 24th
      69        6435 :     const uint16_t manufacturer_date = ((2021 - 1980) << 9) + (04 << 5) + 24;
      70        6435 :     set_register(SMBusBattDevReg::MANUFACTURE_DATE, manufacturer_date);
      71             : 
      72             :     // TODO: manufacturer data
      73        6435 : }
      74             : 
      75   536279826 : void SITL::SIM_BattMonitor_SMBus::update(const class Aircraft &aircraft)
      76             : {
      77   536279826 :     const uint32_t now = AP_HAL::millis();
      78   536279826 :     if (now - last_update_ms > 100) {
      79     4419849 :         const float millivolts = AP::sitl()->state.battery_voltage * 1000.0f;
      80     4419849 :         set_register(SMBusBattDevReg::VOLTAGE, uint16_t(millivolts));
      81             :         // FIXME: is this REALLY what the hardware will do?
      82     4419849 :         const int16_t current = constrain_int32(AP::sitl()->state.battery_current*-1000, INT16_MIN, INT16_MAX);
      83     4419849 :         set_register(SMBusBattDevReg::CURRENT, current);
      84     4419849 :         last_update_ms = now;
      85             :     }
      86   536279826 : }