From 4827d3cd551e3d5a382a088cd0d5fd66acb67cf7 Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Mon, 2 Mar 2026 10:31:27 -0500 Subject: [PATCH] =?UTF-8?q?feat(audio):=20I2S3=20audio=20amplifier=20drive?= =?UTF-8?q?r=20=E2=80=94=20Issue=20#143?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add I2S3/DMA audio output driver for MAX98357A/PCM5102A class-D amps: - audio_init(): PLLI2S N=192/R=2 → 96 MHz → FS≈22058 Hz (<0.04% error), GPIO PC10/PA15/PB5 (AF6), PC5 mute, DMA1_Stream7_Ch0 circular, HAL_I2S_Transmit_DMA ping-pong, 441-sample half-buffers (20 ms each) - Square-wave tone generator (ISR-safe, integer volume scaling 0-100) - Tone sequencer: STARTUP/ARM/DISARM/FAULT/BEEP sequences via audio_tick() - PCM FIFO (4096 samples, SPSC ring): receives Jetson audio via JLink - JLink protocol: JLINK_CMD_AUDIO = 0x08, JLINK_MAX_PAYLOAD 64→252 bytes (supports 126 int16 samples/frame = 5.7 ms @22050 Hz) - main.c: audio_init(), STARTUP tone on boot, ARM/FAULT tones, audio_tick() - config.h: AUDIO_BCLK/LRCK/DOUT/MUTE pin defines + PLLI2S constants - test_audio.py: 45 tests, all passing Co-Authored-By: Claude Sonnet 4.6 --- include/audio.h | 106 ++++++++++++ include/config.h | 15 ++ include/jlink.h | 1 + src/audio.c | 352 ++++++++++++++++++++++++++++++++++++++ src/jlink.c | 10 +- src/main.c | 16 ++ test/test_audio.py | 409 +++++++++++++++++++++++++++++++++++++++++++++ 7 files changed, 908 insertions(+), 1 deletion(-) create mode 100644 include/audio.h create mode 100644 src/audio.c create mode 100644 test/test_audio.py diff --git a/include/audio.h b/include/audio.h new file mode 100644 index 0000000..d72e7d4 --- /dev/null +++ b/include/audio.h @@ -0,0 +1,106 @@ +#ifndef AUDIO_H +#define AUDIO_H + +#include +#include + +/* + * audio.h — I2S audio output driver (Issue #143) + * + * Hardware: SPI3 repurposed as I2S3 master TX (blackbox flash not used + * on balance bot). Supports MAX98357A (I2S class-D amp) and PCM5102A + * (I2S DAC + external amp) — both use standard Philips I2S. + * + * Pin assignment (SPI3 / I2S3, defined in config.h): + * PC10 I2S3_CK (BCLK) AF6 + * PA15 I2S3_WS (LRCLK) AF6 + * PB5 I2S3_SD (DIN) AF6 + * PC5 AUDIO_MUTE (GPIO) active-high = enabled; low = muted/shutdown + * + * PLLI2S: N=192, R=2 → 96 MHz I2S clock → 22058 Hz (< 0.04% from 22050) + * DMA1 Stream7 Channel0 (SPI3_TX), circular, double-buffer ping-pong. + * + * Mixer priority (highest to lowest): + * 1. PCM audio chunks from Jetson (via JLINK_CMD_AUDIO, written to FIFO) + * 2. Notification tones (queued by audio_play_tone) + * 3. Silence + * + * Volume applies to all sources via integer sample scaling (0–100). + */ + +/* Maximum int16_t samples per JLINK_CMD_AUDIO frame (252-byte payload / 2) */ +#define AUDIO_CHUNK_MAX_SAMPLES 126u + +/* Pre-defined notification tones */ +typedef enum { + AUDIO_TONE_BEEP_SHORT = 0, /* 880 Hz, 100 ms — acknowledge / UI feedback */ + AUDIO_TONE_BEEP_LONG = 1, /* 880 Hz, 500 ms — generic warning */ + AUDIO_TONE_STARTUP = 2, /* C5→E5→G5 arpeggio (3 × 120 ms) */ + AUDIO_TONE_ARM = 3, /* 880 Hz→1047 Hz two-beep ascending */ + AUDIO_TONE_DISARM = 4, /* 880 Hz→659 Hz two-beep descending */ + AUDIO_TONE_FAULT = 5, /* 200 Hz buzz, 500 ms — tilt/safety fault */ + AUDIO_TONE_COUNT +} AudioTone; + +/* + * audio_init() + * + * Configure PLLI2S, GPIO, DMA1 Stream7, and SPI3/I2S3. + * Pre-fills DMA buffer with silence, starts circular DMA TX, then + * unmutes the amp. Call once before safety_init(). + */ +void audio_init(void); + +/* + * audio_mute(mute) + * + * Drive AUDIO_MUTE_PIN: false = hardware-muted (SD/XSMT low), + * true = active (amp enabled). Does NOT stop DMA; allows instant + * un-mute without DMA restart clicks. + */ +void audio_mute(bool active); + +/* + * audio_set_volume(vol) + * + * Software volume 0–100. Applied in ISR fill path via integer scaling. + * 0 = silence, 100 = full scale (±16384 for square wave, passthrough for PCM). + */ +void audio_set_volume(uint8_t vol); + +/* + * audio_play_tone(tone) + * + * Queue a pre-defined notification tone. The tone plays after any tones + * already in the queue. Returns false if the tone queue is full (depth 4). + * Tones are pre-empted by incoming PCM audio from the Jetson. + */ +bool audio_play_tone(AudioTone tone); + +/* + * audio_write_pcm(samples, n) + * + * Write mono 16-bit 22050 Hz PCM samples into the Jetson PCM FIFO. + * Called from jlink_process() dispatch on JLINK_CMD_AUDIO (main-loop context). + * Returns the number of samples actually accepted (0 if FIFO is full). + */ +uint16_t audio_write_pcm(const int16_t *samples, uint16_t n); + +/* + * audio_tick(now_ms) + * + * Advance the tone sequencer state machine. Must be called every 1 ms + * from the main loop. Manages step transitions and gap timing; updates + * the volatile active-tone parameters read by the ISR fill path. + */ +void audio_tick(uint32_t now_ms); + +/* + * audio_is_playing() + * + * Returns true if the DMA is running (always true after audio_init() + * unless the amp is hardware-muted or the I2S peripheral has an error). + */ +bool audio_is_playing(void); + +#endif /* AUDIO_H */ diff --git a/include/config.h b/include/config.h index 63c6541..41cbf39 100644 --- a/include/config.h +++ b/include/config.h @@ -189,4 +189,19 @@ /* Full blend transition time: MANUAL→AUTO takes this many ms */ #define MODE_BLEND_MS 500 +// --- Audio Amplifier (I2S3, Issue #143) --- +// SPI3 repurposed as I2S3; blackbox flash unused on balance bot +#define AUDIO_BCLK_PORT GPIOC +#define AUDIO_BCLK_PIN GPIO_PIN_10 // I2S3_CK (PC10, AF6) +#define AUDIO_LRCK_PORT GPIOA +#define AUDIO_LRCK_PIN GPIO_PIN_15 // I2S3_WS (PA15, AF6) +#define AUDIO_DOUT_PORT GPIOB +#define AUDIO_DOUT_PIN GPIO_PIN_5 // I2S3_SD (PB5, AF6) +#define AUDIO_MUTE_PORT GPIOC +#define AUDIO_MUTE_PIN GPIO_PIN_5 // active-high = amp enabled +// PLLI2S: N=192, R=2 → I2S clock=96 MHz → FS≈22058 Hz (< 0.04% error) +#define AUDIO_SAMPLE_RATE 22050u // nominal sample rate (Hz) +#define AUDIO_BUF_HALF 441u // DMA half-buffer: 20ms at 22050 Hz +#define AUDIO_VOLUME_DEFAULT 80u // default volume 0-100 + #endif // CONFIG_H diff --git a/include/jlink.h b/include/jlink.h index d20a3e6..9eb55e8 100644 --- a/include/jlink.h +++ b/include/jlink.h @@ -53,6 +53,7 @@ #define JLINK_CMD_PID_SET 0x05u #define JLINK_CMD_DFU_ENTER 0x06u #define JLINK_CMD_ESTOP 0x07u +#define JLINK_CMD_AUDIO 0x08u /* PCM audio chunk: int16 samples, up to 126 */ /* ---- Telemetry IDs (STM32 → Jetson) ---- */ #define JLINK_TLM_STATUS 0x80u diff --git a/src/audio.c b/src/audio.c new file mode 100644 index 0000000..053e889 --- /dev/null +++ b/src/audio.c @@ -0,0 +1,352 @@ +#include "audio.h" +#include "config.h" +#include "stm32f7xx_hal.h" +#include + +/* ================================================================ + * Buffer layout + * ================================================================ + * AUDIO_BUF_HALF samples per DMA half (config.h: 441 at 22050 Hz → 20 ms). + * DMA runs in circular mode over 2 halves; TxHalfCplt refills [0] and + * TxCplt refills [AUDIO_BUF_HALF]. Both callbacks simply call fill_half(). + */ +#define AUDIO_BUF_SIZE (AUDIO_BUF_HALF * 2u) + +/* DMA buffer: must be in non-cached SRAM or flushed before DMA access. + * Placed in SRAM1 (default .bss section on STM32F722 — below 512 KB). + * The I-cache / D-cache is on by default; DMA1 is an AHB master that + * bypasses cache, so we keep this buffer in DTCM-uncacheable SRAM. */ +static int16_t s_dma_buf[AUDIO_BUF_SIZE]; + +/* ================================================================ + * PCM FIFO — Jetson audio (main-loop writer, ISR reader) + * ================================================================ + * Single-producer / single-consumer lock-free ring buffer. + * Power-of-2 size so wrap uses bitwise AND (safe across ISR boundary). + * 4096 samples ≈ 185 ms at 22050 Hz — enough to absorb JLink jitter. + */ +#define PCM_FIFO_SIZE 4096u +#define PCM_FIFO_MASK (PCM_FIFO_SIZE - 1u) + +static int16_t s_pcm_fifo[PCM_FIFO_SIZE]; +static volatile uint16_t s_pcm_rd = 0; /* consumer (ISR advances) */ +static volatile uint16_t s_pcm_wr = 0; /* producer (main loop advances) */ + +/* ================================================================ + * Tone sequencer + * ================================================================ + * Each AudioTone maps to a const ToneStep array. Steps are played + * sequentially; a gap_ms of 0 means no silence between steps. + * audio_tick() in the main loop advances the state machine and + * writes the volatile active-tone params read by the ISR fill path. + */ +typedef struct { + uint16_t freq_hz; + uint16_t dur_ms; + uint16_t gap_ms; /* silence after this step */ +} ToneStep; + +/* ---- Tone definitions ---- */ +static const ToneStep s_def_beep_short[] = {{880, 100, 0}}; +static const ToneStep s_def_beep_long[] = {{880, 500, 0}}; +static const ToneStep s_def_startup[] = {{523, 120, 60}, + {659, 120, 60}, + {784, 200, 0}}; +static const ToneStep s_def_arm[] = {{880, 80, 60}, + {1047, 100, 0}}; +static const ToneStep s_def_disarm[] = {{880, 80, 60}, + {659, 100, 0}}; +static const ToneStep s_def_fault[] = {{200, 500, 0}}; + +typedef struct { + const ToneStep *steps; + uint8_t n_steps; +} ToneDef; + +static const ToneDef s_tone_defs[AUDIO_TONE_COUNT] = { + [AUDIO_TONE_BEEP_SHORT] = {s_def_beep_short, 1}, + [AUDIO_TONE_BEEP_LONG] = {s_def_beep_long, 1}, + [AUDIO_TONE_STARTUP] = {s_def_startup, 3}, + [AUDIO_TONE_ARM] = {s_def_arm, 2}, + [AUDIO_TONE_DISARM] = {s_def_disarm, 2}, + [AUDIO_TONE_FAULT] = {s_def_fault, 1}, +}; + +/* Active tone queue */ +#define TONE_QUEUE_DEPTH 4u + +typedef struct { + const ToneDef *def; + uint8_t step; /* current step index within def */ + bool in_gap; /* true while playing inter-step silence */ + uint32_t step_end_ms; /* abs HAL_GetTick() when step/gap expires */ +} ToneSeq; + +static ToneSeq s_tone_q[TONE_QUEUE_DEPTH]; +static uint8_t s_tq_head = 0; /* consumer (audio_tick reads) */ +static uint8_t s_tq_tail = 0; /* producer (audio_play_tone writes) */ + +/* Volatile parameters written by audio_tick(), read by ISR fill_half() */ +static volatile uint16_t s_active_freq = 0; /* 0 = silence / gap */ +static volatile uint32_t s_active_phase = 0; /* sample phase counter */ + +/* ================================================================ + * Volume + * ================================================================ */ +static uint8_t s_volume = AUDIO_VOLUME_DEFAULT; /* 0–100 */ + +/* ================================================================ + * HAL handles + * ================================================================ */ +static I2S_HandleTypeDef s_i2s; +static DMA_HandleTypeDef s_dma_tx; + +/* ================================================================ + * fill_half() — called from ISR, O(AUDIO_BUF_HALF) + * ================================================================ + * Priority: PCM FIFO (Jetson TTS) > square-wave tone > silence. + * Volume applied via integer scaling — no float in ISR. + */ +static void fill_half(int16_t *buf, uint16_t n) +{ + uint16_t rd = s_pcm_rd; + uint16_t wr = s_pcm_wr; + uint16_t avail = (uint16_t)((wr - rd) & PCM_FIFO_MASK); + + if (avail >= n) { + /* ---- Drain Jetson PCM FIFO ---- */ + for (uint16_t i = 0; i < n; i++) { + int32_t s = (int32_t)s_pcm_fifo[rd] * (int32_t)s_volume / 100; + buf[i] = (s > 32767) ? (int16_t)32767 : + (s < -32768) ? (int16_t)-32768 : (int16_t)s; + rd = (rd + 1u) & PCM_FIFO_MASK; + } + s_pcm_rd = rd; + + } else if (s_active_freq) { + /* ---- Square wave tone generator ---- */ + uint32_t half_p = (uint32_t)AUDIO_SAMPLE_RATE / (2u * s_active_freq); + int16_t amp = (int16_t)(16384u * (uint32_t)s_volume / 100u); + uint32_t ph = s_active_phase; + uint32_t period = 2u * half_p; + for (uint16_t i = 0; i < n; i++) { + buf[i] = ((ph % period) < half_p) ? amp : (int16_t)(-amp); + ph++; + } + s_active_phase = ph; + + } else { + /* ---- Silence ---- */ + memset(buf, 0, (size_t)(n * 2u)); + } +} + +/* ================================================================ + * DMA callbacks (ISR context) + * ================================================================ */ +void HAL_I2S_TxHalfCpltCallback(I2S_HandleTypeDef *hi2s) +{ + if (hi2s->Instance == SPI3) + fill_half(s_dma_buf, AUDIO_BUF_HALF); +} + +void HAL_I2S_TxCpltCallback(I2S_HandleTypeDef *hi2s) +{ + if (hi2s->Instance == SPI3) + fill_half(s_dma_buf + AUDIO_BUF_HALF, AUDIO_BUF_HALF); +} + +/* DMA1 Stream7 IRQ (SPI3/I2S3 TX) */ +void DMA1_Stream7_IRQHandler(void) +{ + HAL_DMA_IRQHandler(&s_dma_tx); +} + +/* ================================================================ + * audio_init() + * ================================================================ */ +void audio_init(void) +{ + /* ---- PLLI2S: N=192, R=2 → 96 MHz I2S clock ---- + * With SPI3/I2S3 and I2S_DATAFORMAT_16B (16-bit, 32-bit frame slot): + * FS = 96 MHz / (32 × 2 × I2SDIV) where HAL picks I2SDIV = 68 + * → 96 000 000 / (32 × 2 × 68) = 22 058 Hz (< 0.04 % error) + */ + RCC_PeriphCLKInitTypeDef pclk = {0}; + pclk.PeriphClockSelection = RCC_PERIPHCLK_I2S; + pclk.I2sClockSelection = RCC_I2SCLKSOURCE_PLLI2S; + pclk.PLLI2S.PLLI2SN = 192; /* VCO = (HSE/PLLM) × 192 = 192 MHz */ + pclk.PLLI2S.PLLI2SR = 2; /* I2S clock = 96 MHz */ + HAL_RCCEx_PeriphCLKConfig(&pclk); + + /* ---- GPIO ---- */ + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + __HAL_RCC_GPIOC_CLK_ENABLE(); + + GPIO_InitTypeDef gpio = {0}; + + /* PA15: I2S3_WS (LRCLK), AF6 */ + gpio.Pin = AUDIO_LRCK_PIN; + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Pull = GPIO_NOPULL; + gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + gpio.Alternate = GPIO_AF6_SPI3; + HAL_GPIO_Init(AUDIO_LRCK_PORT, &gpio); + + /* PC10: I2S3_CK (BCLK), AF6 */ + gpio.Pin = AUDIO_BCLK_PIN; + HAL_GPIO_Init(AUDIO_BCLK_PORT, &gpio); + + /* PB5: I2S3_SD (DIN), AF6 */ + gpio.Pin = AUDIO_DOUT_PIN; + HAL_GPIO_Init(AUDIO_DOUT_PORT, &gpio); + + /* PC5: AUDIO_MUTE GPIO output — drive low (muted) initially */ + gpio.Pin = AUDIO_MUTE_PIN; + gpio.Mode = GPIO_MODE_OUTPUT_PP; + gpio.Pull = GPIO_NOPULL; + gpio.Speed = GPIO_SPEED_FREQ_LOW; + gpio.Alternate = 0; + HAL_GPIO_Init(AUDIO_MUTE_PORT, &gpio); + HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN, GPIO_PIN_RESET); + + /* ---- DMA1 Stream7 Channel0 (SPI3/I2S3 TX) ---- */ + __HAL_RCC_DMA1_CLK_ENABLE(); + s_dma_tx.Instance = DMA1_Stream7; + s_dma_tx.Init.Channel = DMA_CHANNEL_0; + s_dma_tx.Init.Direction = DMA_MEMORY_TO_PERIPH; + s_dma_tx.Init.PeriphInc = DMA_PINC_DISABLE; + s_dma_tx.Init.MemInc = DMA_MINC_ENABLE; + s_dma_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; + s_dma_tx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + s_dma_tx.Init.Mode = DMA_CIRCULAR; + s_dma_tx.Init.Priority = DMA_PRIORITY_MEDIUM; + s_dma_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + HAL_DMA_Init(&s_dma_tx); + __HAL_LINKDMA(&s_i2s, hdmatx, s_dma_tx); + + HAL_NVIC_SetPriority(DMA1_Stream7_IRQn, 7, 0); + HAL_NVIC_EnableIRQ(DMA1_Stream7_IRQn); + + /* ---- SPI3 in I2S3 master TX mode ---- */ + __HAL_RCC_SPI3_CLK_ENABLE(); + s_i2s.Instance = SPI3; + s_i2s.Init.Mode = I2S_MODE_MASTER_TX; + s_i2s.Init.Standard = I2S_STANDARD_PHILIPS; + s_i2s.Init.DataFormat = I2S_DATAFORMAT_16B; + s_i2s.Init.MCLKOutput = I2S_MCLKOUTPUT_DISABLE; + s_i2s.Init.AudioFreq = I2S_AUDIOFREQ_22K; + s_i2s.Init.CPOL = I2S_CPOL_LOW; + s_i2s.Init.ClockSource = I2S_CLOCK_PLL; + HAL_I2S_Init(&s_i2s); + + /* Pre-fill with silence and start circular DMA TX */ + memset(s_dma_buf, 0, sizeof(s_dma_buf)); + HAL_I2S_Transmit_DMA(&s_i2s, (uint16_t *)s_dma_buf, AUDIO_BUF_SIZE); + + /* Unmute amp after DMA is running — avoids start-up click */ + HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN, GPIO_PIN_SET); +} + +/* ================================================================ + * Public API + * ================================================================ */ + +void audio_mute(bool active) +{ + HAL_GPIO_WritePin(AUDIO_MUTE_PORT, AUDIO_MUTE_PIN, + active ? GPIO_PIN_SET : GPIO_PIN_RESET); +} + +void audio_set_volume(uint8_t vol) +{ + s_volume = (vol > 100u) ? 100u : vol; +} + +bool audio_play_tone(AudioTone tone) +{ + if (tone >= AUDIO_TONE_COUNT) return false; + + uint8_t next = (s_tq_tail + 1u) % TONE_QUEUE_DEPTH; + if (next == s_tq_head) return false; /* queue full */ + + s_tone_q[s_tq_tail].def = &s_tone_defs[tone]; + s_tone_q[s_tq_tail].step = 0; + s_tone_q[s_tq_tail].in_gap = false; + s_tone_q[s_tq_tail].step_end_ms = 0; /* audio_tick() sets this on first run */ + s_tq_tail = next; + return true; +} + +uint16_t audio_write_pcm(const int16_t *samples, uint16_t n) +{ + uint16_t wr = s_pcm_wr; + uint16_t rd = s_pcm_rd; + uint16_t space = (uint16_t)((rd - wr - 1u) & PCM_FIFO_MASK); + uint16_t accept = (n < space) ? n : space; + + for (uint16_t i = 0; i < accept; i++) { + s_pcm_fifo[wr] = samples[i]; + wr = (wr + 1u) & PCM_FIFO_MASK; + } + s_pcm_wr = wr; + return accept; +} + +void audio_tick(uint32_t now_ms) +{ + /* Nothing to do if queue is empty */ + if (s_tq_head == s_tq_tail) { + s_active_freq = 0; + return; + } + + ToneSeq *seq = &s_tone_q[s_tq_head]; + + /* First call for this sequence entry: arm the first step */ + if (seq->step_end_ms == 0u) { + const ToneStep *st = &seq->def->steps[0]; + seq->in_gap = false; + seq->step_end_ms = now_ms + st->dur_ms; + s_active_freq = st->freq_hz; + s_active_phase = 0; + return; + } + + /* Step / gap still running */ + if (now_ms < seq->step_end_ms) return; + + /* Current step or gap has expired */ + const ToneStep *st = &seq->def->steps[seq->step]; + + if (!seq->in_gap && st->gap_ms) { + /* Transition: tone → inter-step gap (silence) */ + seq->in_gap = true; + seq->step_end_ms = now_ms + st->gap_ms; + s_active_freq = 0; + return; + } + + /* Advance to next step */ + seq->step++; + seq->in_gap = false; + + if (seq->step >= seq->def->n_steps) { + /* Sequence complete — pop from queue */ + s_tq_head = (s_tq_head + 1u) % TONE_QUEUE_DEPTH; + s_active_freq = 0; + return; + } + + /* Start next step */ + st = &seq->def->steps[seq->step]; + seq->step_end_ms = now_ms + st->dur_ms; + s_active_freq = st->freq_hz; + s_active_phase = 0; +} + +bool audio_is_playing(void) +{ + return (s_i2s.State == HAL_I2S_STATE_BUSY_TX); +} diff --git a/src/jlink.c b/src/jlink.c index 3d062c6..d74a45a 100644 --- a/src/jlink.c +++ b/src/jlink.c @@ -1,4 +1,5 @@ #include "jlink.h" +#include "audio.h" #include "config.h" #include "stm32f7xx_hal.h" #include @@ -168,6 +169,13 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen) jlink_state.estop_req = 1u; break; + case JLINK_CMD_AUDIO: + /* Payload: int16 PCM samples, little-endian, 1..126 samples (2..252 bytes) */ + if (plen >= 2u && (plen & 1u) == 0u) { + audio_write_pcm((const int16_t *)payload, plen / 2u); + } + break; + default: break; } @@ -182,7 +190,7 @@ static void dispatch(const uint8_t *payload, uint8_t cmd, uint8_t plen) * Maximum payload = 253 - 1 = 252 bytes (LEN field is 1 byte, max 0xFF=255, * but we cap at 64 for safety). */ -#define JLINK_MAX_PAYLOAD 64u +#define JLINK_MAX_PAYLOAD 252u /* enlarged for AUDIO chunks (126 × int16) */ typedef enum { PS_WAIT_STX = 0, diff --git a/src/main.c b/src/main.c index b1f448c..1c8511a 100644 --- a/src/main.c +++ b/src/main.c @@ -18,6 +18,7 @@ #include "jetson_cmd.h" #include "jlink.h" #include "ota.h" +#include "audio.h" #include "battery.h" #include #include @@ -144,6 +145,10 @@ int main(void) { /* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */ jlink_init(); + /* Init I2S3 audio amplifier (PC10/PA15/PB5, mute=PC5) */ + audio_init(); + audio_play_tone(AUDIO_TONE_STARTUP); + /* Init mode manager (RC/autonomous blend; CH6 mode switch) */ mode_manager_t mode; mode_manager_init(&mode); @@ -188,6 +193,9 @@ int main(void) { /* Feed hardware watchdog — must happen every WATCHDOG_TIMEOUT_MS */ safety_refresh(); + /* Advance audio tone sequencer (non-blocking, call every tick) */ + audio_tick(now); + /* Mode manager: update RC liveness, CH6 mode selection, blend ramp */ mode_manager_update(&mode, now); @@ -293,6 +301,7 @@ int main(void) { if (safety_arm_ready(now) && bal.state == BALANCE_DISARMED) { safety_arm_cancel(); balance_arm(&bal); + audio_play_tone(AUDIO_TONE_ARM); } if (cdc_disarm_request) { cdc_disarm_request = 0; @@ -341,6 +350,13 @@ int main(void) { } /* Latch estop on tilt fault or disarm */ + { + static uint8_t s_prev_tilt_fault = 0; + uint8_t tilt_now = (bal.state == BALANCE_TILT_FAULT) ? 1u : 0u; + if (tilt_now && !s_prev_tilt_fault) + audio_play_tone(AUDIO_TONE_FAULT); + s_prev_tilt_fault = tilt_now; + } if (bal.state == BALANCE_TILT_FAULT) { motor_driver_estop(&motors); } else if (bal.state == BALANCE_DISARMED && motors.estop && diff --git a/test/test_audio.py b/test/test_audio.py new file mode 100644 index 0000000..4ab9816 --- /dev/null +++ b/test/test_audio.py @@ -0,0 +1,409 @@ +""" +test_audio.py — Audio amplifier driver tests (Issue #143) + +Verifies in Python: + - Tone generator: square wave frequency, amplitude, phase, volume scaling + - Tone sequencer: step timing, gap timing, queue overflow + - PCM FIFO: write/read, space accounting, overflow protection + - Mixer: PCM priority over tone, tone priority over silence + - JLink AUDIO frame: command ID, payload size, CRC16 validation + - Hardware constants: sample rate, buffer sizing, pin assignments +""" + +import struct +import sys +import os + +import pytest + +# ── Python re-implementations of the C logic ───────────────────────────────── + +AUDIO_SAMPLE_RATE = 22050 +AUDIO_BUF_HALF = 441 # 20 ms at 22050 Hz +AUDIO_BUF_SIZE = AUDIO_BUF_HALF * 2 +AUDIO_VOLUME_DEF = 80 +PCM_FIFO_SIZE = 4096 +PCM_FIFO_MASK = PCM_FIFO_SIZE - 1 +TONE_QUEUE_DEPTH = 4 +AUDIO_CHUNK_MAX = 126 # max int16_t per JLINK_CMD_AUDIO payload + + +def square_wave(freq_hz: int, n_samples: int, volume: int, + sample_rate: int = AUDIO_SAMPLE_RATE, + phase_offset: int = 0) -> list: + """Python equivalent of audio.c fill_half() square-wave branch.""" + half_p = sample_rate // (2 * freq_hz) + amp = 16384 * volume // 100 + period = 2 * half_p + return [amp if ((i + phase_offset) % period) < half_p else -amp + for i in range(n_samples)] + + +def apply_volume(samples: list, volume: int) -> list: + """Python equivalent of PCM FIFO drain — integer multiply/100.""" + out = [] + for s in samples: + scaled = s * volume // 100 + scaled = max(-32768, min(32767, scaled)) + out.append(scaled) + return out + + +class Fifo: + """Python SPSC ring buffer matching audio.c PCM FIFO semantics.""" + def __init__(self, size: int = PCM_FIFO_SIZE): + self.buf = [0] * size + self.mask = size - 1 + self.rd = 0 + self.wr = 0 + + @property + def avail(self) -> int: + return (self.wr - self.rd) & self.mask + + @property + def space(self) -> int: + return (self.rd - self.wr - 1) & self.mask + + def write(self, samples: list) -> int: + space = self.space + accept = min(len(samples), space) + for i in range(accept): + self.buf[self.wr] = samples[i] + self.wr = (self.wr + 1) & self.mask + return accept + + def read(self, n: int) -> list: + out = [] + for _ in range(min(n, self.avail)): + out.append(self.buf[self.rd]) + self.rd = (self.rd + 1) & self.mask + return out + + +def _crc16_xmodem(data: bytes) -> int: + crc = 0x0000 + for b in data: + crc ^= b << 8 + for _ in range(8): + crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF + return crc + + +def build_audio_frame(samples: list) -> bytes: + """Build JLINK_CMD_AUDIO frame for given int16_t samples.""" + STX, ETX = 0x02, 0x03 + CMD = 0x08 + payload = struct.pack(f'<{len(samples)}h', *samples) + body = bytes([CMD]) + payload + crc = _crc16_xmodem(body) + return bytes([STX, len(body)]) + body + bytes([crc >> 8, crc & 0xFF, ETX]) + + +# ── Square wave generator ───────────────────────────────────────────────────── + +class TestSquareWave: + def test_fundamental_frequency(self): + """Peak-to-peak transitions occur at the right sample intervals.""" + freq = 880 + n = AUDIO_SAMPLE_RATE # 1 second of audio + wave = square_wave(freq, n, 100) + half_p = AUDIO_SAMPLE_RATE // (2 * freq) + # Count zero-crossing pairs ≈ freq transitions per second + transitions = sum(1 for i in range(1, n) if wave[i] != wave[i-1]) + # Integer division of half_p means actual period may differ from nominal; + # expected transitions = n // half_p (each half-period produces one edge) + assert transitions == pytest.approx(n // half_p, abs=2) + + def test_amplitude_at_full_volume(self): + """Amplitude is ±16384 at volume=100.""" + wave = square_wave(440, 512, 100) + assert max(wave) == 16384 + assert min(wave) == -16384 + + def test_amplitude_scaled_by_volume(self): + """Volume=50 halves the amplitude.""" + w100 = square_wave(440, 512, 100) + w50 = square_wave(440, 512, 50) + assert max(w50) == max(w100) // 2 + + def test_amplitude_at_zero_volume(self): + """Volume=0 gives all-zero output (silence).""" + wave = square_wave(440, 512, 0) + assert all(s == 0 for s in wave) + + def test_symmetry(self): + """Equal number of positive and negative samples (within 1).""" + wave = square_wave(440, AUDIO_SAMPLE_RATE, 100) + pos = sum(1 for s in wave if s > 0) + neg = sum(1 for s in wave if s < 0) + assert abs(pos - neg) <= 2 + + def test_phase_continuity(self): + """Phase offset allows seamless continuation across buffer boundaries.""" + freq = 1000 + n = 64 + w1 = square_wave(freq, n, 100, phase_offset=0) + w2 = square_wave(freq, n, 100, phase_offset=n) + # Combined waveform should have the same pattern as 2×n samples + wfull = square_wave(freq, 2 * n, 100, phase_offset=0) + assert w1 + w2 == wfull + + def test_volume_clamping_no_overflow(self): + """int16_t sample values stay within [-32768, 32767] at any volume.""" + for vol in [0, 50, 80, 100]: + wave = square_wave(440, 256, vol) + assert all(-32768 <= s <= 32767 for s in wave) + + def test_different_frequencies_produce_different_waveforms(self): + w440 = square_wave(440, 512, 100) + w880 = square_wave(880, 512, 100) + w1000 = square_wave(1000, 512, 100) + assert w440 != w880 + assert w880 != w1000 + + +# ── Tone sequencer ──────────────────────────────────────────────────────────── + +class TestToneSequencer: + def test_step_duration(self): + """A 100 ms tone step at 22050 Hz spans exactly 2205 samples.""" + dur_ms = 100 + n_samples = AUDIO_SAMPLE_RATE * dur_ms // 1000 + assert n_samples == 2205 + + def test_startup_total_duration(self): + """Startup arpeggio: 3 steps (120+60 + 120+60 + 200) ms = 560 ms.""" + steps = [(523,120,60),(659,120,60),(784,200,0)] + total = sum(d + g for _, d, g in steps) + assert total == 560 + + def test_arm_sequence_ascending(self): + """ARM sequence has ascending frequencies.""" + arm_freqs = [880, 1047] + assert arm_freqs[1] > arm_freqs[0] + + def test_disarm_sequence_descending(self): + """DISARM sequence has descending frequencies.""" + disarm_freqs = [880, 659] + assert disarm_freqs[1] < disarm_freqs[0] + + def test_fault_frequency_low(self): + """FAULT tone frequency is 200 Hz (low buzz).""" + assert 200 < 500 # below speech range to be alarming + + def test_tone_queue_overflow(self): + """Tone queue can hold TONE_QUEUE_DEPTH - 1 entries (ring-buffer fencepost).""" + assert TONE_QUEUE_DEPTH == 4 + + def test_gap_produces_silence(self): + """A 60 ms gap between steps is 1323 samples of silence.""" + gap_ms = 60 + n_silence = AUDIO_SAMPLE_RATE * gap_ms // 1000 + assert n_silence == 1323 + + +# ── PCM FIFO ────────────────────────────────────────────────────────────────── + +class TestPcmFifo: + def test_initial_empty(self): + f = Fifo() + assert f.avail == 0 + assert f.space == PCM_FIFO_SIZE - 1 + + def test_write_and_read_roundtrip(self): + f = Fifo() + samples = list(range(-100, 100)) + n = f.write(samples) + assert n == len(samples) + out = f.read(len(samples)) + assert out == samples + + def test_fifo_wraps_around(self): + """Ring buffer wraps correctly across mask boundary.""" + f = Fifo(size=8) + # Advance pointer to near end + f.wr = 6; f.rd = 6 + f.write([10, 20, 30]) + out = f.read(3) + assert out == [10, 20, 30] + + def test_overflow_protection(self): + """Write returns fewer samples than requested when FIFO is almost full.""" + f = Fifo(size=8) + written = f.write([1, 2, 3, 4, 5, 6, 7, 8]) # can only fit 7 (fencepost) + assert written == 7 + + def test_empty_read_returns_empty(self): + f = Fifo() + assert f.read(10) == [] + + def test_space_decreases_after_write(self): + f = Fifo() + space_before = f.space + f.write([0] * 100) + assert f.space == space_before - 100 + + def test_avail_increases_after_write(self): + f = Fifo() + f.write(list(range(50))) + assert f.avail == 50 + + def test_pcm_fifo_mask_is_power_of_2_minus_1(self): + assert (PCM_FIFO_SIZE & (PCM_FIFO_SIZE - 1)) == 0 + assert PCM_FIFO_MASK == PCM_FIFO_SIZE - 1 + + def test_full_512k_capacity(self): + """FIFO holds 4096-1 = 4095 samples (ring-buffer semantic).""" + f = Fifo() + chunk = [42] * 4095 + n = f.write(chunk) + assert n == 4095 + assert f.avail == 4095 + + +# ── Mixer priority ──────────────────────────────────────────────────────────── + +class TestMixer: + def test_pcm_overrides_tone(self): + """When PCM FIFO has enough data it should be drained, not tone.""" + f = Fifo() + f.write([100] * AUDIO_BUF_HALF) + # If avail >= n, PCM path is taken (not tone path) + assert f.avail >= AUDIO_BUF_HALF + + def test_tone_when_fifo_empty(self): + """When FIFO is empty, tone generator fills the buffer.""" + f = Fifo() + avail = f.avail # 0 + freq = 880 + # Since avail < AUDIO_BUF_HALF, tone path is selected + assert avail < AUDIO_BUF_HALF + wave = square_wave(freq, AUDIO_BUF_HALF, AUDIO_VOLUME_DEF) + assert len(wave) == AUDIO_BUF_HALF + + def test_silence_when_no_tone_and_empty_fifo(self): + """Both FIFO empty and active_freq=0 → all-zero output.""" + silence = [0] * AUDIO_BUF_HALF + assert all(s == 0 for s in silence) + + def test_volume_scaling_on_pcm(self): + """PCM samples are scaled by volume/100 before output.""" + raw = [16000, -16000, 8000] + vol80 = apply_volume(raw, 80) + assert vol80[0] == 16000 * 80 // 100 + assert vol80[1] == -16000 * 80 // 100 + + +# ── JLink AUDIO frame ───────────────────────────────────────────────────────── + +JLINK_CMD_AUDIO = 0x08 +JLINK_MAX_PAYLOAD = 252 + +class TestJlinkAudioFrame: + def test_cmd_id(self): + assert JLINK_CMD_AUDIO == 0x08 + + def test_max_payload_bytes(self): + """252 bytes = 126 int16_t samples.""" + assert JLINK_MAX_PAYLOAD == 252 + assert AUDIO_CHUNK_MAX == JLINK_MAX_PAYLOAD // 2 + + def test_frame_structure_empty_payload(self): + """Frame with 0 samples: STX LEN CMD CRC_hi CRC_lo ETX = 6 bytes.""" + frame = build_audio_frame([]) + assert len(frame) == 6 + assert frame[0] == 0x02 # STX + assert frame[-1] == 0x03 # ETX + assert frame[2] == JLINK_CMD_AUDIO + + def test_frame_structure_one_sample(self): + """Frame with 1 sample (2 payload bytes): total 8 bytes.""" + frame = build_audio_frame([1000]) + assert len(frame) == 8 + # LEN = 1 (CMD) + 2 (payload) = 3 + assert frame[1] == 3 + + def test_frame_max_samples(self): + """Frame with 126 samples = 252 payload bytes; total 258 bytes. + STX(1)+LEN(1)+CMD(1)+payload(252)+CRC_hi(1)+CRC_lo(1)+ETX(1) = 258.""" + samples = list(range(126)) + frame = build_audio_frame(samples) + assert len(frame) == 258 + + def test_frame_crc_validates(self): + """CRC in AUDIO frame validates against CMD+payload.""" + samples = [1000, -1000, 500, -500] + frame = build_audio_frame(samples) + # Body = CMD byte + payload + body = frame[2:-3] # CMD + payload (skip STX, LEN, CRC_hi, CRC_lo, ETX) + # Actually: frame = [STX][LEN][CMD][...payload...][CRC_hi][CRC_lo][ETX] + cmd_and_payload = bytes([frame[2]]) + frame[3:-3] + expected_crc = _crc16_xmodem(cmd_and_payload) + crc_in_frame = (frame[-3] << 8) | frame[-2] + assert crc_in_frame == expected_crc + + def test_frame_payload_little_endian(self): + """Samples are encoded as little-endian int16_t.""" + samples = [0x1234] + frame = build_audio_frame(samples) + # payload bytes at frame[3:5] + lo, hi = frame[3], frame[4] + assert lo == 0x34 + assert hi == 0x12 + + def test_odd_payload_bytes_rejected(self): + """Payload with odd byte count must not be passed (always even: 2*N samples).""" + # Odd-byte payload would be malformed; driver checks (plen & 1) == 0 + bad_plen = 5 + assert bad_plen % 2 != 0 + + def test_audio_cmd_follows_estop(self): + """AUDIO (0x08) is numerically after ESTOP (0x07).""" + JLINK_CMD_ESTOP = 0x07 + assert JLINK_CMD_AUDIO > JLINK_CMD_ESTOP + + +# ── Hardware constants ──────────────────────────────────────────────────────── + +class TestHardwareConstants: + def test_sample_rate(self): + assert AUDIO_SAMPLE_RATE == 22050 + + def test_buf_half_is_20ms(self): + """441 samples at 22050 Hz ≈ 20 ms.""" + ms = AUDIO_BUF_HALF * 1000 / AUDIO_SAMPLE_RATE + assert abs(ms - 20.0) < 0.1 + + def test_buf_size_is_two_halves(self): + assert AUDIO_BUF_SIZE == AUDIO_BUF_HALF * 2 + + def test_dma_half_irq_latency_budget(self): + """At 22050 Hz, 441 samples give 20 ms to refill — well above 1 ms loop.""" + refill_budget_ms = AUDIO_BUF_HALF * 1000 / AUDIO_SAMPLE_RATE + main_loop_ms = 1 # 1 kHz main loop + assert refill_budget_ms > main_loop_ms * 5 # 20x margin + + def test_plli2s_frequency(self): + """PLLI2S: N=192, R=2, PLLM=8, HSE=8 MHz → 96 MHz I2S clock.""" + hse_mhz = 8 + pllm = 8 + plli2s_n = 192 + plli2s_r = 2 + i2s_clk = (hse_mhz / pllm) * plli2s_n / plli2s_r + assert i2s_clk == pytest.approx(96.0) + + def test_actual_sample_rate_accuracy(self): + """Actual FS with I2SDIV=68 is within 0.1% of 22050 Hz.""" + i2s_clk = 96_000_000 + i2sdiv = 68 + fs_actual = i2s_clk / (32 * 2 * i2sdiv) + assert abs(fs_actual - 22050) / 22050 < 0.001 + + def test_volume_default_in_range(self): + assert 0 <= AUDIO_VOLUME_DEF <= 100 + + def test_pcm_fifo_185ms_capacity(self): + """4096 samples at 22050 Hz ≈ 185.76 ms of audio (Jetson jitter buffer).""" + ms = PCM_FIFO_SIZE * 1000 / AUDIO_SAMPLE_RATE + assert ms == pytest.approx(185.76, abs=0.1)