From 4cfbeb11e328d89d6dee6d7a96d0318a5d7b970b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 21 Nov 2024 23:06:35 -0600 Subject: [PATCH] SITL: add support for ina3221 triple-channel current/voltage sensor --- libraries/SITL/SIM_I2C.cpp | 7 ++ libraries/SITL/SIM_INA3221.cpp | 143 +++++++++++++++++++++++++++++++++ libraries/SITL/SIM_INA3221.h | 89 ++++++++++++++++++++ libraries/SITL/SIM_config.h | 3 + 4 files changed, 242 insertions(+) create mode 100644 libraries/SITL/SIM_INA3221.cpp create mode 100644 libraries/SITL/SIM_INA3221.h diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index 196301942d..8d1b1daf0a 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/libraries/SITL/SIM_I2C.cpp @@ -37,6 +37,7 @@ #include "SIM_MS5525.h" #include "SIM_MS5611.h" #include "SIM_QMC5883L.h" +#include "SIM_INA3221.h" #include @@ -85,6 +86,9 @@ static IS31FL3195 is31fl3195; #if AP_SIM_COMPASS_QMC5883L_ENABLED static QMC5883L qmc5883l; #endif +#if AP_SIM_INA3221_ENABLED +static INA3221 ina3221; +#endif struct i2c_device_at_address { uint8_t bus; @@ -102,6 +106,9 @@ struct i2c_device_at_address { { 1, 0x39, ignored }, // NCP5623C { 1, 0x40, ignored }, // KellerLD { 1, 0x76, ms5525 }, // MS5525: ARSPD_TYPE = 4 +#if AP_SIM_INA3221_ENABLED + { 1, 0x42, ina3221 }, +#endif { 1, 0x77, tsys01 }, { 1, 0x0B, rotoye }, // Rotoye: BATTx_MONITOR 19, BATTx_I2C_ADDR 13 { 2, 0x0B, maxell }, // Maxell: BATTx_MONITOR 16, BATTx_I2C_ADDR 13 diff --git a/libraries/SITL/SIM_INA3221.cpp b/libraries/SITL/SIM_INA3221.cpp new file mode 100644 index 0000000000..3a12d36342 --- /dev/null +++ b/libraries/SITL/SIM_INA3221.cpp @@ -0,0 +1,143 @@ +#include "SIM_config.h" + +#if AP_SIM_INA3221_ENABLED + +#include "SIM_INA3221.h" + +#include + +SITL::INA3221::INA3221() +{ + writable_registers.set(0); + writable_registers.set(7); + writable_registers.set(8); + writable_registers.set(9); + writable_registers.set(10); + writable_registers.set(11); + writable_registers.set(12); + writable_registers.set(14); + writable_registers.set(15); + writable_registers.set(16); + writable_registers.set(254); + writable_registers.set(255); + + reset(); +} + +void SITL::INA3221::reset() +{ + // from page 24 of datasheet: + registers.byname.configuration.word = 0x7127; + registers.byname.Channel_1_Shunt_Voltage = 0x0; + registers.byname.Channel_1_Bus_Voltage = 0x0; + registers.byname.Channel_2_Shunt_Voltage = 0x0; + registers.byname.Channel_2_Bus_Voltage = 0x0; + registers.byname.Channel_3_Shunt_Voltage = 0x0; + registers.byname.Channel_3_Bus_Voltage = 0x0; + registers.byname.Channel_1_CriticalAlertLimit = 0x7FF8; + registers.byname.Channel_1_WarningAlertLimit = 0x7FF8; + registers.byname.Channel_2_CriticalAlertLimit = 0x7FF8; + registers.byname.Channel_2_WarningAlertLimit = 0x7FF8; + registers.byname.Channel_3_CriticalAlertLimit = 0x7FF8; + registers.byname.Channel_3_WarningAlertLimit = 0x7FF8; + registers.byname.Shunt_VoltageSum = 0x0; + registers.byname.Shunt_VoltageSumLimit = 0x7FFE; + registers.byname.Mask_Enable = 0x0002; + registers.byname.Power_ValidUpperLimit = 0x2710; + registers.byname.Power_ValidLowerLimit = 0x2328; + registers.byname.ManufacturerID = MANUFACTURER_ID; + registers.byname.Die_ID = DIE_ID; +} + +int SITL::INA3221::rdwr(I2C::i2c_rdwr_ioctl_data *&data) +{ + if (data->nmsgs == 2) { + 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_addr = data->msgs[0].buf[0]; + const uint16_t register_value = registers.word[reg_addr]; + data->msgs[1].buf[0] = register_value >> 8; + data->msgs[1].buf[1] = register_value & 0xFF; + data->msgs[1].len = 2; + return 0; + } + + if (data->nmsgs == 1) { + 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]; + registers.word[reg_addr] = register_value; + return 0; + } + + return -1; +}; + +static uint16_t convert_voltage(float voltage) { + return (voltage / 26) * 32768; +} + + +void SITL::INA3221::update(const class Aircraft &aircraft) +{ + if (registers.byname.configuration.bits.reset != 0) { + reset(); + } + + // update readings + if (registers.byname.configuration.bits.mode == 0b000 || + registers.byname.configuration.bits.mode == 0b100) { + // power-off + return; + } + + const bool update_shunt = registers.byname.configuration.bits.mode & 0b001; + const bool update_bus = registers.byname.configuration.bits.mode & 0b010; + if ((registers.byname.configuration.bits.mode & 0b100) == 0) { + // single-shot only + registers.byname.configuration.bits.mode &= ~0b011; + } + + // channel 2 gets the first simulated battery's voltage and current: + // see 8.6.6.2 on page 27 for the whole "40uV" thing + if (registers.byname.configuration.bits.ch1_enable) { + if (update_bus) { + float fakevoltage = 12.3; + registers.byname.Channel_1_Bus_Voltage = fakevoltage; // FIXME + } + if (update_shunt) { + float fakecurrent = 7.6; + registers.byname.Channel_1_Shunt_Voltage = fakecurrent/0.56; // FIXME + } + } + + if (registers.byname.configuration.bits.ch2_enable) { + if (update_shunt) { + registers.byname.Channel_2_Shunt_Voltage = AP::sitl()->state.battery_current/26 * 32768; // FIXME + } + if (update_bus) { + registers.byname.Channel_2_Bus_Voltage = AP::sitl()->state.battery_voltage/26 * 32768; // FIXME + } + } + + if (registers.byname.configuration.bits.ch3_enable) { + if (update_bus) { + registers.byname.Channel_3_Bus_Voltage = convert_voltage(3.1415); + } + if (update_shunt) { + registers.byname.Channel_3_Shunt_Voltage = 2.718/0.56; + } + } + +} + +#endif // AP_SIM_INA3221_ENABLED diff --git a/libraries/SITL/SIM_INA3221.h b/libraries/SITL/SIM_INA3221.h new file mode 100644 index 0000000000..93924c2421 --- /dev/null +++ b/libraries/SITL/SIM_INA3221.h @@ -0,0 +1,89 @@ +#include "SIM_config.h" + +#if AP_SIM_INA3221_ENABLED + +#include "SIM_I2CDevice.h" + +#include + +/* + +Testing: + +param set BATT_MONITOR 30 +reboot +param set BATT_I2C_ADDR 0x42 +param set BATT_I2C_BUS 1 +param set BATT_CHANNEL 2 +reboot + +*/ + +namespace SITL { + +class INA3221 : public I2CDevice +{ +public: + + INA3221(); + + int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override; + void update(const class Aircraft &aircraft) override; + +private: + + static const uint16_t MANUFACTURER_ID = 0b0101010001001001; // from datasheet p25 + static const uint16_t DIE_ID = 0b0011001000100000; // from datasheet p25 + + // All 16-bit INA3221 registers are two 8-bit bytes via the I2C + // interface. Table 4 shows a register map for the INA3221. + union Registers { + uint16_t word[256]; + struct PACKED ByName { + union { + uint16_t word; + struct PACKED { + uint16_t mode : 3; + uint16_t shunt_voltage_conversiontime : 3; + uint16_t bus_voltage_conversiontime : 3; + uint16_t averaging_mode : 3; + uint16_t ch1_enable : 1; + uint16_t ch2_enable : 1; + uint16_t ch3_enable : 1; + uint16_t reset : 1; + } bits; + } configuration; + uint16_t Channel_1_Shunt_Voltage; + uint16_t Channel_1_Bus_Voltage; + uint16_t Channel_2_Shunt_Voltage; + uint16_t Channel_2_Bus_Voltage; + uint16_t Channel_3_Shunt_Voltage; + uint16_t Channel_3_Bus_Voltage; + uint16_t Channel_1_CriticalAlertLimit; + uint16_t Channel_1_WarningAlertLimit; + uint16_t Channel_2_CriticalAlertLimit; + uint16_t Channel_2_WarningAlertLimit; + uint16_t Channel_3_CriticalAlertLimit; + uint16_t Channel_3_WarningAlertLimit; + uint16_t Shunt_VoltageSum; + uint16_t Shunt_VoltageSumLimit; + uint16_t Mask_Enable; + uint16_t Power_ValidUpperLimit; + uint16_t Power_ValidLowerLimit; + uint16_t unused[236]; + uint16_t ManufacturerID; + uint16_t Die_ID; + } byname; + } registers; + + // 256 2-byte registers: + assert_storage_size assert_storage_size_registers_reg; + + Bitmask<256> writable_registers; + + void reset(); +}; + +} // namespace SITL + +#endif // AP_SIM_INA3221_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 557c3077ed..d417577249 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -144,3 +144,6 @@ #define AP_SIM_GIMBAL_ENABLED (AP_SIM_SOLOGIMBAL_ENABLED) #endif +#ifndef AP_SIM_INA3221_ENABLED +#define AP_SIM_INA3221_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif