SITL: add Maxell SMBus battery support

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -95,6 +95,7 @@ void SITL_State::_sitl_setup(const char *home_str)
         sitl_model->set_gripper_epm(&_sitl->gripper_epm_sim);
         sitl_model->set_parachute(&_sitl->parachute_sim);
         sitl_model->set_precland(&_sitl->precland_sim);
+        _sitl->i2c_sim.init();
         sitl_model->set_i2c(&_sitl->i2c_sim);
 
         if (_use_fg_view) {

libraries/SITL/SIM_BattMonitor_SMBus.cpp

@@ -0,0 +1,76 @@
+#include "SIM_BattMonitor_SMBus.h"
+
+#include <AP_Stats/AP_Stats.h>
+
+SITL::SIM_BattMonitor_SMBus::SIM_BattMonitor_SMBus() :
+    I2CRegisters_16Bit()
+{
+    add_register("Temperature", SMBusBattDevReg::TEMP, O_RDONLY);
+    add_register("Voltage", SMBusBattDevReg::VOLTAGE, O_RDONLY);
+    add_register("Current", SMBusBattDevReg::CURRENT, O_RDONLY);
+    add_register("Remaining Capacity", SMBusBattDevReg::REMAINING_CAPACITY, O_RDONLY);
+    add_register("Full Charge Capacity", SMBusBattDevReg::FULL_CHARGE_CAPACITY, O_RDONLY);
+    add_register("Cycle_Count", SMBusBattDevReg::CYCLE_COUNT, O_RDONLY);
+    add_register("Specification Info", SMBusBattDevReg::SPECIFICATION_INFO, O_RDONLY);
+    add_register("Serial", SMBusBattDevReg::SERIAL, O_RDONLY);
+    add_register("Manufacture Name", SMBusBattDevReg::MANUFACTURE_NAME, O_RDONLY);
+    add_register("Manufacture Data", SMBusBattDevReg::MANUFACTURE_DATA, O_RDONLY);
+
+     set_register(SMBusBattDevReg::TEMP, (int16_t)(15 + 273.15));
+     // see update for voltage
+     // see update for current
+     // TODO: remaining capacity
+     // TODO: full capacity
+     set_register(SMBusBattDevReg::CYCLE_COUNT, (uint16_t(39U)));
+
+     // Set SPECIFICATION_INFO
+     union {
+         struct {
+             uint8_t revision : 4;
+             uint8_t version: 4;
+             uint8_t vscale: 4;
+             uint8_t ipscale: 4;
+         } fields;
+         uint16_t word;
+     } specinfo;
+
+     specinfo.fields.revision = 0b0001;  // version 1
+//     specinfo.fields.version = 0b0011;  // 1.1 with PEC; TODO!
+     specinfo.fields.version = 0b0001;  // 1.0
+     specinfo.fields.vscale = 0b0000;  // unknown...
+     specinfo.fields.ipscale = 0b0000;  // unknown...
+     set_register(SMBusBattDevReg::SPECIFICATION_INFO, specinfo.word);
+
+     set_register(SMBusBattDevReg::SERIAL, (uint16_t)12345);
+
+     // Set MANUFACTURE_NAME
+     const char *manufacturer_name = "ArduPilot";
+     set_register(SMBusBattDevReg::MANUFACTURE_NAME, uint16_t(manufacturer_name[0]<<8|strlen(manufacturer_name)));
+     uint8_t i = 1;  // already sent the first byte out....
+     while (i < strlen(manufacturer_name)) {
+         const uint8_t a = manufacturer_name[i++];
+         uint8_t b = 0;
+         if (i < strlen(manufacturer_name)) {
+             b = manufacturer_name[i];
+         }
+         i++;
+         const uint16_t value = b<<8 | a;
+         add_register("Name", SMBusBattDevReg::MANUFACTURE_NAME + i/2, O_RDONLY);
+         set_register(SMBusBattDevReg::MANUFACTURE_NAME + i/2, value);
+     }
+     // TODO: manufacturer data
+}
+
+
+void SITL::SIM_BattMonitor_SMBus::update(const class Aircraft &aircraft)
+{
+    const uint32_t now = AP_HAL::millis();
+    if (now - last_update_ms > 100) {
+        const float millivolts = AP::sitl()->state.battery_voltage * 1000.0f;
+        set_register(SMBusBattDevReg::VOLTAGE, uint16_t(millivolts));
+        // FIXME: is this REALLY what the hardware will do?
+        const int16_t current = constrain_int32(AP::sitl()->state.battery_current*-1000, -32768, 32767);
+        set_register(SMBusBattDevReg::CURRENT, current);
+        last_update_ms = now;
+    }
+}

libraries/SITL/SIM_BattMonitor_SMBus.h

@@ -0,0 +1,36 @@
+#include "SIM_I2CDevice.h"
+
+namespace SITL {
+
+class SMBusBattDevReg : public I2CRegEnum {
+public:
+    static const uint8_t TEMP = 0x08;                 // Temperature
+    static const uint8_t VOLTAGE = 0x09;              // Voltage
+    static const uint8_t CURRENT = 0x0A;              // Current
+    static const uint8_t REMAINING_CAPACITY = 0x0F;   // Remaining Capacity
+    static const uint8_t FULL_CHARGE_CAPACITY = 0x10; // Full Charge Capacity
+    static const uint8_t CYCLE_COUNT = 0x17;          // Cycle Count
+    static const uint8_t SPECIFICATION_INFO = 0x1A;   // Specification Info
+    static const uint8_t SERIAL = 0x1C;               // Serial Number
+    static const uint8_t MANUFACTURE_NAME = 0x20;     // Manufacture Name
+    static const uint8_t MANUFACTURE_DATA = 0x23;     // Manufacture Data
+};
+
+class SIM_BattMonitor_SMBus : public I2CDevice, protected I2CRegisters_16Bit
+{
+public:
+
+    SIM_BattMonitor_SMBus();
+
+    virtual void update(const class Aircraft &aircraft) override;
+
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override {
+        return I2CRegisters_16Bit::rdwr(data);
+    }
+
+private:
+
+    uint32_t last_update_ms;
+};
+
+} // namespace SITL

libraries/SITL/SIM_BattMonitor_SMBus_Generic.cpp

@@ -0,0 +1,61 @@
+#include "SIM_BattMonitor_SMBus_Generic.h"
+
+SITL::SIM_BattMonitor_SMBus_Generic::SIM_BattMonitor_SMBus_Generic() :
+    SIM_BattMonitor_SMBus()
+{
+}
+
+void SITL::SIM_BattMonitor_SMBus_Generic::init()
+{
+    switch (cellcount()) {
+    case 12:
+        add_register("Cell12", SMBusBattGenericDevReg::CELL12, O_RDONLY);
+        FALLTHROUGH;
+    case 11:
+        add_register("Cell11", SMBusBattGenericDevReg::CELL11, O_RDONLY);
+        FALLTHROUGH;
+    case 10:
+        add_register("Cell10", SMBusBattGenericDevReg::CELL10, O_RDONLY);
+        FALLTHROUGH;
+    case 9:
+        add_register("Cell9", SMBusBattGenericDevReg::CELL9, O_RDONLY);
+        FALLTHROUGH;
+    case 8:
+        add_register("Cell8", SMBusBattGenericDevReg::CELL8, O_RDONLY);
+        FALLTHROUGH;
+    case 7:
+        add_register("Cell7", SMBusBattGenericDevReg::CELL7, O_RDONLY);
+        FALLTHROUGH;
+    case 6:
+        add_register("Cell6", SMBusBattGenericDevReg::CELL6, O_RDONLY);
+        FALLTHROUGH;
+    case 5:
+        add_register("Cell5", SMBusBattGenericDevReg::CELL5, O_RDONLY);
+        FALLTHROUGH;
+    case 4:
+        add_register("Cell4", SMBusBattGenericDevReg::CELL4, O_RDONLY);
+        FALLTHROUGH;
+    case 3:
+        add_register("Cell3", SMBusBattGenericDevReg::CELL3, O_RDONLY);
+        FALLTHROUGH;
+    case 2:
+        add_register("Cell2", SMBusBattGenericDevReg::CELL2, O_RDONLY);
+        FALLTHROUGH;
+    case 1:
+        add_register("Cell1", SMBusBattGenericDevReg::CELL1, O_RDONLY);
+        return;
+    default:
+        AP_HAL::panic("Bad cellcount %u", cellcount());
+    }
+}
+
+void SITL::SIM_BattMonitor_SMBus_Generic::update(const class Aircraft &aircraft)
+{
+    SIM_BattMonitor_SMBus::update(aircraft);
+
+    // pretend to have three cells connected
+    const float millivolts = AP::sitl()->state.battery_voltage * 1000.0f;
+    set_register(SMBusBattGenericDevReg::CELL1, uint16_t(millivolts/3.0f - 100.0f));
+    set_register(SMBusBattGenericDevReg::CELL2, uint16_t(millivolts/3.0f));
+    set_register(SMBusBattGenericDevReg::CELL3, uint16_t(millivolts/3.0f + 100.0f));
+}

libraries/SITL/SIM_BattMonitor_SMBus_Generic.h

@@ -0,0 +1,33 @@
+#include "SIM_BattMonitor_SMBus.h"
+
+namespace SITL {
+
+class SMBusBattGenericDevReg : public SMBusBattDevReg {
+public:
+    static const uint8_t CELL1 = 0x3f;
+    static const uint8_t CELL2 = 0x3e;
+    static const uint8_t CELL3 = 0x3d;
+    static const uint8_t CELL4 = 0x3c;
+    static const uint8_t CELL5 = 0x3b;
+    static const uint8_t CELL6 = 0x3a;
+    static const uint8_t CELL7 = 0x39;
+    static const uint8_t CELL8 = 0x38;
+    static const uint8_t CELL9 = 0x37;
+    static const uint8_t CELL10 = 0x36;
+    static const uint8_t CELL11 = 0x35;
+    static const uint8_t CELL12 = 0x34;
+};
+
+class SIM_BattMonitor_SMBus_Generic : public SIM_BattMonitor_SMBus
+{
+public:
+
+    SIM_BattMonitor_SMBus_Generic();
+    void init() override;
+    void update(const class Aircraft &aircraft) override;
+
+    virtual uint8_t cellcount() const = 0;
+
+};
+
+} // namespace SITL

libraries/SITL/SIM_BattMonitor_SMBus_Maxell.cpp

@@ -0,0 +1,7 @@
+#include "SIM_BattMonitor_SMBus_Maxell.h"
+
+SITL::Maxell::Maxell() :
+    SIM_BattMonitor_SMBus_Generic()
+{
+    set_register(SMBusBattGenericDevReg::SERIAL, (uint16_t)37);
+}

libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h

@@ -0,0 +1,26 @@
+#include "SIM_BattMonitor_SMBus_Generic.h"
+
+#include <AP_Common/Bitmask.h>
+
+/*
+
+Testing:
+
+param set BATT_MONITOR 16
+reboot
+
+*/
+
+namespace SITL {
+
+class Maxell : public SIM_BattMonitor_SMBus_Generic
+{
+public:
+
+    Maxell();
+
+    uint8_t cellcount() const override { return 3; }
+
+};
+
+} // namespace SITL

libraries/SITL/SIM_I2C.cpp

@@ -23,6 +23,7 @@
 #include "SIM_I2C.h"
 #include "SIM_ToshibaLED.h"
 #include "SIM_MaxSonarI2CXL.h"
+#include "SIM_BattMonitor_SMBus_Maxell.h"
 
 #include <signal.h>
 
@@ -43,6 +44,7 @@ static IgnoredI2CDevice ignored;
 
 static ToshibaLED toshibaled;
 static MaxSonarI2CXL maxsonari2cxl;
+static Maxell maxell;
 
 struct i2c_device_at_address {
     uint8_t bus;
@@ -55,8 +57,16 @@ struct i2c_device_at_address {
     { 1, 0x40, ignored }, // KellerLD
     { 1, 0x70, maxsonari2cxl },
     { 1, 0x76, ignored }, // MS56XX
+    { 2, 0x0B, maxell },
 };
 
+void I2C::init()
+{
+    for (auto &i : i2c_devices) {
+        i.device.init();
+    }
+}
+
 void I2C::update(const class Aircraft &aircraft)
 {
     for (auto daa : i2c_devices) {

libraries/SITL/SIM_I2C.h

@@ -29,6 +29,8 @@ class I2C {
 public:
     I2C() {}
 
+    void init();
+
     // update i2c state
     void update(const class Aircraft &aircraft);
 

libraries/SITL/SIM_I2CDevice.cpp

@@ -0,0 +1,76 @@
+#include "SIM_I2CDevice.h"
+
+void SITL::I2CRegisters::add_register(const char *name, uint8_t reg, int8_t mode)
+{
+    // ::fprintf(stderr, "Adding register %u (0x%02x) (%s)\n", reg, reg, name);
+    regname[reg] = name;
+    if (mode == O_RDONLY || mode == O_RDWR) {
+        readable_registers.set((uint8_t)reg);
+    }
+    if (mode == O_WRONLY || mode == O_RDWR) {
+        writable_registers.set((uint8_t)reg);
+    }
+}
+
+void SITL::I2CRegisters_16Bit::set_register(uint8_t reg, uint16_t value)
+{
+    if (regname[reg] == nullptr) {
+        AP_HAL::panic("Setting un-named register %u", reg);
+    }
+    // ::fprintf(stderr, "Setting %u (0x%02x) (%s) to 0x%02x (%c)\n", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value, value);
+    word[reg] = htobe16(value);
+}
+
+void SITL::I2CRegisters_16Bit::set_register(uint8_t reg, int16_t value)
+{
+    if (regname[reg] == nullptr) {
+        AP_HAL::panic("Setting un-named register %u", reg);
+    }
+    // ::fprintf(stderr, "Setting %s (%u) to 0x%02x (%c)\n", regname[reg], (unsigned)reg, (signed)value, value);
+    word[reg] = htobe16(value);
+}
+
+int SITL::I2CRegisters_16Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
+{
+    if (data->nmsgs == 2) {
+        // data read request
+        if (data->msgs[0].flags != 0) {
+            AP_HAL::panic("Unexpected flags");
+        }
+        if (data->msgs[1].flags != I2C_M_RD) {
+            AP_HAL::panic("Unexpected flags");
+        }
+        const uint8_t reg_base_addr = data->msgs[0].buf[0];
+        uint8_t bytes_copied = 0;
+        while (bytes_copied < data->msgs[1].len) {
+            const uint8_t reg_addr = reg_base_addr + bytes_copied/2;
+            if (!readable_registers.get(reg_addr)) {
+                // ::printf("Register 0x%02x is not readable!\n", reg_addr);
+                return -1;
+            }
+            const uint16_t register_value = word[reg_addr];
+            data->msgs[1].buf[bytes_copied++] = register_value >> 8;
+            if (bytes_copied < data->msgs[1].len) {
+                data->msgs[1].buf[bytes_copied++] = register_value & 0xFF;
+            }
+        }
+        data->msgs[1].len = bytes_copied;
+        return 0;
+    }
+
+    if (data->nmsgs == 1) {
+        // data write request
+        if (data->msgs[0].flags != 0) {
+            AP_HAL::panic("Unexpected flags");
+        }
+        const uint8_t reg_addr = data->msgs[0].buf[0];
+        if (!writable_registers.get(reg_addr)) {
+            AP_HAL::panic("Register 0x%02x is not writable!", reg_addr);
+        }
+        const uint16_t register_value = data->msgs[0].buf[2] << 8 | data->msgs[0].buf[1];
+        word[reg_addr] = register_value;
+        return 0;
+    }
+
+    return -1;
+};

libraries/SITL/SIM_I2CDevice.h

@@ -6,8 +6,40 @@
 
 namespace SITL {
 
+class I2CRegEnum {
+    // a class to hold register addresses as an enumeration
+};
+
+class I2CRegisters {
+
+protected:
+
+    virtual int rdwr(I2C::i2c_rdwr_ioctl_data *&data) = 0;
+
+    void add_register(const char *name, uint8_t reg, int8_t mode);
+
+    const char *regname[256];
+    Bitmask<256> writable_registers;
+    Bitmask<256> readable_registers;
+
+};
+
+class I2CRegisters_16Bit : public I2CRegisters {
+public:
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
+    void set_register(uint8_t reg, uint16_t value);
+    void set_register(uint8_t reg, int16_t value);
+
+protected:
+
+    uint16_t word[256];
+};
+
+
 class I2CDevice {
 public:
+    virtual void init() {}
+
     virtual void update(const class Aircraft &aircraft) { }
 
     virtual int rdwr(I2C::i2c_rdwr_ioctl_data *&data) = 0;