feat: Remove ELRS arm requirement for autonomous operation (Issue #512)
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<speed>,<steer> every ≤200ms See AUTONOMOUS_ARMING.md for complete protocol and testing checklist. Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
This commit is contained in:
parent
b0c2b5564d
commit
e4116dffc0
136
AUTONOMOUS_ARMING.md
Normal file
136
AUTONOMOUS_ARMING.md
Normal file
@ -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<spd>,<str> — 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<spd>,<str>' 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)
|
||||||
224
phone/INSTALL_MOTOR_TEST.md
Normal file
224
phone/INSTALL_MOTOR_TEST.md
Normal file
@ -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 <jetson-ip>
|
||||||
|
```
|
||||||
|
|
||||||
|
### 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 <jetson-ip>
|
||||||
|
nc -zv <jetson-ip> 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/
|
||||||
177
phone/MOTOR_TEST_JOYSTICK.md
Normal file
177
phone/MOTOR_TEST_JOYSTICK.md
Normal file
@ -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 <jetson-ip>
|
||||||
|
```
|
||||||
|
|
||||||
|
## 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 <ip>`, 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)
|
||||||
384
phone/motor_test_joystick.py
Normal file
384
phone/motor_test_joystick.py
Normal file
@ -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()
|
||||||
21
src/main.c
21
src/main.c
@ -333,11 +333,12 @@ int main(void) {
|
|||||||
power_mgmt_activity();
|
power_mgmt_activity();
|
||||||
}
|
}
|
||||||
|
|
||||||
/* RC CH5 kill switch: disarm immediately if RC is alive and CH5 off.
|
/* RC CH5 kill switch: emergency stop if RC is alive and CH5 off.
|
||||||
* Applies regardless of active mode (CH5 always has kill authority). */
|
* 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) {
|
if (mode.rc_alive && !crsf_state.armed && bal.state == BALANCE_ARMED) {
|
||||||
safety_arm_cancel();
|
motor_driver_estop(&motors);
|
||||||
balance_disarm(&bal);
|
|
||||||
}
|
}
|
||||||
/* RC failsafe: disarm if signal lost AFTER RC was previously alive.
|
/* RC failsafe: disarm if signal lost AFTER RC was previously alive.
|
||||||
* Prevents auto-disarm in USB-only mode (crsf_state.last_rx_ms == 0). */
|
* 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) */
|
/* Tilt fault buzzer alert (one-shot on fault edge) */
|
||||||
safety_alert_tilt_fault(bal.state == BALANCE_TILT_FAULT);
|
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;
|
static bool s_rc_armed_prev = false;
|
||||||
bool rc_armed_now = safety_rc_alive(now) && crsf_state.armed;
|
bool rc_armed_now = safety_rc_alive(now) && crsf_state.armed;
|
||||||
@ -363,11 +367,14 @@ int main(void) {
|
|||||||
safety_arm_start(now);
|
safety_arm_start(now);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (!rc_armed_now && s_rc_armed_prev) {
|
if (!rc_armed_now && s_rc_armed_prev && safety_rc_alive(now)) {
|
||||||
/* Falling edge: cancel pending arm or disarm if already armed */
|
/* Falling edge with RC still alive: RC explicitly de-armed.
|
||||||
|
* Cancel pending arm or disarm if already armed. */
|
||||||
safety_arm_cancel();
|
safety_arm_cancel();
|
||||||
if (bal.state == BALANCE_ARMED) balance_disarm(&bal);
|
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;
|
s_rc_armed_prev = rc_armed_now;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user