tof_control/device/
ina226.rs

1#![allow(unused)]
2use crate::constant::*;
3
4use i2cdev::core::*;
5use i2cdev::linux::{LinuxI2CDevice, LinuxI2CError};
6
7// Register
8const CONFIG: u16 = 0x00;
9const SHUNT: u16 = 0x01;
10const BUS: u16 = 0x02;
11const POWER: u16 = 0x03;
12const CURRENT: u16 = 0x04;
13const CALIB: u16 = 0x05;
14// Configuration Parameters
15const CONFIG_RST: u16 = 0xC000;
16const CONFIG_AVG_1: u16 = 0x4000;
17const CONFIG_AVG_4: u16 = 0x4200;
18const CONFIG_AVG_16: u16 = 0x4400;
19const CONFIG_AVG_64: u16 = 0x4600;
20const CONFIG_AVG_128: u16 = 0x4800;
21const CONFIG_AVG_256: u16 = 0x4A00;
22const CONFIG_AVG_512: u16 = 0x4C00;
23const CONFIG_AVG_1024: u16 = 0x4E00;
24const CONFIG_VBUSCT_140: u16 = 0x4000;
25const CONFIG_VBUSCT_204: u16 = 0x4040;
26const CONFIG_VBUSCT_332: u16 = 0x4080;
27const CONFIG_VBUSCT_588: u16 = 0x40C0;
28const CONFIG_VBUSCT_1100: u16 = 0x4100;
29const CONFIG_VBUSCT_2116: u16 = 0x4140;
30const CONFIG_VBUSCT_4156: u16 = 0x4180;
31const CONFIG_VBUSCT_8244: u16 = 0x41C0;
32const CONFIG_VSHCT_140: u16 = 0x4000;
33const CONFIG_VSHCT_204: u16 = 0x4008;
34const CONFIG_VSHCT_332: u16 = 0x4010;
35const CONFIG_VSHCT_588: u16 = 0x4018;
36const CONFIG_VSHCT_1100: u16 = 0x4020;
37const CONFIG_VSHCT_2116: u16 = 0x4028;
38const CONFIG_VSHCT_4156: u16 = 0x4030;
39const CONFIG_VSHCT_8244: u16 = 0x4038;
40const CONFIG_MODE_PDS: u16 = 0x4000; // Power-Down (or Shutdown)
41const CONFIG_MODE_SVT: u16 = 0x4001; // Shunt Voltage, Triggered
42const CONFIG_MODE_BVT: u16 = 0x4002; // Bus Voltage, Triggered
43const CONFIG_MODE_SBT: u16 = 0x4003; // Shunt and Bus, Triggered
44const CONFIG_MODE_PDS_2: u16 = 0x4004; // Power-Down (or Shutdown) 2
45const CONFIG_MODE_SVC: u16 = 0x4005; // Shunt Voltage, Continuous
46const CONFIG_MODE_BVC: u16 = 0x4006; // Bus Voltage, Continuous
47const CONFIG_MODE_SBC: u16 = 0x4007; // Shunt and Bus, Continuous
48
49pub struct INA226 {
50    bus: u8,
51    address: u16,
52    rshunt: f32, // shunt resistance value
53    mec: f32,    // maximum expected current
54}
55
56impl INA226 {
57    pub fn new(bus: u8, address: u16, rshunt: f32, mec: f32) -> Self {
58        Self {
59            bus,
60            address,
61            rshunt,
62            mec,
63        }
64    }
65    pub fn configure(&self) -> Result<(), LinuxI2CError> {
66        let mut dev = LinuxI2CDevice::new(&format!("/dev/i2c-{}", self.bus), self.address)?;
67
68        let config = CONFIG_AVG_16 | CONFIG_VBUSCT_332 | CONFIG_VSHCT_332 | CONFIG_MODE_SBT;
69        dev.smbus_write_i2c_block_data(CONFIG as u8, &config.to_be_bytes())
70    }
71    fn calibrate(&self, dev: &mut LinuxI2CDevice) -> Result<[f32; 2], LinuxI2CError> {
72        let c_lsb = self.mec / 2f32.powf(15.0);
73        let p_lsb = 25.0 * c_lsb;
74        let cal = 0.00512 / (c_lsb * self.rshunt);
75
76        dev.smbus_write_i2c_block_data(CALIB as u8, &(cal as u16).to_be_bytes())?;
77
78        let cal_lsb = [c_lsb, p_lsb];
79
80        Ok(cal_lsb)
81    }
82    pub fn read(&self) -> Result<[f32; 3], LinuxI2CError> {
83        let mut dev = LinuxI2CDevice::new(&format!("/dev/i2c-{}", self.bus), self.address)?;
84
85        let shunt_voltage = self.read_shunt_voltage(&mut dev)?;
86        let bus_voltage = self.read_bus_voltage(&mut dev)?;
87        let cal_lsb = self.calibrate(&mut dev)?;
88
89        let current = self.read_current(&mut dev, cal_lsb[0])?;
90        let power = self.read_power(&mut dev, cal_lsb[1])?;
91
92        let vcp = [bus_voltage, current, power];
93
94        Ok(vcp)
95    }
96    fn read_shunt_voltage(&self, dev: &mut LinuxI2CDevice) -> Result<f32, LinuxI2CError> {
97        let shunt_voltage_raw = dev.smbus_read_i2c_block_data(SHUNT as u8, 2)?;
98        let mut shunt_voltage_adc =
99            ((shunt_voltage_raw[0] as u16) << 8) | (shunt_voltage_raw[1] as u16);
100        let mut sign: f32 = 1.0;
101        if shunt_voltage_adc >= 0x8000 {
102            sign = -1.0;
103            shunt_voltage_adc = (shunt_voltage_adc & 0x7FFF) + 1;
104        }
105
106        let shunt_voltage = sign * (shunt_voltage_adc as f32) * 0.0000025;
107
108        Ok(shunt_voltage)
109    }
110    fn read_bus_voltage(&self, dev: &mut LinuxI2CDevice) -> Result<f32, LinuxI2CError> {
111        let bus_voltage_raw = dev.smbus_read_i2c_block_data(BUS as u8, 2)?;
112        let bus_voltage_adc = ((bus_voltage_raw[0] as u16) << 8) | (bus_voltage_raw[1] as u16);
113        let bus_voltage = (bus_voltage_adc as f32) * 0.00125;
114
115        Ok(bus_voltage)
116    }
117    fn read_current(&self, dev: &mut LinuxI2CDevice, c_lsb: f32) -> Result<f32, LinuxI2CError> {
118        let mut current_raw = dev.smbus_read_i2c_block_data(CURRENT as u8, 2)?;
119        let mut current_adc = ((current_raw[0] as u16) << 8) | (current_raw[1] as u16);
120        // while current_adc == 0 {
121        //     current_raw = dev.smbus_read_i2c_block_data(CURRENT as u8, 2)?;
122        //     current_adc = ((current_raw[0] as u16) << 8) | (current_raw[1] as u16);
123        // }
124        let mut sign: f32 = 1.0;
125        if current_adc >= 0x8000 {
126            sign = -1.0;
127            current_adc = (current_adc & 0x7FFF) + 1;
128        }
129
130        let current = sign * (current_adc as f32) * c_lsb;
131
132        Ok(current)
133    }
134    fn read_power(&self, dev: &mut LinuxI2CDevice, p_lsb: f32) -> Result<f32, LinuxI2CError> {
135        let mut power_raw = dev.smbus_read_i2c_block_data(POWER as u8, 2)?;
136        let mut power_adc = ((power_raw[0] as u16) << 8) | (power_raw[1] as u16);
137        // while power_adc == 0 {
138        //     power_raw = dev.smbus_read_i2c_block_data(POWER as u8, 2)?;
139        //     power_adc = ((power_raw[0] as u16) << 8) | (power_raw[1] as u16);
140        // }
141        let power = (power_adc as f32) * p_lsb;
142
143        Ok(power)
144    }
145    fn reset(&self) -> Result<(), LinuxI2CError> {
146        let mut dev = LinuxI2CDevice::new(&format!("/dev/i2c-{}", self.bus), self.address)?;
147        dev.smbus_write_i2c_block_data(CONFIG as u8, &CONFIG_RST.to_be_bytes())
148    }
149}
tof_control/device/ina226.rs

tof_control/device/
ina226.rs
