Merge pull request 'feat(safety): remote e-stop over 4G MQTT (Issue #63)' (#69) from sl-firmware/remote-estop into main

include/safety.h

@@ -12,8 +12,17 @@
  *  - RC signal timeout monitoring
  *  - Tilt fault alert via buzzer
  *  - Arm hold interlock (must hold arm for ARMING_HOLD_MS)
+ *  - Remote e-stop over 4G MQTT (CDC 'E'/'F'/'Z' commands)
  */
 
+typedef enum {
+    ESTOP_CLEAR            = 0,
+    ESTOP_TILT             = 1,
+    ESTOP_RC_KILL          = 2,
+    ESTOP_REMOTE           = 3,
+    ESTOP_CELLULAR_TIMEOUT = 4,
+} EstopSource;
+
 /*
  * safety_init() — call once in main() after HAL_Init().
  * Starts IWDG with WATCHDOG_TIMEOUT_MS timeout from config.h.
@@ -48,4 +57,9 @@ void safety_arm_start(uint32_t now);  /* Call when arm requested */
 bool safety_arm_ready(uint32_t now);  /* Poll until true, then arm */
 void safety_arm_cancel(void);         /* Cancel pending arm */
 
+void        safety_remote_estop(EstopSource src);
+void        safety_remote_estop_clear(void);
+EstopSource safety_get_estop(void);
+bool        safety_remote_estop_active(void);
+
 #endif /* SAFETY_H */

include/status.h

@@ -13,7 +13,8 @@ void status_boot_beep(void);
  *  LED1 solid + LED2 off  → disarmed, IMU OK
  *  LED1 solid + LED2 solid → armed
  *  Both slow blink         → tilt fault
+ *  Both fast blink (200ms) -- remote e-stop active (highest priority)
  *  LED1 slow blink + LED2 solid → IMU error (solid LED2 = always-on indicator)
  */
-void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault);
+void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault, int remote_estop);
 #endif

jetson/docker-compose.yml

@@ -209,10 +209,8 @@ services:
 
 
   # -- Remote e-stop bridge (MQTT over 4G -> STM32 CDC) ----------------------
-  # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E
+  # Subscribes to saltybot/estop MQTT topic. {"kill":true} -> 'E\r\n' to STM32.
-' to STM32.
+  # Cellular watchdog: 5s MQTT drop in AUTO mode -> 'F\r\n' (ESTOP_CELLULAR_TIMEOUT).
-  # Cellular watchdog: 5s MQTT drop in AUTO mode -> 'F
-' (ESTOP_CELLULAR_TIMEOUT).
   remote-estop:
     image: saltybot/ros2-humble:jetson-orin
     build:

jetson/ros2_ws/src/saltybot_bridge/config/estop_params.yaml

@@ -0,0 +1,12 @@
+remote_estop_node:
+  ros__parameters:
+    serial_port:               /dev/stm32-bridge
+    baud_rate:                 921600
+    mqtt_host:                 "mqtt.example.com"
+    mqtt_port:                 1883
+    mqtt_user:                 "saltybot"
+    mqtt_password:             ""
+    mqtt_topic:                "saltybot/estop"
+    cellular_timeout_s:        5.0
+    auto_estop_on_disconnect:  true
+    mqtt_keepalive:            10

jetson/ros2_ws/src/saltybot_bridge/launch/remote_estop.launch.py

@@ -0,0 +1,18 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    pkg = get_package_share_directory('saltybot_bridge')
+    params_file = os.path.join(pkg, 'config', 'estop_params.yaml')
+    return LaunchDescription([
+        Node(
+            package='saltybot_bridge',
+            executable='remote_estop_node',
+            name='remote_estop_node',
+            parameters=[params_file],
+            output='screen',
+        ),
+    ])

jetson/ros2_ws/src/saltybot_bridge/package.xml

@@ -12,6 +12,7 @@
   <license>MIT</license>
 
   <depend>rclpy</depend>
+  <depend>python3-paho-mqtt</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>diagnostic_msgs</depend>

