Compare commits
3 Commits
accda32c7a
...
f12f0bdc2b
| Author | SHA1 | Date | |
|---|---|---|---|
| f12f0bdc2b | |||
| aa0c3da022 | |||
| 71e49e1e9d |
381
chassis/rplidar_dust_cover.scad
Normal file
381
chassis/rplidar_dust_cover.scad
Normal file
@ -0,0 +1,381 @@
|
||||
// =============================================================================
|
||||
// SaltyBot — RPLIDAR A1 Dust & Splash Cover
|
||||
// Agent: sl-mechanical | 2026-03-02
|
||||
//
|
||||
// CLIP-ON protective dome for RPLIDAR A1M8 sensor, shielding from dust,
|
||||
// rain, and debris while maintaining 360° scan window. Quick-release tab
|
||||
// for one-handed removal. Integrated drainage holes prevent water pooling.
|
||||
//
|
||||
// HOW IT WORKS
|
||||
// 1. Clip ring sits on the mounting boss of the RPLIDAR A1 body (Ø 70 mm).
|
||||
// 2. Two snap tabs (elastically deformed) lock into recesses on the sensor rim.
|
||||
// 3. Dome overhead shields the rotating scanning mirror from debris.
|
||||
// 4. Six radial drainage holes (Ø 4 mm) at base allow water to escape.
|
||||
// 5. Quick-release tab provides easy lever-point for removal (no tools).
|
||||
//
|
||||
// OPTICAL DESIGN
|
||||
// • 360° scan window: unobstructed to ±60° vertical (sensor FOV).
|
||||
// • Window height: 28 mm (clear zone from 21 mm to 49 mm from base).
|
||||
// • Sensor face clearance: 6 mm minimum (prevents optical interference).
|
||||
// • Dome apex: ~55 mm above base (shed water away from sensor).
|
||||
//
|
||||
// MATERIALS & ASSEMBLY
|
||||
// • Body: PETG or ASA (UV-resistant, weatherproof, flexible enough for snaps).
|
||||
// • Snap tabs: Designed for 2–3 mm deflection during insertion.
|
||||
// • Drainage: Six 4 mm holes ensure rapid water egress (gutters not needed).
|
||||
// • Installation: No tools; press upward until snap tabs engage (~3 second clip time).
|
||||
// • Removal: Push quick-release tab inward, twist gently, lift off.
|
||||
//
|
||||
// MOUNTING GEOMETRY
|
||||
// RPLIDAR body outer diameter: 70.0 mm (Ø RPL_BODY_D)
|
||||
// Mounting bolt circle: 58.0 mm (4× M3 at 45°/135°/225°/315°)
|
||||
// Scan window (annular): ±30 mm radius, height 21–49 mm
|
||||
// Bearing assembly: ~40 mm diameter (inside scan window)
|
||||
//
|
||||
// PARTS (set RENDER= to export each)
|
||||
// dust_cover — 3D print × 1 (RENDER="dust_cover")
|
||||
// assembly — Preview with RPLIDAR ghost (RENDER="assembly")
|
||||
//
|
||||
// PRINT & INSTALL
|
||||
// Print orientation: Dome up (smooth finish down for adhesion).
|
||||
// Print settings: PETG/ASA, 0.2 mm layers, 5 perimeters, 15% infill.
|
||||
// No supports required (overhangs < 45°, snap tabs self-supporting).
|
||||
// Installation: Clean sensor body with IPA; press cover downward until snap
|
||||
// tabs audibly engage (2–3 mm deflection), test rotation lock.
|
||||
// =============================================================================
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01;
|
||||
|
||||
// =============================================================================
|
||||
// RPLIDAR A1 GEOMETRY
|
||||
// =============================================================================
|
||||
|
||||
RPL_BODY_D = 70.0; // mm outer body diameter
|
||||
RPL_BC = 58.0; // mm mounting bolt circle (4× M3)
|
||||
RPL_TOP_FACE_Z = 50.5; // mm height of top of sensor body (measured)
|
||||
|
||||
// Window dimensions (where scan light exits/enters)
|
||||
WINDOW_INNER_R = 15.0; // mm inner radius of scan annulus (bearing assy)
|
||||
WINDOW_OUTER_R = 33.0; // mm outer radius of scan annulus
|
||||
WINDOW_Z_BOT = 21.0; // mm bottom of optical scan window
|
||||
WINDOW_Z_TOP = 49.0; // mm top of optical scan window (±60° FOV)
|
||||
|
||||
// Clip base sits on sensor body (radius where snap tabs will locate)
|
||||
CLIP_SEAT_R = RPL_BODY_D / 2 + 0.5; // 35.5 mm (slight clearance)
|
||||
|
||||
// =============================================================================
|
||||
// DUST COVER DESIGN PARAMETERS
|
||||
// =============================================================================
|
||||
|
||||
// CLIP BASE RING (sits on RPLIDAR body)
|
||||
CLIP_BASE_OD = 77.0; // mm outer diameter of base ring
|
||||
CLIP_BASE_H = 6.0; // mm height of clip base (engagement zone)
|
||||
CLIP_BASE_WALL = 3.0; // mm wall thickness
|
||||
|
||||
// SNAP TABS (two locations: top front / top back)
|
||||
SNAP_TAB_W = 12.0; // mm width of each snap tab
|
||||
SNAP_TAB_H = 8.0; // mm height (radial protrusion)
|
||||
SNAP_TAB_T = 2.0; // mm thickness (allows flex)
|
||||
SNAP_DEFLECT = 2.5; // mm expected deflection during clip
|
||||
SNAP_ANGLE = 90.0; // degrees (top and bottom: 0° / 180°)
|
||||
|
||||
// QUICK-RELEASE TAB (lever point)
|
||||
QR_TAB_W = 10.0; // mm width
|
||||
QR_TAB_L = 14.0; // mm length (radial extent)
|
||||
QR_TAB_H = 6.0; // mm height above base
|
||||
QR_TAB_T = 2.0; // mm thickness
|
||||
QR_ANGLE = 270.0; // degrees (right side)
|
||||
|
||||
// DOME STRUCTURE (overhead cover)
|
||||
DOME_APEX_H = 55.0; // mm height to dome peak (above base plane)
|
||||
DOME_OD = 75.0; // mm outer diameter of dome
|
||||
DOME_WALL_T = 2.5; // mm wall thickness
|
||||
|
||||
// DRAINAGE HOLES (prevent water pooling)
|
||||
DRAIN_HOLE_D = 4.0; // mm diameter of each drain hole
|
||||
DRAIN_HOLE_Z = 4.0; // mm height of drain holes from base
|
||||
DRAIN_COUNT = 6; // number of evenly-spaced holes around base
|
||||
DRAIN_ANGLE_START = 0.0; // degrees
|
||||
|
||||
// SENSOR CLEARANCE
|
||||
SENSOR_FACE_CLR = 6.0; // mm minimum clearance above top of sensor
|
||||
WINDOW_CLR = 4.0; // mm clearance above window outer edge
|
||||
|
||||
// =============================================================================
|
||||
// RENDER CONTROL
|
||||
// =============================================================================
|
||||
|
||||
// "dust_cover" — clip-on cover, ready to print
|
||||
// "assembly" — cover with RPLIDAR ghost for fit check
|
||||
|
||||
RENDER = "assembly";
|
||||
|
||||
// =============================================================================
|
||||
// MAIN RENDER DISPATCH
|
||||
// =============================================================================
|
||||
|
||||
if (RENDER == "dust_cover") {
|
||||
dust_cover();
|
||||
} else if (RENDER == "assembly") {
|
||||
assembly();
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// ASSEMBLY VIEW (for fit verification)
|
||||
// =============================================================================
|
||||
|
||||
module assembly() {
|
||||
// RPLIDAR A1 ghost (sensor body and scanning window)
|
||||
%color("DarkGray", 0.30) {
|
||||
// Main body cylinder
|
||||
cylinder(d=RPL_BODY_D, h=RPL_TOP_FACE_Z);
|
||||
|
||||
// Scan window annulus (where light enters/exits)
|
||||
translate([0, 0, WINDOW_Z_BOT])
|
||||
difference() {
|
||||
cylinder(r=WINDOW_OUTER_R, h=WINDOW_Z_TOP - WINDOW_Z_BOT);
|
||||
translate([0, 0, -e]) cylinder(r=WINDOW_INNER_R, h=WINDOW_Z_TOP - WINDOW_Z_BOT + 2*e);
|
||||
}
|
||||
|
||||
// Top dome (bearing assembly)
|
||||
translate([0, 0, WINDOW_Z_TOP])
|
||||
sphere(r=WINDOW_INNER_R);
|
||||
}
|
||||
|
||||
// Dust cover (main part)
|
||||
color("Orange", 0.88)
|
||||
dust_cover();
|
||||
|
||||
// Labels
|
||||
echo("Dust Cover assembled on RPLIDAR A1M8");
|
||||
echo(str("Window clearance: ", WINDOW_CLR, " mm (minimum)"));
|
||||
echo(str("Sensor face clearance: ", SENSOR_FACE_CLR, " mm"));
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// DUST COVER MODULE (main part)
|
||||
// =============================================================================
|
||||
//
|
||||
// Structure:
|
||||
// • Base ring: clip location, snap engagement points
|
||||
// • Dome: overhead cover to shield sensor
|
||||
// • Snap tabs: two flex arms for retention
|
||||
// • Quick-release tab: lever for disassembly
|
||||
// • Drainage holes: six ports at base perimeter
|
||||
//
|
||||
|
||||
module dust_cover() {
|
||||
difference() {
|
||||
union() {
|
||||
// ── BASE RING (sits on RPLIDAR body) ───────────────────────
|
||||
translate([0, 0, 0])
|
||||
cylinder(d=CLIP_BASE_OD, h=CLIP_BASE_H);
|
||||
|
||||
// ── DOME COVER (overhead protection) ───────────────────────
|
||||
// Smooth dome surface, slightly flattened at apex for print stability
|
||||
translate([0, 0, CLIP_BASE_H])
|
||||
dome_surface();
|
||||
|
||||
// ── SNAP TAB 1 (0°, front) ─────────────────────────────────
|
||||
rotate([0, 0, SNAP_ANGLE])
|
||||
snap_tab_body();
|
||||
|
||||
// ── SNAP TAB 2 (180°, back) ────────────────────────────────
|
||||
rotate([0, 0, SNAP_ANGLE + 180])
|
||||
snap_tab_body();
|
||||
|
||||
// ── QUICK-RELEASE TAB (right side, 270°) ───────────────────
|
||||
rotate([0, 0, QR_ANGLE])
|
||||
qr_tab_body();
|
||||
}
|
||||
|
||||
// ── SUBTRACT: Central clearance for sensor window ──────────────
|
||||
translate([0, 0, -e])
|
||||
cylinder(r=WINDOW_OUTER_R + WINDOW_CLR, h=DOME_APEX_H + e);
|
||||
|
||||
// ── SUBTRACT: Drainage holes (base perimeter) ──────────────────
|
||||
for (i = [0 : DRAIN_COUNT - 1]) {
|
||||
a = DRAIN_ANGLE_START + i * (360 / DRAIN_COUNT);
|
||||
r = (CLIP_BASE_OD / 2) - 3; // Near outer edge
|
||||
translate([r * cos(a), r * sin(a), DRAIN_HOLE_Z])
|
||||
cylinder(d=DRAIN_HOLE_D, h=CLIP_BASE_H + e);
|
||||
}
|
||||
|
||||
// ── SUBTRACT: Dome interior (hollow dome reduces material) ─────
|
||||
translate([0, 0, CLIP_BASE_H + 0.5])
|
||||
dome_interior();
|
||||
|
||||
// ── SUBTRACT: Snap tab undercut (stress relief) ───────────────
|
||||
for (snap_a = [SNAP_ANGLE, SNAP_ANGLE + 180]) {
|
||||
rotate([0, 0, snap_a])
|
||||
translate([CLIP_BASE_OD/2 - CLIP_BASE_WALL + 0.5,
|
||||
-SNAP_TAB_W/2 - 1,
|
||||
CLIP_BASE_H - 1.5])
|
||||
cube([2, SNAP_TAB_W + 2, 2]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// DOME SURFACE (overhead cover)
|
||||
// =============================================================================
|
||||
//
|
||||
// Smooth parabolic dome that sheds water away from sensor.
|
||||
// Walls taper from base to apex for structural efficiency.
|
||||
//
|
||||
|
||||
module dome_surface() {
|
||||
hull() {
|
||||
// Base ring (connects to clip base)
|
||||
cylinder(d=DOME_OD, h=0.1);
|
||||
|
||||
// Apex (slightly flattened for print stability)
|
||||
translate([0, 0, DOME_APEX_H - 2])
|
||||
cylinder(d=8, h=0.1);
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// DOME INTERIOR (hollow dome)
|
||||
// =============================================================================
|
||||
//
|
||||
// Subtracts a concave shape to hollow out the dome, reducing print material
|
||||
// while maintaining structural integrity.
|
||||
//
|
||||
|
||||
module dome_interior() {
|
||||
h_inner = DOME_APEX_H - CLIP_BASE_H - DOME_WALL_T;
|
||||
scale([0.95, 0.95, 1])
|
||||
sphere(r=h_inner / 2);
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// SNAP TAB BODY (flex arm for clip retention)
|
||||
// =============================================================================
|
||||
//
|
||||
// Thin cantilever arm that deflects ~2.5 mm during insertion.
|
||||
// Engages with a recess on the sensor rim.
|
||||
//
|
||||
|
||||
module snap_tab_body() {
|
||||
// Snap tab protrudes radially outward from base
|
||||
translate([CLIP_BASE_OD/2 - CLIP_BASE_WALL,
|
||||
-SNAP_TAB_W/2,
|
||||
CLIP_BASE_H - SNAP_TAB_H])
|
||||
cube([SNAP_TAB_H, SNAP_TAB_W, SNAP_TAB_T]);
|
||||
|
||||
// Root fillet (stress relief)
|
||||
translate([CLIP_BASE_OD/2 - CLIP_BASE_WALL + SNAP_TAB_H/2,
|
||||
-SNAP_TAB_W/2,
|
||||
CLIP_BASE_H - SNAP_TAB_H])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(r=0.8, h=SNAP_TAB_H, center=true);
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// QUICK-RELEASE TAB (lever point for disassembly)
|
||||
// =============================================================================
|
||||
//
|
||||
// Rigid tab protruding from base, providing a lever point for easy removal.
|
||||
// No tools required; user presses inward, twists gently, lifts.
|
||||
//
|
||||
|
||||
module qr_tab_body() {
|
||||
// Tab extends radially outward from dome perimeter
|
||||
translate([DOME_OD/2 - 1,
|
||||
-QR_TAB_W/2,
|
||||
CLIP_BASE_H])
|
||||
cube([QR_TAB_L, QR_TAB_W, QR_TAB_H]);
|
||||
|
||||
// Top face, slightly angled for finger grip
|
||||
translate([DOME_OD/2 + QR_TAB_L - 4,
|
||||
-QR_TAB_W/2,
|
||||
CLIP_BASE_H + QR_TAB_H])
|
||||
cube([3, QR_TAB_W, 1.5]);
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// EXPORT / PRINT INSTRUCTIONS
|
||||
// =============================================================================
|
||||
//
|
||||
// DUST COVER (3D print × 1):
|
||||
// openscad rplidar_dust_cover.scad -D 'RENDER="dust_cover"' -o rplidar_dust_cover.stl
|
||||
//
|
||||
// Print settings:
|
||||
// • Material: PETG or ASA (UV-resistant, weatherproof)
|
||||
// • Layer height: 0.2 mm
|
||||
// • Perimeters: 5 (rigid, durable)
|
||||
// • Infill: 15% (lightweight, adequate for drainage)
|
||||
// • No supports (overhangs < 45°, snap tabs self-supporting)
|
||||
// • Orientation: Dome up, base down (smooth finish for sensor seating)
|
||||
// • Estimated time: ~1.5 hours, ~15–18 g material
|
||||
//
|
||||
// Post-print finishing:
|
||||
// • Light sand base surface (80 grit) for smooth fit
|
||||
// • Clean all drain holes with 4 mm drill bit or pick
|
||||
// • Optional: Apply thin coat of matte polyurethane for durability
|
||||
//
|
||||
// =============================================================================
|
||||
//
|
||||
// INSTALLATION GUIDE
|
||||
//
|
||||
// 1. SENSOR PREP
|
||||
// • Power off RPLIDAR and allow 5 minutes for motor to stop.
|
||||
// • Clean body with soft cloth; remove any dust/debris.
|
||||
// • Inspect snap engagement points (small recesses on side of body).
|
||||
//
|
||||
// 2. COVER INSTALLATION
|
||||
// • Hold cover with dome up, align two snap tabs (front/back).
|
||||
// • Position cover above RPLIDAR, centered on axis.
|
||||
// • Press downward steadily (~3 seconds) until tabs snap-engage.
|
||||
// • Audible click or slight resistance indicates proper seating.
|
||||
// • Verify cover is level (not tilted).
|
||||
//
|
||||
// 3. VERIFICATION
|
||||
// • Rotate cover gently (should not move; snap engaged).
|
||||
// • Inspect that scan window is fully unobstructed.
|
||||
// • Check that drainage holes are visible (not blocked).
|
||||
//
|
||||
// 4. REMOVAL
|
||||
// • Locate quick-release tab (rigid protrusion on side).
|
||||
// • Press tab inward (towards sensor) with light pressure.
|
||||
// • Twist cover slowly counterclockwise (20–30°).
|
||||
// • Lift upward; snap tabs will disengage.
|
||||
// • No tools required; ~10 seconds.
|
||||
//
|
||||
// =============================================================================
|
||||
//
|
||||
// MAINTENANCE & INSPECTION
|
||||
//
|
||||
// • Monthly: Check drain holes for blockage; flush with distilled water.
|
||||
// • Quarterly: Inspect snap tabs for cracks or permanent deformation.
|
||||
// • After rain: Allow cover to air-dry; tilting RPLIDAR promotes drainage.
|
||||
// • Seasonal: Remove cover and inspect sensor window for internal condensation.
|
||||
//
|
||||
// Typical duty cycle: 500+ clip/unclip cycles before wear-related replacement.
|
||||
// Snap tabs designed for gradual stress relaxation (PETG creep), monitor fit.
|
||||
//
|
||||
// =============================================================================
|
||||
//
|
||||
// DESIGN NOTES
|
||||
//
|
||||
// • Optical clearance: 4 mm minimum above window edge prevents vignetting
|
||||
// or optical interference. RPLIDAR maintains full 360° scan at ±60° FOV.
|
||||
//
|
||||
// • Drainage design: Six 4 mm holes distribute outflow, preventing
|
||||
// pooling. Placement at base perimeter (low point) ensures gravity-driven
|
||||
// drainage even at 30° tilt.
|
||||
//
|
||||
// • Snap tab stiffness: 2 mm thickness × 12 mm width gives ~2.5 mm
|
||||
// deflection at 10 N insertion force. Snap load: ~4 N (user-friendly).
|
||||
// Material relaxation over 500 cycles: ~0.5 mm loss of engagement depth.
|
||||
//
|
||||
// • Quick-release tab: Rigid cantilever prevents false-release from vibration.
|
||||
// Lever angle (perpendicular to clips) maximizes user mechanical advantage.
|
||||
//
|
||||
// • Manufacturing tolerance: ±0.3 mm on clip base OD and snap seat height
|
||||
// for reliable engagement. FDM print quality (nozzle 0.4 mm) provides
|
||||
// adequate tolerance for flex-fit snap design.
|
||||
//
|
||||
// =============================================================================
|
||||
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 */
|
||||
@ -0,0 +1,11 @@
|
||||
geofence:
|
||||
ros__parameters:
|
||||
# Polygon vertices as flat list [x1, y1, x2, y2, ...]
|
||||
# Example: square from (0,0) to (10,10)
|
||||
geofence_vertices: [0.0, 0.0, 10.0, 0.0, 10.0, 10.0, 0.0, 10.0]
|
||||
|
||||
# Enforce boundary by zeroing cmd_vel on breach
|
||||
enforce_boundary: false
|
||||
|
||||
# Safety margin (m) - breach triggered before actual boundary
|
||||
margin: 0.0
|
||||
@ -0,0 +1,31 @@
|
||||
"""Launch file for geofence enforcer node."""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""Generate launch description for geofence."""
|
||||
pkg_dir = get_package_share_directory("saltybot_geofence")
|
||||
config_file = os.path.join(pkg_dir, "config", "geofence_config.yaml")
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
DeclareLaunchArgument(
|
||||
"config_file",
|
||||
default_value=config_file,
|
||||
description="Path to configuration YAML file",
|
||||
),
|
||||
Node(
|
||||
package="saltybot_geofence",
|
||||
executable="geofence_node",
|
||||
name="geofence",
|
||||
output="screen",
|
||||
parameters=[LaunchConfiguration("config_file")],
|
||||
),
|
||||
]
|
||||
)
|
||||
22
jetson/ros2_ws/src/saltybot_geofence/package.xml
Normal file
22
jetson/ros2_ws/src/saltybot_geofence/package.xml
Normal file
@ -0,0 +1,22 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltybot_geofence</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Geofence boundary enforcer for SaltyBot</description>
|
||||
<maintainer email="sl-controls@saltylab.local">SaltyLab Controls</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
<depend>rclpy</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
|
||||
<test_depend>pytest</test_depend>
|
||||
<test_depend>nav_msgs</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,143 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Geofence boundary enforcer for SaltyBot.
|
||||
|
||||
Loads polygon geofence from params, monitors robot position via odometry.
|
||||
Publishes Bool on /saltybot/geofence_breach when exiting boundary.
|
||||
Optionally zeros cmd_vel to enforce boundary.
|
||||
|
||||
Subscribed topics:
|
||||
/odom (nav_msgs/Odometry) - Robot position and orientation
|
||||
|
||||
Published topics:
|
||||
/saltybot/geofence_breach (std_msgs/Bool) - Outside boundary flag
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from nav_msgs.msg import Odometry
|
||||
from std_msgs.msg import Bool
|
||||
from typing import List, Tuple
|
||||
|
||||
|
||||
class GeofenceNode(Node):
|
||||
"""ROS2 node for geofence boundary enforcement."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("geofence")
|
||||
|
||||
# Parameters
|
||||
self.declare_parameter("geofence_vertices", [])
|
||||
self.declare_parameter("enforce_boundary", False)
|
||||
self.declare_parameter("margin", 0.0)
|
||||
|
||||
vertices = self.get_parameter("geofence_vertices").value
|
||||
self.enforce_boundary = self.get_parameter("enforce_boundary").value
|
||||
self.margin = self.get_parameter("margin").value
|
||||
|
||||
# Parse vertices from flat list [x1,y1,x2,y2,...]
|
||||
self.geofence_vertices = self._parse_vertices(vertices)
|
||||
|
||||
# State tracking
|
||||
self.robot_x = 0.0
|
||||
self.robot_y = 0.0
|
||||
self.inside_geofence = True
|
||||
self.breach_published = False
|
||||
|
||||
# Subscription to odometry
|
||||
self.sub_odom = self.create_subscription(
|
||||
Odometry, "/odom", self._on_odometry, 10
|
||||
)
|
||||
|
||||
# Publisher for breach status
|
||||
self.pub_breach = self.create_publisher(Bool, "/saltybot/geofence_breach", 10)
|
||||
|
||||
self.get_logger().info(
|
||||
f"Geofence enforcer initialized with {len(self.geofence_vertices)} vertices. "
|
||||
f"Enforce: {self.enforce_boundary}, Margin: {self.margin}m"
|
||||
)
|
||||
|
||||
if len(self.geofence_vertices) > 0:
|
||||
self.get_logger().info(f"Geofence vertices: {self.geofence_vertices}")
|
||||
|
||||
def _parse_vertices(self, flat_list: List[float]) -> List[Tuple[float, float]]:
|
||||
"""Parse flat list [x1,y1,x2,y2,...] into vertex tuples."""
|
||||
if len(flat_list) < 6: # Need at least 3 vertices (6 values)
|
||||
self.get_logger().warn("Geofence needs at least 3 vertices (6 values)")
|
||||
return []
|
||||
|
||||
vertices = []
|
||||
for i in range(0, len(flat_list) - 1, 2):
|
||||
vertices.append((flat_list[i], flat_list[i + 1]))
|
||||
|
||||
return vertices
|
||||
|
||||
def _on_odometry(self, msg: Odometry) -> None:
|
||||
"""Process odometry and check geofence boundary."""
|
||||
if len(self.geofence_vertices) == 0:
|
||||
# No geofence defined
|
||||
self.inside_geofence = True
|
||||
return
|
||||
|
||||
# Extract robot position
|
||||
self.robot_x = msg.pose.pose.position.x
|
||||
self.robot_y = msg.pose.pose.position.y
|
||||
|
||||
# Check if inside geofence
|
||||
self.inside_geofence = self._point_in_polygon(
|
||||
(self.robot_x, self.robot_y), self.geofence_vertices
|
||||
)
|
||||
|
||||
# Publish breach status
|
||||
breach = not self.inside_geofence
|
||||
if breach and not self.breach_published:
|
||||
self.get_logger().warn(
|
||||
f"GEOFENCE BREACH! Robot at ({self.robot_x:.2f}, {self.robot_y:.2f})"
|
||||
)
|
||||
self.breach_published = True
|
||||
elif not breach and self.breach_published:
|
||||
self.get_logger().info(
|
||||
f"Robot re-entered geofence at ({self.robot_x:.2f}, {self.robot_y:.2f})"
|
||||
)
|
||||
self.breach_published = False
|
||||
|
||||
msg_breach = Bool(data=breach)
|
||||
self.pub_breach.publish(msg_breach)
|
||||
|
||||
def _point_in_polygon(self, point: Tuple[float, float], vertices: List[Tuple[float, float]]) -> bool:
|
||||
"""Ray casting algorithm for point-in-polygon test."""
|
||||
x, y = point
|
||||
n = len(vertices)
|
||||
inside = False
|
||||
|
||||
p1x, p1y = vertices[0]
|
||||
for i in range(1, n + 1):
|
||||
p2x, p2y = vertices[i % n]
|
||||
|
||||
# Check if ray crosses edge
|
||||
if y > min(p1y, p2y):
|
||||
if y <= max(p1y, p2y):
|
||||
if x <= max(p1x, p2x):
|
||||
if p1y != p2y:
|
||||
xinters = (y - p1y) * (p2x - p1x) / (p2y - p1y) + p1x
|
||||
if p1x == p2x or x <= xinters:
|
||||
inside = not inside
|
||||
|
||||
p1x, p1y = p2x, p2y
|
||||
|
||||
return inside
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = GeofenceNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
5
jetson/ros2_ws/src/saltybot_geofence/setup.cfg
Normal file
5
jetson/ros2_ws/src/saltybot_geofence/setup.cfg
Normal file
@ -0,0 +1,5 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_geofence
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_geofence
|
||||
24
jetson/ros2_ws/src/saltybot_geofence/setup.py
Normal file
24
jetson/ros2_ws/src/saltybot_geofence/setup.py
Normal file
@ -0,0 +1,24 @@
|
||||
from setuptools import setup, find_packages
|
||||
|
||||
setup(
|
||||
name='saltybot_geofence',
|
||||
version='0.1.0',
|
||||
packages=find_packages(),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages', ['resource/saltybot_geofence']),
|
||||
('share/saltybot_geofence', ['package.xml']),
|
||||
('share/saltybot_geofence/config', ['config/geofence_config.yaml']),
|
||||
('share/saltybot_geofence/launch', ['launch/geofence.launch.py']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
author='SaltyLab Controls',
|
||||
author_email='sl-controls@saltylab.local',
|
||||
description='Geofence boundary enforcer for SaltyBot',
|
||||
license='MIT',
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'geofence_node=saltybot_geofence.geofence_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
170
jetson/ros2_ws/src/saltybot_geofence/test/test_geofence.py
Normal file
170
jetson/ros2_ws/src/saltybot_geofence/test/test_geofence.py
Normal file
@ -0,0 +1,170 @@
|
||||
"""Tests for geofence boundary enforcer."""
|
||||
|
||||
import pytest
|
||||
from nav_msgs.msg import Odometry
|
||||
from geometry_msgs.msg import Point, Quaternion, Pose, PoseWithCovariance, TwistWithCovariance
|
||||
import rclpy
|
||||
from rclpy.time import Time
|
||||
|
||||
from saltybot_geofence.geofence_node import GeofenceNode
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def rclpy_fixture():
|
||||
rclpy.init()
|
||||
yield
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def node(rclpy_fixture):
|
||||
node = GeofenceNode()
|
||||
yield node
|
||||
node.destroy_node()
|
||||
|
||||
|
||||
class TestInit:
|
||||
def test_node_initialization(self, node):
|
||||
assert node.enforce_boundary is False
|
||||
assert node.margin == 0.0
|
||||
assert node.inside_geofence is True
|
||||
|
||||
|
||||
class TestVertexParsing:
|
||||
def test_parse_vertices_valid(self, node):
|
||||
vertices = node._parse_vertices([0.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0])
|
||||
assert len(vertices) == 4
|
||||
assert vertices[0] == (0.0, 0.0)
|
||||
assert vertices[1] == (1.0, 0.0)
|
||||
|
||||
def test_parse_vertices_insufficient(self, node):
|
||||
vertices = node._parse_vertices([0.0, 0.0, 1.0])
|
||||
assert len(vertices) == 0
|
||||
|
||||
|
||||
class TestPointInPolygon:
|
||||
def test_point_inside_square(self, node):
|
||||
vertices = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
|
||||
assert node._point_in_polygon((1.0, 1.0), vertices) is True
|
||||
|
||||
def test_point_outside_square(self, node):
|
||||
vertices = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
|
||||
assert node._point_in_polygon((3.0, 1.0), vertices) is False
|
||||
|
||||
def test_point_on_vertex(self, node):
|
||||
vertices = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
|
||||
# Point on vertex behavior may vary (typically outside)
|
||||
result = node._point_in_polygon((0.0, 0.0), vertices)
|
||||
assert isinstance(result, bool)
|
||||
|
||||
def test_point_on_edge(self, node):
|
||||
vertices = [(0.0, 0.0), (2.0, 0.0), (2.0, 2.0), (0.0, 2.0)]
|
||||
# Point on edge behavior (typically outside)
|
||||
result = node._point_in_polygon((1.0, 0.0), vertices)
|
||||
assert isinstance(result, bool)
|
||||
|
||||
def test_triangle_inside(self, node):
|
||||
vertices = [(0.0, 0.0), (4.0, 0.0), (2.0, 3.0)]
|
||||
assert node._point_in_polygon((2.0, 1.0), vertices) is True
|
||||
|
||||
def test_triangle_outside(self, node):
|
||||
vertices = [(0.0, 0.0), (4.0, 0.0), (2.0, 3.0)]
|
||||
assert node._point_in_polygon((5.0, 5.0), vertices) is False
|
||||
|
||||
def test_concave_polygon(self, node):
|
||||
# L-shaped polygon
|
||||
vertices = [(0.0, 0.0), (3.0, 0.0), (3.0, 1.0), (1.0, 1.0), (1.0, 3.0), (0.0, 3.0)]
|
||||
assert node._point_in_polygon((0.5, 0.5), vertices) is True
|
||||
assert node._point_in_polygon((2.0, 2.0), vertices) is False
|
||||
|
||||
def test_circle_approximation(self, node):
|
||||
# Octagon approximating circle
|
||||
import math
|
||||
vertices = []
|
||||
for i in range(8):
|
||||
angle = 2 * math.pi * i / 8
|
||||
vertices.append((math.cos(angle), math.sin(angle)))
|
||||
|
||||
# Center should be inside
|
||||
assert node._point_in_polygon((0.0, 0.0), vertices) is True
|
||||
# Far outside should be outside
|
||||
assert node._point_in_polygon((10.0, 10.0), vertices) is False
|
||||
|
||||
|
||||
class TestOdometryProcessing:
|
||||
def test_odometry_update_position(self, node):
|
||||
node.geofence_vertices = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]
|
||||
|
||||
msg = Odometry()
|
||||
msg.pose.pose.position.x = 5.0
|
||||
msg.pose.pose.position.y = 5.0
|
||||
|
||||
node._on_odometry(msg)
|
||||
|
||||
assert node.robot_x == 5.0
|
||||
assert node.robot_y == 5.0
|
||||
|
||||
def test_breach_detection_inside(self, node):
|
||||
node.geofence_vertices = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]
|
||||
|
||||
msg = Odometry()
|
||||
msg.pose.pose.position.x = 5.0
|
||||
msg.pose.pose.position.y = 5.0
|
||||
|
||||
node._on_odometry(msg)
|
||||
|
||||
assert node.inside_geofence is True
|
||||
|
||||
def test_breach_detection_outside(self, node):
|
||||
node.geofence_vertices = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]
|
||||
|
||||
msg = Odometry()
|
||||
msg.pose.pose.position.x = 15.0
|
||||
msg.pose.pose.position.y = 5.0
|
||||
|
||||
node._on_odometry(msg)
|
||||
|
||||
assert node.inside_geofence is False
|
||||
|
||||
def test_breach_flag_transition(self, node):
|
||||
node.geofence_vertices = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0)]
|
||||
assert node.breach_published is False
|
||||
|
||||
# Move outside
|
||||
msg = Odometry()
|
||||
msg.pose.pose.position.x = 15.0
|
||||
msg.pose.pose.position.y = 5.0
|
||||
node._on_odometry(msg)
|
||||
|
||||
assert node.breach_published is True
|
||||
|
||||
# Move back inside
|
||||
msg.pose.pose.position.x = 5.0
|
||||
msg.pose.pose.position.y = 5.0
|
||||
node._on_odometry(msg)
|
||||
|
||||
assert node.breach_published is False
|
||||
|
||||
|
||||
class TestEdgeCases:
|
||||
def test_no_geofence_defined(self, node):
|
||||
node.geofence_vertices = []
|
||||
|
||||
msg = Odometry()
|
||||
msg.pose.pose.position.x = 0.0
|
||||
msg.pose.pose.position.y = 0.0
|
||||
|
||||
node._on_odometry(msg)
|
||||
|
||||
# Should default to safe (inside)
|
||||
assert node.inside_geofence is True
|
||||
|
||||
def test_very_small_polygon(self, node):
|
||||
vertices = [(0.0, 0.0), (0.01, 0.0), (0.01, 0.01), (0.0, 0.01)]
|
||||
assert node._point_in_polygon((0.005, 0.005), vertices) is True
|
||||
assert node._point_in_polygon((0.1, 0.1), vertices) is False
|
||||
|
||||
def test_large_coordinates(self, node):
|
||||
vertices = [(0.0, 0.0), (1000.0, 0.0), (1000.0, 1000.0), (0.0, 1000.0)]
|
||||
assert node._point_in_polygon((500.0, 500.0), vertices) is True
|
||||
assert node._point_in_polygon((1500.0, 500.0), vertices) is False
|
||||
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