diff --git a/libraries/SITL/SIM_BattMonitor_SMBus_Generic.cpp b/libraries/SITL/SIM_BattMonitor_SMBus_Generic.cpp index 459fc379ad..b343d895b5 100644 --- a/libraries/SITL/SIM_BattMonitor_SMBus_Generic.cpp +++ b/libraries/SITL/SIM_BattMonitor_SMBus_Generic.cpp @@ -8,6 +8,12 @@ SITL::SIM_BattMonitor_SMBus_Generic::SIM_BattMonitor_SMBus_Generic() : void SITL::SIM_BattMonitor_SMBus_Generic::init() { switch (cellcount()) { + case 14: + add_register("Cell14", SMBusBattGenericDevReg::CELL14, O_RDONLY); + FALLTHROUGH; + case 13: + add_register("Cell13", SMBusBattGenericDevReg::CELL13, O_RDONLY); + FALLTHROUGH; case 12: add_register("Cell12", SMBusBattGenericDevReg::CELL12, O_RDONLY); FALLTHROUGH; @@ -54,8 +60,59 @@ 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)); + const float millivolts = aircraft.get_battery_voltage() * 1000.0f; + uint16_t value_even = -1; + uint16_t value_odd = -1; + const uint8_t _connected_cells = connected_cells(); + if (millivolts > 0) { + const float volts_per_cell = millivolts/float(_connected_cells); + value_even = uint16_t(volts_per_cell - 100.0f); + value_odd = uint16_t(volts_per_cell + 100.0f); + } + switch (_connected_cells) { + case 14: + set_register(SMBusBattGenericDevReg::CELL14, value_even); + FALLTHROUGH; + case 13: + set_register(SMBusBattGenericDevReg::CELL13, value_odd); + FALLTHROUGH; + case 12: + set_register(SMBusBattGenericDevReg::CELL12, value_even); + FALLTHROUGH; + case 11: + set_register(SMBusBattGenericDevReg::CELL11, value_odd); + FALLTHROUGH; + case 10: + set_register(SMBusBattGenericDevReg::CELL10, value_even); + FALLTHROUGH; + case 9: + set_register(SMBusBattGenericDevReg::CELL9, value_odd); + FALLTHROUGH; + case 8: + set_register(SMBusBattGenericDevReg::CELL8, value_even); + FALLTHROUGH; + case 7: + set_register(SMBusBattGenericDevReg::CELL7, value_odd); + FALLTHROUGH; + case 6: + set_register(SMBusBattGenericDevReg::CELL6, value_even); + FALLTHROUGH; + case 5: + set_register(SMBusBattGenericDevReg::CELL5, value_odd); + FALLTHROUGH; + case 4: + set_register(SMBusBattGenericDevReg::CELL4, value_even); + FALLTHROUGH; + case 3: + set_register(SMBusBattGenericDevReg::CELL3, value_odd); + FALLTHROUGH; + case 2: + set_register(SMBusBattGenericDevReg::CELL2, value_even); + FALLTHROUGH; + case 1: + set_register(SMBusBattGenericDevReg::CELL1, value_odd); + return; + default: + AP_HAL::panic("Bad connected_cellcount %u", _connected_cells); + } } diff --git a/libraries/SITL/SIM_BattMonitor_SMBus_Generic.h b/libraries/SITL/SIM_BattMonitor_SMBus_Generic.h index e24ae5466d..5260568a15 100644 --- a/libraries/SITL/SIM_BattMonitor_SMBus_Generic.h +++ b/libraries/SITL/SIM_BattMonitor_SMBus_Generic.h @@ -18,6 +18,8 @@ public: static const uint8_t CELL10 = 0x36; static const uint8_t CELL11 = 0x35; static const uint8_t CELL12 = 0x34; + static const uint8_t CELL13 = 0x33; + static const uint8_t CELL14 = 0x32; }; class SIM_BattMonitor_SMBus_Generic : public SIM_BattMonitor_SMBus @@ -30,6 +32,7 @@ public: virtual uint8_t cellcount() const = 0; + virtual uint8_t connected_cells() const { return 3; } }; } // namespace SITL diff --git a/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h b/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h index 9be6d74a8f..5652b48cbd 100644 --- a/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h +++ b/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h @@ -19,7 +19,8 @@ public: Maxell(); - uint8_t cellcount() const override { return 3; } + uint8_t cellcount() const override { return 14; } + uint8_t connected_cells() const override { return 14; } }; diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index 6a957be258..5322a4e7a0 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/libraries/SITL/SIM_I2C.cpp @@ -70,6 +70,7 @@ struct i2c_device_at_address { { 1, 0x76, ignored }, // MS56XX { 1, 0x77, tsys01 }, { 1, 0x0B, rotoye }, + { 2, 0x0B, maxell }, { 2, 0x28, airspeed_dlvr }, };