jetson/ros2_ws/src/saltybot_bridge/saltybot_bridge/remote_estop_node.py

@@ -0,0 +1,167 @@
+"""
+remote_estop_node.py -- Remote e-stop bridge: MQTT -> STM32 USB CDC
+
+{"kill": true}  -> writes 'E\n' to STM32 (ESTOP_REMOTE, immediate motor cutoff)
+{"kill": false} -> writes 'Z\n' to STM32 (clear latch, robot can re-arm)
+
+Cellular watchdog: if MQTT link drops for > cellular_timeout_s while in
+AUTO mode, automatically sends 'F\n' (ESTOP_CELLULAR_TIMEOUT).
+
+Publishes /saltybot/estop_active (std_msgs/Bool).
+"""
+
+import json
+import time
+import threading
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Bool, String
+
+import paho.mqtt.client as mqtt
+import serial
+
+
+class RemoteEstopNode(Node):
+    def __init__(self):
+        super().__init__('remote_estop_node')
+
+        self.declare_parameter('serial_port', '/dev/stm32-bridge')
+        self.declare_parameter('baud_rate', 921600)
+        self.declare_parameter('mqtt_host', 'mqtt.example.com')
+        self.declare_parameter('mqtt_port', 1883)
+        self.declare_parameter('mqtt_user', 'saltybot')
+        self.declare_parameter('mqtt_password', '')
+        self.declare_parameter('mqtt_topic', 'saltybot/estop')
+        self.declare_parameter('cellular_timeout_s', 5.0)
+        self.declare_parameter('auto_estop_on_disconnect', True)
+        self.declare_parameter('mqtt_keepalive', 10)
+
+        self._serial_port = self.get_parameter('serial_port').value
+        self._baud_rate = self.get_parameter('baud_rate').value
+        self._mqtt_host = self.get_parameter('mqtt_host').value
+        self._mqtt_port = self.get_parameter('mqtt_port').value
+        self._mqtt_user = self.get_parameter('mqtt_user').value
+        self._mqtt_password = self.get_parameter('mqtt_password').value
+        self._mqtt_topic = self.get_parameter('mqtt_topic').value
+        self._cellular_timeout_s = self.get_parameter('cellular_timeout_s').value
+        self._auto_estop_on_disconnect = self.get_parameter('auto_estop_on_disconnect').value
+        self._mqtt_keepalive = self.get_parameter('mqtt_keepalive').value
+
+        self._ser = serial.Serial(self._serial_port, self._baud_rate, timeout=0.1)
+        self._estop_pub = self.create_publisher(Bool, '/saltybot/estop_active', 10)
+        self._mode_sub = self.create_subscription(
+            String, '/saltybot/balance_state', self._balance_state_cb, 10)
+        self._auto_mode = False
+        self._estop_active = False
+        self._last_mqtt_ok = time.monotonic()
+        self._watchdog_fired = False
+        self._lock = threading.Lock()
+
+        self._mqtt = mqtt.Client(client_id='saltybot-estop')
+        self._mqtt.username_pw_set(self._mqtt_user, self._mqtt_password)
+        self._mqtt.on_connect = self._on_mqtt_connect
+        self._mqtt.on_disconnect = self._on_mqtt_disconnect
+        self._mqtt.on_message = self._on_mqtt_message
+        self._mqtt.will_set(self._mqtt_topic + '/status', 'OFFLINE', qos=1, retain=True)
+        try:
+            self._mqtt.connect_async(self._mqtt_host, self._mqtt_port, self._mqtt_keepalive)
+            self._mqtt.loop_start()
+        except Exception as e:
+            self.get_logger().error(f'MQTT connect failed: {e}')
+
+        self.create_timer(1.0, self._watchdog_cb)
+        self.get_logger().info(
+            f'remote_estop_node started -- MQTT {self._mqtt_host}:{self._mqtt_port}')
+
+    def _on_mqtt_connect(self, client, userdata, flags, rc):
+        if rc == 0:
+            client.subscribe(self._mqtt_topic, qos=1)
+            with self._lock:
+                self._last_mqtt_ok = time.monotonic()
+                self._watchdog_fired = False
+        else:
+            self.get_logger().warning(f'MQTT connect refused rc={rc}')
+
+    def _on_mqtt_disconnect(self, client, userdata, rc):
+        self.get_logger().warning(f'MQTT disconnected rc={rc}')
+
+    def _on_mqtt_message(self, client, userdata, msg):
+        with self._lock:
+            self._last_mqtt_ok = time.monotonic()
+            self._watchdog_fired = False
+        try:
+            payload = json.loads(msg.payload.decode('utf-8'))
+            kill = payload.get('kill', False)
+        except (json.JSONDecodeError, UnicodeDecodeError) as e:
+            self.get_logger().error(f'Bad MQTT payload: {e}')
+            return
+        if kill:
+            self._send_estop('E')
+            self.get_logger().warning('MQTT kill=true -> ESTOP_REMOTE')
+        else:
+            self._send_clear()
+            self.get_logger().info('MQTT kill=false -> estop clear')
+
+    def _balance_state_cb(self, msg: String):
+        self._auto_mode = '"md":2' in msg.data
+
+    def _watchdog_cb(self):
+        if not self._auto_estop_on_disconnect:
+            return
+        with self._lock:
+            elapsed = time.monotonic() - self._last_mqtt_ok
+            fired = self._watchdog_fired
+        if self._auto_mode and elapsed > self._cellular_timeout_s and not fired:
+            with self._lock:
+                self._watchdog_fired = True
+            self.get_logger().error(
+                f'Cellular link lost {elapsed:.1f}s in AUTO mode -> ESTOP_CELLULAR_TIMEOUT')
+            self._send_estop('F')
+
+    def _send_estop(self, cmd: str):
+        try:
+            self._ser.write(f'{cmd}\n'.encode())
+            self._ser.flush()
+        except serial.SerialException as e:
+            self.get_logger().error(f'Serial write error: {e}')
+        with self._lock:
+            self._estop_active = True
+        msg = Bool()
+        msg.data = True
+        self._estop_pub.publish(msg)
+
+    def _send_clear(self):
+        try:
+            self._ser.write(b'Z\n')
+            self._ser.flush()
+        except serial.SerialException as e:
+            self.get_logger().error(f'Serial write error: {e}')
+        with self._lock:
+            self._estop_active = False
+        msg = Bool()
+        msg.data = False
+        self._estop_pub.publish(msg)
+
+    def destroy_node(self):
+        self._mqtt.loop_stop()
+        self._mqtt.disconnect()
+        if self._ser.is_open:
+            self._ser.close()
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RemoteEstopNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_bridge/setup.py

