#include "ina219.h" #include "config.h" #include "i2c1.h" #include /* ================================================================ * INA219 Register Definitions * ================================================================ */ #define INA219_REG_CONFIG 0x00 #define INA219_REG_SHUNT_VOLTAGE 0x01 #define INA219_REG_BUS_VOLTAGE 0x02 #define INA219_REG_POWER 0x03 #define INA219_REG_CURRENT 0x04 #define INA219_REG_CALIBRATION 0x05 /* Configuration Register Bits */ #define INA219_CONFIG_RESET (1 << 15) #define INA219_CONFIG_BRNG_16V (0 << 13) #define INA219_CONFIG_BRNG_32V (1 << 13) #define INA219_CONFIG_PGA_40MV (0 << 11) #define INA219_CONFIG_PGA_80MV (1 << 11) #define INA219_CONFIG_PGA_160MV (2 << 11) #define INA219_CONFIG_PGA_320MV (3 << 11) #define INA219_CONFIG_BADC_9BIT (0 << 7) #define INA219_CONFIG_BADC_10BIT (1 << 7) #define INA219_CONFIG_BADC_11BIT (2 << 7) #define INA219_CONFIG_BADC_12BIT (3 << 7) #define INA219_CONFIG_SADC_9BIT (0 << 3) #define INA219_CONFIG_SADC_10BIT (1 << 3) #define INA219_CONFIG_SADC_11BIT (2 << 3) #define INA219_CONFIG_SADC_12BIT (3 << 3) #define INA219_CONFIG_MODE_SHUNT (0 << 0) #define INA219_CONFIG_MODE_BUSVOLT (1 << 0) #define INA219_CONFIG_MODE_BOTH (3 << 0) /* I2C Addresses */ #define INA219_ADDR_LEFT_MOTOR 0x40 /* A0=A1=GND */ #define INA219_ADDR_RIGHT_MOTOR 0x41 /* A0=SDA, A1=GND */ /* ================================================================ * Internal State * ================================================================ */ typedef struct { uint8_t i2c_addr; uint16_t calibration_value; uint16_t current_lsb_ua; /* Current LSB in µA */ uint16_t power_lsb_uw; /* Power LSB in µW */ } INA219State; static INA219State s_ina219[INA219_COUNT] = { [INA219_LEFT_MOTOR] = {.i2c_addr = INA219_ADDR_LEFT_MOTOR}, [INA219_RIGHT_MOTOR] = {.i2c_addr = INA219_ADDR_RIGHT_MOTOR} }; /* ================================================================ * I2C Helper Functions * ================================================================ */ static bool i2c_write_register(uint8_t addr, uint8_t reg, uint16_t value) { uint8_t buf[3] = {reg, (uint8_t)(value >> 8), (uint8_t)(value & 0xFF)}; return i2c1_write(addr, buf, sizeof(buf)) == 0; } static bool i2c_read_register(uint8_t addr, uint8_t reg, uint16_t *value) { uint8_t buf[2]; if (i2c1_write(addr, ®, 1) != 0) return false; if (i2c1_read(addr, buf, sizeof(buf)) != 0) return false; *value = ((uint16_t)buf[0] << 8) | buf[1]; return true; } /* ================================================================ * Public API * ================================================================ */ void ina219_init(void) { /* Ensure I2C1 is initialized before calling this */ /* Auto-calibrate both sensors for typical motor monitoring: * - Max current: 5A * - Shunt resistor: 0.1Ω * - LSB: 160µA (5A / 32768) */ ina219_calibrate(INA219_LEFT_MOTOR, 5000, 100); ina219_calibrate(INA219_RIGHT_MOTOR, 5000, 100); } void ina219_calibrate(INA219Sensor sensor, uint16_t max_current_ma, uint16_t shunt_ohms_milli) { if (sensor >= INA219_COUNT) return; INA219State *s = &s_ina219[sensor]; /* Calculate current LSB: max_current / 32768 (15-bit signed register) * LSB unit: µA * Example: 5000mA / 32768 ≈ 152.6µA → use 160µA (round up for safety) */ uint32_t current_lsb_ua = ((uint32_t)max_current_ma * 1000 + 32767) / 32768; s->current_lsb_ua = (uint16_t)current_lsb_ua; /* Power LSB = 20 × current_lsb_ua (20µW per 1µA of current LSB) */ s->power_lsb_uw = 20 * current_lsb_ua; /* Calibration register: (0.04096) / (current_lsb_ua × shunt_ohms_milli / 1000) * Simplified: 40960 / (current_lsb_ua × shunt_ohms_milli) */ uint32_t calibration = 40960 / ((uint32_t)current_lsb_ua * shunt_ohms_milli / 1000); if (calibration > 65535) calibration = 65535; s->calibration_value = (uint16_t)calibration; /* Write calibration register */ i2c_write_register(s->i2c_addr, INA219_REG_CALIBRATION, s->calibration_value); /* Configure for continuous conversion mode (12-bit ADC for both shunt and bus) * Config: 32V range, 160mV PGA, 12-bit ADC, continuous mode */ uint16_t config = INA219_CONFIG_BRNG_32V | INA219_CONFIG_PGA_160MV | INA219_CONFIG_BADC_12BIT | INA219_CONFIG_SADC_12BIT | INA219_CONFIG_MODE_BOTH; i2c_write_register(s->i2c_addr, INA219_REG_CONFIG, config); } bool ina219_read(INA219Sensor sensor, INA219Data *data) { if (sensor >= INA219_COUNT || !data) return false; INA219State *s = &s_ina219[sensor]; uint8_t addr = s->i2c_addr; uint16_t reg_value; /* Read shunt voltage (register 0x01) */ if (!i2c_read_register(addr, INA219_REG_SHUNT_VOLTAGE, ®_value)) return false; int16_t shunt_raw = (int16_t)reg_value; data->shunt_voltage_uv = shunt_raw * 10; /* 10µV/LSB */ /* Read bus voltage (register 0x02) */ if (!i2c_read_register(addr, INA219_REG_BUS_VOLTAGE, ®_value)) return false; uint16_t bus_raw = (reg_value >> 3) & 0x1FFF; /* 13-bit voltage, 4mV/LSB */ data->bus_voltage_mv = bus_raw * 4; /* Read current (register 0x04) — requires calibration */ if (!i2c_read_register(addr, INA219_REG_CURRENT, ®_value)) return false; int16_t current_raw = (int16_t)reg_value; data->current_ma = (current_raw * (int32_t)s->current_lsb_ua) / 1000; /* Read power (register 0x03) — in units of power_lsb */ if (!i2c_read_register(addr, INA219_REG_POWER, ®_value)) return false; uint32_t power_raw = reg_value; data->power_mw = (power_raw * (uint32_t)s->power_lsb_uw) / 1000; return true; } bool ina219_read_bus_voltage_mv(INA219Sensor sensor, uint16_t *voltage_mv) { if (sensor >= INA219_COUNT || !voltage_mv) return false; INA219State *s = &s_ina219[sensor]; uint16_t reg_value; if (!i2c_read_register(s->i2c_addr, INA219_REG_BUS_VOLTAGE, ®_value)) return false; uint16_t bus_raw = (reg_value >> 3) & 0x1FFF; *voltage_mv = bus_raw * 4; return true; } bool ina219_read_current_ma(INA219Sensor sensor, int16_t *current_ma) { if (sensor >= INA219_COUNT || !current_ma) return false; INA219State *s = &s_ina219[sensor]; uint16_t reg_value; if (!i2c_read_register(s->i2c_addr, INA219_REG_CURRENT, ®_value)) return false; int16_t current_raw = (int16_t)reg_value; *current_ma = (current_raw * (int32_t)s->current_lsb_ua) / 1000; return true; } bool ina219_read_power_mw(INA219Sensor sensor, uint32_t *power_mw) { if (sensor >= INA219_COUNT || !power_mw) return false; INA219State *s = &s_ina219[sensor]; uint16_t reg_value; if (!i2c_read_register(s->i2c_addr, INA219_REG_POWER, ®_value)) return false; uint32_t power_raw = reg_value; *power_mw = (power_raw * (uint32_t)s->power_lsb_uw) / 1000; return true; } void ina219_alert_enable(INA219Sensor sensor, uint16_t current_limit_ma) { if (sensor >= INA219_COUNT) return; INA219State *s = &s_ina219[sensor]; /* Alert limit register: set to current threshold * Current threshold = (limit_ma × 1000) / current_lsb_ua */ int16_t limit_raw = ((int32_t)current_limit_ma * 1000) / s->current_lsb_ua; if (limit_raw > 32767) limit_raw = 32767; /* Enable alert on over-limit, latching mode */ uint16_t alert_config = limit_raw; i2c_write_register(s->i2c_addr, 0x06, alert_config); /* Alert register */ } void ina219_alert_disable(INA219Sensor sensor) { if (sensor >= INA219_COUNT) return; INA219State *s = &s_ina219[sensor]; /* Write 0 to alert register to disable */ i2c_write_register(s->i2c_addr, 0x06, 0); } void ina219_reset(INA219Sensor sensor) { if (sensor >= INA219_COUNT) return; INA219State *s = &s_ina219[sensor]; /* Set reset bit in config register */ i2c_write_register(s->i2c_addr, INA219_REG_CONFIG, INA219_CONFIG_RESET); /* Wait for reset to complete (~1ms) */ uint32_t start = 0; /* In real code, use HAL_GetTick() */ while (start < 2) start++; /* Simple delay */ /* Re-calibrate after reset */ ina219_calibrate(sensor, 5000, 100); }