sl-controls
2b06161cb4
Adds ADC-based motor current sensing with configurable overload threshold, soft PWM limiting, hard cutoff on sustained overload, and auto-recovery. Changes: - include/motor_current.h: MotorCurrentState enum (NORMAL/SOFT_LIMIT/COOLDOWN), thresholds (5A hard, 4A soft, 2s overload, 10s cooldown), full API - src/motor_current.c: reads battery_adc_get_current_ma() each tick (reuses existing ADC3 IN13/PC3 DMA sampling); linear PWM scale in soft-limit zone (scale256 fixed-point); fault counter + one-tick fault_pending flag for main-loop fault log integration; telemetry at MOTOR_CURR_TLM_HZ (5 Hz) - include/pid_flash.h: add pid_sched_entry_t (16 bytes), pid_sched_flash_t (128 bytes at 0x0807FF40), PID_SCHED_MAX_BANDS=6, pid_flash_load_schedule(), pid_flash_save_all() — fixes missing types needed by jlink.h (Issue #550) - src/pid_flash.c: implement flash_write_words() helper, pid_flash_load_schedule(), pid_flash_save_all() — single sector-7 erase covers both schedule and PID records - include/jlink.h: add JLINK_TLM_MOTOR_CURRENT (0x86), jlink_tlm_motor_current_t (8 bytes: current_ma, limit_pct, state, fault_count), jlink_send_motor_current_tlm() - src/jlink.c: implement jlink_send_motor_current_tlm() (14-byte frame) Motor overload state machine: MC_NORMAL : current_ma < 4000 mA — full PWM authority MC_SOFT_LIMIT : 4000-5000 mA — linear reduction (0% at 4A → 100% at 5A) MC_COOLDOWN : >5A sustained 2s → zero output for 10s then NORMAL Main-loop integration: motor_current_tick(now_ms); if (motor_current_fault_pending()) fault_log_append(FAULT_MOTOR_OVERCURRENT); cmd = motor_current_apply_limit(balance_pid_output()); motor_current_send_tlm(now_ms); Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
122 lines
3.9 KiB
C
122 lines
3.9 KiB
C
#ifndef MOTOR_CURRENT_H
|
|
#define MOTOR_CURRENT_H
|
|
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
|
|
/*
|
|
* motor_current — ADC-based motor current monitoring and overload protection
|
|
* for Issue #584.
|
|
*
|
|
* Hardware:
|
|
* ADC3 IN13 (PC3, ADC_CURR_PIN) is already sampled by battery_adc.c via
|
|
* DMA2_Stream0 circular. This module reads battery_adc_get_current_ma()
|
|
* each tick rather than running a second ADC, since total discharge current
|
|
* on this single-motor balance bot equals motor current plus ~30 mA overhead.
|
|
*
|
|
* Behaviour:
|
|
* MC_NORMAL : current_ma < MOTOR_CURR_SOFT_MA — full output
|
|
* MC_SOFT_LIMIT : current_ma in [SOFT_MA, HARD_MA) — linear PWM reduction
|
|
* MC_COOLDOWN : hard cutoff latched after HARD_MA sustained for
|
|
* MOTOR_CURR_OVERLOAD_MS (2 s) — zero output for
|
|
* MOTOR_CURR_COOLDOWN_MS (10 s), then MC_NORMAL
|
|
*
|
|
* Soft limit formula (MC_SOFT_LIMIT):
|
|
* scale = (HARD_MA - current_ma) / (HARD_MA - SOFT_MA) [0..1]
|
|
* limited_cmd = (int16_t)(cmd * scale)
|
|
*
|
|
* Fault event:
|
|
* On each hard-cutoff trip, s_fault_count is incremented (saturates at 255)
|
|
* and motor_current_fault_pending() returns true for one main-loop tick so
|
|
* the caller can append a fault log entry.
|
|
*
|
|
* Main-loop integration (pseudo-code):
|
|
*
|
|
* void main_loop_tick(uint32_t now_ms) {
|
|
* battery_adc_tick(now_ms);
|
|
* motor_current_tick(now_ms);
|
|
*
|
|
* if (motor_current_fault_pending())
|
|
* fault_log_append(FAULT_MOTOR_OVERCURRENT);
|
|
*
|
|
* int16_t cmd = balance_pid_output();
|
|
* cmd = motor_current_apply_limit(cmd);
|
|
* motor_driver_update(&g_motor, cmd, steer, now_ms);
|
|
*
|
|
* motor_current_send_tlm(now_ms); // rate-limited to MOTOR_CURR_TLM_HZ
|
|
* }
|
|
*/
|
|
|
|
/* ---- Thresholds ---- */
|
|
#define MOTOR_CURR_HARD_MA 5000u /* 5 A — hard cutoff level */
|
|
#define MOTOR_CURR_SOFT_MA 4000u /* 4 A — soft-limit onset (80% of hard) */
|
|
#define MOTOR_CURR_OVERLOAD_MS 2000u /* sustained over HARD_MA before fault */
|
|
#define MOTOR_CURR_COOLDOWN_MS 10000u /* zero-output recovery period (ms) */
|
|
#define MOTOR_CURR_TLM_HZ 5u /* JLINK_TLM_MOTOR_CURRENT publish rate */
|
|
|
|
/* ---- State enum ---- */
|
|
typedef enum {
|
|
MC_NORMAL = 0,
|
|
MC_SOFT_LIMIT = 1,
|
|
MC_COOLDOWN = 2,
|
|
} MotorCurrentState;
|
|
|
|
/* ---- API ---- */
|
|
|
|
/*
|
|
* motor_current_init() — reset all state.
|
|
* Call once during system init, after battery_adc_init().
|
|
*/
|
|
void motor_current_init(void);
|
|
|
|
/*
|
|
* motor_current_tick(now_ms) — evaluate ADC reading, update state machine.
|
|
* Call from main loop after battery_adc_tick(), at any rate ≥ 10 Hz.
|
|
* Non-blocking (<1 µs).
|
|
*/
|
|
void motor_current_tick(uint32_t now_ms);
|
|
|
|
/*
|
|
* motor_current_apply_limit(cmd) — scale motor command by current-limit factor.
|
|
* MC_NORMAL: returns cmd unchanged.
|
|
* MC_SOFT_LIMIT: returns cmd scaled down linearly.
|
|
* MC_COOLDOWN: returns 0.
|
|
* Call after motor_current_tick() each loop iteration.
|
|
*/
|
|
int16_t motor_current_apply_limit(int16_t cmd);
|
|
|
|
/*
|
|
* motor_current_is_faulted() — true while in MC_COOLDOWN (output zeroed).
|
|
*/
|
|
bool motor_current_is_faulted(void);
|
|
|
|
/*
|
|
* motor_current_state() — current state machine state.
|
|
*/
|
|
MotorCurrentState motor_current_state(void);
|
|
|
|
/*
|
|
* motor_current_ma() — most recent ADC reading used by the state machine (mA).
|
|
*/
|
|
int32_t motor_current_ma(void);
|
|
|
|
/*
|
|
* motor_current_fault_count() — lifetime hard-cutoff trip counter (0..255).
|
|
*/
|
|
uint8_t motor_current_fault_count(void);
|
|
|
|
/*
|
|
* motor_current_fault_pending() — true for exactly one tick after a hard
|
|
* cutoff trip fires. Main loop should append a fault log entry and then the
|
|
* flag clears automatically on the next call.
|
|
*/
|
|
bool motor_current_fault_pending(void);
|
|
|
|
/*
|
|
* motor_current_send_tlm(now_ms) — transmit JLINK_TLM_MOTOR_CURRENT (0x86)
|
|
* frame to Jetson. Rate-limited to MOTOR_CURR_TLM_HZ; safe to call every tick.
|
|
*/
|
|
void motor_current_send_tlm(uint32_t now_ms);
|
|
|
|
#endif /* MOTOR_CURRENT_H */
|