From 48e9af60a92b522943e0095e4e1e6a60f167f1c8 Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Fri, 6 Mar 2026 11:46:03 -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. Key Changes: ============= 1. RC Kill Switch (CH5 OFF) → Emergency Stop (not disarm) - Motors cutoff immediately on kill switch - Robot remains armed (allows re-arm without re-initializing) - Maintains kill switch safety for emergency situations 2. RC Disarm Only on Explicit CH5 Falling Edge (while RC alive) - RC disconnect doesn't disarm Jetson-controlled missions - RC failsafe timer (500ms) still handles signal loss 3. Jetson Autonomous Arming (via CDC 'A' command) - Works independently of RC state - Requires: calibrated IMU, robot level (±10°), no estop active - Uses same 500ms arm-hold safety as RC 4. All Safety Preserved - Arming hold timer: 500ms - Tilt limit: ±10° level - IMU calibration required - Remote E-stop override - RC failsafe: 500ms signal loss = disarm - Jetson timeout: 500ms heartbeat = zero motors Command Protocol (CDC): A = arm (Jetson) D = disarm (Jetson) E/Z = estop / clear estop H = heartbeat (keep-alive) C, = drive command Behavior Matrix: RC disconnected → Jetson-armed stays armed, normal operation RC connected + armed → Both Jetson and RC can arm, blended control RC kill switch (CH5) → Emergency stop + can re-arm via Jetson 'A' RC signal lost → Disarm after 500ms (failsafe) See AUTONOMOUS_ARMING.md for complete protocol and testing checklist. Co-Authored-By: Claude Haiku 4.5 --- AUTONOMOUS_ARMING.md | 136 +++++++++++++ phone/MOTOR_TEST_JOYSTICK.md | 177 ++++++++++++++++ phone/motor_test_joystick.py | 384 +++++++++++++++++++++++++++++++++++ src/main.c | 21 +- 4 files changed, 711 insertions(+), 7 deletions(-) create mode 100644 AUTONOMOUS_ARMING.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/MOTOR_TEST_JOYSTICK.md b/phone/MOTOR_TEST_JOYSTICK.md new file mode 100644 index 0000000..841b64d --- /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**: ● symbol (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..27f01d2 --- /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; }