seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity

Merge pull request 'feat(power): STOP-mode sleep/wake power manager — Issue #178' (#186) from sl-firmware/issue-178-power-mgmt into main

Browse Source
This commit is contained in:
sl-jetson 2026-03-02 10:56:52 -05:00
commit 077f26d9d6
7 changed files with 1020 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
include/config.h
View File

@ -189,6 +189,18 @@
/* Full blend transition time: MANUAL→AUTO takes this many ms */
#define MODE_BLEND_MS 500
// --- Power Management (STOP mode, Issue #178) ---
#define PM_IDLE_TIMEOUT_MS 30000u // 30s no activity → PM_SLEEP_PENDING
#define PM_FADE_MS 3000u // LED fade-out duration before STOP entry
#define PM_LED_PERIOD_MS 2000u // sleep-pending triangle-wave period (ms)
// Estimated per-subsystem currents (mA) — used for JLINK_TLM_POWER telemetry
#define PM_CURRENT_BASE_MA 30 // SPI1(IMU)+UART4(CRSF)+USART1(JLink)+core
#define PM_CURRENT_AUDIO_MA 8 // I2S3 + amplifier quiescent
#define PM_CURRENT_OSD_MA 5 // SPI2 OSD (MAX7456)
#define PM_CURRENT_DEBUG_MA 1 // UART5 + USART6
#define PM_CURRENT_STOP_MA 1 // MCU in STOP mode (< 1 mA)
#define PM_TLM_HZ 1 // JLINK_TLM_POWER transmit rate (Hz)
// --- Audio Amplifier (I2S3, Issue #143) ---
// SPI3 repurposed as I2S3; blackbox flash unused on balance bot
#define AUDIO_BCLK_PORT GPIOC

19
include/jlink.h
View File

@ -54,9 +54,11 @@
#define JLINK_CMD_DFU_ENTER 0x06u
#define JLINK_CMD_ESTOP 0x07u
#define JLINK_CMD_AUDIO 0x08u /* PCM audio chunk: int16 samples, up to 126 */
#define JLINK_CMD_SLEEP 0x09u /* no payload; request STOP-mode sleep */
/* ---- Telemetry IDs (STM32 → Jetson) ---- */
#define JLINK_TLM_STATUS 0x80u
#define JLINK_TLM_POWER 0x81u /* jlink_tlm_power_t (11 bytes) */
/* ---- Telemetry STATUS payload (20 bytes, packed) ---- */
typedef struct __attribute__((packed)) {
@ -77,6 +79,15 @@ typedef struct __attribute__((packed)) {
uint8_t fw_patch;
} jlink_tlm_status_t; /* 20 bytes */
/* ---- Telemetry POWER payload (11 bytes, packed) ---- */
typedef struct __attribute__((packed)) {
uint8_t power_state; /* PowerState: 0=ACTIVE,1=SLEEP_PENDING,2=SLEEPING,3=WAKING */
uint16_t est_total_ma; /* estimated total current draw (mA) */
uint16_t est_audio_ma; /* estimated I2S3+amp current (mA); 0 if gated */
uint16_t est_osd_ma; /* estimated OSD SPI2 current (mA); 0 if gated */
uint32_t idle_ms; /* ms since last cmd_vel activity */
} jlink_tlm_power_t; /* 11 bytes */
/* ---- Volatile state (read from main loop) ---- */
typedef struct {
/* Drive command — updated on JLINK_CMD_DRIVE */
@ -99,6 +110,8 @@ typedef struct {
/* DFU reboot request — set by parser, cleared by main loop */
volatile uint8_t dfu_req;
/* Sleep request — set by JLINK_CMD_SLEEP, cleared by main loop */
volatile uint8_t sleep_req;
} JLinkState;
extern volatile JLinkState jlink_state;
@ -130,4 +143,10 @@ void jlink_send_telemetry(const jlink_tlm_status_t *status);
*/
void jlink_process(void);
/*
* jlink_send_power_telemetry(power) — build and transmit a JLINK_TLM_POWER
* frame (17 bytes) at PM_TLM_HZ. Call from main loop when not in STOP mode.
*/
void jlink_send_power_telemetry(const jlink_tlm_power_t *power);
#endif /* JLINK_H */

96
include/power_mgmt.h Normal file
View File

@ -0,0 +1,96 @@
#ifndef POWER_MGMT_H
#define POWER_MGMT_H
#include <stdint.h>
#include <stdbool.h>
/*
* power_mgmt — STM32F7 STOP-mode sleep/wake manager (Issue #178).
*
* State machine:
* PM_ACTIVE ──(idle ≥ PM_IDLE_TIMEOUT_MS or sleep cmd)──► PM_SLEEP_PENDING
* PM_SLEEP_PENDING ──(fade complete, ≥ PM_FADE_MS)──► PM_SLEEPING (WFI)
* PM_SLEEPING ──(EXTI wake)──► PM_WAKING ──(clocks restored)──► PM_ACTIVE
*
* Any call to power_mgmt_activity() during SLEEP_PENDING or SLEEPING
* immediately transitions back toward PM_ACTIVE.
*
* Wake sources (EXTI, falling edge on UART idle-high RX pin or IMU INT):
* EXTI1 PA1 UART4_RX — CRSF/ELRS start bit
* EXTI7 PB7 USART1_RX — JLink start bit
* EXTI4 PC4 MPU6000 INT — IMU motion (handler owned by mpu6000.c)
*
* Peripheral gating on sleep entry (clock disable, state preserved):
* Disabled: SPI3/I2S3 (audio amp), SPI2 (OSD), USART6, UART5 (debug)
* Active: SPI1 (IMU), UART4 (CRSF), USART1 (JLink), I2C1 (baro/mag)
*
* Sleep LED (LED1, active-low PC15):
* PM_SLEEP_PENDING: triangle-wave pulse, period PM_LED_PERIOD_MS
* All other states: 0 (caller uses normal LED logic)
*
* IWDG:
* Fed immediately before WFI. STOP wakeup <10 ms typical — well within
* WATCHDOG_TIMEOUT_MS (50 ms).
*
* Safety interlock:
* Caller MUST NOT call power_mgmt_tick() while armed; call
* power_mgmt_activity() instead to keep the idle timer reset.
*
* JLink integration:
* JLINK_CMD_SLEEP (0x09) → power_mgmt_request_sleep()
* Any valid JLink frame → power_mgmt_activity() (handled in main loop)
*/
typedef enum {
PM_ACTIVE = 0, /* Normal, all peripherals running */
PM_SLEEP_PENDING = 1, /* Idle timeout reached; LED fade-out in progress */
PM_SLEEPING = 2, /* In STOP mode (WFI); execution blocked in tick() */
PM_WAKING = 3, /* Transitional; clocks/peripherals being restored */
} PowerState;
/* ---- API ---- */
/*
* power_mgmt_init() — configure wake EXTI lines (EXTI1, EXTI7).
* Call after crsf_init() and jlink_init().
*/
void power_mgmt_init(void);
/*
* power_mgmt_activity() — record cmd_vel event (CRSF frame, JLink frame).
* Resets idle timer; aborts any pending/active sleep.
*/
void power_mgmt_activity(void);
/*
* power_mgmt_request_sleep() — force sleep regardless of idle timer
* (called on JLINK_CMD_SLEEP). Next tick() enters PM_SLEEP_PENDING.
*/
void power_mgmt_request_sleep(void);
/*
* power_mgmt_tick(now_ms) — drive state machine. May block in WFI during
* STOP mode. Returns state after this tick.
* MUST NOT be called while balance_state == BALANCE_ARMED.
*/
PowerState power_mgmt_tick(uint32_t now_ms);
/* power_mgmt_state() — non-blocking read of current state. */
PowerState power_mgmt_state(void);
/*
* power_mgmt_led_brightness() — 0-255 brightness for sleep-pending pulse.
* Returns 0 when not in PM_SLEEP_PENDING; caller uses normal LED logic.
*/
uint8_t power_mgmt_led_brightness(void);
/*
* power_mgmt_current_ma() — estimated total current draw (mA) based on
* gating state; populated in JLINK_TLM_POWER telemetry.
*/
uint16_t power_mgmt_current_ma(void);
/* power_mgmt_idle_ms() — ms elapsed since last power_mgmt_activity() call. */
uint32_t power_mgmt_idle_ms(void);
#endif /* POWER_MGMT_H */

30
src/jlink.c
View File

@ -176,6 +176,11 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen)
}
break;
case JLINK_CMD_SLEEP:
/* Payload-less; main loop calls power_mgmt_request_sleep() */
jlink_state.sleep_req = 1u;
break;
default:
break;
}
@ -290,3 +295,28 @@ void jlink_send_telemetry(const jlink_tlm_status_t *status)
HAL_UART_Transmit(&s_uart, frame, sizeof(frame), 5u);
}
/* ---- jlink_send_power_telemetry() ---- */
void jlink_send_power_telemetry(const jlink_tlm_power_t *power)
{
/*
* Frame: [STX][LEN][0x81][11 bytes POWER][CRC_hi][CRC_lo][ETX]
* LEN = 1 (CMD) + 11 (payload) = 12; total = 17 bytes
* At 921600 baud: 17×10/921600 ≈ 0.18 ms — safe to block.
*/
static uint8_t frame[17];
const uint8_t plen = (uint8_t)sizeof(jlink_tlm_power_t); /* 11 */
const uint8_t len = 1u + plen; /* 12 */
frame[0] = JLINK_STX;
frame[1] = len;
frame[2] = JLINK_TLM_POWER;
memcpy(&frame[3], power, 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;
HAL_UART_Transmit(&s_uart, frame, sizeof(frame), 5u);
}

45
src/main.c
View File

@ -19,6 +19,7 @@
#include "jlink.h"
#include "ota.h"
#include "audio.h"
#include "power_mgmt.h"
#include "battery.h"
#include <math.h>
#include <string.h>
@ -149,6 +150,9 @@ int main(void) {
audio_init();
audio_play_tone(AUDIO_TONE_STARTUP);
/* Init power management — STOP-mode sleep/wake, wake EXTIs configured */
power_mgmt_init();
/* Init mode manager (RC/autonomous blend; CH6 mode switch) */
mode_manager_t mode;
mode_manager_init(&mode);
@ -183,6 +187,8 @@ int main(void) {
uint32_t esc_tick = 0;
uint32_t crsf_telem_tick = 0; /* CRSF uplink telemetry TX timer */
uint32_t jlink_tlm_tick = 0; /* Jetson binary telemetry TX timer */
uint32_t pm_tlm_tick = 0; /* JLINK_TLM_POWER transmit timer */
uint8_t pm_pwm_phase = 0; /* Software PWM counter for sleep LED */
const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */
while (1) {
@ -196,6 +202,23 @@ int main(void) {
/* Advance audio tone sequencer (non-blocking, call every tick) */
audio_tick(now);
/* Sleep LED: software PWM on LED1 (active-low PC15) driven by PM brightness.
* pm_pwm_phase rolls over each ms; brightness sets duty cycle 0-255. */
pm_pwm_phase++;
{
uint8_t pm_bright = power_mgmt_led_brightness();
if (pm_bright > 0u) {
bool led_on = (pm_pwm_phase < pm_bright);
HAL_GPIO_WritePin(LED1_PORT, LED1_PIN,
led_on ? GPIO_PIN_RESET : GPIO_PIN_SET);
}
}
/* Power manager tick — may block in WFI (STOP mode) when disarmed */
if (bal.state != BALANCE_ARMED) {
power_mgmt_tick(now);
}
/* Mode manager: update RC liveness, CH6 mode selection, blend ramp */
mode_manager_update(&mode, now);
@ -249,6 +272,16 @@ int main(void) {
* never returns when disarmed — MCU resets into DFU mode. */
ota_enter_dfu(bal.state == BALANCE_ARMED);
}
if (jlink_state.sleep_req) {
jlink_state.sleep_req = 0u;
power_mgmt_request_sleep();
}
/* Power management: CRSF/JLink activity or armed state resets idle timer */
if (crsf_is_active(now) || jlink_is_active(now) ||
bal.state == BALANCE_ARMED) {
power_mgmt_activity();
}
/* RC CH5 kill switch: disarm immediately if RC is alive and CH5 off.
* Applies regardless of active mode (CH5 always has kill authority). */
@ -424,6 +457,18 @@ int main(void) {
jlink_send_telemetry(&tlm);
}
/* JLINK_TLM_POWER telemetry at PM_TLM_HZ (1 Hz) */
if (now - pm_tlm_tick >= (1000u / PM_TLM_HZ)) {
pm_tlm_tick = now;
jlink_tlm_power_t pow;
pow.power_state = (uint8_t)power_mgmt_state();
pow.est_total_ma = power_mgmt_current_ma();
pow.est_audio_ma = (uint16_t)(power_mgmt_state() == PM_SLEEPING ? 0u : PM_CURRENT_AUDIO_MA);
pow.est_osd_ma = (uint16_t)(power_mgmt_state() == PM_SLEEPING ? 0u : PM_CURRENT_OSD_MA);
pow.idle_ms = power_mgmt_idle_ms();
jlink_send_power_telemetry(&pow);
}
/* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
if (cdc_streaming && imu_calibrated() && now - send_tick >= 20) {
send_tick = now;

251
src/power_mgmt.c Normal file
View File

@ -0,0 +1,251 @@
#include "power_mgmt.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>
/* ---- Internal state ---- */
static PowerState s_state = PM_ACTIVE;
static uint32_t s_last_active = 0;
static uint32_t s_fade_start = 0;
static bool s_sleep_req = false;
static bool s_peripherals_gated = false;
/* ---- EXTI wake-source configuration ---- */
/*
* EXTI1 → PA1 (UART4_RX / CRSF): falling edge (UART start bit)
* EXTI7 → PB7 (USART1_RX / JLink): falling edge
* EXTI4 → PC4 (MPU6000 INT): already configured by mpu6000_init();
* we just ensure IMR bit is set.
*
* GPIO pins remain in their current AF mode; EXTI is pad-level and
* fires independently of the AF setting.
*/
static void enable_wake_exti(void)
{
__HAL_RCC_SYSCFG_CLK_ENABLE();
/* EXTI1: PA1 (UART4_RX) — SYSCFG EXTICR1[7:4] = 0000 (PA) */
SYSCFG->EXTICR[0] = (SYSCFG->EXTICR[0] & ~(0xFu << 4)) | (0x0u << 4);
EXTI->FTSR |= (1u << 1);
EXTI->RTSR &= ~(1u << 1);
EXTI->PR = (1u << 1); /* clear pending */
EXTI->IMR |= (1u << 1);
HAL_NVIC_SetPriority(EXTI1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(EXTI1_IRQn);
/* EXTI7: PB7 (USART1_RX) — SYSCFG EXTICR2[15:12] = 0001 (PB) */
SYSCFG->EXTICR[1] = (SYSCFG->EXTICR[1] & ~(0xFu << 12)) | (0x1u << 12);
EXTI->FTSR |= (1u << 7);
EXTI->RTSR &= ~(1u << 7);
EXTI->PR = (1u << 7);
EXTI->IMR |= (1u << 7);
HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
/* EXTI4: PC4 (MPU6000 INT) — handler in mpu6000.c; just ensure IMR set */
EXTI->IMR |= (1u << 4);
}
static void disable_wake_exti(void)
{
/* Mask UART RX wake EXTIs now that UART peripherals handle traffic */
EXTI->IMR &= ~(1u << 1);
EXTI->IMR &= ~(1u << 7);
/* Leave EXTI4 (IMU data-ready) always unmasked */
}
/* ---- Peripheral clock gating ---- */
/*
* Clock-only gate (no force-reset): peripheral register state is preserved.
* On re-enable, DMA circular transfers resume without reinitialisation.
*/
static void gate_peripherals(void)
{
if (s_peripherals_gated) return;
__HAL_RCC_SPI3_CLK_DISABLE(); /* I2S3 / audio amplifier */
__HAL_RCC_SPI2_CLK_DISABLE(); /* OSD MAX7456 */
__HAL_RCC_USART6_CLK_DISABLE(); /* legacy Jetson CDC */
__HAL_RCC_UART5_CLK_DISABLE(); /* debug UART */
s_peripherals_gated = true;
}
static void ungate_peripherals(void)
{
if (!s_peripherals_gated) return;
__HAL_RCC_SPI3_CLK_ENABLE();
__HAL_RCC_SPI2_CLK_ENABLE();
__HAL_RCC_USART6_CLK_ENABLE();
__HAL_RCC_UART5_CLK_ENABLE();
s_peripherals_gated = false;
}
/* ---- PLL clock restore after STOP mode ---- */
/*
* After STOP wakeup SYSCLK = HSI (16 MHz). Re-lock PLL for 216 MHz.
* PLLM=8, PLLN=216, PLLP=2, PLLQ=9 — STM32F722 @ 216 MHz, HSI source.
*
* HAL_RCC_ClockConfig() calls HAL_InitTick() which resets uwTick to 0;
* save and restore it so existing timeouts remain valid across sleep.
*/
extern volatile uint32_t uwTick;
static void restore_clocks(void)
{
uint32_t saved_tick = uwTick;
RCC_OscInitTypeDef osc = {0};
osc.OscillatorType = RCC_OSCILLATORTYPE_HSI;
osc.HSIState = RCC_HSI_ON;
osc.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
osc.PLL.PLLState = RCC_PLL_ON;
osc.PLL.PLLSource = RCC_PLLSOURCE_HSI;
osc.PLL.PLLM = 8;
osc.PLL.PLLN = 216;
osc.PLL.PLLP = RCC_PLLP_DIV2;
osc.PLL.PLLQ = 9;
HAL_RCC_OscConfig(&osc);
RCC_ClkInitTypeDef clk = {0};
clk.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK |
RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
clk.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
clk.AHBCLKDivider = RCC_SYSCLK_DIV1;
clk.APB1CLKDivider = RCC_HCLK_DIV4; /* 54 MHz */
clk.APB2CLKDivider = RCC_HCLK_DIV2; /* 108 MHz */
HAL_RCC_ClockConfig(&clk, FLASH_LATENCY_7);
uwTick = saved_tick; /* restore — HAL_InitTick() reset it to 0 */
}
/* ---- EXTI IRQ handlers (wake-only: clear pending bit and return) ---- */
/*
* These handlers fire once on wakeup. After restore_clocks() the respective
* UART peripherals resume normal DMA/IDLE-interrupt operation.
*
* NOTE: If EXTI9_5_IRQHandler is already defined elsewhere in the project,
* merge that handler with this one.
*/
void EXTI1_IRQHandler(void)
{
if (EXTI->PR & (1u << 1)) EXTI->PR = (1u << 1);
}
void EXTI9_5_IRQHandler(void)
{
/* Clear any pending EXTI5-9 lines (PB7 = EXTI7 is our primary wake) */
uint32_t pr = EXTI->PR & 0x3E0u;
if (pr) EXTI->PR = pr;
}
/* ---- LED brightness (integer arithmetic, no float, called from main loop) ---- */
/*
* Triangle wave: 0→255→0 over PM_LED_PERIOD_MS.
* Only active during PM_SLEEP_PENDING; returns 0 otherwise.
*/
uint8_t power_mgmt_led_brightness(void)
{
if (s_state != PM_SLEEP_PENDING) return 0u;
uint32_t phase = (HAL_GetTick() - s_fade_start) % PM_LED_PERIOD_MS;
uint32_t half = PM_LED_PERIOD_MS / 2u;
if (phase < half)
return (uint8_t)(phase * 255u / half);
else
return (uint8_t)((PM_LED_PERIOD_MS - phase) * 255u / half);
}
/* ---- Current estimate ---- */
uint16_t power_mgmt_current_ma(void)
{
if (s_state == PM_SLEEPING)
return (uint16_t)PM_CURRENT_STOP_MA;
uint16_t ma = (uint16_t)PM_CURRENT_BASE_MA;
if (!s_peripherals_gated) {
ma += (uint16_t)(PM_CURRENT_AUDIO_MA + PM_CURRENT_OSD_MA +
PM_CURRENT_DEBUG_MA);
}
return ma;
}
/* ---- Idle elapsed ---- */
uint32_t power_mgmt_idle_ms(void)
{
return HAL_GetTick() - s_last_active;
}
/* ---- Public API ---- */
void power_mgmt_init(void)
{
s_state = PM_ACTIVE;
s_last_active = HAL_GetTick();
s_fade_start = 0;
s_sleep_req = false;
s_peripherals_gated = false;
enable_wake_exti();
}
void power_mgmt_activity(void)
{
s_last_active = HAL_GetTick();
if (s_state != PM_ACTIVE) {
s_sleep_req = false;
s_state = PM_WAKING; /* resolved to PM_ACTIVE on next tick() */
}
}
void power_mgmt_request_sleep(void)
{
s_sleep_req = true;
}
PowerState power_mgmt_state(void)
{
return s_state;
}
PowerState power_mgmt_tick(uint32_t now_ms)
{
switch (s_state) {
case PM_ACTIVE:
if (s_sleep_req || (now_ms - s_last_active) >= PM_IDLE_TIMEOUT_MS) {
s_sleep_req = false;
s_fade_start = now_ms;
s_state = PM_SLEEP_PENDING;
}
break;
case PM_SLEEP_PENDING:
if ((now_ms - s_fade_start) >= PM_FADE_MS) {
gate_peripherals();
enable_wake_exti();
s_state = PM_SLEEPING;
/* Feed IWDG: wakeup <10 ms << WATCHDOG_TIMEOUT_MS (50 ms) */
IWDG->KR = 0xAAAAu;
/* === STOP MODE ENTRY — execution resumes here on EXTI wake === */
HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFI);
/* === WAKEUP POINT (< 10 ms latency) === */
restore_clocks();
ungate_peripherals();
disable_wake_exti();
s_last_active = HAL_GetTick();
s_state = PM_ACTIVE;
}
break;
case PM_SLEEPING:
/* Unreachable: WFI is inline in PM_SLEEP_PENDING above */
break;
case PM_WAKING:
/* Set by power_mgmt_activity() during SLEEP_PENDING/SLEEPING */
ungate_peripherals();
s_state = PM_ACTIVE;
break;
}
return s_state;
}

567
test/test_power_mgmt.py Normal file
View File

@ -0,0 +1,567 @@
"""
test_power_mgmt.py — unit tests for Issue #178 power management module.
Models the PM state machine, LED brightness, peripheral gating, current
estimates, JLink protocol extension, and hardware timing budgets in Python.
"""
import struct
import pytest
# ---------------------------------------------------------------------------
# Constants (mirror config.h / power_mgmt.h)
# ---------------------------------------------------------------------------
PM_IDLE_TIMEOUT_MS = 30_000
PM_FADE_MS = 3_000
PM_LED_PERIOD_MS = 2_000
PM_CURRENT_BASE_MA = 30 # SPI1(IMU) + UART4(CRSF) + USART1(JLink) + core
PM_CURRENT_AUDIO_MA = 8 # I2S3 + amp quiescent
PM_CURRENT_OSD_MA = 5 # SPI2 OSD MAX7456
PM_CURRENT_DEBUG_MA = 1 # UART5 + USART6
PM_CURRENT_STOP_MA = 1 # MCU in STOP mode (< 1 mA)
PM_CURRENT_ACTIVE_ALL = (PM_CURRENT_BASE_MA + PM_CURRENT_AUDIO_MA +
PM_CURRENT_OSD_MA + PM_CURRENT_DEBUG_MA)
# JLink additions
JLINK_STX = 0x02
JLINK_ETX = 0x03
JLINK_CMD_SLEEP = 0x09
JLINK_TLM_STATUS = 0x80
JLINK_TLM_POWER = 0x81
# Power states
PM_ACTIVE = 0
PM_SLEEP_PENDING = 1
PM_SLEEPING = 2
PM_WAKING = 3
# WATCHDOG_TIMEOUT_MS from config.h
WATCHDOG_TIMEOUT_MS = 50
# ---------------------------------------------------------------------------
# CRC-16/XModem helper (poly 0x1021, init 0x0000)
# ---------------------------------------------------------------------------
def crc16_xmodem(data: bytes) -> int:
crc = 0x0000
for b in data:
crc ^= b << 8
for _ in range(8):
crc = (crc << 1) ^ 0x1021 if crc & 0x8000 else crc << 1
crc &= 0xFFFF
return crc
def build_frame(cmd: int, payload: bytes = b"") -> bytes:
data = bytes([cmd]) + payload
length = len(data)
crc = crc16_xmodem(data)
return bytes([JLINK_STX, length, *data, crc >> 8, crc & 0xFF, JLINK_ETX])
# ---------------------------------------------------------------------------
# Python model of the power_mgmt state machine (mirrors power_mgmt.c)
# ---------------------------------------------------------------------------
class PowerMgmtSim:
def __init__(self, now: int = 0):
self.state = PM_ACTIVE
self.last_active = now
self.fade_start = 0
self.sleep_req = False
self.peripherals_gated = False
def activity(self, now: int) -> None:
self.last_active = now
if self.state != PM_ACTIVE:
self.sleep_req = False
self.state = PM_WAKING
def request_sleep(self) -> None:
self.sleep_req = True
def led_brightness(self, now: int) -> int:
if self.state != PM_SLEEP_PENDING:
return 0
phase = (now - self.fade_start) % PM_LED_PERIOD_MS
half = PM_LED_PERIOD_MS // 2
if phase < half:
return phase * 255 // half
else:
return (PM_LED_PERIOD_MS - phase) * 255 // half
def current_ma(self) -> int:
if self.state == PM_SLEEPING:
return PM_CURRENT_STOP_MA
ma = PM_CURRENT_BASE_MA
if not self.peripherals_gated:
ma += PM_CURRENT_AUDIO_MA + PM_CURRENT_OSD_MA + PM_CURRENT_DEBUG_MA
return ma
def idle_ms(self, now: int) -> int:
return now - self.last_active
def tick(self, now: int) -> int:
if self.state == PM_ACTIVE:
if self.sleep_req or (now - self.last_active) >= PM_IDLE_TIMEOUT_MS:
self.sleep_req = False
self.fade_start = now
self.state = PM_SLEEP_PENDING
elif self.state == PM_SLEEP_PENDING:
if (now - self.fade_start) >= PM_FADE_MS:
self.peripherals_gated = True
self.state = PM_SLEEPING
# In firmware: WFI blocks here; in test we skip to simulate_wake
elif self.state == PM_WAKING:
self.peripherals_gated = False
self.state = PM_ACTIVE
return self.state
def simulate_wake(self, now: int) -> None:
"""Simulate EXTI wakeup from STOP mode (models HAL_PWR_EnterSTOPMode return)."""
if self.state == PM_SLEEPING:
self.peripherals_gated = False
self.last_active = now
self.state = PM_ACTIVE
# ---------------------------------------------------------------------------
# Tests: Idle timer
# ---------------------------------------------------------------------------
class TestIdleTimer:
def test_stays_active_before_timeout(self):
pm = PowerMgmtSim(now=0)
for t in range(0, PM_IDLE_TIMEOUT_MS, 1000):
assert pm.tick(t) == PM_ACTIVE
def test_enters_sleep_pending_at_timeout(self):
pm = PowerMgmtSim(now=0)
assert pm.tick(PM_IDLE_TIMEOUT_MS - 1) == PM_ACTIVE
assert pm.tick(PM_IDLE_TIMEOUT_MS) == PM_SLEEP_PENDING
def test_activity_resets_idle_timer(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS - 1000)
pm.activity(PM_IDLE_TIMEOUT_MS - 1000) # reset at T=29000
assert pm.tick(PM_IDLE_TIMEOUT_MS) == PM_ACTIVE # 1 s since reset
assert pm.tick(PM_IDLE_TIMEOUT_MS - 1000 + PM_IDLE_TIMEOUT_MS) == PM_SLEEP_PENDING
def test_idle_ms_increases_monotonically(self):
pm = PowerMgmtSim(now=0)
assert pm.idle_ms(0) == 0
assert pm.idle_ms(5000) == 5000
assert pm.idle_ms(29999) == 29999
def test_idle_ms_resets_on_activity(self):
pm = PowerMgmtSim(now=0)
pm.activity(10000)
assert pm.idle_ms(10500) == 500
def test_30s_timeout_matches_spec(self):
assert PM_IDLE_TIMEOUT_MS == 30_000
# ---------------------------------------------------------------------------
# Tests: State machine transitions
# ---------------------------------------------------------------------------
class TestStateMachine:
def test_sleep_req_bypasses_idle_timer(self):
pm = PowerMgmtSim(now=0)
pm.activity(0)
pm.request_sleep()
assert pm.tick(500) == PM_SLEEP_PENDING
def test_fade_complete_enters_sleeping(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS) # → SLEEP_PENDING
assert pm.tick(PM_IDLE_TIMEOUT_MS + PM_FADE_MS) == PM_SLEEPING
def test_fade_not_complete_stays_pending(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS)
assert pm.tick(PM_IDLE_TIMEOUT_MS + PM_FADE_MS - 1) == PM_SLEEP_PENDING
def test_wake_from_stop_returns_active(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS)
pm.tick(PM_IDLE_TIMEOUT_MS + PM_FADE_MS) # → SLEEPING
pm.simulate_wake(PM_IDLE_TIMEOUT_MS + PM_FADE_MS + 5)
assert pm.state == PM_ACTIVE
def test_activity_during_sleep_pending_aborts(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS) # → SLEEP_PENDING
pm.activity(PM_IDLE_TIMEOUT_MS + 100) # abort
assert pm.state == PM_WAKING
pm.tick(PM_IDLE_TIMEOUT_MS + 101)
assert pm.state == PM_ACTIVE
def test_activity_during_sleeping_aborts(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS)
pm.tick(PM_IDLE_TIMEOUT_MS + PM_FADE_MS) # → SLEEPING
pm.activity(PM_IDLE_TIMEOUT_MS + PM_FADE_MS + 3)
assert pm.state == PM_WAKING
pm.tick(PM_IDLE_TIMEOUT_MS + PM_FADE_MS + 4)
assert pm.state == PM_ACTIVE
def test_waking_resolves_on_next_tick(self):
pm = PowerMgmtSim(now=0)
pm.state = PM_WAKING
pm.tick(1000)
assert pm.state == PM_ACTIVE
def test_full_sleep_wake_cycle(self):
pm = PowerMgmtSim(now=0)
# 1. Active
assert pm.tick(100) == PM_ACTIVE
# 2. Idle → sleep pending
assert pm.tick(PM_IDLE_TIMEOUT_MS) == PM_SLEEP_PENDING
# 3. Fade → sleeping
assert pm.tick(PM_IDLE_TIMEOUT_MS + PM_FADE_MS) == PM_SLEEPING
# 4. EXTI wake → active
pm.simulate_wake(PM_IDLE_TIMEOUT_MS + PM_FADE_MS + 8)
assert pm.state == PM_ACTIVE
def test_multiple_sleep_wake_cycles(self):
pm = PowerMgmtSim(now=0)
base = 0
for _ in range(3):
pm.activity(base)
pm.tick(base + PM_IDLE_TIMEOUT_MS)
pm.tick(base + PM_IDLE_TIMEOUT_MS + PM_FADE_MS)
pm.simulate_wake(base + PM_IDLE_TIMEOUT_MS + PM_FADE_MS + 5)
assert pm.state == PM_ACTIVE
base += PM_IDLE_TIMEOUT_MS + PM_FADE_MS + 10
# ---------------------------------------------------------------------------
# Tests: Peripheral gating
# ---------------------------------------------------------------------------
class TestPeripheralGating:
GATED = {'SPI3_I2S3', 'SPI2_OSD', 'USART6', 'UART5_DEBUG'}
ACTIVE = {'SPI1_IMU', 'UART4_CRSF', 'USART1_JLINK', 'I2C1_BARO'}
def test_gated_set_has_four_peripherals(self):
assert len(self.GATED) == 4
def test_no_overlap_between_gated_and_active(self):
assert not (self.GATED & self.ACTIVE)
def test_crsf_uart_not_gated(self):
assert not any('UART4' in p or 'CRSF' in p for p in self.GATED)
def test_jlink_uart_not_gated(self):
assert not any('USART1' in p or 'JLINK' in p for p in self.GATED)
def test_imu_spi_not_gated(self):
assert not any('SPI1' in p or 'IMU' in p for p in self.GATED)
def test_peripherals_gated_on_sleep_entry(self):
pm = PowerMgmtSim(now=0)
assert not pm.peripherals_gated
pm.tick(PM_IDLE_TIMEOUT_MS)
pm.tick(PM_IDLE_TIMEOUT_MS + PM_FADE_MS) # → SLEEPING
assert pm.peripherals_gated
def test_peripherals_ungated_on_wake(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS)
pm.tick(PM_IDLE_TIMEOUT_MS + PM_FADE_MS)
pm.simulate_wake(PM_IDLE_TIMEOUT_MS + PM_FADE_MS + 5)
assert not pm.peripherals_gated
def test_peripherals_not_gated_in_sleep_pending(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS) # → SLEEP_PENDING
assert not pm.peripherals_gated
def test_peripherals_ungated_if_activity_during_pending(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS)
pm.activity(PM_IDLE_TIMEOUT_MS + 100)
pm.tick(PM_IDLE_TIMEOUT_MS + 101)
assert not pm.peripherals_gated
# ---------------------------------------------------------------------------
# Tests: LED brightness
# ---------------------------------------------------------------------------
class TestLedBrightness:
def test_zero_when_active(self):
pm = PowerMgmtSim(now=0)
assert pm.led_brightness(5000) == 0
def test_zero_when_sleeping(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS)
pm.tick(PM_IDLE_TIMEOUT_MS + PM_FADE_MS) # → SLEEPING
assert pm.led_brightness(PM_IDLE_TIMEOUT_MS + PM_FADE_MS + 100) == 0
def test_zero_when_waking(self):
pm = PowerMgmtSim(now=0)
pm.state = PM_WAKING
assert pm.led_brightness(1000) == 0
def test_zero_at_phase_start(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS) # fade_start = PM_IDLE_TIMEOUT_MS
assert pm.led_brightness(PM_IDLE_TIMEOUT_MS) == 0
def test_max_at_half_period(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS)
t = PM_IDLE_TIMEOUT_MS + PM_LED_PERIOD_MS // 2
assert pm.led_brightness(t) == 255
def test_zero_at_full_period(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS)
t = PM_IDLE_TIMEOUT_MS + PM_LED_PERIOD_MS
assert pm.led_brightness(t) == 0
def test_symmetric_about_half_period(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS)
quarter = PM_LED_PERIOD_MS // 4
three_quarter = 3 * PM_LED_PERIOD_MS // 4
b1 = pm.led_brightness(PM_IDLE_TIMEOUT_MS + quarter)
b2 = pm.led_brightness(PM_IDLE_TIMEOUT_MS + three_quarter)
assert abs(b1 - b2) <= 1 # allow 1 LSB for integer division
def test_range_0_to_255(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS)
for dt in range(0, PM_LED_PERIOD_MS * 3, 37):
b = pm.led_brightness(PM_IDLE_TIMEOUT_MS + dt)
assert 0 <= b <= 255
def test_repeats_over_multiple_periods(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS)
# Sample at same phase in periods 0, 1, 2 — should be equal
phase = PM_LED_PERIOD_MS // 3
b0 = pm.led_brightness(PM_IDLE_TIMEOUT_MS + phase)
b1 = pm.led_brightness(PM_IDLE_TIMEOUT_MS + PM_LED_PERIOD_MS + phase)
b2 = pm.led_brightness(PM_IDLE_TIMEOUT_MS + 2 * PM_LED_PERIOD_MS + phase)
assert b0 == b1 == b2
def test_period_is_2s(self):
assert PM_LED_PERIOD_MS == 2000
# ---------------------------------------------------------------------------
# Tests: Power / current estimates
# ---------------------------------------------------------------------------
class TestPowerEstimates:
def test_active_includes_all_subsystems(self):
pm = PowerMgmtSim(now=0)
assert pm.current_ma() == PM_CURRENT_ACTIVE_ALL
def test_sleeping_returns_stop_ma(self):
pm = PowerMgmtSim(now=0)
pm.tick(PM_IDLE_TIMEOUT_MS)
pm.tick(PM_IDLE_TIMEOUT_MS + PM_FADE_MS) # → SLEEPING
assert pm.current_ma() == PM_CURRENT_STOP_MA
def test_gated_returns_base_only(self):
pm = PowerMgmtSim(now=0)
pm.peripherals_gated = True
assert pm.current_ma() == PM_CURRENT_BASE_MA
def test_stop_current_less_than_active(self):
assert PM_CURRENT_STOP_MA < PM_CURRENT_ACTIVE_ALL
def test_stop_current_at_most_1ma(self):
assert PM_CURRENT_STOP_MA <= 1
def test_active_current_reasonable(self):
# Should be < 100 mA (just MCU + peripherals, no motors)
assert PM_CURRENT_ACTIVE_ALL < 100
def test_audio_subsystem_estimate(self):
assert PM_CURRENT_AUDIO_MA > 0
def test_osd_subsystem_estimate(self):
assert PM_CURRENT_OSD_MA > 0
def test_total_equals_sum_of_parts(self):
total = (PM_CURRENT_BASE_MA + PM_CURRENT_AUDIO_MA +
PM_CURRENT_OSD_MA + PM_CURRENT_DEBUG_MA)
assert total == PM_CURRENT_ACTIVE_ALL
# ---------------------------------------------------------------------------
# Tests: JLink protocol extension
# ---------------------------------------------------------------------------
class TestJlinkProtocol:
def test_sleep_cmd_id(self):
assert JLINK_CMD_SLEEP == 0x09
def test_sleep_follows_audio_cmd(self):
JLINK_CMD_AUDIO = 0x08
assert JLINK_CMD_SLEEP == JLINK_CMD_AUDIO + 1
def test_power_tlm_id(self):
assert JLINK_TLM_POWER == 0x81
def test_power_tlm_follows_status_tlm(self):
assert JLINK_TLM_POWER == JLINK_TLM_STATUS + 1
def test_sleep_frame_length(self):
# SLEEP has no payload: STX(1)+LEN(1)+CMD(1)+CRC(2)+ETX(1) = 6
frame = build_frame(JLINK_CMD_SLEEP)
assert len(frame) == 6
def test_sleep_frame_sentinels(self):
frame = build_frame(JLINK_CMD_SLEEP)
assert frame[0] == JLINK_STX
assert frame[-1] == JLINK_ETX
def test_sleep_frame_len_field(self):
frame = build_frame(JLINK_CMD_SLEEP)
assert frame[1] == 1 # LEN = 1 (CMD only, no payload)
def test_sleep_frame_cmd_byte(self):
frame = build_frame(JLINK_CMD_SLEEP)
assert frame[2] == JLINK_CMD_SLEEP
def test_sleep_frame_crc_valid(self):
frame = build_frame(JLINK_CMD_SLEEP)
calc = crc16_xmodem(bytes([JLINK_CMD_SLEEP]))
rx = (frame[-3] << 8) | frame[-2]
assert rx == calc
def test_power_tlm_frame_length(self):
# jlink_tlm_power_t = 11 bytes
# Frame: STX(1)+LEN(1)+CMD(1)+payload(11)+CRC(2)+ETX(1) = 17
POWER_TLM_PAYLOAD_LEN = 11
expected = 1 + 1 + 1 + POWER_TLM_PAYLOAD_LEN + 2 + 1
assert expected == 17
def test_power_tlm_payload_struct(self):
"""jlink_tlm_power_t: u8 power_state, u16 est_total_ma,
u16 est_audio_ma, u16 est_osd_ma, u32 idle_ms = 11 bytes."""
fmt = "<BHHHI"
size = struct.calcsize(fmt)
assert size == 11
def test_power_tlm_frame_crc_valid(self):
power_state = PM_ACTIVE
est_total_ma = PM_CURRENT_ACTIVE_ALL
est_audio_ma = PM_CURRENT_AUDIO_MA
est_osd_ma = PM_CURRENT_OSD_MA
idle_ms = 5000
payload = struct.pack("<BHHHI", power_state, est_total_ma,
est_audio_ma, est_osd_ma, idle_ms)
frame = build_frame(JLINK_TLM_POWER, payload)
assert frame[0] == JLINK_STX
assert frame[-1] == JLINK_ETX
data_for_crc = bytes([JLINK_TLM_POWER]) + payload
expected_crc = crc16_xmodem(data_for_crc)
rx_crc = (frame[-3] << 8) | frame[-2]
assert rx_crc == expected_crc
# ---------------------------------------------------------------------------
# Tests: Wake latency and IWDG budget
# ---------------------------------------------------------------------------
class TestWakeLatencyBudget:
# STM32F722 STOP-mode wakeup: HSI ready ~2 ms + PLL lock ~2 ms ≈ 4 ms
ESTIMATED_WAKE_MS = 10 # conservative upper bound
def test_wake_latency_within_50ms(self):
assert self.ESTIMATED_WAKE_MS < WATCHDOG_TIMEOUT_MS
def test_watchdog_timeout_is_50ms(self):
assert WATCHDOG_TIMEOUT_MS == 50
def test_iwdg_feed_before_wfi_is_safe(self):
# Time from IWDG feed to next feed after wake:
# ~1 ms (loop overhead) + ESTIMATED_WAKE_MS + ~1 ms = ~12 ms
time_from_feed_to_next_ms = 1 + self.ESTIMATED_WAKE_MS + 1
assert time_from_feed_to_next_ms < WATCHDOG_TIMEOUT_MS
def test_fade_ms_positive(self):
assert PM_FADE_MS > 0
def test_fade_ms_less_than_idle_timeout(self):
assert PM_FADE_MS < PM_IDLE_TIMEOUT_MS
def test_stop_mode_wake_much_less_than_50ms(self):
# PLL startup on STM32F7: HSI on (0 ms, already running) +
# PLL lock ~2 ms + SysTick re-init ~0.1 ms ≈ 3 ms
pll_lock_ms = 3
overhead_ms = 1
total_ms = pll_lock_ms + overhead_ms
assert total_ms < 50
def test_wake_exti_sources_count(self):
"""Three wake sources: EXTI1 (CRSF), EXTI7 (JLink), EXTI4 (IMU)."""
wake_sources = ['EXTI1_UART4_CRSF', 'EXTI7_USART1_JLINK', 'EXTI4_IMU_INT']
assert len(wake_sources) == 3
def test_uwTick_must_be_restored_after_stop(self):
"""HAL_RCC_ClockConfig resets uwTick to 0; restore_clocks() saves it."""
# Verify the pattern: save uwTick → HAL calls → restore uwTick
saved_tick = 12345
# Simulate HAL_InitTick() resetting to 0
uw_tick_after_hal = 0
restored = saved_tick # power_mgmt.c: uwTick = saved_tick
assert restored == saved_tick
assert uw_tick_after_hal != saved_tick # HAL reset it
# ---------------------------------------------------------------------------
# Tests: Hardware constants
# ---------------------------------------------------------------------------
class TestHardwareConstants:
def test_pll_params_for_216mhz(self):
"""PLLM=8, PLLN=216, PLLP=2 → VCO=216*2=432 MHz, SYSCLK=216 MHz."""
HSI_MHZ = 16
PLLM = 8
PLLN = 216
PLLP = 2
vco_mhz = HSI_MHZ / PLLM * PLLN
sysclk = vco_mhz / PLLP
assert sysclk == pytest.approx(216.0, rel=1e-6)
def test_apb1_54mhz(self):
"""APB1 = SYSCLK / 4 = 54 MHz."""
assert 216 / 4 == 54
def test_apb2_108mhz(self):
"""APB2 = SYSCLK / 2 = 108 MHz."""
assert 216 / 2 == 108
def test_flash_latency_7_required_at_216mhz(self):
"""STM32F7 at 2.7-3.3 V: 7 wait states for 210-216 MHz."""
FLASH_LATENCY = 7
assert FLASH_LATENCY == 7
def test_exti1_for_pa1(self):
"""SYSCFG EXTICR1[7:4] = 0x0 selects PA for EXTI1."""
PA_SOURCE = 0x0
assert PA_SOURCE == 0x0
def test_exti7_for_pb7(self):
"""SYSCFG EXTICR2[15:12] = 0x1 selects PB for EXTI7."""
PB_SOURCE = 0x1
assert PB_SOURCE == 0x1
def test_exticr_indices(self):
"""EXTI1 → EXTICR[0], EXTI7 → EXTICR[1]."""
assert 1 // 4 == 0 # EXTI1 is in EXTICR[0]
assert 7 // 4 == 1 # EXTI7 is in EXTICR[1]
def test_exti7_shift_in_exticr2(self):
"""EXTI7 field is at bits [15:12] of EXTICR[1] → shift = (7%4)*4 = 12."""
shift = (7 % 4) * 4
assert shift == 12
def test_idle_timeout_30s(self):
assert PM_IDLE_TIMEOUT_MS == 30_000