Compare commits

...

5 Commits

Author SHA1 Message Date
c3ada4a156 feat(audio): I2S3 audio amplifier driver — Issue #143
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 <noreply@anthropic.com>
2026-03-02 10:34:35 -05:00
566cfc8811 Merge pull request 'fix: IWDG reset during gyro recal — refresh at i=0 not i=39 (P0 #42)' (#172) from sl-firmware/gyro-recal-button into main 2026-03-02 10:34:20 -05:00
e43c82d132 Merge pull request 'feat(webui): settings & configuration panel (Issue #160)' (#166) from sl-webui/issue-160-settings into main 2026-03-02 10:27:24 -05:00
da3ee19688 feat(webui): settings & configuration panel (Issue #160)
Some checks failed
social-bot integration tests / Lint (flake8 + pep257) (pull_request) Failing after 2s
social-bot integration tests / Core integration tests (mock sensors, no GPU) (pull_request) Has been skipped
social-bot integration tests / Latency profiling (GPU, Orin) (pull_request) Has been cancelled
- useSettings.js: PID parameter catalogue, step-response simulation,
  ROS2 parameter apply via rcl_interfaces/srv/SetParameters, sensor
  param management, firmware info extraction from /diagnostics,
  diagnostics bundle export, JSON backup/restore, localStorage persist
- SettingsPanel.jsx: 6-view panel (PID, Sensors, Network, Firmware,
  Diagnostics, Backup); StepResponseCanvas with stable/oscillating/
  unstable colour-coding; GainSlider with range+number input; weight-
  class tabs (empty/light/heavy); parameter validation badges
- App.jsx: CONFIG tab group (purple), settings tab render, FLEET_TABS
  set to gate ConnectionBar and footer for fleet/missions/settings

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-03-02 10:26:42 -05:00
cf0a5a3583 fix: IWDG reset during gyro recal — refresh at i=0 not i=39 (P0 #42)
i%40==39 fired the first IWDG refresh only after 40ms of calibration.
Combined with ~10ms of main loop overhead before entering calibrate(),
total elapsed since last refresh could exceed the 50ms IWDG window.

Change to i%40==0: first refresh fires at i=0 (<1ms after entry),
subsequent refreshes every 40ms — safely within the 50ms window.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
2026-02-28 22:04:27 -05:00
11 changed files with 1653 additions and 5 deletions

106
include/audio.h Normal file
View File

@ -0,0 +1,106 @@
#ifndef AUDIO_H
#define AUDIO_H
#include <stdint.h>
#include <stdbool.h>
/*
* 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 (0100).
*/
/* 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 0100. 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 */

View File

@ -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

View File

@ -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

352
src/audio.c Normal file
View File

@ -0,0 +1,352 @@
#include "audio.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>
/* ================================================================
* 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; /* 0100 */
/* ================================================================
* 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);
}

View File

@ -1,4 +1,5 @@
#include "jlink.h"
#include "audio.h"
#include "config.h"
#include "stm32f7xx_hal.h"
#include <string.h>
@ -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,

View File

@ -18,6 +18,7 @@
#include "jetson_cmd.h"
#include "jlink.h"
#include "ota.h"
#include "audio.h"
#include "battery.h"
#include <math.h>
#include <string.h>
@ -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 &&

View File

@ -68,8 +68,10 @@ void mpu6000_calibrate(void) {
sum_gy += raw.gy;
sum_gz += raw.gz;
HAL_Delay(1);
/* Refresh IWDG every 40ms — safe during re-cal with watchdog running */
if (i % 40 == 39) safety_refresh();
/* Refresh IWDG every 40ms, starting immediately (i=0) — the gap between
* safety_refresh() at the top of the main loop and entry here can be
* ~10ms, so we must refresh on i=0 to avoid the 50ms IWDG window. */
if (i % 40 == 0) safety_refresh();
}
s_bias_gx = (float)sum_gx / GYRO_CAL_SAMPLES;

409
test/test_audio.py Normal file
View File

@ -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)

View File

@ -38,6 +38,9 @@ import { FleetPanel } from './components/FleetPanel.jsx';
// Mission planner (issue #145)
import { MissionPlanner } from './components/MissionPlanner.jsx';
// Settings panel (issue #160)
import { SettingsPanel } from './components/SettingsPanel.jsx';
const TAB_GROUPS = [
{
label: 'SOCIAL',
@ -70,8 +73,17 @@ const TAB_GROUPS = [
{ id: 'missions', label: 'Missions' },
],
},
{
label: 'CONFIG',
color: 'text-purple-600',
tabs: [
{ id: 'settings', label: 'Settings' },
],
},
];
const FLEET_TABS = new Set(['fleet', 'missions']);
const DEFAULT_WS_URL = 'ws://localhost:9090';
function ConnectionBar({ url, setUrl, connected, error }) {
@ -142,7 +154,7 @@ export default function App() {
<span className="text-orange-500 font-bold tracking-widest text-sm"> SALTYBOT</span>
<span className="text-cyan-800 text-xs hidden sm:inline">DASHBOARD</span>
</div>
{activeTab !== 'fleet' && (
{!FLEET_TABS.has(activeTab) && (
<ConnectionBar url={wsUrl} setUrl={setWsUrl} connected={connected} error={error} />
)}
</header>
@ -197,11 +209,13 @@ export default function App() {
{activeTab === 'fleet' && <FleetPanel />}
{activeTab === 'missions' && <MissionPlanner />}
{activeTab === 'settings' && <SettingsPanel subscribe={subscribe} callService={callService} connected={connected} wsUrl={wsUrl} />}
</main>
{/* ── Footer ── */}
<footer className="bg-[#070712] border-t border-cyan-950 px-4 py-1.5 flex items-center justify-between text-xs text-gray-700 shrink-0">
{activeTab !== 'fleet' ? (
{!FLEET_TABS.has(activeTab) ? (
<>
<span>rosbridge: <code className="text-gray-600">{wsUrl}</code></span>
<span className={connected ? 'text-green-700' : 'text-red-900'}>

View File

@ -0,0 +1,452 @@
/**
* SettingsPanel.jsx System configuration dashboard.
*
* Sub-views: PID | Sensors | Network | Firmware | Diagnostics | Backup
*
* Props:
* subscribe, callService from useRosbridge
* connected rosbridge connection status
* wsUrl current rosbridge WebSocket URL
*/
import { useState, useEffect, useRef, useCallback } from 'react';
import {
useSettings, PID_PARAMS, SENSOR_PARAMS, PID_NODE,
simulateStepResponse, validatePID,
} from '../hooks/useSettings.js';
const VIEWS = ['PID', 'Sensors', 'Network', 'Firmware', 'Diagnostics', 'Backup'];
function ValidationBadges({ warnings }) {
if (!warnings?.length) return (
<div className="flex items-center gap-1.5 text-xs text-green-400">
<span className="w-1.5 h-1.5 rounded-full bg-green-400 inline-block"/>
All gains within safe bounds
</div>
);
return (
<div className="space-y-1">
{warnings.map((w, i) => (
<div key={i} className={`flex items-start gap-2 text-xs rounded px-2 py-1 border ${
w.level === 'error'
? 'bg-red-950 border-red-800 text-red-400'
: 'bg-amber-950 border-amber-800 text-amber-300'
}`}>
<span className="shrink-0">{w.level === 'error' ? '✕' : '⚠'}</span>
{w.msg}
</div>
))}
</div>
);
}
function ApplyResult({ result }) {
if (!result) return null;
return (
<div className={`text-xs rounded px-2 py-1 border ${
result.ok
? 'bg-green-950 border-green-800 text-green-400'
: 'bg-red-950 border-red-800 text-red-400'
}`}>{result.ok ? '✓ ' : '✕ '}{result.msg}</div>
);
}
function StepResponseCanvas({ kp, ki, kd }) {
const canvasRef = useRef(null);
useEffect(() => {
const canvas = canvasRef.current;
if (!canvas) return;
const ctx = canvas.getContext('2d');
const W = canvas.width, H = canvas.height;
const PAD = { top: 12, right: 16, bottom: 24, left: 40 };
const CW = W - PAD.left - PAD.right;
const CH = H - PAD.top - PAD.bottom;
ctx.clearRect(0, 0, W, H);
ctx.fillStyle = '#050510';
ctx.fillRect(0, 0, W, H);
const data = simulateStepResponse(kp, ki, kd);
if (!data.length) return;
const tail = data.slice(-20);
const maxTail = Math.max(...tail.map(d => Math.abs(d.theta)));
const finalMax = Math.max(...data.map(d => Math.abs(d.theta)));
const isUnstable = finalMax > 45;
const isOscillating = !isUnstable && maxTail > 1.0;
const lineColor = isUnstable ? '#ef4444' : isOscillating ? '#f59e0b' : '#22c55e';
const yMax = Math.min(90, Math.max(10, finalMax * 1.2));
const yMin = -Math.min(5, yMax * 0.1);
const tx = (t) => PAD.left + (t / 2.4) * CW;
const ty = (v) => PAD.top + CH - ((v - yMin) / (yMax - yMin)) * CH;
ctx.strokeStyle = '#0d1b2a'; ctx.lineWidth = 1;
for (let v = -10; v <= 90; v += 5) {
if (v < yMin || v > yMax) continue;
ctx.beginPath(); ctx.moveTo(PAD.left, ty(v)); ctx.lineTo(PAD.left + CW, ty(v)); ctx.stroke();
}
for (let t = 0; t <= 2.4; t += 0.4) {
ctx.beginPath(); ctx.moveTo(tx(t), PAD.top); ctx.lineTo(tx(t), PAD.top + CH); ctx.stroke();
}
ctx.strokeStyle = lineColor; ctx.lineWidth = 2;
ctx.shadowBlur = isUnstable ? 0 : 4; ctx.shadowColor = lineColor;
ctx.beginPath();
data.forEach((d, i) => {
const x = tx(d.t), y = ty(Math.max(yMin, Math.min(yMax, d.theta)));
i === 0 ? ctx.moveTo(x, y) : ctx.lineTo(x, y);
});
ctx.stroke(); ctx.shadowBlur = 0;
ctx.fillStyle = '#4b5563'; ctx.font = '8px monospace'; ctx.textAlign = 'right';
[0, 5, 10, 20, 45, 90].filter(v => v >= yMin && v <= yMax).forEach(v => {
ctx.fillText(`${v}°`, PAD.left - 3, ty(v) + 3);
});
ctx.textAlign = 'center';
[0, 0.5, 1.0, 1.5, 2.0, 2.4].forEach(t => {
ctx.fillText(`${t.toFixed(1)}`, tx(t), PAD.top + CH + 14);
});
const label = isUnstable ? 'UNSTABLE' : isOscillating ? 'OSCILLATING' : 'STABLE';
ctx.fillStyle = lineColor; ctx.font = 'bold 9px monospace'; ctx.textAlign = 'right';
ctx.fillText(label, PAD.left + CW, PAD.top + 10);
}, [kp, ki, kd]);
return (
<canvas ref={canvasRef} width={380} height={140}
className="w-full block rounded bg-gray-950 border border-gray-800" />
);
}
function GainSlider({ param, value, onChange }) {
const pct = ((value - param.min) / (param.max - param.min)) * 100;
return (
<div className="space-y-0.5">
<div className="flex items-center justify-between text-xs">
<span className="text-gray-500">{param.label}</span>
<input type="number"
className="w-16 text-right bg-gray-900 border border-gray-700 rounded px-1 py-0.5 text-xs text-cyan-200 focus:outline-none focus:border-cyan-700"
value={typeof value === 'boolean' ? value : Number(value).toFixed(param.step < 0.1 ? 3 : param.step < 1 ? 2 : 1)}
step={param.step} min={param.min} max={param.max}
onChange={e => onChange(param.key, param.type === 'bool' ? e.target.checked : parseFloat(e.target.value))}
/>
</div>
{param.type !== 'bool' && (
<div className="relative h-1.5 bg-gray-800 rounded overflow-hidden">
<div className="absolute h-full rounded" style={{ width: `${pct}%`, background: '#06b6d4' }} />
<input type="range" className="absolute inset-0 w-full opacity-0 cursor-pointer h-full"
min={param.min} max={param.max} step={param.step} value={value}
onChange={e => onChange(param.key, parseFloat(e.target.value))} />
</div>
)}
</div>
);
}
function PIDView({ gains, setGains, applyPIDGains, applying, applyResult, connected }) {
const [activeClass, setActiveClass] = useState('empty');
const warnings = validatePID(gains);
const classKeys = {
empty: ['kp_empty','ki_empty','kd_empty'],
light: ['kp_light','ki_light','kd_light'],
heavy: ['kp_heavy','ki_heavy','kd_heavy'],
};
const overrideKeys = ['override_enabled','override_kp','override_ki','override_kd'];
const clampKeys = ['kp_min','kp_max','ki_min','ki_max','kd_min','kd_max'];
const miscKeys = ['control_rate','balance_setpoint_rad'];
const handleChange = (key, val) => setGains(g => ({ ...g, [key]: val }));
const previewKp = gains.override_enabled ? gains.override_kp : gains[`kp_${activeClass}`] ?? 15;
const previewKi = gains.override_enabled ? gains.override_ki : gains[`ki_${activeClass}`] ?? 0.5;
const previewKd = gains.override_enabled ? gains.override_kd : gains[`kd_${activeClass}`] ?? 1.5;
const paramsByKey = Object.fromEntries(PID_PARAMS.map(p => [p.key, p]));
return (
<div className="space-y-4">
<div className="flex items-center gap-2 flex-wrap">
<div className="text-cyan-700 text-xs font-bold tracking-widest">PID GAIN EDITOR</div>
<span className={`text-xs px-1.5 py-0.5 rounded border ml-auto ${connected ? 'text-green-400 border-green-800' : 'text-gray-600 border-gray-700'}`}>
{connected ? 'LIVE' : 'OFFLINE'}
</span>
</div>
<div className="space-y-1">
<div className="text-gray-600 text-xs">Simulated step response ({gains.override_enabled ? 'override' : activeClass} gains · 5° disturbance)</div>
<StepResponseCanvas kp={previewKp} ki={previewKi} kd={previewKd} />
</div>
<div className="flex gap-0.5 border-b border-gray-800">
{Object.keys(classKeys).map(c => (
<button key={c} onClick={() => setActiveClass(c)}
className={`px-3 py-1.5 text-xs font-bold tracking-wide border-b-2 transition-colors ${activeClass===c?'border-cyan-500 text-cyan-300':'border-transparent text-gray-600 hover:text-gray-300'}`}
>{c.toUpperCase()}</button>
))}
{[['override','border-amber-500 text-amber-300'],['clamp','border-purple-500 text-purple-300'],['misc','border-gray-400 text-gray-200']].map(([id, activeStyle]) => (
<button key={id} onClick={() => setActiveClass(id)}
className={`px-3 py-1.5 text-xs font-bold tracking-wide border-b-2 transition-colors ${activeClass===id?activeStyle:'border-transparent text-gray-600 hover:text-gray-300'}`}
>{id.toUpperCase()}</button>
))}
</div>
<div className="grid grid-cols-1 sm:grid-cols-3 gap-3">
{(activeClass==='override'?overrideKeys:activeClass==='clamp'?clampKeys:activeClass==='misc'?miscKeys:classKeys[activeClass]).map(key => {
const p = paramsByKey[key]; if (!p) return null;
return <GainSlider key={key} param={p} value={gains[key]??p.default} onChange={handleChange} />;
})}
</div>
<ValidationBadges warnings={warnings} />
<div className="flex gap-2 items-center flex-wrap">
<button onClick={() => applyPIDGains()} disabled={applying||warnings.some(w=>w.level==='error')}
className="px-4 py-1.5 rounded bg-cyan-950 border border-cyan-700 text-cyan-300 hover:bg-cyan-900 text-xs font-bold disabled:opacity-40">
{applying?'Applying…':connected?'Apply to Robot':'Save Locally'}
</button>
{warnings.some(w=>w.level==='error')&&<span className="text-red-500 text-xs">Fix errors before applying</span>}
<ApplyResult result={applyResult} />
</div>
</div>
);
}
function SensorsView({ sensors, setSensors, applySensorParams, applying, applyResult, connected }) {
const handleChange = (key, val) => setSensors(s => ({ ...s, [key]: val }));
const grouped = {};
SENSOR_PARAMS.forEach(p => { if (!grouped[p.node]) grouped[p.node] = []; grouped[p.node].push(p); });
return (
<div className="space-y-4">
<div className="text-cyan-700 text-xs font-bold tracking-widest">SENSOR CONFIGURATION</div>
{Object.entries(grouped).map(([node, params]) => (
<div key={node} className="bg-gray-950 border border-gray-800 rounded-lg p-3 space-y-3">
<div className="text-gray-500 text-xs font-bold font-mono">{node}</div>
<div className="grid grid-cols-1 sm:grid-cols-2 gap-3">
{params.map(p => p.type === 'bool' ? (
<label key={p.key} className="flex items-center gap-2 text-xs cursor-pointer">
<div onClick={() => handleChange(p.key, !sensors[p.key])}
className={`w-8 h-4 rounded-full relative cursor-pointer transition-colors ${sensors[p.key]?'bg-cyan-700':'bg-gray-700'}`}>
<span className={`absolute top-0.5 w-3 h-3 rounded-full bg-white transition-all ${sensors[p.key]?'left-4':'left-0.5'}`}/>
</div>
<span className="text-gray-400">{p.label}</span>
</label>
) : (
<GainSlider key={p.key} param={p} value={sensors[p.key]??p.default} onChange={handleChange} />
))}
</div>
</div>
))}
<div className="flex gap-2 items-center flex-wrap">
<button onClick={() => applySensorParams()} disabled={applying}
className="px-4 py-1.5 rounded bg-cyan-950 border border-cyan-700 text-cyan-300 hover:bg-cyan-900 text-xs font-bold disabled:opacity-40">
{applying?'Applying…':connected?'Apply to Robot':'Save Locally'}
</button>
<ApplyResult result={applyResult} />
</div>
</div>
);
}
function NetworkView({ wsUrl, connected }) {
const [ddsDomain, setDdsDomain] = useState(() => parseInt(localStorage.getItem('saltybot_dds_domain')||'0',10));
const [saved, setSaved] = useState(false);
const urlObj = (() => { try { return new URL(wsUrl); } catch { return null; } })();
const saveDDS = () => { localStorage.setItem('saltybot_dds_domain', String(ddsDomain)); setSaved(true); setTimeout(()=>setSaved(false),2000); };
return (
<div className="space-y-4">
<div className="text-cyan-700 text-xs font-bold tracking-widest">NETWORK SETTINGS</div>
<div className="bg-gray-950 border border-gray-800 rounded-lg p-3 space-y-2">
<div className="text-gray-500 text-xs font-bold">ROSBRIDGE WEBSOCKET</div>
<div className="grid grid-cols-2 gap-2 text-xs">
{[['URL', wsUrl, 'text-cyan-300 font-mono truncate'], ['Host', urlObj?.hostname??'—', 'text-gray-300 font-mono'],
['Port', urlObj?.port??'9090', 'text-gray-300 font-mono'],
['Status', connected?'CONNECTED':'DISCONNECTED', connected?'text-green-400':'text-red-400']
].map(([k, v, cls]) => (<><div key={k} className="text-gray-600">{k}</div><div className={cls}>{v}</div></>))}
</div>
</div>
<div className="bg-gray-950 border border-gray-800 rounded-lg p-3 space-y-3">
<div className="text-gray-500 text-xs font-bold">ROS2 DDS DOMAIN</div>
<div className="flex items-center gap-3">
<label className="text-gray-500 text-xs w-24">Domain ID</label>
<input type="number" min="0" max="232"
className="w-20 bg-gray-900 border border-gray-700 rounded px-2 py-1 text-xs text-gray-200 focus:outline-none focus:border-cyan-700"
value={ddsDomain} onChange={e=>setDdsDomain(parseInt(e.target.value)||0)} />
<button onClick={saveDDS} className="px-3 py-1 rounded bg-cyan-950 border border-cyan-700 text-cyan-300 hover:bg-cyan-900 text-xs">Save</button>
{saved && <span className="text-green-400 text-xs">Saved</span>}
</div>
<div className="text-gray-700 text-xs">Set ROS_DOMAIN_ID on the robot to match. Range 0232.</div>
</div>
<div className="bg-gray-950 border border-gray-800 rounded-lg p-3 space-y-1 text-xs text-gray-600">
<div className="text-gray-500 font-bold text-xs">REFERENCE PORTS</div>
<div>Web dashboard: <code className="text-gray-400">8080</code></div>
<div>Rosbridge: <code className="text-gray-400">ws://&lt;host&gt;:9090</code></div>
<div>RTSP: <code className="text-gray-400">rtsp://&lt;host&gt;:8554/panoramic</code></div>
<div>MJPEG: <code className="text-gray-400">http://&lt;host&gt;:8080/stream?topic=/camera/panoramic/compressed</code></div>
</div>
</div>
);
}
function FirmwareView({ firmwareInfo, startFirmwareWatch, connected }) {
useEffect(() => { if (!connected) return; const unsub = startFirmwareWatch(); return unsub; }, [connected, startFirmwareWatch]);
const formatUptime = (s) => { if (!s) return '—'; return `${Math.floor(s/3600)}h ${Math.floor((s%3600)/60)}m`; };
const rows = [
['STM32 Firmware', firmwareInfo?.stm32Version ?? '—'],
['Jetson SW', firmwareInfo?.jetsonVersion ?? '—'],
['Last OTA Update',firmwareInfo?.lastOtaDate ?? '—'],
['Hostname', firmwareInfo?.hostname ?? '—'],
['ROS Distro', firmwareInfo?.rosDistro ?? '—'],
['Uptime', formatUptime(firmwareInfo?.uptimeS)],
];
return (
<div className="space-y-4">
<div className="flex items-center gap-2">
<div className="text-cyan-700 text-xs font-bold tracking-widest">FIRMWARE INFO</div>
{!connected && <span className="text-gray-600 text-xs">(connect to robot to fetch live info)</span>}
</div>
<div className="bg-gray-950 border border-gray-800 rounded-lg overflow-hidden">
{rows.map(([label, value], i) => (
<div key={label} className={`flex items-center px-4 py-2.5 text-xs ${i%2===0?'bg-gray-950':'bg-[#070712]'}`}>
<span className="text-gray-500 w-36 shrink-0">{label}</span>
<span className={`font-mono ${value==='—'?'text-gray-700':'text-cyan-300'}`}>{value}</span>
</div>
))}
</div>
{!firmwareInfo && connected && (
<div className="text-amber-700 text-xs border border-amber-900 rounded p-2">
Waiting for /diagnostics Ensure firmware diagnostics keys (stm32_fw_version etc.) are published.
</div>
)}
</div>
);
}
function DiagnosticsView({ exportDiagnosticsBundle, subscribe, connected }) {
const [diagData, setDiagData] = useState(null);
const [balanceData, setBalanceData] = useState(null);
const [exporting, setExporting] = useState(false);
useEffect(() => {
if (!connected || !subscribe) return;
const u1 = subscribe('/diagnostics', 'diagnostic_msgs/DiagnosticArray', msg => setDiagData(msg));
const u2 = subscribe('/saltybot/balance_state', 'std_msgs/String', msg => {
try { setBalanceData(JSON.parse(msg.data)); } catch {}
});
return () => { u1?.(); u2?.(); };
}, [connected, subscribe]);
const errorCount = (diagData?.status??[]).filter(s=>s.level>=2).length;
const warnCount = (diagData?.status??[]).filter(s=>s.level===1).length;
const handleExport = () => {
setExporting(true);
exportDiagnosticsBundle(balanceData?{'live':{balanceState:balanceData}}:{}, {});
setTimeout(()=>setExporting(false), 1000);
};
return (
<div className="space-y-4">
<div className="text-cyan-700 text-xs font-bold tracking-widest">DIAGNOSTICS EXPORT</div>
<div className="grid grid-cols-3 gap-3">
{[['ERRORS',errorCount,errorCount?'text-red-400':'text-gray-600'],['WARNINGS',warnCount,warnCount?'text-amber-400':'text-gray-600'],['NODES',diagData?.status?.length??0,'text-gray-400']].map(([l,c,cl])=>(
<div key={l} className="bg-gray-950 border border-gray-800 rounded p-3 text-center">
<div className={`text-2xl font-bold ${cl}`}>{c}</div>
<div className="text-gray-600 text-xs mt-0.5">{l}</div>
</div>
))}
</div>
<div className="bg-gray-950 border border-gray-800 rounded-lg p-4 space-y-3">
<div className="text-gray-400 text-sm font-bold">Download Diagnostics Bundle</div>
<div className="text-gray-600 text-xs">Bundle: PID gains, sensor settings, firmware info, live /diagnostics, balance state, timestamp.</div>
<button onClick={handleExport} disabled={exporting}
className="px-4 py-2 rounded bg-cyan-950 border border-cyan-700 text-cyan-300 hover:bg-cyan-900 text-xs font-bold disabled:opacity-40">
{exporting?'Preparing…':'Download JSON Bundle'}
</button>
<div className="text-gray-700 text-xs">
For rosbag: <code className="text-gray-600">ros2 bag record -o saltybot_diag /diagnostics /saltybot/balance_state /odom</code>
</div>
</div>
{diagData?.status?.length>0 && (
<div className="space-y-1 max-h-64 overflow-y-auto">
{[...diagData.status].sort((a,b)=>(b.level??0)-(a.level??0)).map((s,i)=>(
<div key={i} className={`flex items-center gap-2 px-3 py-1.5 rounded border text-xs ${s.level>=2?'bg-red-950 border-red-800':s.level===1?'bg-amber-950 border-amber-800':'bg-gray-950 border-gray-800'}`}>
<div className={`w-1.5 h-1.5 rounded-full shrink-0 ${s.level>=2?'bg-red-400':s.level===1?'bg-amber-400':'bg-green-400'}`}/>
<span className="text-gray-300 font-bold truncate flex-1">{s.name}</span>
<span className={s.level>=2?'text-red-400':s.level===1?'text-amber-400':'text-gray-600'}>{s.message||(s.level===0?'OK':`L${s.level}`)}</span>
</div>
))}
</div>
)}
</div>
);
}
function BackupView({ exportSettingsJSON, importSettingsJSON }) {
const [importText, setImportText] = useState('');
const [showImport, setShowImport] = useState(false);
const [msg, setMsg] = useState(null);
const showMsg = (text, ok=true) => { setMsg({text,ok}); setTimeout(()=>setMsg(null),4000); };
const handleExport = () => {
const a = document.createElement('a');
a.href = URL.createObjectURL(new Blob([exportSettingsJSON()],{type:'application/json'}));
a.download = `saltybot-settings-${new Date().toISOString().slice(0,10)}.json`;
a.click(); showMsg('Settings exported');
};
const handleImport = () => {
try { importSettingsJSON(importText); setImportText(''); setShowImport(false); showMsg('Settings imported'); }
catch(e) { showMsg('Import failed: '+e.message, false); }
};
return (
<div className="space-y-4 max-w-lg">
<div className="text-cyan-700 text-xs font-bold tracking-widest">BACKUP & RESTORE</div>
<div className="bg-gray-950 border border-gray-800 rounded-lg p-4 space-y-3">
<div className="text-gray-400 text-sm font-bold">Export Settings</div>
<div className="text-gray-600 text-xs">Saves PID gains, sensor config and UI preferences to JSON.</div>
<button onClick={handleExport} className="px-4 py-2 rounded bg-cyan-950 border border-cyan-700 text-cyan-300 hover:bg-cyan-900 text-xs font-bold">Export JSON</button>
</div>
<div className="bg-gray-950 border border-gray-800 rounded-lg p-4 space-y-3">
<div className="text-gray-400 text-sm font-bold">Restore Settings</div>
<div className="text-gray-600 text-xs">Load a previously exported settings JSON. Click Apply after import to push to the robot.</div>
<button onClick={() => setShowImport(s=>!s)} className="px-4 py-2 rounded border border-gray-700 text-gray-400 hover:text-gray-200 text-xs">
{showImport?'Cancel':'Import JSON…'}
</button>
{showImport && (
<div className="space-y-2">
<textarea rows={8} className="w-full bg-gray-900 border border-gray-700 rounded px-2 py-1.5 text-xs text-gray-200 focus:outline-none font-mono"
placeholder="Paste exported settings JSON here…" value={importText} onChange={e=>setImportText(e.target.value)} />
<button disabled={!importText.trim()} onClick={handleImport}
className="px-3 py-1 rounded bg-cyan-950 border border-cyan-700 text-cyan-300 hover:bg-cyan-900 text-xs disabled:opacity-40">Restore</button>
</div>
)}
</div>
{msg && (
<div className={`text-xs rounded px-2 py-1 border ${msg.ok?'bg-green-950 border-green-800 text-green-400':'bg-red-950 border-red-800 text-red-400'}`}>{msg.text}</div>
)}
<div className="bg-gray-950 border border-gray-800 rounded-lg p-3 text-xs text-gray-600">
Settings stored in localStorage key <code className="text-gray-500">saltybot_settings_v1</code>. Export to persist across browsers.
</div>
</div>
);
}
export function SettingsPanel({ subscribe, callService, connected = false, wsUrl = '' }) {
const [view, setView] = useState('PID');
const settings = useSettings({ callService, subscribe });
return (
<div className="space-y-4">
<div className="flex gap-0.5 border-b border-gray-800 overflow-x-auto">
{VIEWS.map(v => (
<button key={v} onClick={() => setView(v)}
className={`px-3 py-2 text-xs font-bold tracking-wider whitespace-nowrap border-b-2 transition-colors ${
view===v ? 'border-cyan-500 text-cyan-300' : 'border-transparent text-gray-600 hover:text-gray-300'
}`}>{v.toUpperCase()}</button>
))}
</div>
{view==='PID' && <PIDView gains={settings.gains} setGains={settings.setGains} applyPIDGains={settings.applyPIDGains} applying={settings.applying} applyResult={settings.applyResult} connected={connected} />}
{view==='Sensors' && <SensorsView sensors={settings.sensors} setSensors={settings.setSensors} applySensorParams={settings.applySensorParams} applying={settings.applying} applyResult={settings.applyResult} connected={connected} />}
{view==='Network' && <NetworkView wsUrl={wsUrl} connected={connected} />}
{view==='Firmware' && <FirmwareView firmwareInfo={settings.firmwareInfo} startFirmwareWatch={settings.startFirmwareWatch} connected={connected} />}
{view==='Diagnostics' && <DiagnosticsView exportDiagnosticsBundle={settings.exportDiagnosticsBundle} subscribe={subscribe} connected={connected} />}
{view==='Backup' && <BackupView exportSettingsJSON={settings.exportSettingsJSON} importSettingsJSON={settings.importSettingsJSON} />}
</div>
);
}

View File

@ -0,0 +1,273 @@
/**
* useSettings.js ROS2 parameter I/O + settings persistence + validation.
*
* Wraps rcl_interfaces/srv/GetParameters and SetParameters calls via
* the rosbridge callService helper passed from useRosbridge.
*
* Known nodes and their parameters:
* adaptive_pid_node PID gains (empty/light/heavy), clamp bounds, overrides
* rtsp_server_node video fps, bitrate, resolution, RTSP port
* uwb_driver_node baudrate, range limits, Kalman filter noise
*
* Validation rules:
* kp_min 540, ki_min 05, kd_min 010
* override gains must be within [min, max]
* control_rate: 50200 Hz
* balance_setpoint_rad: ±0.1 rad
*
* PID step-response simulator (pure JS, no ROS):
* Inverted pendulum: θ''(t) = g/L · θ(t) 1/m· · u(t)
* Parameters: L0.40 m, m3 kg, g=9.81
* Simulate 120 steps at dt=0.02 s (2.4 s total) with 5° step input.
*/
import { useState, useCallback } from 'react';
const STORAGE_KEY = 'saltybot_settings_v1';
// ── Parameter catalogue ────────────────────────────────────────────────────────
export const PID_NODE = 'adaptive_pid_node';
export const PID_PARAMS = [
{ key: 'kp_empty', default: 15.0, min: 0, max: 60, step: 0.5, label: 'Kp empty' },
{ key: 'ki_empty', default: 0.5, min: 0, max: 10, step: 0.05, label: 'Ki empty' },
{ key: 'kd_empty', default: 1.5, min: 0, max: 20, step: 0.1, label: 'Kd empty' },
{ key: 'kp_light', default: 18.0, min: 0, max: 60, step: 0.5, label: 'Kp light' },
{ key: 'ki_light', default: 0.6, min: 0, max: 10, step: 0.05, label: 'Ki light' },
{ key: 'kd_light', default: 2.0, min: 0, max: 20, step: 0.1, label: 'Kd light' },
{ key: 'kp_heavy', default: 22.0, min: 0, max: 60, step: 0.5, label: 'Kp heavy' },
{ key: 'ki_heavy', default: 0.8, min: 0, max: 10, step: 0.05, label: 'Ki heavy' },
{ key: 'kd_heavy', default: 2.5, min: 0, max: 20, step: 0.1, label: 'Kd heavy' },
{ key: 'kp_min', default: 5.0, min: 0, max: 60, step: 0.5, label: 'Kp min (clamp)' },
{ key: 'kp_max', default: 40.0, min: 0, max: 80, step: 0.5, label: 'Kp max (clamp)' },
{ key: 'ki_min', default: 0.0, min: 0, max: 10, step: 0.05, label: 'Ki min (clamp)' },
{ key: 'ki_max', default: 5.0, min: 0, max: 20, step: 0.05, label: 'Ki max (clamp)' },
{ key: 'kd_min', default: 0.0, min: 0, max: 20, step: 0.1, label: 'Kd min (clamp)' },
{ key: 'kd_max', default: 10.0, min: 0, max: 40, step: 0.1, label: 'Kd max (clamp)' },
{ key: 'override_enabled', default: false, type: 'bool', label: 'Override gains' },
{ key: 'override_kp', default: 15.0, min: 0, max: 60, step: 0.5, label: 'Override Kp' },
{ key: 'override_ki', default: 0.5, min: 0, max: 10, step: 0.05, label: 'Override Ki' },
{ key: 'override_kd', default: 1.5, min: 0, max: 20, step: 0.1, label: 'Override Kd' },
{ key: 'control_rate', default: 100.0, min: 50, max: 200, step: 5, label: 'Control rate (Hz)' },
{ key: 'balance_setpoint_rad', default: 0.0, min: -0.1, max: 0.1, step: 0.001, label: 'Balance setpoint (rad)' },
];
export const SENSOR_PARAMS = [
{ node: 'rtsp_server_node', key: 'fps', default: 15, min: 5, max: 60, step: 1, label: 'Camera FPS' },
{ node: 'rtsp_server_node', key: 'bitrate_kbps',default: 4000, min: 500,max: 20000,step: 500, label: 'Camera bitrate (kbps)' },
{ node: 'rtsp_server_node', key: 'rtsp_port', default: 8554, min: 1024,max: 65535,step: 1, label: 'RTSP port' },
{ node: 'rtsp_server_node', key: 'use_nvenc', default: true, type: 'bool', label: 'NVENC hardware encode' },
{ node: 'uwb_driver_node', key: 'max_range_m', default: 8.0, min: 1, max: 20, step: 0.5, label: 'UWB max range (m)' },
{ node: 'uwb_driver_node', key: 'kf_process_noise', default: 0.1, min: 0.001, max: 1.0, step: 0.001, label: 'UWB Kalman noise' },
];
// ── Validation ─────────────────────────────────────────────────────────────────
export function validatePID(gains) {
const warnings = [];
const { kp_empty, ki_empty, kd_empty, kp_max, ki_max, kd_max,
override_enabled, override_kp, override_ki, override_kd,
control_rate, balance_setpoint_rad } = gains;
if (kp_empty > 35)
warnings.push({ level: 'warn', msg: `Kp empty (${kp_empty}) is high — risk of oscillation.` });
if (ki_empty > 3)
warnings.push({ level: 'warn', msg: `Ki empty (${ki_empty}) is high — risk of integral windup.` });
if (kp_empty > kp_max)
warnings.push({ level: 'error', msg: `Kp empty (${kp_empty}) exceeds kp_max (${kp_max}).` });
if (ki_empty > ki_max)
warnings.push({ level: 'error', msg: `Ki empty (${ki_empty}) exceeds ki_max (${ki_max}).` });
if (kp_empty > 0 && kd_empty / kp_empty < 0.05)
warnings.push({ level: 'warn', msg: `Low Kd/Kp ratio — under-damped response likely.` });
if (override_enabled) {
if (override_kp > kp_max)
warnings.push({ level: 'error', msg: `Override Kp (${override_kp}) exceeds kp_max (${kp_max}).` });
if (override_ki > ki_max)
warnings.push({ level: 'error', msg: `Override Ki (${override_ki}) exceeds ki_max (${ki_max}).` });
if (override_kd > kd_max)
warnings.push({ level: 'error', msg: `Override Kd (${override_kd}) exceeds kd_max (${kd_max}).` });
}
if (control_rate > 150)
warnings.push({ level: 'warn', msg: `Control rate ${control_rate} Hz — ensure STM32 UART can keep up.` });
if (Math.abs(balance_setpoint_rad) > 0.05)
warnings.push({ level: 'warn', msg: `Setpoint |${balance_setpoint_rad?.toFixed(3)}| rad > 3° — intentional lean?` });
return warnings;
}
// ── Step-response simulation ───────────────────────────────────────────────────
export function simulateStepResponse(kp, ki, kd) {
const dt = 0.02;
const N = 120;
const g = 9.81;
const L = 0.40;
const m = 3.0;
const step = 5 * Math.PI / 180;
let theta = step;
let omega = 0;
let integral = 0;
let prevErr = theta;
const result = [];
for (let i = 0; i < N; i++) {
const t = i * dt;
const err = -theta;
integral += err * dt;
integral = Math.max(-2, Math.min(2, integral));
const deriv = (err - prevErr) / dt;
prevErr = err;
const u = kp * err + ki * integral + kd * deriv;
const alpha = (g / L) * theta - u / (m * L * L);
omega += alpha * dt;
theta += omega * dt;
result.push({ t, theta: theta * 180 / Math.PI, u: Math.min(100, Math.max(-100, u)) });
if (Math.abs(theta) > Math.PI / 2) {
for (let j = i + 1; j < N; j++)
result.push({ t: j * dt, theta: theta > 0 ? 90 : -90, u: 0 });
break;
}
}
return result;
}
// ── Hook ──────────────────────────────────────────────────────────────────────
export function useSettings({ callService, subscribe } = {}) {
const [gains, setGains] = useState(() => {
try {
const s = JSON.parse(localStorage.getItem(STORAGE_KEY));
return s?.gains ?? Object.fromEntries(PID_PARAMS.map(p => [p.key, p.default]));
} catch { return Object.fromEntries(PID_PARAMS.map(p => [p.key, p.default])); }
});
const [sensors, setSensors] = useState(() => {
try {
const s = JSON.parse(localStorage.getItem(STORAGE_KEY));
return s?.sensors ?? Object.fromEntries(SENSOR_PARAMS.map(p => [p.key, p.default]));
} catch { return Object.fromEntries(SENSOR_PARAMS.map(p => [p.key, p.default])); }
});
const [firmwareInfo, setFirmwareInfo] = useState(null);
const [applying, setApplying] = useState(false);
const [applyResult, setApplyResult] = useState(null);
const persist = useCallback((newGains, newSensors) => {
localStorage.setItem(STORAGE_KEY, JSON.stringify({
gains: newGains ?? gains,
sensors: newSensors ?? sensors,
savedAt: Date.now(),
}));
}, [gains, sensors]);
const buildSetRequest = useCallback((params) => ({
parameters: params.map(({ name, value }) => {
let type = 3; let v = { double_value: value };
if (typeof value === 'boolean') { type = 1; v = { bool_value: value }; }
else if (Number.isInteger(value)) { type = 2; v = { integer_value: value }; }
return { name, value: { type, ...v } };
}),
}), []);
const applyPIDGains = useCallback(async (overrideGains) => {
const toApply = overrideGains ?? gains;
setApplying(true); setApplyResult(null);
const params = PID_PARAMS.map(p => ({ name: p.key, value: toApply[p.key] ?? p.default }));
if (!callService) {
setGains(toApply); persist(toApply, null); setApplying(false);
setApplyResult({ ok: true, msg: 'Saved locally (not connected)' }); return;
}
try {
await new Promise((resolve, reject) => {
callService(`/${PID_NODE}/set_parameters`, 'rcl_interfaces/srv/SetParameters',
buildSetRequest(params), (res) => {
res.results?.every(r => r.successful) ? resolve() :
reject(new Error(res.results?.find(r => !r.successful)?.reason ?? 'failed'));
});
setTimeout(() => reject(new Error('timeout')), 5000);
});
setGains(toApply); persist(toApply, null);
setApplyResult({ ok: true, msg: 'Parameters applied to adaptive_pid_node' });
} catch (e) {
setApplyResult({ ok: false, msg: String(e.message) });
}
setApplying(false);
}, [gains, callService, buildSetRequest, persist]);
const applySensorParams = useCallback(async (overrideParams) => {
const toApply = overrideParams ?? sensors;
setApplying(true); setApplyResult(null);
if (!callService) {
setSensors(toApply); persist(null, toApply); setApplying(false);
setApplyResult({ ok: true, msg: 'Saved locally (not connected)' }); return;
}
const byNode = {};
SENSOR_PARAMS.forEach(p => {
if (!byNode[p.node]) byNode[p.node] = [];
byNode[p.node].push({ name: p.key, value: toApply[p.key] ?? p.default });
});
try {
for (const [node, params] of Object.entries(byNode)) {
await new Promise((resolve, reject) => {
callService(`/${node}/set_parameters`, 'rcl_interfaces/srv/SetParameters',
buildSetRequest(params), resolve);
setTimeout(() => reject(new Error(`timeout on ${node}`)), 5000);
});
}
setSensors(toApply); persist(null, toApply);
setApplyResult({ ok: true, msg: 'Sensor parameters applied' });
} catch (e) { setApplyResult({ ok: false, msg: String(e.message) }); }
setApplying(false);
}, [sensors, callService, buildSetRequest, persist]);
const startFirmwareWatch = useCallback(() => {
if (!subscribe) return () => {};
return subscribe('/diagnostics', 'diagnostic_msgs/DiagnosticArray', (msg) => {
const info = {};
for (const status of msg.status ?? []) {
for (const kv of status.values ?? []) {
if (kv.key === 'stm32_fw_version') info.stm32Version = kv.value;
if (kv.key === 'jetson_sw_version') info.jetsonVersion = kv.value;
if (kv.key === 'last_ota_date') info.lastOtaDate = kv.value;
if (kv.key === 'jetson_hostname') info.hostname = kv.value;
if (kv.key === 'ros_distro') info.rosDistro = kv.value;
if (kv.key === 'uptime_s') info.uptimeS = parseFloat(kv.value);
}
}
if (Object.keys(info).length > 0) setFirmwareInfo(fi => ({ ...fi, ...info }));
});
}, [subscribe]);
const exportSettingsJSON = useCallback(() =>
JSON.stringify({ gains, sensors, exportedAt: new Date().toISOString() }, null, 2),
[gains, sensors]);
const importSettingsJSON = useCallback((json) => {
const data = JSON.parse(json);
if (data.gains) { setGains(data.gains); persist(data.gains, null); }
if (data.sensors) { setSensors(data.sensors); persist(null, data.sensors); }
}, [persist]);
const exportDiagnosticsBundle = useCallback((robotData, connections) => {
const bundle = {
exportedAt: new Date().toISOString(),
settings: { gains, sensors },
firmwareInfo,
fleet: Object.entries(connections ?? {}).map(([id, c]) => ({ id, ...c, data: robotData?.[id] })),
};
const a = document.createElement('a');
a.href = URL.createObjectURL(new Blob([JSON.stringify(bundle, null, 2)], { type: 'application/json' }));
a.download = `saltybot-diagnostics-${Date.now()}.json`;
a.click();
}, [gains, sensors, firmwareInfo]);
return {
gains, setGains, sensors, setSensors, firmwareInfo,
applying, applyResult,
applyPIDGains, applySensorParams, startFirmwareWatch,
exportSettingsJSON, importSettingsJSON, exportDiagnosticsBundle,
validate: () => validatePID(gains),
};
}