feat: Motor current monitoring (Issue #584) #594
@ -44,6 +44,7 @@
|
||||
* 0x83 PID_RESULT - jlink_tlm_pid_result_t (13 bytes), sent after PID_SAVE (Issue #531)
|
||||
* 0x84 GIMBAL_STATE - jlink_tlm_gimbal_state_t (10 bytes, Issue #547)
|
||||
* 0x85 SCHED - jlink_tlm_sched_t (1+N*16 bytes), sent on SCHED_GET (Issue #550)
|
||||
* 0x86 MOTOR_CURRENT - jlink_tlm_motor_current_t (8 bytes, Issue #584)
|
||||
*
|
||||
* Priority: CRSF RC always takes precedence. Jetson steer/speed only applied
|
||||
* when mode_manager_active() == MODE_AUTONOMOUS (CH6 high). In RC_MANUAL and
|
||||
@ -81,6 +82,7 @@
|
||||
#define JLINK_TLM_PID_RESULT 0x83u /* jlink_tlm_pid_result_t (13 bytes, Issue #531) */
|
||||
#define JLINK_TLM_GIMBAL_STATE 0x84u /* jlink_tlm_gimbal_state_t (10 bytes, Issue #547) */
|
||||
#define JLINK_TLM_SCHED 0x85u /* jlink_tlm_sched_t (1+N*16 bytes, Issue #550) */
|
||||
#define JLINK_TLM_MOTOR_CURRENT 0x86u /* jlink_tlm_motor_current_t (8 bytes, Issue #584) */
|
||||
|
||||
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
|
||||
typedef struct __attribute__((packed)) {
|
||||
@ -148,6 +150,16 @@ typedef struct __attribute__((packed)) {
|
||||
pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* up to 6 x 16 = 96 bytes */
|
||||
} jlink_tlm_sched_t; /* 1 + 96 = 97 bytes max */
|
||||
|
||||
/* ---- Telemetry MOTOR_CURRENT payload (8 bytes, packed) Issue #584 ---- */
|
||||
/* Published at MOTOR_CURR_TLM_HZ; reports measured current and protection state. */
|
||||
typedef struct __attribute__((packed)) {
|
||||
int32_t current_ma; /* filtered battery/motor current (mA, + = discharge) */
|
||||
uint8_t limit_pct; /* soft-limit reduction applied: 0=none, 100=full cutoff */
|
||||
uint8_t state; /* MotorCurrentState: 0=NORMAL,1=SOFT_LIMIT,2=COOLDOWN */
|
||||
uint8_t fault_count; /* lifetime hard-cutoff trips (saturates at 255) */
|
||||
uint8_t _pad; /* reserved */
|
||||
} jlink_tlm_motor_current_t; /* 8 bytes */
|
||||
|
||||
/* ---- Volatile state (read from main loop) ---- */
|
||||
typedef struct {
|
||||
/* Drive command - updated on JLINK_CMD_DRIVE */
|
||||
@ -232,4 +244,11 @@ void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm);
|
||||
*/
|
||||
JLinkSchedSetBuf *jlink_get_sched_set(void);
|
||||
|
||||
/*
|
||||
* jlink_send_motor_current_tlm(tlm) - transmit JLINK_TLM_MOTOR_CURRENT (0x86)
|
||||
* frame (14 bytes total) to Jetson. Issue #584.
|
||||
* Rate-limiting is handled by motor_current_send_tlm(); call from there only.
|
||||
*/
|
||||
void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm);
|
||||
|
||||
#endif /* JLINK_H */
|
||||
|
||||
121
include/motor_current.h
Normal file
121
include/motor_current.h
Normal file
@ -0,0 +1,121 @@
|
||||
#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 */
|
||||
@ -31,6 +31,36 @@ typedef struct __attribute__((packed)) {
|
||||
uint8_t _pad[48]; /* padding to 64 bytes */
|
||||
} pid_flash_t;
|
||||
|
||||
/* ---- Gain schedule flash storage (Issue #550) ---- */
|
||||
|
||||
/* Maximum number of speed-band entries in the gain schedule table */
|
||||
#define PID_SCHED_MAX_BANDS 6u
|
||||
|
||||
/*
|
||||
* Sector 7 layout (128KB at 0x08060000):
|
||||
* 0x0807FF40 pid_sched_flash_t (128 bytes) — gain schedule record
|
||||
* 0x0807FFC0 pid_flash_t ( 64 bytes) — single PID record (existing)
|
||||
* Both records are written in a single sector erase via pid_flash_save_all().
|
||||
*/
|
||||
#define PID_SCHED_FLASH_ADDR 0x0807FF40UL
|
||||
#define PID_SCHED_MAGIC 0x534C5402UL /* 'SLT\x02' — version 2 */
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
float speed_mps; /* velocity breakpoint (m/s) */
|
||||
float kp;
|
||||
float ki;
|
||||
float kd;
|
||||
} pid_sched_entry_t; /* 16 bytes */
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint32_t magic; /* PID_SCHED_MAGIC when valid */
|
||||
uint8_t num_bands; /* valid entries (1..PID_SCHED_MAX_BANDS) */
|
||||
uint8_t flags; /* reserved, must be 0 */
|
||||
uint8_t _pad0[2];
|
||||
pid_sched_entry_t bands[PID_SCHED_MAX_BANDS]; /* 6 × 16 = 96 bytes */
|
||||
uint8_t _pad1[24]; /* total = 4+1+1+2+96+24 = 128 bytes */
|
||||
} pid_sched_flash_t; /* 128 bytes */
|
||||
|
||||
/*
|
||||
* pid_flash_load() — read saved PID from flash.
|
||||
* Returns true and fills *kp/*ki/*kd if magic is valid.
|
||||
@ -39,10 +69,27 @@ typedef struct __attribute__((packed)) {
|
||||
bool pid_flash_load(float *kp, float *ki, float *kd);
|
||||
|
||||
/*
|
||||
* pid_flash_save() — erase sector 7 and write Kp/Ki/Kd.
|
||||
* pid_flash_save() — erase sector 7 and write Kp/Ki/Kd (single-PID only).
|
||||
* Use pid_flash_save_all() to save both single-PID and schedule atomically.
|
||||
* Must not be called while armed (flash erase takes ~1s and stalls the CPU).
|
||||
* Returns true on success.
|
||||
*/
|
||||
bool pid_flash_save(float kp, float ki, float kd);
|
||||
|
||||
/*
|
||||
* pid_flash_load_schedule() — read gain schedule from flash.
|
||||
* Returns true and fills out_entries[0..n-1] and *out_n if magic is valid.
|
||||
* Returns false if no valid schedule stored.
|
||||
*/
|
||||
bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n);
|
||||
|
||||
/*
|
||||
* pid_flash_save_all() — erase sector 7 once and atomically write both:
|
||||
* - pid_sched_flash_t at PID_SCHED_FLASH_ADDR (0x0807FF40)
|
||||
* - pid_flash_t at PID_FLASH_STORE_ADDR (0x0807FFC0)
|
||||
* Must not be called while armed. Returns true on success.
|
||||
*/
|
||||
bool pid_flash_save_all(float kp_single, float ki_single, float kd_single,
|
||||
const pid_sched_entry_t *entries, uint8_t num_bands);
|
||||
|
||||
#endif /* PID_FLASH_H */
|
||||
|
||||
25
src/jlink.c
25
src/jlink.c
@ -482,6 +482,31 @@ void jlink_send_gimbal_state(const jlink_tlm_gimbal_state_t *state)
|
||||
jlink_tx_locked(frame, sizeof(frame));
|
||||
}
|
||||
|
||||
/* ---- jlink_send_motor_current_tlm() -- Issue #584 ---- */
|
||||
void jlink_send_motor_current_tlm(const jlink_tlm_motor_current_t *tlm)
|
||||
{
|
||||
/*
|
||||
* Frame: [STX][LEN][0x86][8 bytes MOTOR_CURRENT][CRC_hi][CRC_lo][ETX]
|
||||
* LEN = 1 (CMD) + 8 (payload) = 9; total frame = 14 bytes.
|
||||
* At 921600 baud: 14x10/921600 ~0.15 ms -- safe to block.
|
||||
*/
|
||||
static uint8_t frame[14];
|
||||
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_motor_current_t); /* 8 */
|
||||
const uint8_t len = 1u + plen; /* 9 */
|
||||
|
||||
frame[0] = JLINK_STX;
|
||||
frame[1] = len;
|
||||
frame[2] = JLINK_TLM_MOTOR_CURRENT;
|
||||
memcpy(&frame[3], tlm, plen);
|
||||
|
||||
uint16_t crc = crc16_xmodem(&frame[2], len);
|
||||
frame[3 + plen] = (uint8_t)(crc >> 8);
|
||||
frame[3 + plen + 1] = (uint8_t)(crc & 0xFFu);
|
||||
frame[3 + plen + 2] = JLINK_ETX;
|
||||
|
||||
jlink_tx_locked(frame, sizeof(frame));
|
||||
}
|
||||
|
||||
/* ---- jlink_send_sched_telemetry() -- Issue #550 ---- */
|
||||
void jlink_send_sched_telemetry(const jlink_tlm_sched_t *tlm)
|
||||
{
|
||||
|
||||
183
src/motor_current.c
Normal file
183
src/motor_current.c
Normal file
@ -0,0 +1,183 @@
|
||||
/*
|
||||
* 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);
|
||||
}
|
||||
104
src/pid_flash.c
104
src/pid_flash.c
@ -70,3 +70,107 @@ bool pid_flash_save(float kp, float ki, float kd)
|
||||
stored->ki == ki &&
|
||||
stored->kd == kd);
|
||||
}
|
||||
|
||||
/* ---- Helper: write arbitrary bytes as 32-bit words ---- */
|
||||
/*
|
||||
* Writes 'len' bytes from 'src' to flash at 'addr'.
|
||||
* len must be a multiple of 4. Flash must already be unlocked.
|
||||
* Returns HAL_OK on success, or first failure status.
|
||||
*/
|
||||
static HAL_StatusTypeDef flash_write_words(uint32_t addr,
|
||||
const void *src,
|
||||
uint32_t len)
|
||||
{
|
||||
const uint32_t *p = (const uint32_t *)src;
|
||||
HAL_StatusTypeDef rc = HAL_OK;
|
||||
for (uint32_t i = 0; i < len / 4u; i++) {
|
||||
rc = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, addr, p[i]);
|
||||
if (rc != HAL_OK) return rc;
|
||||
addr += 4u;
|
||||
}
|
||||
return HAL_OK;
|
||||
}
|
||||
|
||||
/* ---- pid_flash_load_schedule() ---- */
|
||||
bool pid_flash_load_schedule(pid_sched_entry_t *out_entries, uint8_t *out_n)
|
||||
{
|
||||
const pid_sched_flash_t *p = (const pid_sched_flash_t *)PID_SCHED_FLASH_ADDR;
|
||||
|
||||
if (p->magic != PID_SCHED_MAGIC) return false;
|
||||
if (p->num_bands == 0u || p->num_bands > PID_SCHED_MAX_BANDS) return false;
|
||||
|
||||
*out_n = p->num_bands;
|
||||
for (uint8_t i = 0; i < p->num_bands; i++) {
|
||||
out_entries[i] = p->bands[i];
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ---- pid_flash_save_all() ---- */
|
||||
bool pid_flash_save_all(float kp_single, float ki_single, float kd_single,
|
||||
const pid_sched_entry_t *entries, uint8_t num_bands)
|
||||
{
|
||||
if (num_bands == 0u || num_bands > PID_SCHED_MAX_BANDS) return false;
|
||||
|
||||
HAL_StatusTypeDef rc;
|
||||
|
||||
rc = HAL_FLASH_Unlock();
|
||||
if (rc != HAL_OK) return false;
|
||||
|
||||
/* Single erase of sector 7 covers both records */
|
||||
FLASH_EraseInitTypeDef erase = {
|
||||
.TypeErase = FLASH_TYPEERASE_SECTORS,
|
||||
.Sector = PID_FLASH_SECTOR,
|
||||
.NbSectors = 1,
|
||||
.VoltageRange = PID_FLASH_SECTOR_VOLTAGE,
|
||||
};
|
||||
uint32_t sector_error = 0;
|
||||
rc = HAL_FLASHEx_Erase(&erase, §or_error);
|
||||
if (rc != HAL_OK || sector_error != 0xFFFFFFFFUL) {
|
||||
HAL_FLASH_Lock();
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Build and write schedule record at PID_SCHED_FLASH_ADDR */
|
||||
pid_sched_flash_t srec;
|
||||
memset(&srec, 0xFF, sizeof(srec));
|
||||
srec.magic = PID_SCHED_MAGIC;
|
||||
srec.num_bands = num_bands;
|
||||
srec.flags = 0u;
|
||||
for (uint8_t i = 0; i < num_bands; i++) {
|
||||
srec.bands[i] = entries[i];
|
||||
}
|
||||
|
||||
rc = flash_write_words(PID_SCHED_FLASH_ADDR, &srec, sizeof(srec));
|
||||
if (rc != HAL_OK) {
|
||||
HAL_FLASH_Lock();
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Build and write single-PID record at PID_FLASH_STORE_ADDR */
|
||||
pid_flash_t prec;
|
||||
memset(&prec, 0xFF, sizeof(prec));
|
||||
prec.magic = PID_FLASH_MAGIC;
|
||||
prec.kp = kp_single;
|
||||
prec.ki = ki_single;
|
||||
prec.kd = kd_single;
|
||||
|
||||
rc = flash_write_words(PID_FLASH_STORE_ADDR, &prec, sizeof(prec));
|
||||
if (rc != HAL_OK) {
|
||||
HAL_FLASH_Lock();
|
||||
return false;
|
||||
}
|
||||
|
||||
HAL_FLASH_Lock();
|
||||
|
||||
/* Verify both records */
|
||||
const pid_sched_flash_t *sv = (const pid_sched_flash_t *)PID_SCHED_FLASH_ADDR;
|
||||
const pid_flash_t *pv = (const pid_flash_t *)PID_FLASH_STORE_ADDR;
|
||||
|
||||
return (sv->magic == PID_SCHED_MAGIC &&
|
||||
sv->num_bands == num_bands &&
|
||||
pv->magic == PID_FLASH_MAGIC &&
|
||||
pv->kp == kp_single &&
|
||||
pv->ki == ki_single &&
|
||||
pv->kd == kd_single);
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user