saltylab-firmware/src/ina219.c

#include "ina219.h"
#include "config.h"
#include "i2c1.h"
#include <string.h>

/* ================================================================
 * 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)
{
    (void)addr; (void)reg; (void)value;
    /* TODO: Implement using HAL_I2C_Master_Transmit with hi2c1 */
    return false;
}

static bool i2c_read_register(uint8_t addr, uint8_t reg, uint16_t *value)
{
    (void)addr; (void)reg;
    /* TODO: Implement using HAL_I2C_Master_Transmit/Receive with hi2c1 */
    if (value) *value = 0;
    return false;
}

/* ================================================================
 * 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, &reg_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, &reg_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, &reg_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, &reg_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, &reg_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, &reg_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, &reg_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);
}