@@ -12,10 +12,12 @@ setup(
         (f"share/{package_name}/launch", [
             "launch/bridge.launch.py",
             "launch/cmd_vel_bridge.launch.py",
+            "launch/remote_estop.launch.py",
         ]),
         (f"share/{package_name}/config", [
             "config/bridge_params.yaml",
             "config/cmd_vel_bridge_params.yaml",
+            "config/estop_params.yaml",
         ]),
     ],
     install_requires=["setuptools", "pyserial"],
@@ -33,6 +35,7 @@ setup(
             "saltybot_cmd_node  = saltybot_bridge.saltybot_cmd_node:main",
             # Nav2 cmd_vel bridge: velocity limits + ramp + deadman + mode gate
             "cmd_vel_bridge_node = saltybot_bridge.cmd_vel_bridge_node:main",
+            "remote_estop_node = saltybot_bridge.remote_estop_node:main",
         ],
     },
 )

jetson/ros2_ws/src/saltybot_bridge/test/test_remote_estop.py

@@ -0,0 +1,105 @@
+"""Unit tests for remote_estop_node."""
+
+import json
+import time
+import threading
+from unittest.mock import MagicMock, patch
+
+import pytest
+
+
+def make_node(auto_estop=True, timeout=0.2):
+    with patch("saltybot_bridge.remote_estop_node.serial.Serial"), \
+         patch("saltybot_bridge.remote_estop_node.mqtt.Client"), \
+         patch("saltybot_bridge.remote_estop_node.rclpy"), \
+         patch("saltybot_bridge.remote_estop_node.Node.__init__", return_value=None):
+
+        from saltybot_bridge.remote_estop_node import RemoteEstopNode
+        node = RemoteEstopNode.__new__(RemoteEstopNode)
+        node._ser = MagicMock()
+        node._ser.is_open = True
+        node._mqtt = MagicMock()
+        node._mqtt_host = "localhost"
+        node._mqtt_port = 1883
+        node._mqtt_user = "test"
+        node._mqtt_password = ""
+        node._mqtt_topic = "test/estop"
+        node._mqtt_keepalive = 10
+        node._cellular_timeout_s = timeout
+        node._auto_estop_on_disconnect = auto_estop
+        node._auto_mode = False
+        node._estop_active = False
+        node._last_mqtt_ok = time.monotonic()
+        node._watchdog_fired = False
+        node._lock = threading.Lock()
+        node._estop_pub = MagicMock()
+        node.get_logger = MagicMock(return_value=MagicMock(
+            info=MagicMock(), warning=MagicMock(), error=MagicMock()))
+        return node
+
+
+def mqtt_msg(payload_dict):
+    msg = MagicMock()
+    msg.payload = json.dumps(payload_dict).encode()
+    return msg
+
+
+def test_kill_true_sends_E():
+    node = make_node()
+    t0 = time.monotonic()
+    node._on_mqtt_message(None, None, mqtt_msg({"kill": True}))
+    assert (time.monotonic() - t0) < 0.5
+    node._ser.write.assert_called_once_with(b'E\n')
+
+
+def test_kill_false_sends_Z():
+    node = make_node()
+    node._on_mqtt_message(None, None, mqtt_msg({"kill": False}))
+    node._ser.write.assert_called_once_with(b'Z\n')
+
+
+def test_kill_true_publishes_active_true():
+    node = make_node()
+    node._on_mqtt_message(None, None, mqtt_msg({"kill": True}))
+    assert node._estop_pub.publish.call_args[0][0].data is True
+
+
+def test_bad_json_no_write():
+    node = make_node()
+    bad = MagicMock()
+    bad.payload = b"NOT JSON"
+    node._on_mqtt_message(None, None, bad)
+    node._ser.write.assert_not_called()
+
+
+def test_watchdog_fires_F_in_auto_after_timeout():
+    node = make_node(auto_estop=True, timeout=0.05)
+    node._auto_mode = True
+    node._last_mqtt_ok = time.monotonic() - 1.0
+    node._watchdog_cb()
+    node._ser.write.assert_called_once_with(b'F\n')
+
+
+def test_watchdog_no_fire_if_not_auto():
+    node = make_node(auto_estop=True, timeout=0.05)
+    node._auto_mode = False
+    node._last_mqtt_ok = time.monotonic() - 1.0
+    node._watchdog_cb()
+    node._ser.write.assert_not_called()
+
+
+def test_watchdog_only_fires_once():
+    node = make_node(auto_estop=True, timeout=0.05)
+    node._auto_mode = True
+    node._last_mqtt_ok = time.monotonic() - 1.0
+    node._watchdog_cb()
+    node._watchdog_cb()
+    assert node._ser.write.call_count == 1
+
+
+def test_watchdog_disabled_by_param():
+    node = make_node(auto_estop=False, timeout=0.05)
+    node._auto_mode = True
+    node._last_mqtt_ok = time.monotonic() - 1.0
+    node._watchdog_cb()
+    node._ser.write.assert_not_called()

