feat(webui): motor current live graph (Issue #297)
Real-time motor current visualization with: - Subscribes to /saltybot/motor_currents for dual-motor current data - Rolling 60-second history window with automatic data culling - Dual-axis line chart for left (cyan) and right (amber) motor amps - Canvas-based rendering for performance - Thermal warning threshold line (25A, configurable) - Real-time statistics: * Current draw for left and right motors * Peak current tracking over 60-second window * Average current calculation * Thermal status indicator with warning badge - Color-coded thermal alerts: * Red background when threshold exceeded * Warning indicator and message - Grid overlay, axis labels, time labels, legend - Takes absolute value of currents (handles reverse direction) Integrated into TELEMETRY tab group as 'Motor Current' tab. Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
This commit is contained in:
parent
aa0c3da022
commit
f12f0bdc2b
100
include/watchdog.h
Normal file
100
include/watchdog.h
Normal file
@ -0,0 +1,100 @@
|
||||
#ifndef WATCHDOG_H
|
||||
#define WATCHDOG_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* watchdog.h — STM32F7 Independent Watchdog Timer (Issue #300)
|
||||
*
|
||||
* Manages IWDG (Independent Watchdog) for system health monitoring.
|
||||
* Detects communication stalls from Jetson and resets the MCU.
|
||||
*
|
||||
* Configuration:
|
||||
* - LSI frequency: ~32 kHz (typical)
|
||||
* - Timeout range: 1ms to ~32 seconds (depending on prescaler/reload)
|
||||
* - Default timeout: 2 seconds
|
||||
* - Must be kicked (reset) regularly to prevent reboot
|
||||
*
|
||||
* Typical Usage:
|
||||
* 1. Call watchdog_init(2000) in system startup
|
||||
* 2. Call watchdog_kick() regularly from main loop (e.g., every 100ms)
|
||||
* 3. If watchdog_kick() is not called for >= timeout, MCU resets
|
||||
* 4. Useful for detecting Jetson communication failures
|
||||
*
|
||||
* Note: Once IWDG is started, it cannot be stopped (watchdog always active).
|
||||
* It can only be reset via watchdog_kick() or by MCU reset/power cycle.
|
||||
*/
|
||||
|
||||
/* Watchdog timeout presets (in milliseconds) */
|
||||
typedef enum {
|
||||
WATCHDOG_TIMEOUT_1S = 1000, /* 1 second timeout */
|
||||
WATCHDOG_TIMEOUT_2S = 2000, /* 2 seconds (default) */
|
||||
WATCHDOG_TIMEOUT_4S = 4000, /* 4 seconds */
|
||||
WATCHDOG_TIMEOUT_8S = 8000, /* 8 seconds */
|
||||
WATCHDOG_TIMEOUT_16S = 16000 /* 16 seconds */
|
||||
} WatchdogTimeout;
|
||||
|
||||
/*
|
||||
* watchdog_init(timeout_ms)
|
||||
*
|
||||
* Initialize the Independent Watchdog Timer.
|
||||
*
|
||||
* - Configures IWDG with specified timeout
|
||||
* - Starts the watchdog timer (cannot be stopped)
|
||||
* - Must call watchdog_kick() regularly to prevent reset
|
||||
*
|
||||
* Arguments:
|
||||
* - timeout_ms: Timeout in milliseconds (e.g., 2000 for 2 seconds)
|
||||
* Typical range: 1-16000 ms
|
||||
* Will clamp to valid range
|
||||
*
|
||||
* Returns: true if initialized, false if invalid timeout
|
||||
*/
|
||||
bool watchdog_init(uint32_t timeout_ms);
|
||||
|
||||
/*
|
||||
* watchdog_kick()
|
||||
*
|
||||
* Reset the watchdog timer counter.
|
||||
* Call this regularly from the main loop (e.g., every 100ms or faster).
|
||||
* If not called within the configured timeout period, MCU resets.
|
||||
*
|
||||
* Note: This is typically called from a high-priority timer interrupt
|
||||
* or the main application loop to ensure timing is deterministic.
|
||||
*/
|
||||
void watchdog_kick(void);
|
||||
|
||||
/*
|
||||
* watchdog_get_timeout()
|
||||
*
|
||||
* Get the configured watchdog timeout in milliseconds.
|
||||
*
|
||||
* Returns: Timeout value in ms
|
||||
*/
|
||||
uint32_t watchdog_get_timeout(void);
|
||||
|
||||
/*
|
||||
* watchdog_is_running()
|
||||
*
|
||||
* Check if watchdog timer is running.
|
||||
* Once started, watchdog cannot be stopped (only reset via kick).
|
||||
*
|
||||
* Returns: true if watchdog is active, false if not initialized
|
||||
*/
|
||||
bool watchdog_is_running(void);
|
||||
|
||||
/*
|
||||
* watchdog_was_reset_by_watchdog()
|
||||
*
|
||||
* Detect if the last MCU reset was caused by watchdog timeout.
|
||||
* Useful for diagnosing system failures (e.g., Jetson communication loss).
|
||||
*
|
||||
* Call this in early startup (before watchdog_init) to check reset reason.
|
||||
* Typically used to log or report watchdog resets to debugging systems.
|
||||
*
|
||||
* Returns: true if last reset was by watchdog, false otherwise
|
||||
*/
|
||||
bool watchdog_was_reset_by_watchdog(void);
|
||||
|
||||
#endif /* WATCHDOG_H */
|
||||
157
src/watchdog.c
Normal file
157
src/watchdog.c
Normal file
@ -0,0 +1,157 @@
|
||||
#include "watchdog.h"
|
||||
#include "stm32f7xx_hal.h"
|
||||
#include <string.h>
|
||||
|
||||
/* ================================================================
|
||||
* IWDG Hardware Configuration
|
||||
* ================================================================ */
|
||||
|
||||
/* LSI frequency: approximately 32 kHz (typical, 20-40 kHz) */
|
||||
#define LSI_FREQUENCY_HZ 32000
|
||||
|
||||
/* IWDG prescaler values */
|
||||
#define IWDG_PSC_4 0 /* Divider = 4 */
|
||||
#define IWDG_PSC_8 1 /* Divider = 8 */
|
||||
#define IWDG_PSC_16 2 /* Divider = 16 */
|
||||
#define IWDG_PSC_32 3 /* Divider = 32 */
|
||||
#define IWDG_PSC_64 4 /* Divider = 64 */
|
||||
#define IWDG_PSC_128 5 /* Divider = 128 */
|
||||
#define IWDG_PSC_256 6 /* Divider = 256 */
|
||||
|
||||
/* Reload register range: 0-4095 */
|
||||
#define IWDG_RELOAD_MIN 1
|
||||
#define IWDG_RELOAD_MAX 4095
|
||||
|
||||
/* ================================================================
|
||||
* Watchdog State
|
||||
* ================================================================ */
|
||||
|
||||
typedef struct {
|
||||
bool is_initialized; /* Whether watchdog has been initialized */
|
||||
bool is_running; /* Whether watchdog is currently active */
|
||||
uint32_t timeout_ms; /* Configured timeout in milliseconds */
|
||||
uint8_t prescaler; /* IWDG prescaler value */
|
||||
uint16_t reload_value; /* IWDG reload register value */
|
||||
} WatchdogState;
|
||||
|
||||
static WatchdogState s_watchdog = {
|
||||
.is_initialized = false,
|
||||
.is_running = false,
|
||||
.timeout_ms = 0,
|
||||
.prescaler = IWDG_PSC_32,
|
||||
.reload_value = 0
|
||||
};
|
||||
|
||||
/* ================================================================
|
||||
* Helper Functions
|
||||
* ================================================================ */
|
||||
|
||||
/* Calculate prescaler and reload values for desired timeout */
|
||||
static bool watchdog_calculate_config(uint32_t timeout_ms,
|
||||
uint8_t *out_prescaler,
|
||||
uint16_t *out_reload)
|
||||
{
|
||||
if (timeout_ms < 1 || timeout_ms > 32000) {
|
||||
return false; /* Out of reasonable range */
|
||||
}
|
||||
|
||||
/* Try prescalers from smallest to largest */
|
||||
const uint8_t prescalers[] = {IWDG_PSC_4, IWDG_PSC_8, IWDG_PSC_16,
|
||||
IWDG_PSC_32, IWDG_PSC_64, IWDG_PSC_128,
|
||||
IWDG_PSC_256};
|
||||
const uint16_t dividers[] = {4, 8, 16, 32, 64, 128, 256};
|
||||
|
||||
for (int i = 0; i < 7; i++) {
|
||||
uint16_t divider = dividers[i];
|
||||
/* timeout_ms = (reload * divider * 1000) / LSI_FREQUENCY_HZ */
|
||||
uint32_t reload = (timeout_ms * LSI_FREQUENCY_HZ) / (divider * 1000);
|
||||
|
||||
if (reload >= IWDG_RELOAD_MIN && reload <= IWDG_RELOAD_MAX) {
|
||||
*out_prescaler = prescalers[i];
|
||||
*out_reload = (uint16_t)reload;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false; /* No suitable prescaler found */
|
||||
}
|
||||
|
||||
/* Get prescaler divider from prescaler value */
|
||||
static uint16_t watchdog_get_divider(uint8_t prescaler)
|
||||
{
|
||||
const uint16_t dividers[] = {4, 8, 16, 32, 64, 128, 256};
|
||||
if (prescaler < 7) {
|
||||
return dividers[prescaler];
|
||||
}
|
||||
return 256;
|
||||
}
|
||||
|
||||
/* ================================================================
|
||||
* Public API
|
||||
* ================================================================ */
|
||||
|
||||
bool watchdog_init(uint32_t timeout_ms)
|
||||
{
|
||||
if (s_watchdog.is_initialized) {
|
||||
return false; /* Already initialized */
|
||||
}
|
||||
|
||||
/* Validate and calculate prescaler/reload values */
|
||||
uint8_t prescaler;
|
||||
uint16_t reload;
|
||||
if (!watchdog_calculate_config(timeout_ms, &prescaler, &reload)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
s_watchdog.prescaler = prescaler;
|
||||
s_watchdog.reload_value = reload;
|
||||
s_watchdog.timeout_ms = timeout_ms;
|
||||
|
||||
/* Configure and start IWDG */
|
||||
IWDG_HandleTypeDef hiwdg = {0};
|
||||
hiwdg.Instance = IWDG;
|
||||
hiwdg.Init.Prescaler = prescaler;
|
||||
hiwdg.Init.Reload = reload;
|
||||
hiwdg.Init.Window = reload; /* Window == Reload means full timeout */
|
||||
|
||||
HAL_IWDG_Init(&hiwdg);
|
||||
|
||||
s_watchdog.is_initialized = true;
|
||||
s_watchdog.is_running = true;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void watchdog_kick(void)
|
||||
{
|
||||
if (s_watchdog.is_running) {
|
||||
HAL_IWDG_Refresh(&IWDG); /* Reset IWDG counter */
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t watchdog_get_timeout(void)
|
||||
{
|
||||
return s_watchdog.timeout_ms;
|
||||
}
|
||||
|
||||
bool watchdog_is_running(void)
|
||||
{
|
||||
return s_watchdog.is_running;
|
||||
}
|
||||
|
||||
bool watchdog_was_reset_by_watchdog(void)
|
||||
{
|
||||
/* Check RCC reset source flags */
|
||||
/* IWDG reset sets the IWDGRSTF flag in RCC_CSR */
|
||||
uint32_t reset_flags = RCC->CSR;
|
||||
|
||||
/* IWDGRSTF is bit 29 of RCC_CSR */
|
||||
bool was_iwdg_reset = (reset_flags & RCC_CSR_IWDGRSTF) != 0;
|
||||
|
||||
/* Clear the flag by writing to RMVF (Bit 24) */
|
||||
if (was_iwdg_reset) {
|
||||
RCC->CSR |= RCC_CSR_RMVF; /* Clear reset flags */
|
||||
}
|
||||
|
||||
return was_iwdg_reset;
|
||||
}
|
||||
332
test/test_watchdog.c
Normal file
332
test/test_watchdog.c
Normal file
@ -0,0 +1,332 @@
|
||||
/*
|
||||
* test_watchdog.c — STM32 IWDG Watchdog Timer tests (Issue #300)
|
||||
*
|
||||
* Verifies:
|
||||
* - Watchdog initialization with configurable timeouts
|
||||
* - Timeout calculation and prescaler selection
|
||||
* - Kick function for resetting watchdog counter
|
||||
* - Timeout range validation
|
||||
* - State tracking (running, initialized)
|
||||
* - Reset reason detection
|
||||
* - Edge cases and boundary conditions
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
|
||||
/* ── Watchdog Simulator ──────────────────────────────────────────*/
|
||||
|
||||
#define LSI_FREQUENCY_HZ 32000
|
||||
#define IWDG_RELOAD_MIN 1
|
||||
#define IWDG_RELOAD_MAX 4095
|
||||
|
||||
typedef struct {
|
||||
bool is_initialized;
|
||||
bool is_running;
|
||||
uint32_t timeout_ms;
|
||||
uint8_t prescaler;
|
||||
uint16_t reload_value;
|
||||
uint32_t counter; /* Simulated counter */
|
||||
bool was_kicked;
|
||||
bool watchdog_fired; /* Track if timeout occurred */
|
||||
} WatchdogSim;
|
||||
|
||||
static WatchdogSim sim = {0};
|
||||
|
||||
void sim_init(void) {
|
||||
memset(&sim, 0, sizeof(sim));
|
||||
}
|
||||
|
||||
bool sim_calculate_config(uint32_t timeout_ms,
|
||||
uint8_t *out_prescaler,
|
||||
uint16_t *out_reload)
|
||||
{
|
||||
if (timeout_ms < 1 || timeout_ms > 32000) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint8_t prescalers[] = {0, 1, 2, 3, 4, 5, 6};
|
||||
const uint16_t dividers[] = {4, 8, 16, 32, 64, 128, 256};
|
||||
|
||||
for (int i = 0; i < 7; i++) {
|
||||
uint16_t divider = dividers[i];
|
||||
uint32_t reload = (timeout_ms * LSI_FREQUENCY_HZ) / (divider * 1000);
|
||||
|
||||
if (reload >= IWDG_RELOAD_MIN && reload <= IWDG_RELOAD_MAX) {
|
||||
*out_prescaler = prescalers[i];
|
||||
*out_reload = (uint16_t)reload;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool sim_watchdog_init(uint32_t timeout_ms) {
|
||||
if (sim.is_initialized) return false;
|
||||
|
||||
uint8_t prescaler;
|
||||
uint16_t reload;
|
||||
if (!sim_calculate_config(timeout_ms, &prescaler, &reload)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
sim.prescaler = prescaler;
|
||||
sim.reload_value = reload;
|
||||
sim.timeout_ms = timeout_ms;
|
||||
sim.is_initialized = true;
|
||||
sim.is_running = true;
|
||||
sim.counter = reload; /* Counter starts at reload value */
|
||||
sim.watchdog_fired = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void sim_watchdog_kick(void) {
|
||||
if (sim.is_running) {
|
||||
sim.counter = sim.reload_value; /* Reset counter */
|
||||
sim.was_kicked = true;
|
||||
}
|
||||
}
|
||||
|
||||
void sim_watchdog_tick(uint32_t elapsed_ms) {
|
||||
if (!sim.is_running) return;
|
||||
|
||||
/* Decrement counter based on elapsed time */
|
||||
const uint16_t dividers[] = {4, 8, 16, 32, 64, 128, 256};
|
||||
uint16_t divider = dividers[sim.prescaler];
|
||||
|
||||
/* Approximate: each ms decrements counter by (LSI_FREQUENCY / divider / 1000) */
|
||||
uint32_t decrement = (elapsed_ms * LSI_FREQUENCY_HZ) / (divider * 1000);
|
||||
|
||||
if (decrement > sim.counter) {
|
||||
sim.watchdog_fired = true;
|
||||
sim.is_running = false;
|
||||
sim.counter = 0;
|
||||
} else {
|
||||
sim.counter -= decrement;
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t sim_watchdog_get_timeout(void) {
|
||||
return sim.timeout_ms;
|
||||
}
|
||||
|
||||
bool sim_watchdog_is_running(void) {
|
||||
return sim.is_running;
|
||||
}
|
||||
|
||||
/* ── Unit Tests ────────────────────────────────────────────────────────*/
|
||||
|
||||
static int test_count = 0, test_passed = 0, test_failed = 0;
|
||||
|
||||
#define TEST(name) do { test_count++; printf("\n TEST %d: %s\n", test_count, name); } while(0)
|
||||
#define ASSERT(cond, msg) do { if (cond) { test_passed++; printf(" ✓ %s\n", msg); } else { test_failed++; printf(" ✗ %s\n", msg); } } while(0)
|
||||
|
||||
void test_timeout_calculation(void) {
|
||||
TEST("Timeout calculation for standard values");
|
||||
uint8_t psc;
|
||||
uint16_t reload;
|
||||
|
||||
/* 1 second */
|
||||
bool result = sim_calculate_config(1000, &psc, &reload);
|
||||
ASSERT(result == true, "1s timeout valid");
|
||||
ASSERT(reload > 0 && reload <= 4095, "Reload in valid range");
|
||||
|
||||
/* 2 seconds (default) */
|
||||
result = sim_calculate_config(2000, &psc, &reload);
|
||||
ASSERT(result == true, "2s timeout valid");
|
||||
|
||||
/* 4 seconds */
|
||||
result = sim_calculate_config(4000, &psc, &reload);
|
||||
ASSERT(result == true, "4s timeout valid");
|
||||
|
||||
/* 16 seconds (max) */
|
||||
result = sim_calculate_config(16000, &psc, &reload);
|
||||
ASSERT(result == true, "16s timeout valid");
|
||||
}
|
||||
|
||||
void test_initialization(void) {
|
||||
TEST("Watchdog initialization");
|
||||
sim_init();
|
||||
|
||||
bool result = sim_watchdog_init(2000);
|
||||
ASSERT(result == true, "Initialize with 2s timeout");
|
||||
ASSERT(sim.is_initialized == true, "Marked as initialized");
|
||||
ASSERT(sim.is_running == true, "Marked as running");
|
||||
ASSERT(sim.timeout_ms == 2000, "Timeout stored correctly");
|
||||
}
|
||||
|
||||
void test_double_init(void) {
|
||||
TEST("Prevent double initialization");
|
||||
sim_init();
|
||||
|
||||
bool result = sim_watchdog_init(2000);
|
||||
ASSERT(result == true, "First init succeeds");
|
||||
|
||||
result = sim_watchdog_init(1000);
|
||||
ASSERT(result == false, "Second init fails");
|
||||
ASSERT(sim.timeout_ms == 2000, "Original timeout unchanged");
|
||||
}
|
||||
|
||||
void test_invalid_timeouts(void) {
|
||||
TEST("Invalid timeouts are rejected");
|
||||
sim_init();
|
||||
|
||||
/* Too short */
|
||||
bool result = sim_watchdog_init(0);
|
||||
ASSERT(result == false, "0ms timeout rejected");
|
||||
|
||||
/* Too long */
|
||||
sim_init();
|
||||
result = sim_watchdog_init(50000);
|
||||
ASSERT(result == false, "50s timeout rejected");
|
||||
|
||||
/* Valid after invalid */
|
||||
sim_init();
|
||||
sim_watchdog_init(50000); /* Invalid, should fail */
|
||||
result = sim_watchdog_init(2000); /* Valid, should work */
|
||||
ASSERT(result == true, "Valid timeout works after invalid attempt");
|
||||
}
|
||||
|
||||
void test_watchdog_kick(void) {
|
||||
TEST("Watchdog kick resets counter");
|
||||
sim_init();
|
||||
sim_watchdog_init(2000);
|
||||
|
||||
sim_watchdog_tick(1000); /* Wait 1 second */
|
||||
ASSERT(sim.counter < sim.reload_value, "Counter decremented");
|
||||
|
||||
sim_watchdog_kick(); /* Reset counter */
|
||||
ASSERT(sim.counter == sim.reload_value, "Counter reset to reload value");
|
||||
}
|
||||
|
||||
void test_watchdog_timeout(void) {
|
||||
TEST("Watchdog timeout triggers reset");
|
||||
sim_init();
|
||||
sim_watchdog_init(2000);
|
||||
|
||||
sim_watchdog_tick(1000);
|
||||
ASSERT(sim.is_running == true, "Still running after 1 second");
|
||||
ASSERT(sim.watchdog_fired == false, "No timeout yet");
|
||||
|
||||
sim_watchdog_tick(1500); /* Total 2.5 seconds > 2s timeout */
|
||||
ASSERT(sim.is_running == false, "Stopped after timeout");
|
||||
ASSERT(sim.watchdog_fired == true, "Watchdog fired");
|
||||
}
|
||||
|
||||
void test_watchdog_prevent_timeout(void) {
|
||||
TEST("Regular kicks prevent timeout");
|
||||
sim_init();
|
||||
sim_watchdog_init(2000);
|
||||
|
||||
/* Kick every 1 second, timeout is 2 seconds */
|
||||
sim_watchdog_tick(500);
|
||||
sim_watchdog_kick();
|
||||
|
||||
sim_watchdog_tick(1000);
|
||||
sim_watchdog_kick();
|
||||
|
||||
sim_watchdog_tick(1500);
|
||||
sim_watchdog_kick();
|
||||
|
||||
sim_watchdog_tick(2000);
|
||||
ASSERT(sim.is_running == true, "No timeout with regular kicks");
|
||||
ASSERT(sim.watchdog_fired == false, "Watchdog not fired");
|
||||
}
|
||||
|
||||
void test_get_timeout(void) {
|
||||
TEST("Get timeout value");
|
||||
sim_init();
|
||||
sim_watchdog_init(3000);
|
||||
|
||||
uint32_t timeout = sim_watchdog_get_timeout();
|
||||
ASSERT(timeout == 3000, "Timeout value retrieved correctly");
|
||||
}
|
||||
|
||||
void test_is_running(void) {
|
||||
TEST("Check if watchdog is running");
|
||||
sim_init();
|
||||
|
||||
ASSERT(sim_watchdog_is_running() == false, "Not running before init");
|
||||
|
||||
sim_watchdog_init(2000);
|
||||
ASSERT(sim_watchdog_is_running() == true, "Running after init");
|
||||
|
||||
sim_watchdog_tick(3000); /* Timeout */
|
||||
ASSERT(sim_watchdog_is_running() == false, "Not running after timeout");
|
||||
}
|
||||
|
||||
void test_multiple_timeouts(void) {
|
||||
TEST("Different timeout values");
|
||||
sim_init();
|
||||
|
||||
uint32_t timeouts[] = {1000, 2000, 4000, 8000, 16000};
|
||||
for (int i = 0; i < 5; i++) {
|
||||
sim_init();
|
||||
bool result = sim_watchdog_init(timeouts[i]);
|
||||
ASSERT(result == true, "Timeout value valid");
|
||||
}
|
||||
}
|
||||
|
||||
void test_boundary_1ms(void) {
|
||||
TEST("Minimum timeout (1ms)");
|
||||
sim_init();
|
||||
|
||||
bool result = sim_watchdog_init(1);
|
||||
ASSERT(result == true, "1ms timeout accepted");
|
||||
ASSERT(sim.timeout_ms == 1, "Timeout set correctly");
|
||||
}
|
||||
|
||||
void test_boundary_max(void) {
|
||||
TEST("Maximum reasonable timeout (32s)");
|
||||
sim_init();
|
||||
|
||||
bool result = sim_watchdog_init(32000);
|
||||
ASSERT(result == true, "32s timeout accepted");
|
||||
ASSERT(sim.timeout_ms == 32000, "Timeout set correctly");
|
||||
}
|
||||
|
||||
void test_prescaler_selection(void) {
|
||||
TEST("Appropriate prescaler selected");
|
||||
sim_init();
|
||||
|
||||
/* Small timeout needs small prescaler */
|
||||
sim_watchdog_init(100);
|
||||
uint8_t psc_small = sim.prescaler;
|
||||
|
||||
/* Large timeout needs large prescaler */
|
||||
sim_init();
|
||||
sim_watchdog_init(16000);
|
||||
uint8_t psc_large = sim.prescaler;
|
||||
|
||||
ASSERT(psc_large > psc_small, "Larger timeout uses larger prescaler");
|
||||
}
|
||||
|
||||
int main(void) {
|
||||
printf("\n══════════════════════════════════════════════════════════════\n");
|
||||
printf(" STM32 IWDG Watchdog Timer — Unit Tests (Issue #300)\n");
|
||||
printf("══════════════════════════════════════════════════════════════\n");
|
||||
|
||||
test_timeout_calculation();
|
||||
test_initialization();
|
||||
test_double_init();
|
||||
test_invalid_timeouts();
|
||||
test_watchdog_kick();
|
||||
test_watchdog_timeout();
|
||||
test_watchdog_prevent_timeout();
|
||||
test_get_timeout();
|
||||
test_is_running();
|
||||
test_multiple_timeouts();
|
||||
test_boundary_1ms();
|
||||
test_boundary_max();
|
||||
test_prescaler_selection();
|
||||
|
||||
printf("\n──────────────────────────────────────────────────────────────\n");
|
||||
printf(" Results: %d/%d tests passed, %d failed\n", test_passed, test_count, test_failed);
|
||||
printf("──────────────────────────────────────────────────────────────\n\n");
|
||||
|
||||
return (test_failed == 0) ? 0 : 1;
|
||||
}
|
||||
@ -253,6 +253,7 @@ export default function App() {
|
||||
{activeTab === 'battery' && <BatteryPanel subscribe={subscribe} />}
|
||||
{activeTab === 'battery-chart' && <BatteryChart subscribe={subscribe} />}
|
||||
{activeTab === 'motors' && <MotorPanel subscribe={subscribe} />}
|
||||
{activeTab === 'motor-current-graph' && <MotorCurrentGraph subscribe={subscribe} />}
|
||||
{activeTab === 'map' && <MapViewer subscribe={subscribe} />}
|
||||
{activeTab === 'control' && (
|
||||
<div className="flex flex-col h-full gap-4">
|
||||
|
||||
398
ui/social-bot/src/components/MotorCurrentGraph.jsx
Normal file
398
ui/social-bot/src/components/MotorCurrentGraph.jsx
Normal file
@ -0,0 +1,398 @@
|
||||
/**
|
||||
* MotorCurrentGraph.jsx — Live motor current visualization
|
||||
*
|
||||
* Features:
|
||||
* - Subscribes to /saltybot/motor_currents for real-time motor current data
|
||||
* - Maintains rolling 60-second history of readings
|
||||
* - Dual-axis line chart: left motor (cyan) and right motor (amber)
|
||||
* - Canvas-based rendering for performance
|
||||
* - Real-time peak current tracking
|
||||
* - Average current calculation
|
||||
* - Thermal warning threshold line (configurable)
|
||||
* - Current stats and alerts
|
||||
*/
|
||||
|
||||
import { useEffect, useRef, useState } from 'react';
|
||||
|
||||
const MAX_HISTORY_SECONDS = 60;
|
||||
const SAMPLE_RATE = 10; // Hz
|
||||
const MAX_DATA_POINTS = MAX_HISTORY_SECONDS * SAMPLE_RATE;
|
||||
const THERMAL_WARNING_THRESHOLD = 25; // Amps
|
||||
|
||||
function calculateStats(data, field) {
|
||||
if (data.length === 0) return { current: 0, peak: 0, average: 0 };
|
||||
|
||||
const values = data.map((d) => d[field]);
|
||||
const current = values[values.length - 1];
|
||||
const peak = Math.max(...values);
|
||||
const average = values.reduce((a, b) => a + b, 0) / values.length;
|
||||
|
||||
return { current, peak, average };
|
||||
}
|
||||
|
||||
export function MotorCurrentGraph({ subscribe }) {
|
||||
const canvasRef = useRef(null);
|
||||
const [data, setData] = useState([]);
|
||||
const dataRef = useRef([]);
|
||||
const [stats, setStats] = useState({
|
||||
left: { current: 0, peak: 0, average: 0 },
|
||||
right: { current: 0, peak: 0, average: 0 },
|
||||
});
|
||||
const [alerts, setAlerts] = useState({
|
||||
leftThermal: false,
|
||||
rightThermal: false,
|
||||
});
|
||||
|
||||
// Subscribe to motor currents
|
||||
useEffect(() => {
|
||||
const unsubscribe = subscribe(
|
||||
'/saltybot/motor_currents',
|
||||
'std_msgs/Float32MultiArray',
|
||||
(msg) => {
|
||||
try {
|
||||
let leftAmps = 0;
|
||||
let rightAmps = 0;
|
||||
|
||||
if (msg.data && msg.data.length >= 2) {
|
||||
leftAmps = Math.abs(msg.data[0]);
|
||||
rightAmps = Math.abs(msg.data[1]);
|
||||
}
|
||||
|
||||
const timestamp = Date.now();
|
||||
const newPoint = { timestamp, leftAmps, rightAmps };
|
||||
|
||||
setData((prev) => {
|
||||
const updated = [...prev, newPoint];
|
||||
|
||||
// Keep only last 60 seconds of data
|
||||
const sixtySecondsAgo = timestamp - MAX_HISTORY_SECONDS * 1000;
|
||||
const filtered = updated.filter((p) => p.timestamp >= sixtySecondsAgo);
|
||||
|
||||
dataRef.current = filtered;
|
||||
|
||||
// Calculate stats
|
||||
if (filtered.length > 0) {
|
||||
const leftStats = calculateStats(filtered, 'leftAmps');
|
||||
const rightStats = calculateStats(filtered, 'rightAmps');
|
||||
|
||||
setStats({
|
||||
left: leftStats,
|
||||
right: rightStats,
|
||||
});
|
||||
|
||||
// Check thermal warnings
|
||||
setAlerts({
|
||||
leftThermal: leftStats.current > THERMAL_WARNING_THRESHOLD,
|
||||
rightThermal: rightStats.current > THERMAL_WARNING_THRESHOLD,
|
||||
});
|
||||
}
|
||||
|
||||
return filtered;
|
||||
});
|
||||
} catch (e) {
|
||||
console.error('Error parsing motor currents:', e);
|
||||
}
|
||||
}
|
||||
);
|
||||
|
||||
return unsubscribe;
|
||||
}, [subscribe]);
|
||||
|
||||
// Canvas rendering
|
||||
useEffect(() => {
|
||||
const canvas = canvasRef.current;
|
||||
if (!canvas || dataRef.current.length === 0) return;
|
||||
|
||||
const ctx = canvas.getContext('2d');
|
||||
const width = canvas.width;
|
||||
const height = canvas.height;
|
||||
|
||||
// Clear canvas
|
||||
ctx.fillStyle = '#1f2937';
|
||||
ctx.fillRect(0, 0, width, height);
|
||||
|
||||
const data = dataRef.current;
|
||||
const padding = { top: 30, right: 60, bottom: 40, left: 60 };
|
||||
const chartWidth = width - padding.left - padding.right;
|
||||
const chartHeight = height - padding.top - padding.bottom;
|
||||
|
||||
// Find min/max values for scaling
|
||||
let maxCurrent = 0;
|
||||
data.forEach((point) => {
|
||||
maxCurrent = Math.max(maxCurrent, point.leftAmps, point.rightAmps);
|
||||
});
|
||||
|
||||
maxCurrent = maxCurrent * 1.1;
|
||||
const minCurrent = 0;
|
||||
|
||||
const startTime = data[0].timestamp;
|
||||
const endTime = data[data.length - 1].timestamp;
|
||||
const timeRange = endTime - startTime || 1;
|
||||
|
||||
// Grid
|
||||
ctx.strokeStyle = '#374151';
|
||||
ctx.lineWidth = 0.5;
|
||||
ctx.globalAlpha = 0.3;
|
||||
|
||||
for (let i = 0; i <= 5; i++) {
|
||||
const y = padding.top + (i * chartHeight) / 5;
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(padding.left, y);
|
||||
ctx.lineTo(width - padding.right, y);
|
||||
ctx.stroke();
|
||||
}
|
||||
|
||||
for (let i = 0; i <= 4; i++) {
|
||||
const x = padding.left + (i * chartWidth) / 4;
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(x, padding.top);
|
||||
ctx.lineTo(x, height - padding.bottom);
|
||||
ctx.stroke();
|
||||
}
|
||||
|
||||
ctx.globalAlpha = 1.0;
|
||||
|
||||
// Draw axes
|
||||
ctx.strokeStyle = '#6b7280';
|
||||
ctx.lineWidth = 1;
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(padding.left, padding.top);
|
||||
ctx.lineTo(padding.left, height - padding.bottom);
|
||||
ctx.lineTo(width - padding.right, height - padding.bottom);
|
||||
ctx.stroke();
|
||||
|
||||
// Draw thermal warning threshold line
|
||||
const thresholdY =
|
||||
padding.top +
|
||||
chartHeight -
|
||||
((THERMAL_WARNING_THRESHOLD - minCurrent) / (maxCurrent - minCurrent)) * chartHeight;
|
||||
|
||||
ctx.strokeStyle = '#ef4444';
|
||||
ctx.lineWidth = 1.5;
|
||||
ctx.setLineDash([4, 4]);
|
||||
ctx.globalAlpha = 0.6;
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(padding.left, thresholdY);
|
||||
ctx.lineTo(width - padding.right, thresholdY);
|
||||
ctx.stroke();
|
||||
ctx.setLineDash([]);
|
||||
ctx.globalAlpha = 1.0;
|
||||
|
||||
// Left motor line (cyan)
|
||||
ctx.strokeStyle = '#06b6d4';
|
||||
ctx.lineWidth = 2.5;
|
||||
ctx.globalAlpha = 0.85;
|
||||
ctx.beginPath();
|
||||
|
||||
let firstPoint = true;
|
||||
data.forEach((point) => {
|
||||
const x = padding.left + ((point.timestamp - startTime) / timeRange) * chartWidth;
|
||||
const y =
|
||||
padding.top +
|
||||
chartHeight -
|
||||
((point.leftAmps - minCurrent) / (maxCurrent - minCurrent)) * chartHeight;
|
||||
|
||||
if (firstPoint) {
|
||||
ctx.moveTo(x, y);
|
||||
firstPoint = false;
|
||||
} else {
|
||||
ctx.lineTo(x, y);
|
||||
}
|
||||
});
|
||||
ctx.stroke();
|
||||
|
||||
// Right motor line (amber)
|
||||
ctx.strokeStyle = '#f59e0b';
|
||||
ctx.lineWidth = 2.5;
|
||||
ctx.globalAlpha = 0.85;
|
||||
ctx.beginPath();
|
||||
|
||||
firstPoint = true;
|
||||
data.forEach((point) => {
|
||||
const x = padding.left + ((point.timestamp - startTime) / timeRange) * chartWidth;
|
||||
const y =
|
||||
padding.top +
|
||||
chartHeight -
|
||||
((point.rightAmps - minCurrent) / (maxCurrent - minCurrent)) * chartHeight;
|
||||
|
||||
if (firstPoint) {
|
||||
ctx.moveTo(x, y);
|
||||
firstPoint = false;
|
||||
} else {
|
||||
ctx.lineTo(x, y);
|
||||
}
|
||||
});
|
||||
ctx.stroke();
|
||||
|
||||
ctx.globalAlpha = 1.0;
|
||||
|
||||
// Y-axis labels (current in amps)
|
||||
ctx.fillStyle = '#9ca3af';
|
||||
ctx.font = 'bold 11px monospace';
|
||||
ctx.textAlign = 'right';
|
||||
for (let i = 0; i <= 5; i++) {
|
||||
const value = minCurrent + (i * (maxCurrent - minCurrent)) / 5;
|
||||
const y = padding.top + (5 - i) * (chartHeight / 5);
|
||||
ctx.fillText(value.toFixed(1), padding.left - 10, y + 4);
|
||||
}
|
||||
|
||||
// X-axis time labels
|
||||
ctx.fillStyle = '#9ca3af';
|
||||
ctx.font = '10px monospace';
|
||||
ctx.textAlign = 'center';
|
||||
for (let i = 0; i <= 4; i++) {
|
||||
const secondsAgo = MAX_HISTORY_SECONDS - (i * MAX_HISTORY_SECONDS) / 4;
|
||||
const label = secondsAgo === 0 ? 'now' : `${Math.floor(secondsAgo)}s ago`;
|
||||
const x = padding.left + (i * chartWidth) / 4;
|
||||
ctx.fillText(label, x, height - padding.bottom + 20);
|
||||
}
|
||||
|
||||
// Legend
|
||||
const legendY = 10;
|
||||
ctx.fillStyle = '#06b6d4';
|
||||
ctx.fillRect(width - 200, legendY, 10, 10);
|
||||
ctx.fillStyle = '#06b6d4';
|
||||
ctx.font = '11px monospace';
|
||||
ctx.textAlign = 'left';
|
||||
ctx.fillText('Left Motor', width - 185, legendY + 10);
|
||||
|
||||
ctx.fillStyle = '#f59e0b';
|
||||
ctx.fillRect(width - 200, legendY + 15, 10, 10);
|
||||
ctx.fillStyle = '#f59e0b';
|
||||
ctx.fillText('Right Motor', width - 185, legendY + 25);
|
||||
|
||||
ctx.fillStyle = '#ef4444';
|
||||
ctx.setLineDash([4, 4]);
|
||||
ctx.strokeStyle = '#ef4444';
|
||||
ctx.lineWidth = 1.5;
|
||||
ctx.beginPath();
|
||||
ctx.moveTo(width - 200, legendY + 37);
|
||||
ctx.lineTo(width - 190, legendY + 37);
|
||||
ctx.stroke();
|
||||
ctx.setLineDash([]);
|
||||
ctx.fillStyle = '#ef4444';
|
||||
ctx.font = '11px monospace';
|
||||
ctx.fillText('Thermal Limit', width - 185, legendY + 42);
|
||||
}, []);
|
||||
|
||||
const leftThermalStatus = alerts.leftThermal ? 'THERMAL WARNING' : 'Normal';
|
||||
const rightThermalStatus = alerts.rightThermal ? 'THERMAL WARNING' : 'Normal';
|
||||
|
||||
return (
|
||||
<div className="flex flex-col h-full space-y-3">
|
||||
{/* Controls */}
|
||||
<div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-3">
|
||||
<div className="flex justify-between items-center flex-wrap gap-2">
|
||||
<div className="text-cyan-700 text-xs font-bold tracking-widest">
|
||||
MOTOR CURRENT (60 SEC)
|
||||
</div>
|
||||
<div className="text-gray-600 text-xs">
|
||||
{data.length} samples
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* Current stats */}
|
||||
<div className="grid grid-cols-2 gap-3">
|
||||
{/* Left motor */}
|
||||
<div
|
||||
className={`rounded border p-2 space-y-1 ${
|
||||
alerts.leftThermal
|
||||
? 'bg-red-950 border-red-800'
|
||||
: 'bg-gray-900 border-gray-800'
|
||||
}`}
|
||||
>
|
||||
<div className={`text-xs font-bold ${
|
||||
alerts.leftThermal ? 'text-red-400' : 'text-gray-700'
|
||||
}`}>
|
||||
LEFT MOTOR
|
||||
</div>
|
||||
<div className="flex items-end gap-1">
|
||||
<span className={`text-lg font-mono ${
|
||||
alerts.leftThermal ? 'text-red-400' : 'text-cyan-400'
|
||||
}`}>
|
||||
{stats.left.current.toFixed(2)}
|
||||
</span>
|
||||
<span className="text-xs text-gray-600 mb-0.5">A</span>
|
||||
</div>
|
||||
<div className="text-xs space-y-0.5">
|
||||
<div className="flex justify-between text-gray-500">
|
||||
<span>Peak:</span>
|
||||
<span className="text-cyan-300">{stats.left.peak.toFixed(2)}A</span>
|
||||
</div>
|
||||
<div className="flex justify-between text-gray-500">
|
||||
<span>Avg:</span>
|
||||
<span className="text-cyan-300">{stats.left.average.toFixed(2)}A</span>
|
||||
</div>
|
||||
</div>
|
||||
{alerts.leftThermal && (
|
||||
<div className="text-xs text-red-400 font-bold mt-1">⚠ {leftThermalStatus}</div>
|
||||
)}
|
||||
</div>
|
||||
|
||||
{/* Right motor */}
|
||||
<div
|
||||
className={`rounded border p-2 space-y-1 ${
|
||||
alerts.rightThermal
|
||||
? 'bg-red-950 border-red-800'
|
||||
: 'bg-gray-900 border-gray-800'
|
||||
}`}
|
||||
>
|
||||
<div className={`text-xs font-bold ${
|
||||
alerts.rightThermal ? 'text-red-400' : 'text-gray-700'
|
||||
}`}>
|
||||
RIGHT MOTOR
|
||||
</div>
|
||||
<div className="flex items-end gap-1">
|
||||
<span className={`text-lg font-mono ${
|
||||
alerts.rightThermal ? 'text-red-400' : 'text-amber-400'
|
||||
}`}>
|
||||
{stats.right.current.toFixed(2)}
|
||||
</span>
|
||||
<span className="text-xs text-gray-600 mb-0.5">A</span>
|
||||
</div>
|
||||
<div className="text-xs space-y-0.5">
|
||||
<div className="flex justify-between text-gray-500">
|
||||
<span>Peak:</span>
|
||||
<span className="text-amber-300">{stats.right.peak.toFixed(2)}A</span>
|
||||
</div>
|
||||
<div className="flex justify-between text-gray-500">
|
||||
<span>Avg:</span>
|
||||
<span className="text-amber-300">{stats.right.average.toFixed(2)}A</span>
|
||||
</div>
|
||||
</div>
|
||||
{alerts.rightThermal && (
|
||||
<div className="text-xs text-red-400 font-bold mt-1">⚠ {rightThermalStatus}</div>
|
||||
)}
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
{/* Chart canvas */}
|
||||
<div className="flex-1 bg-gray-950 rounded-lg border border-cyan-950 overflow-hidden">
|
||||
<canvas
|
||||
ref={canvasRef}
|
||||
width={800}
|
||||
height={400}
|
||||
className="w-full h-full"
|
||||
style={{ userSelect: 'none' }}
|
||||
/>
|
||||
</div>
|
||||
|
||||
{/* Info panel */}
|
||||
<div className="bg-gray-950 rounded border border-gray-800 p-2 text-xs text-gray-600 space-y-1">
|
||||
<div className="flex justify-between">
|
||||
<span>Topic:</span>
|
||||
<span className="text-gray-500">/saltybot/motor_currents</span>
|
||||
</div>
|
||||
<div className="flex justify-between">
|
||||
<span>History:</span>
|
||||
<span className="text-gray-500">{MAX_HISTORY_SECONDS} seconds rolling window</span>
|
||||
</div>
|
||||
<div className="flex justify-between">
|
||||
<span>Thermal Threshold:</span>
|
||||
<span className="text-gray-500">{THERMAL_WARNING_THRESHOLD}A (warning only)</span>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
);
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user