diff --git a/include/config.h b/include/config.h index 41cbf39..4729252 100644 --- a/include/config.h +++ b/include/config.h @@ -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 diff --git a/include/jlink.h b/include/jlink.h index 9eb55e8..3c70ff4 100644 --- a/include/jlink.h +++ b/include/jlink.h @@ -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 */ diff --git a/include/power_mgmt.h b/include/power_mgmt.h new file mode 100644 index 0000000..391c25b --- /dev/null +++ b/include/power_mgmt.h @@ -0,0 +1,96 @@ +#ifndef POWER_MGMT_H +#define POWER_MGMT_H + +#include +#include + +/* + * 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 */ diff --git a/src/jlink.c b/src/jlink.c index d74a45a..c9a94f3 100644 --- a/src/jlink.c +++ b/src/jlink.c @@ -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); +} diff --git a/src/main.c b/src/main.c index 1c8511a..215adc7 100644 --- a/src/main.c +++ b/src/main.c @@ -19,6 +19,7 @@ #include "jlink.h" #include "ota.h" #include "audio.h" +#include "power_mgmt.h" #include "battery.h" #include #include @@ -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; diff --git a/src/power_mgmt.c b/src/power_mgmt.c new file mode 100644 index 0000000..c5c07d3 --- /dev/null +++ b/src/power_mgmt.c @@ -0,0 +1,251 @@ +#include "power_mgmt.h" +#include "config.h" +#include "stm32f7xx_hal.h" +#include + +/* ---- 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; +} diff --git a/test/test_power_mgmt.py b/test/test_power_mgmt.py new file mode 100644 index 0000000..b7d7c4a --- /dev/null +++ b/test/test_power_mgmt.py @@ -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 = " 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