lib/USB_CDC/include/usbd_cdc_if.h

@@ -14,4 +14,7 @@ void checkForBootloader(void);
 /* PID tuning command interface (written by USB IRQ, read by main loop) */
 extern volatile uint8_t cdc_cmd_ready;
 extern volatile char    cdc_cmd_buf[32];
+
+extern volatile uint8_t cdc_estop_request;
+extern volatile uint8_t cdc_estop_clear_request;
 #endif

lib/USB_CDC/src/usbd_cdc_if.c

@@ -9,6 +9,9 @@ volatile uint8_t  cdc_disarm_request = 0; /* set by D command */
 volatile uint8_t  cdc_recal_request = 0;  /* set by G command — gyro recalibration */
 volatile uint32_t cdc_rx_count = 0;       /* total CDC packets received from host */
 
+volatile uint8_t  cdc_estop_request = 0;
+volatile uint8_t  cdc_estop_clear_request = 0;
+
 /*
  * PID tuning command buffer.
  * CDC_Receive (USB IRQ) copies multi-char commands here.
@@ -143,6 +146,10 @@ static int8_t CDC_Receive(uint8_t *buf, uint32_t *len) {
         case 'G': cdc_recal_request = 1;          break; /* gyro recalibration */
         case 'R': request_bootloader();            break; /* never returns */
 
