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