2024-11-22 01:06:35 -04:00
|
|
|
#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;
|
|
|
|
};
|
|
|
|
|
2024-12-01 14:30:04 -04:00
|
|
|
static uint16_t convert_voltage(float voltage_v) {
|
|
|
|
// 8mV per count, register value is x8 (3 lowest bits not used)
|
|
|
|
float volt_counts = (voltage_v/8e-3)*8;
|
|
|
|
if (volt_counts < INT16_MIN) {
|
|
|
|
volt_counts = INT16_MIN;
|
|
|
|
} else if (volt_counts > INT16_MAX) {
|
|
|
|
volt_counts = INT16_MAX;
|
|
|
|
}
|
|
|
|
|
|
|
|
// register value is signed
|
|
|
|
return (uint16_t)((int16_t)volt_counts);
|
|
|
|
}
|
|
|
|
|
|
|
|
static uint16_t convert_current(float current_a) {
|
|
|
|
// V = IR
|
|
|
|
const float shunt_resistance_ohms = 0.001; // default value in driver
|
|
|
|
const float shunt_voltage = current_a*shunt_resistance_ohms;
|
|
|
|
|
|
|
|
// 40uV per count, register value is x8 (3 lowest bits not used)
|
|
|
|
float shunt_counts = (shunt_voltage/40e-6)*8;
|
|
|
|
if (shunt_counts < INT16_MIN) {
|
|
|
|
shunt_counts = INT16_MIN;
|
|
|
|
} else if (shunt_counts > INT16_MAX) {
|
|
|
|
shunt_counts = INT16_MAX;
|
|
|
|
}
|
|
|
|
|
|
|
|
// register value is signed
|
|
|
|
return (uint16_t)((int16_t)shunt_counts);
|
2024-11-22 01:06:35 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2024-12-01 14:30:04 -04:00
|
|
|
// channel 2 gets the first simulated battery's voltage and current, others are test values:
|
2024-11-22 01:06:35 -04:00
|
|
|
if (registers.byname.configuration.bits.ch1_enable) {
|
2024-12-01 14:30:04 -04:00
|
|
|
// values close to chip limits (assuming 1mOhm current shunt)
|
2024-11-22 01:06:35 -04:00
|
|
|
if (update_bus) {
|
2024-12-01 14:30:04 -04:00
|
|
|
registers.byname.Channel_1_Bus_Voltage = convert_voltage(25); // max of 26V
|
2024-11-22 01:06:35 -04:00
|
|
|
}
|
|
|
|
if (update_shunt) {
|
2024-12-01 14:30:04 -04:00
|
|
|
registers.byname.Channel_1_Shunt_Voltage = convert_current(160); // max of 163.8A
|
2024-11-22 01:06:35 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (registers.byname.configuration.bits.ch2_enable) {
|
|
|
|
if (update_bus) {
|
2024-12-01 14:30:04 -04:00
|
|
|
registers.byname.Channel_2_Bus_Voltage = convert_voltage(AP::sitl()->state.battery_voltage);
|
|
|
|
}
|
|
|
|
if (update_shunt) {
|
|
|
|
registers.byname.Channel_2_Shunt_Voltage = convert_current(AP::sitl()->state.battery_current);
|
2024-11-22 01:06:35 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (registers.byname.configuration.bits.ch3_enable) {
|
2024-12-01 14:30:04 -04:00
|
|
|
// values different from above to test channel switching
|
2024-11-22 01:06:35 -04:00
|
|
|
if (update_bus) {
|
2024-12-01 14:30:04 -04:00
|
|
|
registers.byname.Channel_3_Bus_Voltage = convert_voltage(3.14159);
|
2024-11-22 01:06:35 -04:00
|
|
|
}
|
|
|
|
if (update_shunt) {
|
2024-12-01 14:30:04 -04:00
|
|
|
registers.byname.Channel_3_Shunt_Voltage = convert_current(2.71828);
|
2024-11-22 01:06:35 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif // AP_SIM_INA3221_ENABLED
|