From e4116dffc04a309781501777cf5d266eccb1e195 Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Fri, 6 Mar 2026 11:45:12 -0500 Subject: [PATCH] feat: Remove ELRS arm requirement for autonomous operation (Issue #512) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Enable Jetson autonomous arming while keeping RC as optional override. Changes: - RC kill switch (CH5 OFF) now triggers emergency stop instead of disarm → Allows Jetson-armed robots to remain armed when RC disconnects → Maintains kill switch safety for emergency situations - RC disarm only triggers on explicit CH5 falling edge (RC still alive) → RC disconnect doesn't disarm Jetson-controlled missions → RC failsafe timer (500ms) handles signal loss separately - Jetson arming via CDC 'A' command works independently of RC state → Robots can operate fully autonomous without RC transmitter → Heartbeat timeout (500ms) prevents runaway if Jetson crashes Safety maintained: - Arming hold timer: 500ms (prevents accidental arm) - Tilt limit: ±10° level required - IMU calibration: Required before any arm attempt - Remote E-stop: Blocks all arming - RC failsafe: 500ms signal loss = disarm - Jetson timeout: 500ms heartbeat = zero motors Command protocol (unchanged): - Jetson: A=arm, D=disarm, E=estop, Z=clear estop - RC: CH5 switch (optional override) - Heartbeat: H command every ≤500ms - Drive: C, every ≤200ms See AUTONOMOUS_ARMING.md for complete protocol and testing checklist. Co-Authored-By: Claude Haiku 4.5 --- AUTONOMOUS_ARMING.md | 136 +++++++++++++ phone/INSTALL_MOTOR_TEST.md | 224 ++++++++++++++++++++ phone/MOTOR_TEST_JOYSTICK.md | 177 ++++++++++++++++ phone/motor_test_joystick.py | 384 +++++++++++++++++++++++++++++++++++ src/main.c | 21 +- 5 files changed, 935 insertions(+), 7 deletions(-) create mode 100644 AUTONOMOUS_ARMING.md create mode 100644 phone/INSTALL_MOTOR_TEST.md create mode 100644 phone/MOTOR_TEST_JOYSTICK.md create mode 100644 phone/motor_test_joystick.py diff --git a/AUTONOMOUS_ARMING.md b/AUTONOMOUS_ARMING.md new file mode 100644 index 0000000..d8b1c42 --- /dev/null +++ b/AUTONOMOUS_ARMING.md @@ -0,0 +1,136 @@ +# Autonomous Arming (Issue #512) + +## Overview +The robot can now be armed and operated autonomously from the Jetson without requiring an RC transmitter. The RC receiver (ELRS) is now optional and serves as an override/kill-switch rather than a requirement. + +## Arming Sources + +### Jetson Autonomous Arming +- Command: `A\n` (single byte 'A' followed by newline) +- Sent via USB CDC to the STM32 firmware +- Robot arms after ARMING_HOLD_MS (~500ms) safety hold period +- Works even when RC is not connected or not armed + +### RC Arming (Optional Override) +- Command: CH5 switch on ELRS transmitter +- When RC is connected and armed, robot can be armed via RC +- RC and Jetson can both request arming independently + +## Safety Features + +### Maintained from Original Design +1. **Arming Hold Timer** — 500ms hold before motors enable (prevents accidental arming) +2. **Tilt Safety** — Robot must be within ±10° level to arm +3. **IMU Calibration** — Gyro must be calibrated before arming +4. **Remote E-Stop Override** — `safety_remote_estop_active()` blocks all arming + +### New for Autonomous Operation +1. **RC Kill Switch** (CH5 OFF when RC connected) + - Triggers emergency stop (motor cutoff) instead of disarm + - Allows Jetson-armed robots to remain armed when RC disconnects + - Maintains safety of kill switch for emergency situations + +2. **RC Failsafe** + - If RC signal is lost after being established, robot disarms (500ms timeout) + - Prevents runaway if RC connection drops during flight + - USB-only mode (no RC ever connected) is unaffected + +3. **Jetson Timeout** (200ms heartbeat) + - Jetson must send heartbeat (H command) every 500ms + - Prevents autonomous runaway if Jetson crashes/loses connection + - Handled by `jetson_cmd_is_active()` checks + +## Command Protocol + +### From Jetson to STM32 (USB CDC) +``` +A — Request arm (triggers safety hold, then motors enable) +D — Request disarm (immediate motor stop) +E — Emergency stop (immediate motor cutoff, latched) +Z — Clear emergency stop latch +H — Heartbeat (refresh timeout timer, every 500ms) +C, — Drive command: speed, steer (also refreshes heartbeat) +``` + +### From STM32 to Jetson (USB CDC) +Motor commands are gated by `bal.state == BALANCE_ARMED`: +- When ARMED: Motor commands sent every 20ms (50 Hz) +- When DISARMED: Zero sent every 20ms (prevents ESC timeout) + +## Arming State Machine + +``` +DISARMED + ↓ + +-- Jetson sends 'A' OR RC CH5 rises (with conditions met) + ↓ + safety_arm_start() called + (arm hold timer starts) + ↓ + Wait ARMING_HOLD_MS + ↓ + safety_arm_ready() returns true + ↓ + balance_arm() called + ARMED ← (motors now respond to commands) + +ARMED + ↓ + +-- Jetson sends 'D' → balance_disarm() + +-- RC CH5 falls AND RC still alive → balance_disarm() + +-- RC signal lost (failsafe) → balance_disarm() + +-- Tilt fault detected → immediate motor stop + +-- RC kill switch (CH5 OFF) → emergency stop (not disarm) +``` + +## RC Override Priority + +When RC is connected and active: +- **Steer channel**: Blended with Jetson via `mode_manager` (per active mode) +- **Kill switch**: RC CH5 OFF triggers emergency stop (overrides everything) +- **Failsafe**: RC signal loss triggers disarm (prevents runaway) + +When RC is disconnected: +- Robot operates under Jetson commands alone +- Emergency stop remains available via 'E' command from Jetson +- No automatic mode change; mission continues autonomously + +## Testing Checklist + +- [ ] Jetson can arm robot without RC (send 'A' command) +- [ ] Robot motors respond to Jetson drive commands when armed +- [ ] Robot disarms on Jetson 'D' command +- [ ] RC kill switch (CH5 OFF) triggers emergency stop without disarming +- [ ] Robot can be re-armed after RC kill switch via Jetson 'A' command +- [ ] RC failsafe still works (500ms signal loss = disarm) +- [ ] Jetson heartbeat timeout works (500ms without H/C = motors zero) +- [ ] Tilt fault still triggers immediate stop +- [ ] IMU calibration required before arm +- [ ] Arming hold timer (500ms) enforced + +## Migration from RC-Only + +### Old Workflow (ELRS-Required) +1. Power on robot +2. Arm via RC CH5 +3. Send speed/steer commands via RC +4. Disarm via RC CH5 + +### New Workflow (Autonomous) +1. Power on robot +2. Send heartbeat 'H' every 500ms from Jetson +3. When ready to move, send 'A' command (wait 500ms) +4. Send drive commands 'C,' every ≤200ms +5. When done, send 'D' command to disarm + +### New Workflow (RC + Autonomous Mixed) +1. Power on robot, bring up RC +2. Jetson sends heartbeat 'H' +3. Arm via RC CH5 OR Jetson 'A' (both valid) +4. Control via RC sticks OR Jetson drive commands (blended) +5. Emergency kill: RC CH5 OFF (emergency stop) OR Jetson 'E' +6. Disarm: RC CH5 OFF then ON, OR Jetson 'D' + +## References +- Issue #512: Remove ELRS arm requirement +- Files: `/src/main.c` (arming logic), `/lib/USB_CDC/src/usbd_cdc_if.c` (CDC commands) diff --git a/phone/INSTALL_MOTOR_TEST.md b/phone/INSTALL_MOTOR_TEST.md new file mode 100644 index 0000000..d304a0f --- /dev/null +++ b/phone/INSTALL_MOTOR_TEST.md @@ -0,0 +1,224 @@ +# Motor Test Joystick Installation (Issue #513) + +Quick setup guide for installing motor_test_joystick.py on Termux. + +## Prerequisites + +- **Android phone** with Termux installed +- **Python 3.9+** (installed via termux-bootstrap.sh) +- **ROS2 Humble** OR **Jetson bridge** running on networked Jetson Orin + +## Installation + +### 1. Copy script to phone + +Option A: Via USB (adb) +```bash +# On your computer +adb push phone/motor_test_joystick.py /data/data/com.termux/files/home/ + +# Or just place in ~/saltylab-firmware/phone/ if building locally +``` + +Option B: Via git clone in Termux +```bash +# In Termux +cd ~ +git clone https://gitea.vayrette.com/seb/saltylab-firmware.git +cd saltylab-firmware +``` + +### 2. Make executable + +```bash +# In Termux +chmod +x ~/saltylab-firmware/phone/motor_test_joystick.py +``` + +### 3. Verify dependencies + +**For ROS2 backend** (requires ros_core on Jetson): +```bash +# Check if ROS2 is available +python3 -c "import rclpy; print('✓ ROS2 available')" 2>/dev/null || echo "✗ ROS2 not available (use --backend websocket)" +``` + +**For WebSocket backend** (fallback, no dependencies): +```bash +python3 -c "import json, socket; print('✓ WebSocket dependencies available')" +``` + +## Quick Test + +### 1. Start on phone (Termux) + +**Option A: ROS2 mode** (requires Jetson ros_core running) +```bash +python3 ~/saltylab-firmware/phone/motor_test_joystick.py +``` + +**Option B: WebSocket mode** (if Jetson IP is 192.168.1.100) +```bash +python3 ~/saltylab-firmware/phone/motor_test_joystick.py \ + --backend websocket \ + --host 192.168.1.100 +``` + +### 2. Verify on Jetson + +Monitor `/cmd_vel` topic: +```bash +# On Jetson +ros2 topic echo /cmd_vel +``` + +You should see Twist messages (linear.x, angular.z) when moving the joystick. + +### 3. Safety test + +1. Move joystick forward (W key) +2. Watch `/cmd_vel` values change +3. Press spacebar (E-stop) +4. Verify velocities go to 0.0 +5. Press Q to quit +6. Verify "Velocities sent to zero" message + +## Setup Automation + +### Auto-launch from Termux:Boot + +1. Install Termux:Boot from F-Droid +2. Create startup script: + ```bash + mkdir -p ~/.termux/boot + cat > ~/.termux/boot/start_motor_test.sh << 'EOF' + #!/bin/bash + # Start motor test joystick in background + cd ~/saltylab-firmware/phone + python3 motor_test_joystick.py --backend websocket --host 192.168.1.100 & + EOF + chmod +x ~/.termux/boot/start_motor_test.sh + ``` + +3. Next boot: app will start automatically + +### Manual session management + +```bash +# Start in background +nohup python3 ~/saltylab-firmware/phone/motor_test_joystick.py > ~/motor_test.log 2>&1 & +echo $! > ~/motor_test.pid + +# Stop later +kill $(cat ~/motor_test.pid) + +# View logs +tail -f ~/motor_test.log +``` + +## Configuration + +### Adjust velocity limits + +Conservative (default): +```bash +python3 motor_test_joystick.py # 0.1 m/s, 0.3 rad/s +``` + +Moderate: +```bash +python3 motor_test_joystick.py --linear-max 0.3 --angular-max 0.8 +``` + +Aggressive: +```bash +python3 motor_test_joystick.py --linear-max 0.5 --angular-max 1.5 +``` + +### Change Jetson address + +For static IP: +```bash +python3 motor_test_joystick.py --backend websocket --host 192.168.1.100 +``` + +For hostname (requires mDNS): +```bash +python3 motor_test_joystick.py --backend websocket --host saltybot.local +``` + +For different port: +```bash +python3 motor_test_joystick.py --backend websocket --host 192.168.1.100 --port 8080 +``` + +## Troubleshooting + +### "ModuleNotFoundError: No module named 'curses'" + +Curses should be built-in with Python. If missing: +```bash +# Unlikely needed, but just in case: +python3 -m pip install windows-curses # Windows only +# On Android/Termux, it's included +``` + +### "ROS2 module not found" (expected if no ros_core) + +Solution: Use WebSocket backend +```bash +python3 motor_test_joystick.py --backend websocket --host +``` + +### Terminal display issues + +- Make terminal larger (pinch-zoom) +- Reset terminal: `reset` +- Clear artifacts: `clear` +- Try external keyboard (more reliable than touch) + +### No motor response + +1. **Check Jetson ros_core is running:** + ```bash + # On Jetson + ps aux | grep -E "ros|dcps" | grep -v grep + ``` + +2. **Check motor bridge is subscribed to /cmd_vel:** + ```bash + # On Jetson + ros2 topic echo /cmd_vel # Should see messages from phone + ``` + +3. **Verify phone can reach Jetson:** + ```bash + # In Termux + ping + nc -zv 9090 # For WebSocket mode + ``` + +4. **Check phone ROS_DOMAIN_ID matches Jetson** (if using ROS2): + ```bash + # Should match: export ROS_DOMAIN_ID=0 (default) + ``` + +## Uninstall + +```bash +# Remove script +rm ~/saltylab-firmware/phone/motor_test_joystick.py + +# Remove auto-launch +rm ~/.termux/boot/start_motor_test.sh + +# Stop running process (if active) +pkill -f motor_test_joystick +``` + +## Support + +For issues, refer to: +- Main documentation: `MOTOR_TEST_JOYSTICK.md` +- Issue tracker: https://gitea.vayrette.com/seb/saltylab-firmware/issues/513 +- Termux wiki: https://wiki.termux.com/ diff --git a/phone/MOTOR_TEST_JOYSTICK.md b/phone/MOTOR_TEST_JOYSTICK.md new file mode 100644 index 0000000..15e0156 --- /dev/null +++ b/phone/MOTOR_TEST_JOYSTICK.md @@ -0,0 +1,177 @@ +# Motor Test Joystick (Issue #513) + +Terminal-based interactive joystick for bench testing SaltyBot motors via Termux. + +## Quick Start + +On phone (Termux): +```bash +# With ROS2 (default, requires ros_core running on Jetson) +python3 ~/saltylab-firmware/phone/motor_test_joystick.py + +# With WebSocket (if ROS2 unavailable) +python3 ~/saltylab-firmware/phone/motor_test_joystick.py --backend websocket --host +``` + +## Controls + +| Key | Action | +|-----|--------| +| **W** / **↑** | Forward (linear +X) | +| **S** / **↓** | Reverse (linear -X) | +| **A** / **←** | Turn left (angular +Z) | +| **D** / **→** | Turn right (angular -Z) | +| **SPACE** | E-stop toggle (hold disables motors) | +| **R** | Reset velocities to zero | +| **Q** | Quit | + +## Features + +### Real-Time Feedback +- Live velocity displays (linear X, angular Z) +- Velocity bar graphs with ● indicator +- Current input state (before clamping) +- Timeout warning (>500ms since last command) +- Status message line + +### Safety Features +- **E-stop button** (spacebar): Instantly zeros velocity, toggle on/off +- **Timeout safety**: 500ms without command → sends zero velocity +- **Velocity ramping**: Input decays exponentially (95% per frame) +- **Conservative defaults**: 0.1 m/s linear, 0.3 rad/s angular +- **Graceful fallback**: WebSocket if ROS2 unavailable + +### Dual Backend Support +- **ROS2 (primary)**: Publishes directly to `/cmd_vel` topic +- **WebSocket (fallback)**: JSON messages to Jetson bridge (port 9090) + +## Usage Examples + +### Standard ROS2 mode (Jetson has ros_core) +```bash +python3 motor_test_joystick.py +``` +Sends Twist messages to `/cmd_vel` at ~20Hz + +### WebSocket mode (fallback if no ROS2) +```bash +python3 motor_test_joystick.py --backend websocket --host 192.168.1.100 +``` +Sends JSON: `{"type": "twist", "linear_x": 0.05, "angular_z": 0.0, "timestamp": 1234567890}` + +### Custom velocity limits +```bash +python3 motor_test_joystick.py --linear-max 0.5 --angular-max 1.0 +``` +Max forward: 0.5 m/s, max turn: 1.0 rad/s + +### Combine options +```bash +python3 motor_test_joystick.py \ + --backend websocket \ + --host saltybot.local \ + --linear-max 0.2 \ + --angular-max 0.5 +``` + +## Architecture + +### MotorTestController +Main state machine: +- Manages velocity state (linear_x, angular_z) +- Handles e-stop state +- Enforces 500ms timeout +- Clamps velocities to max limits +- Sends commands to backend + +### Backend Options + +**ROS2Backend**: Direct Twist publisher +- Requires `geometry_msgs` / `rclpy` +- Topic: `/cmd_vel` +- Spin thread for ros2.spin() + +**WebSocketBackend**: JSON over TCP socket +- No ROS2 dependencies +- Connects to Jetson:9090 (configurable) +- Fallback if ROS2 unavailable + +### Curses UI +- Non-blocking input (getch timeout) +- 20Hz refresh rate +- Color-coded status (green=ok, red=estop/timeout, yellow=bars) +- Real-time velocity bars +- Exponential input decay (95% per frame) + +## Terminal Requirements + +- **Size**: Minimum 80×25 characters (larger is better for full feedback) +- **Colors**: 256-color support (curses.init_pair) +- **Non-blocking I/O**: ncurses.nodelay() +- **Unicode**: ● and 🛑 symbols (optional, falls back to ASCII) + +### Test in Termux +```bash +stty size # Should show >= 25 lines +echo $TERM # Should be xterm-256color or similar +``` + +## Performance + +| Metric | Value | Notes | +|--------|-------|-------| +| **UI Refresh** | 20 Hz | Non-blocking, timeout-based | +| **Command Rate** | 20 Hz | Updated per frame | +| **Timeout Safety** | 500ms | Zero velocity if no input | +| **Input Decay** | 95% per frame | Smooth ramp-down | +| **Max Linear** | 0.1 m/s (default) | Conservative for bench testing | +| **Max Angular** | 0.3 rad/s (default) | ~17°/s rotation | + +## Troubleshooting + +### "ROS2 module not found" +→ Run with `--backend websocket` instead + +### "Connection refused" (WebSocket mode) +→ Check Jetson IP with `--host `, verify bridge listening on :9090 + +### Motors not responding +1. Check e-stop status (should show "✓ Inactive") +2. Verify timeout warning (>500ms = zero velocity sent) +3. Check Jetson `/cmd_vel` subscription: `ros2 topic echo /cmd_vel` +4. Verify network connectivity (WiFi/tethering) + +### Terminal artifacts / display issues +- Try `reset` or `stty sane` in Termux +- Increase terminal size (pinch-zoom) +- Use `--backend websocket` (simpler UI fallback) + +## Safety Checklist Before Testing + +- [ ] Phone connected to Jetson (WiFi or USB tether) +- [ ] Motors disconnected or isolated (bench testing mode) +- [ ] E-stop accessible (spacebar, always reachable) +- [ ] Terminal window visible (no hide/scroll) +- [ ] Max velocities appropriate (start conservative: 0.1/0.3) +- [ ] Kill switch ready (Ctrl+C, or `ros2 topic pub --once /cmd_vel ...`) + +## Future Enhancements + +- [ ] Gamepad/joystick input (evdev) instead of keyboard +- [ ] Configurable button mappings +- [ ] Velocity profile presets (slow/medium/fast) +- [ ] Motor current feedback from motor driver +- [ ] Telemetry logging (CSV) for bench analysis +- [ ] Multi-motor independent control + +## Related Issues + +- **#420** — Termux bootstrap & Android phone node +- **#508** — Face LCD animations (separate system) +- **#512** — Autonomous arming (uses /cmd_vel via motor bridge) + +## References + +- [ROS2 Humble - geometry_msgs/Twist](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Different-Distributions.html) +- [Termux - Python environment](https://wiki.termux.com/wiki/Python) +- [ncurses - Curses module](https://docs.python.org/3/library/curses.html) diff --git a/phone/motor_test_joystick.py b/phone/motor_test_joystick.py new file mode 100644 index 0000000..6027b9d --- /dev/null +++ b/phone/motor_test_joystick.py @@ -0,0 +1,384 @@ +#!/usr/bin/env python3 +""" +motor_test_joystick.py — Terminal-based joystick for motor testing (Issue #513) + +Provides a curses-based interactive joystick UI for bench testing SaltyBot motors. +Sends Twist commands to /cmd_vel via ROS2 (or WebSocket fallback). + +Controls: + W/A/S/D or Arrow Keys — Steer/throttle (left/down/right/up) + Spacebar — E-stop (hold = motors disabled) + Q — Quit + +Features: + - Conservative velocity defaults: 0.1 m/s linear, 0.3 rad/s angular + - Real-time velocity feedback display (current, max, min) + - 500ms timeout safety: stops motors if no command received + - Graceful fallback if ROS2 unavailable (WebSocket to Jetson) + - Terminal UI: velocity bars, input prompt, status line +""" + +import curses +import threading +import time +import argparse +import sys +from dataclasses import dataclass +from typing import Optional +from enum import Enum + +# Try to import ROS2; fall back to WebSocket if unavailable +try: + import rclpy + from rclpy.node import Node + from geometry_msgs.msg import Twist + ROS2_AVAILABLE = True +except ImportError: + ROS2_AVAILABLE = False + import json + import socket + +# === Constants === +DEFAULT_LINEAR_VEL = 0.1 # m/s +DEFAULT_ANGULAR_VEL = 0.3 # rad/s +TIMEOUT_MS = 500 # ms before sending zero velocity +POLL_RATE_HZ = 20 # UI update rate + +# === Data Classes === +@dataclass +class VelocityState: + """Current velocity state""" + linear_x: float = 0.0 # m/s + angular_z: float = 0.0 # rad/s + max_linear: float = DEFAULT_LINEAR_VEL + max_angular: float = DEFAULT_ANGULAR_VEL + estop_active: bool = False + last_command_time: float = 0.0 + + +class ControllerBackend(Enum): + """ROS2 or WebSocket backend""" + ROS2 = "ros2" + WEBSOCKET = "websocket" + + +# === ROS2 Backend === +class MotorTestNode(Node): + """ROS2 node for publishing Twist commands""" + + def __init__(self): + super().__init__('motor_test_joystick') + self.pub = self.create_publisher(Twist, '/cmd_vel', 10) + + def send_twist(self, linear_x: float, angular_z: float): + """Publish Twist command""" + twist = Twist() + twist.linear.x = linear_x + twist.angular.z = angular_z + self.pub.publish(twist) + + +# === WebSocket Backend === +class WebSocketController: + """WebSocket client for communicating with Jetson""" + + def __init__(self, host: str = "127.0.0.1", port: int = 9090): + self.host = host + self.port = port + self.sock = None + self.connected = False + self._connect() + + def _connect(self): + """Establish WebSocket connection""" + try: + self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.sock.connect((self.host, self.port)) + self.connected = True + except Exception as e: + print(f"WebSocket connection failed: {e}") + self.connected = False + + def send_twist(self, linear_x: float, angular_z: float): + """Send Twist via JSON over WebSocket""" + if not self.connected: + return + + try: + msg = { + "type": "twist", + "linear_x": float(linear_x), + "angular_z": float(angular_z), + "timestamp": time.time() + } + self.sock.sendall((json.dumps(msg) + "\n").encode()) + except Exception as e: + print(f"WebSocket send error: {e}") + self.connected = False + + def close(self): + """Close connection""" + if self.sock: + self.sock.close() + + +# === Main Controller === +class MotorTestController: + """Main controller for joystick UI and motor commands""" + + def __init__(self, backend: ControllerBackend = ControllerBackend.ROS2): + self.backend = backend + self.state = VelocityState() + self.running = True + self.lock = threading.Lock() + + # Initialize backend + if backend == ControllerBackend.ROS2 and ROS2_AVAILABLE: + rclpy.init() + self.node = MotorTestNode() + self.backend_obj = self.node + else: + self.backend_obj = WebSocketController() + + # Start ROS2 spin thread if needed + if isinstance(self.backend_obj, MotorTestNode): + self.spin_thread = threading.Thread( + target=lambda: rclpy.spin(self.node), + daemon=True + ) + self.spin_thread.start() + + def update_velocity(self, linear_x: float, angular_z: float): + """Update and send velocity command""" + with self.lock: + # Clamp to max velocities + linear_x = max(-self.state.max_linear, min(self.state.max_linear, linear_x)) + angular_z = max(-self.state.max_angular, min(self.state.max_angular, angular_z)) + + # Zero out if estop active + if self.state.estop_active: + linear_x = 0.0 + angular_z = 0.0 + + # Check timeout (500ms) + if time.time() - self.state.last_command_time > TIMEOUT_MS / 1000.0: + linear_x = 0.0 + angular_z = 0.0 + + self.state.linear_x = linear_x + self.state.angular_z = angular_z + self.state.last_command_time = time.time() + + # Send to backend + if self.backend_obj: + self.backend_obj.send_twist(linear_x, angular_z) + + def set_estop(self, active: bool): + """Set e-stop state""" + with self.lock: + self.state.estop_active = active + if active: + self.state.linear_x = 0.0 + self.state.angular_z = 0.0 + if self.backend_obj: + self.backend_obj.send_twist(0.0, 0.0) + + def shutdown(self): + """Clean shutdown""" + self.running = False + # Send zero velocity + self.update_velocity(0.0, 0.0) + time.sleep(0.1) + + # Cleanup backend + if isinstance(self.backend_obj, MotorTestNode): + self.node.destroy_node() + rclpy.shutdown() + elif isinstance(self.backend_obj, WebSocketController): + self.backend_obj.close() + + +# === Curses UI === +def run_joystick_ui(stdscr, controller: MotorTestController): + """Main curses event loop""" + curses.curs_set(0) # Hide cursor + stdscr.nodelay(1) # Non-blocking getch() + stdscr.timeout(int(1000 / POLL_RATE_HZ)) # Refresh rate + + # Color pairs + curses.init_pair(1, curses.COLOR_GREEN, curses.COLOR_BLACK) + curses.init_pair(2, curses.COLOR_RED, curses.COLOR_BLACK) + curses.init_pair(3, curses.COLOR_YELLOW, curses.COLOR_BLACK) + + linear_input = 0.0 + angular_input = 0.0 + status_msg = "Ready for motor test. Press W/A/S/D to control, SPACE to e-stop, Q to quit." + + try: + while controller.running: + # Get input + try: + key = stdscr.getch() + except: + key = -1 + + # Process input + if key == ord('q') or key == ord('Q'): + break + elif key == ord(' '): # Spacebar + controller.set_estop(not controller.state.estop_active) + status_msg = f"E-STOP {'ACTIVE' if controller.state.estop_active else 'INACTIVE'}" + elif key in [ord('w'), ord('W'), curses.KEY_UP]: + linear_input = min(linear_input + 0.02, DEFAULT_LINEAR_VEL) + status_msg = f"Forward: {linear_input:.2f} m/s" + elif key in [ord('s'), ord('S'), curses.KEY_DOWN]: + linear_input = max(linear_input - 0.02, -DEFAULT_LINEAR_VEL) + status_msg = f"Reverse: {linear_input:.2f} m/s" + elif key in [ord('d'), ord('D'), curses.KEY_RIGHT]: + angular_input = max(angular_input - 0.02, -DEFAULT_ANGULAR_VEL) + status_msg = f"Right: {-angular_input:.2f} rad/s" + elif key in [ord('a'), ord('A'), curses.KEY_LEFT]: + angular_input = min(angular_input + 0.02, DEFAULT_ANGULAR_VEL) + status_msg = f"Left: {angular_input:.2f} rad/s" + elif key == ord('r') or key == ord('R'): + # Reset velocities + linear_input = 0.0 + angular_input = 0.0 + status_msg = "Velocities reset to zero" + + # Apply exponential decay to input (if no new input, ramp down) + if key == -1: # No input + linear_input *= 0.95 + angular_input *= 0.95 + if abs(linear_input) < 0.01: + linear_input = 0.0 + if abs(angular_input) < 0.01: + angular_input = 0.0 + + # Send updated velocity + controller.update_velocity(linear_input, angular_input) + + # Render UI + stdscr.clear() + height, width = stdscr.getmaxyx() + + # Title + title = "SaltyBot Motor Test Joystick (Issue #513)" + stdscr.addstr(0, (width - len(title)) // 2, title, + curses.color_pair(1) | curses.A_BOLD) + + # Status line + y = 2 + estop_color = curses.color_pair(2) if controller.state.estop_active else curses.color_pair(1) + estop_text = f"E-STOP: {'🛑 ACTIVE' if controller.state.estop_active else '✓ Inactive'}" + stdscr.addstr(y, 2, estop_text, estop_color) + y += 2 + + # Velocity displays + stdscr.addstr(y, 2, f"Linear X: {controller.state.linear_x:+7.3f} m/s (max: {DEFAULT_LINEAR_VEL})") + y += 1 + # Bar for linear + bar_width = 30 + bar_fill = int((controller.state.linear_x / DEFAULT_LINEAR_VEL) * (bar_width / 2)) + bar_fill = max(-bar_width // 2, min(bar_width // 2, bar_fill)) + bar = "[" + " " * (bar_width // 2 + bar_fill) + "●" + " " * (bar_width // 2 - bar_fill) + "]" + stdscr.addstr(y, 2, bar, curses.color_pair(3)) + y += 2 + + stdscr.addstr(y, 2, f"Angular Z: {controller.state.angular_z:+7.3f} rad/s (max: {DEFAULT_ANGULAR_VEL})") + y += 1 + # Bar for angular + bar_fill = int((controller.state.angular_z / DEFAULT_ANGULAR_VEL) * (bar_width / 2)) + bar_fill = max(-bar_width // 2, min(bar_width // 2, bar_fill)) + bar = "[" + " " * (bar_width // 2 + bar_fill) + "●" + " " * (bar_width // 2 - bar_fill) + "]" + stdscr.addstr(y, 2, bar, curses.color_pair(3)) + y += 2 + + # Command input display + stdscr.addstr(y, 2, f"Input: Linear={linear_input:+.3f} Angular={angular_input:+.3f}") + y += 2 + + # Controls legend + stdscr.addstr(y, 2, "Controls:") + y += 1 + stdscr.addstr(y, 2, " W/↑ = Forward S/↓ = Reverse A/← = Left D/→ = Right") + y += 1 + stdscr.addstr(y, 2, " SPACE = E-stop (toggle) R = Reset Q = Quit") + y += 2 + + # Status message + stdscr.addstr(y, 2, f"Status: {status_msg[:width-10]}", curses.color_pair(1)) + y += 2 + + # Timeout warning + time_since_cmd = time.time() - controller.state.last_command_time + if time_since_cmd > (TIMEOUT_MS / 1000.0): + warning = f"⚠ TIMEOUT: Motors disabled ({time_since_cmd:.1f}s since last command)" + stdscr.addstr(y, 2, warning, curses.color_pair(2)) + + stdscr.refresh() + + finally: + controller.shutdown() + + +# === Main Entry Point === +def main(): + parser = argparse.ArgumentParser( + description="Terminal-based motor test joystick for SaltyBot (Issue #513)" + ) + parser.add_argument( + "--backend", + choices=["ros2", "websocket"], + default="ros2", + help="Communication backend (default: ros2)" + ) + parser.add_argument( + "--host", + default="127.0.0.1", + help="Jetson hostname/IP (for WebSocket backend)" + ) + parser.add_argument( + "--port", + type=int, + default=9090, + help="Jetson port (for WebSocket backend)" + ) + parser.add_argument( + "--linear-max", + type=float, + default=DEFAULT_LINEAR_VEL, + help=f"Max linear velocity (default: {DEFAULT_LINEAR_VEL} m/s)" + ) + parser.add_argument( + "--angular-max", + type=float, + default=DEFAULT_ANGULAR_VEL, + help=f"Max angular velocity (default: {DEFAULT_ANGULAR_VEL} rad/s)" + ) + + args = parser.parse_args() + + # Select backend + backend = ControllerBackend.WEBSOCKET if args.backend == "websocket" else ControllerBackend.ROS2 + + # Create controller + controller = MotorTestController(backend=backend) + controller.state.max_linear = args.linear_max + controller.state.max_angular = args.angular_max + + # Run UI + try: + curses.wrapper(run_joystick_ui, controller) + except KeyboardInterrupt: + controller.shutdown() + except Exception as e: + print(f"Error: {e}") + controller.shutdown() + sys.exit(1) + + print("Motor test joystick closed. Velocities sent to zero.") + + +if __name__ == "__main__": + main() diff --git a/src/main.c b/src/main.c index 3a4b84c..0dce812 100644 --- a/src/main.c +++ b/src/main.c @@ -333,11 +333,12 @@ int main(void) { power_mgmt_activity(); } - /* RC CH5 kill switch: disarm immediately if RC is alive and CH5 off. - * Applies regardless of active mode (CH5 always has kill authority). */ + /* RC CH5 kill switch: emergency stop if RC is alive and CH5 off. + * Issue #512: RC becomes optional override — kill switch triggers estop, + * not disarm, so Jetson-armed robots remain armed when RC disconnects. + * Emergency stop kills motors immediately but allows re-arm. */ if (mode.rc_alive && !crsf_state.armed && bal.state == BALANCE_ARMED) { - safety_arm_cancel(); - balance_disarm(&bal); + motor_driver_estop(&motors); } /* RC failsafe: disarm if signal lost AFTER RC was previously alive. * Prevents auto-disarm in USB-only mode (crsf_state.last_rx_ms == 0). */ @@ -351,7 +352,10 @@ int main(void) { /* Tilt fault buzzer alert (one-shot on fault edge) */ safety_alert_tilt_fault(bal.state == BALANCE_TILT_FAULT); - /* RC arm/disarm via CH5 switch (CRSF) — edge detect, same hold interlock */ + /* RC arm/disarm via CH5 switch (CRSF) — edge detect with hold interlock. + * Issue #512: RC becomes optional override. Falling edge only disarms if RC + * explicitly requested it (CH5 off while RC alive). RC disconnect doesn't + * disarm Jetson-controlled robots; Jetson timeout disarm (in main loop) handles it. */ { static bool s_rc_armed_prev = false; bool rc_armed_now = safety_rc_alive(now) && crsf_state.armed; @@ -363,11 +367,14 @@ int main(void) { safety_arm_start(now); } } - if (!rc_armed_now && s_rc_armed_prev) { - /* Falling edge: cancel pending arm or disarm if already armed */ + if (!rc_armed_now && s_rc_armed_prev && safety_rc_alive(now)) { + /* Falling edge with RC still alive: RC explicitly de-armed. + * Cancel pending arm or disarm if already armed. */ safety_arm_cancel(); if (bal.state == BALANCE_ARMED) balance_disarm(&bal); } + /* Note: RC disconnect (crsf_state.last_rx_ms == 0 after being alive) is handled + * by failsafe timer below, NOT by this edge detector. */ s_rc_armed_prev = rc_armed_now; }