tof_control/device/
ina219.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 = 0x8000;
16const CONFIG_BRNG_16: u16 = 0x0000;
17const CONFIG_BRNG_32: u16 = 0x2000;
18const CONFIG_PG_40: u16 = 0x0000;
19const CONFIG_PG_80: u16 = 0x0800;
20const CONFIG_PG_160: u16 = 0x1000;
21const CONFIG_PG_320: u16 = 0x1800;
22const CONFIG_BADC_9B: u16 = 0x0000;
23const CONFIG_BADC_10B: u16 = 0x0080;
24const CONFIG_BADC_11B: u16 = 0x0100;
25const CONFIG_BADC_12B_D: u16 = 0x0180;
26const CONFIG_BADC_12B: u16 = 0x0400;
27const CONFIG_BADC_2S: u16 = 0x0480;
28const CONFIG_BADC_4S: u16 = 0x0500;
29const CONFIG_BADC_8S: u16 = 0x0580;
30const CONFIG_BADC_16S: u16 = 0x0600;
31const CONFIG_BADC_32S: u16 = 0x0680;
32const CONFIG_BADC_64S: u16 = 0x0700;
33const CONFIG_BADC_128S: u16 = 0x0780;
34const CONFIG_SADC_9B: u16 = 0x0000;
35const CONFIG_SADC_10B: u16 = 0x0008;
36const CONFIG_SADC_11B: u16 = 0x0010;
37const CONFIG_SADC_12B_D: u16 = 0x0018;
38const CONFIG_SADC_12B: u16 = 0x0040;
39const CONFIG_SADC_2S: u16 = 0x0048;
40const CONFIG_SADC_4S: u16 = 0x0050;
41const CONFIG_SADC_8S: u16 = 0x0058;
42const CONFIG_SADC_16S: u16 = 0x0060;
43const CONFIG_SADC_32S: u16 = 0x0068;
44const CONFIG_SADC_64S: u16 = 0x0070;
45const CONFIG_SADC_128S: u16 = 0x0078;
46const CONFIG_MODE_PD: u16 = 0x0000; // Power-Down
47const CONFIG_MODE_SVT: u16 = 0x0001; // Shunt Voltage, Triggered
48const CONFIG_MODE_BVT: u16 = 0x0002; // Bus Voltage, Triggered
49const CONFIG_MODE_SBT: u16 = 0x0003; // Shunt and Bus, Triggered
50const CONFIG_MODE_ADO: u16 = 0x0004; // ADC off (disabled)
51const CONFIG_MODE_SVC: u16 = 0x0005; // Shunt Voltage, Continuous
52const CONFIG_MODE_BVC: u16 = 0x0006; // Bus Voltage, Continuous
53const CONFIG_MODE_SBC: u16 = 0x0007; // Shunt and Bus, Continuous
54
55pub struct INA219 {
56    bus: u8,
57    address: u16,
58    rshunt: f32, // shunt resistance value
59    mec: f32,    // maximum expected current
60}
61
62impl INA219 {
63    pub fn new(bus: u8, address: u16, rshunt: f32, mec: f32) -> Self {
64        Self {
65            bus,
66            address,
67            rshunt,
68            mec,
69        }
70    }
71    pub fn configure(&self) -> Result<(), LinuxI2CError> {
72        let mut dev = LinuxI2CDevice::new(&format!("/dev/i2c-{}", self.bus), self.address)?;
73
74        let config =
75            CONFIG_BRNG_32 | CONFIG_PG_320 | CONFIG_BADC_16S | CONFIG_SADC_16S | CONFIG_MODE_SBT;
76        dev.smbus_write_i2c_block_data(CONFIG as u8, &config.to_be_bytes())
77    }
78    fn calibrate(&self, dev: &mut LinuxI2CDevice) -> Result<[f32; 2], LinuxI2CError> {
79        let c_lsb = self.mec / 2f32.powf(15.0);
80        let p_lsb = 20.0 * c_lsb;
81        let cal = 0.04096 / (c_lsb * self.rshunt);
82
83        dev.smbus_write_i2c_block_data(CALIB as u8, &(cal as u16).to_be_bytes())?;
84
85        let cal_lsb = [c_lsb, p_lsb];
86
87        Ok(cal_lsb)
88    }
89    pub fn read(&self) -> Result<[f32; 3], LinuxI2CError> {
90        let mut dev = LinuxI2CDevice::new(&format!("/dev/i2c-{}", self.bus), self.address)?;
91
92        let shunt_voltage = self.read_shunt_voltage(&mut dev)?;
93        let bus_voltage = self.read_bus_voltage(&mut dev)?;
94
95        let cal_lsb = self.calibrate(&mut dev)?;
96
97        let current = self.read_current(&mut dev, cal_lsb[0])?;
98        let power = self.read_power(&mut dev, cal_lsb[1])?;
99
100        let vcp = [bus_voltage, current, power];
101
102        Ok(vcp)
103    }
104    fn read_shunt_voltage(&self, dev: &mut LinuxI2CDevice) -> Result<f32, LinuxI2CError> {
105        let shunt_voltage_raw = dev.smbus_read_i2c_block_data(SHUNT as u8, 2)?;
106        let mut shunt_voltage_adc =
107            ((shunt_voltage_raw[0] as u16) << 8) | (shunt_voltage_raw[1] as u16);
108        let mut sign: f32 = 1.0;
109        if shunt_voltage_adc >= 0x8000 {
110            sign = -1.0;
111            shunt_voltage_adc = (shunt_voltage_adc & 0x7FFF) + 1;
112        }
113
114        let shunt_voltage = sign * (shunt_voltage_adc as f32) * 0.00001;
115
116        Ok(shunt_voltage)
117    }
118    fn read_bus_voltage(&self, dev: &mut LinuxI2CDevice) -> Result<f32, LinuxI2CError> {
119        let bus_voltage_raw = dev.smbus_read_i2c_block_data(BUS as u8, 2)?;
120        let bus_voltage_adc =
121            (((bus_voltage_raw[0] as u16) << 8) | (bus_voltage_raw[1] as u16)) >> 3 & 0x1FFF;
122        let bus_voltage = (bus_voltage_adc as f32) * 0.004;
123
124        Ok(bus_voltage)
125    }
126    fn read_current(&self, dev: &mut LinuxI2CDevice, c_lsb: f32) -> Result<f32, LinuxI2CError> {
127        let mut current_raw = dev.smbus_read_i2c_block_data(CURRENT as u8, 2)?;
128        let mut current_adc = ((current_raw[0] as u16) << 8) | (current_raw[1] as u16);
129        // while current_adc == 0 {
130        //     current_raw = dev.smbus_read_i2c_block_data(CURRENT as u8, 2)?;
131        //     current_adc = ((current_raw[0] as u16) << 8) | (current_raw[1] as u16);
132        // }
133        let mut sign: f32 = 1.0;
134        if current_adc >= 0x8000 {
135            sign = -1.0;
136            current_adc = (current_adc & 0x7FFF) + 1;
137        }
138
139        let current = sign * (current_adc as f32) * c_lsb;
140
141        Ok(current)
142    }
143    fn read_power(&self, dev: &mut LinuxI2CDevice, p_lsb: f32) -> Result<f32, LinuxI2CError> {
144        let mut power_raw = dev.smbus_read_i2c_block_data(POWER as u8, 2)?;
145        let mut power_adc = ((power_raw[0] as u16) << 8) | (power_raw[1] as u16);
146        // while power_adc == 0 {
147        //     power_raw = dev.smbus_read_i2c_block_data(POWER as u8, 2)?;
148        //     power_adc = ((power_raw[0] as u16) << 8) | (power_raw[1] as u16);
149        // }
150        let power = (power_adc as f32) * p_lsb;
151
152        Ok(power)
153    }
154    fn reset(&self) -> Result<(), LinuxI2CError> {
155        let mut dev = LinuxI2CDevice::new(&format!("/dev/i2c-{}", self.bus), self.address)?;
156        dev.smbus_write_i2c_block_data(CONFIG as u8, &CONFIG_RST.to_be_bytes())
157    }
158}
tof_control/device/ina219.rs

tof_control/device/
ina219.rs