+        case 'E': cdc_estop_request = 1;       break;
+        case 'F': cdc_estop_request = 2;       break;
+        case 'Z': cdc_estop_clear_request = 1; break;
+
         /*
          * PID tuning: P<kp>  I<ki>  D<kd>  T<setpoint>  M<max_speed>  ?
          * Copy to cmd buffer; main loop parses float (avoids sscanf in IRQ).

src/main.c

@@ -25,6 +25,8 @@ extern volatile uint8_t  cdc_arm_request;    /* set by A command */
 extern volatile uint8_t  cdc_disarm_request; /* set by D command */
 extern volatile uint8_t  cdc_recal_request;  /* set by G command */
 extern volatile uint32_t cdc_rx_count;       /* total CDC packets received */
+extern volatile uint8_t  cdc_estop_request;
+extern volatile uint8_t  cdc_estop_clear_request;
 
 /*
  * Apply a PID tuning command string from the USB terminal.
@@ -178,6 +180,20 @@ int main(void) {
         /* Mode manager: update RC liveness, CH6 mode selection, blend ramp */
         mode_manager_update(&mode, now);
 
+        if (cdc_estop_request) {
+            EstopSource src = (cdc_estop_request == 1) ? ESTOP_REMOTE : ESTOP_CELLULAR_TIMEOUT;
+            cdc_estop_request = 0;
+            safety_remote_estop(src);
+            safety_arm_cancel();
+            balance_disarm(&bal);
+            motor_driver_estop(&motors);
+        }
+        if (cdc_estop_clear_request) {
+            cdc_estop_clear_request = 0;
+            if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED)
+                safety_remote_estop_clear();
+        }
+
         /* RC CH5 kill switch: disarm immediately if RC is alive and CH5 off.
          * Applies regardless of active mode (CH5 always has kill authority). */
         if (mode.rc_alive && !crsf_state.armed && bal.state == BALANCE_ARMED) {
@@ -202,7 +218,8 @@ int main(void) {
             bool rc_armed_now = safety_rc_alive(now) && crsf_state.armed;
             if (rc_armed_now && !s_rc_armed_prev) {
                 /* Rising edge: start arm hold (motors enable after ARMING_HOLD_MS) */
-                if (mpu6000_is_calibrated() &&
+                if (!safety_remote_estop_active() &&
+                        mpu6000_is_calibrated() &&
                         bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
                     safety_arm_start(now);
                 }
@@ -218,8 +235,8 @@ int main(void) {
         /* Handle arm/disarm requests from USB with arm-hold interlock */
         if (cdc_arm_request) {
             cdc_arm_request = 0;
-            /* Block arming until gyro calibration is complete */
+            if (!safety_remote_estop_active() &&
-            if (mpu6000_is_calibrated() &&
+                    mpu6000_is_calibrated() &&
                     bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
                 safety_arm_start(now);
             }
@@ -269,7 +286,8 @@ int main(void) {
         /* Latch estop on tilt fault or disarm */
         if (bal.state == BALANCE_TILT_FAULT) {
             motor_driver_estop(&motors);
-        } else if (bal.state == BALANCE_DISARMED && motors.estop) {
+        } else if (bal.state == BALANCE_DISARMED && motors.estop &&
+                   !safety_remote_estop_active()) {
             motor_driver_estop_clear(&motors);
         }
 
@@ -353,10 +371,14 @@ int main(void) {
                     p += n; rem -= n;
                 }
                 /* Jetson active flag, USB TX/RX packet counters */
-                n = snprintf(p, rem, ",\"ja\":%d,\"txc\":%u,\"rxc\":%u}\n",
+                int es; EstopSource _rs = safety_get_estop();
+                if (_rs >= ESTOP_REMOTE) es = (int)_rs;
+                else if (bal.state == BALANCE_TILT_FAULT) es = ESTOP_TILT;
+                else es = ESTOP_CLEAR;
+                n = snprintf(p, rem, ",\"ja\":%d,\"txc\":%u,\"rxc\":%u,\"es\":%d}\n",
                     jetson_cmd_is_active(now) ? 1 : 0,
                     (unsigned)tx_count,
-                    (unsigned)cdc_rx_count);
+                    (unsigned)cdc_rx_count, es);
                 len = (int)(p + n - buf);
             } else {
                 len = snprintf(buf, sizeof(buf), "{\"err\":%d}\n", imu_ret);
@@ -367,7 +389,8 @@ int main(void) {
 
         status_update(now, (imu_ret == 0),
                       (bal.state == BALANCE_ARMED),
-                      (bal.state == BALANCE_TILT_FAULT));
+                      (bal.state == BALANCE_TILT_FAULT),
+                      safety_remote_estop_active());
         HAL_Delay(1);
     }
 }

src/safety.c

@@ -33,6 +33,8 @@ static bool     s_arm_pending  = false;
 /* Tilt fault alert state — edge-detect to fire buzzer once */
 static bool s_was_faulted = false;
 
+static EstopSource s_estop_source = ESTOP_CLEAR;
+
 void safety_init(void) {
     hiwdg.Instance       = IWDG;
     hiwdg.Init.Prescaler = IWDG_PRESCALER;
@@ -76,3 +78,8 @@ bool safety_arm_ready(uint32_t now) {
 void safety_arm_cancel(void) {
     s_arm_pending = false;
 }
+
+void safety_remote_estop(EstopSource src) { s_estop_source = src; }
+void safety_remote_estop_clear(void) { s_estop_source = ESTOP_CLEAR; }
+EstopSource safety_get_estop(void) { return s_estop_source; }
+bool safety_remote_estop_active(void) { return s_estop_source >= ESTOP_REMOTE; }

src/status.c

@@ -42,11 +42,14 @@ void status_boot_beep(void) {
     HAL_GPIO_WritePin(BEEPER_PORT, BEEPER_PIN, GPIO_PIN_RESET);
 }
 
-void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault) {
+void status_update(uint32_t tick, int imu_ok, int armed, int tilt_fault, int remote_estop) {
-    /* Solid = good, blink = needs attention (1 Hz, 500ms half-period) */
+    GPIO_PinState fast_blink = ((tick / 200) % 2) ? GPIO_PIN_RESET : GPIO_PIN_SET;
     GPIO_PinState blink = ((tick / 500) % 2) ? GPIO_PIN_RESET  /* ON half */
                                               : GPIO_PIN_SET;   /* OFF half */
-    if (!imu_ok) {
+    if (remote_estop) {
+        HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, fast_blink);
+        HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, fast_blink);
+    } else if (!imu_ok) {
         /* IMU error: LED1 blinking (attention), LED2 solid ON */
         HAL_GPIO_WritePin(LED1_PORT, LED1_PIN, blink);
         HAL_GPIO_WritePin(LED2_PORT, LED2_PIN, GPIO_PIN_RESET);