Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: add support for ina3221 triple-channel current/voltage sensor

This commit is contained in:
Peter Barker 2024-11-21 23:06:35 -06:00 committed by Thomas Watson
parent 5bc3b629f4
commit 4cfbeb11e3
4 changed files with 242 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/SITL/SIM_I2C.cpp
View File

@ -37,6 +37,7 @@
#include "SIM_MS5525.h" #include "SIM_MS5525.h"
#include "SIM_MS5611.h" #include "SIM_MS5611.h"
#include "SIM_QMC5883L.h" #include "SIM_QMC5883L.h"
#include "SIM_INA3221.h"
#include <signal.h> #include <signal.h>
@ -85,6 +86,9 @@ static IS31FL3195 is31fl3195;
#if AP_SIM_COMPASS_QMC5883L_ENABLED #if AP_SIM_COMPASS_QMC5883L_ENABLED
static QMC5883L qmc5883l; static QMC5883L qmc5883l;
#endif #endif
#if AP_SIM_INA3221_ENABLED
static INA3221 ina3221;
#endif
struct i2c_device_at_address { struct i2c_device_at_address {
uint8_t bus; uint8_t bus;
@ -102,6 +106,9 @@ struct i2c_device_at_address {
{ 1, 0x39, ignored }, // NCP5623C { 1, 0x39, ignored }, // NCP5623C
{ 1, 0x40, ignored }, // KellerLD { 1, 0x40, ignored }, // KellerLD
{ 1, 0x76, ms5525 }, // MS5525: ARSPD_TYPE = 4 { 1, 0x76, ms5525 }, // MS5525: ARSPD_TYPE = 4
#if AP_SIM_INA3221_ENABLED
{ 1, 0x42, ina3221 },
#endif
{ 1, 0x77, tsys01 }, { 1, 0x77, tsys01 },
{ 1, 0x0B, rotoye }, // Rotoye: BATTx_MONITOR 19, BATTx_I2C_ADDR 13 { 1, 0x0B, rotoye }, // Rotoye: BATTx_MONITOR 19, BATTx_I2C_ADDR 13
{ 2, 0x0B, maxell }, // Maxell: BATTx_MONITOR 16, BATTx_I2C_ADDR 13 { 2, 0x0B, maxell }, // Maxell: BATTx_MONITOR 16, BATTx_I2C_ADDR 13

143
libraries/SITL/SIM_INA3221.cpp Normal file
View File

@ -0,0 +1,143 @@
#include "SIM_config.h"
#if AP_SIM_INA3221_ENABLED
#include "SIM_INA3221.h"
#include <SITL/SITL.h>
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

89
libraries/SITL/SIM_INA3221.h Normal file
View File

@ -0,0 +1,89 @@
#include "SIM_config.h"
#if AP_SIM_INA3221_ENABLED
#include "SIM_I2CDevice.h"
#include <AP_Common/Bitmask.h>
/*
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<Registers::ByName, 512> assert_storage_size_registers_reg;
Bitmask<256> writable_registers;
void reset();
};
} // namespace SITL
#endif // AP_SIM_INA3221_ENABLED

3
libraries/SITL/SIM_config.h
View File

@ -144,3 +144,6 @@
#define AP_SIM_GIMBAL_ENABLED (AP_SIM_SOLOGIMBAL_ENABLED) #define AP_SIM_GIMBAL_ENABLED (AP_SIM_SOLOGIMBAL_ENABLED)
#endif #endif
#ifndef AP_SIM_INA3221_ENABLED
#define AP_SIM_INA3221_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif