From 602a9592cefb4812da76d4e39d716aa4b64ac334 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 21 Oct 2020 09:41:31 +1100 Subject: [PATCH] SITL: add Maxell SMBus battery support --- libraries/AP_HAL_SITL/SITL_State.cpp | 1 + libraries/SITL/SIM_BattMonitor_SMBus.cpp | 76 +++++++++++++++++++ libraries/SITL/SIM_BattMonitor_SMBus.h | 36 +++++++++ .../SITL/SIM_BattMonitor_SMBus_Generic.cpp | 61 +++++++++++++++ .../SITL/SIM_BattMonitor_SMBus_Generic.h | 33 ++++++++ .../SITL/SIM_BattMonitor_SMBus_Maxell.cpp | 7 ++ libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h | 26 +++++++ libraries/SITL/SIM_I2C.cpp | 10 +++ libraries/SITL/SIM_I2C.h | 2 + libraries/SITL/SIM_I2CDevice.cpp | 76 +++++++++++++++++++ libraries/SITL/SIM_I2CDevice.h | 32 ++++++++ 11 files changed, 360 insertions(+) create mode 100644 libraries/SITL/SIM_BattMonitor_SMBus.cpp create mode 100644 libraries/SITL/SIM_BattMonitor_SMBus.h create mode 100644 libraries/SITL/SIM_BattMonitor_SMBus_Generic.cpp create mode 100644 libraries/SITL/SIM_BattMonitor_SMBus_Generic.h create mode 100644 libraries/SITL/SIM_BattMonitor_SMBus_Maxell.cpp create mode 100644 libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h create mode 100644 libraries/SITL/SIM_I2CDevice.cpp diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index ac76d79ab1..2a500db007 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/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) { diff --git a/libraries/SITL/SIM_BattMonitor_SMBus.cpp b/libraries/SITL/SIM_BattMonitor_SMBus.cpp new file mode 100644 index 0000000000..4d827335b2 --- /dev/null +++ b/libraries/SITL/SIM_BattMonitor_SMBus.cpp @@ -0,0 +1,76 @@ +#include "SIM_BattMonitor_SMBus.h" + +#include + +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; + } +} diff --git a/libraries/SITL/SIM_BattMonitor_SMBus.h b/libraries/SITL/SIM_BattMonitor_SMBus.h new file mode 100644 index 0000000000..f4eb89ec28 --- /dev/null +++ b/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 diff --git a/libraries/SITL/SIM_BattMonitor_SMBus_Generic.cpp b/libraries/SITL/SIM_BattMonitor_SMBus_Generic.cpp new file mode 100644 index 0000000000..459fc379ad --- /dev/null +++ b/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)); +} diff --git a/libraries/SITL/SIM_BattMonitor_SMBus_Generic.h b/libraries/SITL/SIM_BattMonitor_SMBus_Generic.h new file mode 100644 index 0000000000..aecd12a885 --- /dev/null +++ b/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 diff --git a/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.cpp b/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.cpp new file mode 100644 index 0000000000..f71579100c --- /dev/null +++ b/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); +} diff --git a/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h b/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h new file mode 100644 index 0000000000..9be6d74a8f --- /dev/null +++ b/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h @@ -0,0 +1,26 @@ +#include "SIM_BattMonitor_SMBus_Generic.h" + +#include + +/* + +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 diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index d3130640fd..5068ae521a 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/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 @@ -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) { diff --git a/libraries/SITL/SIM_I2C.h b/libraries/SITL/SIM_I2C.h index 489558d441..e85994276f 100644 --- a/libraries/SITL/SIM_I2C.h +++ b/libraries/SITL/SIM_I2C.h @@ -29,6 +29,8 @@ class I2C { public: I2C() {} + void init(); + // update i2c state void update(const class Aircraft &aircraft); diff --git a/libraries/SITL/SIM_I2CDevice.cpp b/libraries/SITL/SIM_I2CDevice.cpp new file mode 100644 index 0000000000..7cac846c10 --- /dev/null +++ b/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; +}; diff --git a/libraries/SITL/SIM_I2CDevice.h b/libraries/SITL/SIM_I2CDevice.h index 6f80145f57..703212d58d 100644 --- a/libraries/SITL/SIM_I2CDevice.h +++ b/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;