saltylab-firmware/src/motor_current.c

/*
 * motor_current.c — ADC-based motor current monitoring and overload protection
 * (Issue #584).
 *
 * Reads battery discharge current from battery_adc_get_current_ma() (ADC3 IN13,
 * PC3), which is already DMA-sampled by battery_adc.c.  Implements:
 *
 *   1. Soft current limiting: linear PWM reduction when current exceeds
 *      MOTOR_CURR_SOFT_MA (4 A, 80% of hard threshold).
 *
 *   2. Hard cutoff: if current stays above MOTOR_CURR_HARD_MA (5 A) for
 *      MOTOR_CURR_OVERLOAD_MS (2 s), output is zeroed.  A fault event is
 *      signalled via motor_current_fault_pending() for one tick so the main
 *      loop can append a fault log entry.
 *
 *   3. Auto-recovery: after MOTOR_CURR_COOLDOWN_MS (10 s) in MC_COOLDOWN,
 *      state returns to MC_NORMAL and normal PWM authority is restored.
 *
 *   4. Telemetry: JLINK_TLM_MOTOR_CURRENT (0x86) published at
 *      MOTOR_CURR_TLM_HZ (5 Hz) via jlink_send_motor_current_tlm().
 */

#include "motor_current.h"
#include "battery_adc.h"
#include "jlink.h"
#include <stddef.h>

/* ---- Module state ---- */
static MotorCurrentState s_state            = MC_NORMAL;
static int32_t           s_current_ma       = 0;
static uint32_t          s_overload_start   = 0;  /* ms when current first ≥ HARD_MA */
static uint32_t          s_cooldown_start   = 0;  /* ms when cooldown began */
static uint8_t           s_fault_count      = 0;  /* lifetime trip counter */
static uint8_t           s_fault_pending    = 0;  /* cleared after one read */
static uint32_t          s_last_tlm_ms      = 0;  /* rate-limit TLM TX */

/* Soft-limit scale factor in 0..256 fixed-point (256 = 1.0) */
static uint16_t          s_scale256         = 256u;

/* ---- motor_current_init() ---- */
void motor_current_init(void)
{
    s_state          = MC_NORMAL;
    s_current_ma     = 0;
    s_overload_start = 0;
    s_cooldown_start = 0;
    s_fault_count    = 0;
    s_fault_pending  = 0;
    s_last_tlm_ms    = 0;
    s_scale256       = 256u;
}

/* ---- motor_current_tick() ---- */
void motor_current_tick(uint32_t now_ms)
{
    /* Snapshot current from battery ADC (mA, positive = discharge) */
    s_current_ma = battery_adc_get_current_ma();

    /* Use absolute value: protect in both forward and regen braking */
    int32_t abs_ma = s_current_ma;
    if (abs_ma < 0) abs_ma = -abs_ma;

    switch (s_state) {

    case MC_NORMAL:
        s_scale256 = 256u;
        if (abs_ma >= (int32_t)MOTOR_CURR_SOFT_MA) {
            s_state = MC_SOFT_LIMIT;
            /* Track overload onset if already above hard threshold */
            s_overload_start = (abs_ma >= (int32_t)MOTOR_CURR_HARD_MA)
                               ? now_ms : 0u;
        }
        break;

    case MC_SOFT_LIMIT:
        if (abs_ma < (int32_t)MOTOR_CURR_SOFT_MA) {
            /* Recovered below soft threshold */
            s_state          = MC_NORMAL;
            s_overload_start = 0u;
            s_scale256       = 256u;
        } else {
            /* Compute linear scale: 256 at SOFT_MA, 0 at HARD_MA */
            int32_t range = (int32_t)MOTOR_CURR_HARD_MA
                            - (int32_t)MOTOR_CURR_SOFT_MA;
            int32_t over  = abs_ma - (int32_t)MOTOR_CURR_SOFT_MA;
            if (over >= range) {
                s_scale256 = 0u;
            } else {
                /* scale256 = (range - over) * 256 / range */
                s_scale256 = (uint16_t)(((range - over) * 256u) / range);
            }

            /* Track sustained hard-threshold overload */
            if (abs_ma >= (int32_t)MOTOR_CURR_HARD_MA) {
                if (s_overload_start == 0u) {
                    s_overload_start = now_ms;
                } else if ((now_ms - s_overload_start) >= MOTOR_CURR_OVERLOAD_MS) {
                    /* Hard cutoff — trip the fault */
                    if (s_fault_count < 255u) s_fault_count++;
                    s_fault_pending  = 1u;
                    s_cooldown_start = now_ms;
                    s_overload_start = 0u;
                    s_scale256       = 0u;
                    s_state          = MC_COOLDOWN;
                }
            } else {
                /* Current dipped back below HARD_MA — reset overload timer */
                s_overload_start = 0u;
            }
        }
        break;

    case MC_COOLDOWN:
        s_scale256 = 0u;
        if ((now_ms - s_cooldown_start) >= MOTOR_CURR_COOLDOWN_MS) {
            s_state    = MC_NORMAL;
            s_scale256 = 256u;
        }
        break;
    }
}

/* ---- motor_current_apply_limit() ---- */
int16_t motor_current_apply_limit(int16_t cmd)
{
    if (s_scale256 >= 256u) return cmd;
    if (s_scale256 == 0u)   return 0;
    return (int16_t)(((int32_t)cmd * s_scale256) / 256);
}

/* ---- Accessors ---- */
bool motor_current_is_faulted(void)
{
    return s_state == MC_COOLDOWN;
}

MotorCurrentState motor_current_state(void)
{
    return s_state;
}

int32_t motor_current_ma(void)
{
    return s_current_ma;
}

uint8_t motor_current_fault_count(void)
{
    return s_fault_count;
}

bool motor_current_fault_pending(void)
{
    if (!s_fault_pending) return false;
    s_fault_pending = 0u;
    return true;
}

/* ---- motor_current_send_tlm() ---- */
void motor_current_send_tlm(uint32_t now_ms)
{
    if (MOTOR_CURR_TLM_HZ == 0u) return;

    uint32_t interval_ms = 1000u / MOTOR_CURR_TLM_HZ;
    if ((now_ms - s_last_tlm_ms) < interval_ms) return;
    s_last_tlm_ms = now_ms;

    jlink_tlm_motor_current_t tlm;

    tlm.current_ma  = s_current_ma;

    /* limit_pct: 0 = no limiting, 100 = full cutoff */
    if (s_scale256 >= 256u) {
        tlm.limit_pct = 0u;
    } else {
        tlm.limit_pct = (uint8_t)(((256u - s_scale256) * 100u) / 256u);
    }

    tlm.state       = (uint8_t)s_state;
    tlm.fault_count = s_fault_count;

    jlink_send_motor_current_tlm(&tlm);
}