Implement emergency safety handling

Author: jkaflik

README.md

@@ -14,9 +14,9 @@ More details about this firmware can be found in [documentation](https://jkaflik
 - [x] Charging
 - [x] LED status
 - [x] IMU
-- [ ] Emergency mode
-  - [ ] Emergency publisher
-  - [ ] Emergency restart service
+- [x] Emergency mode
+  - [x] Emergency publishers
+  - [x] Latched emergency command topic
 - [ ] Cover panel support
 
 ## Usage
@@ -31,8 +31,35 @@ The firmware uses onboard NeoPixel LED(s) to display the current system status.
 - **Magenta**: Battery discharging
 - **Red**: Battery low
 - **Blue**: IMU sensor failure
+- **Blinking red**: Emergency latch active
 
 When multiple statuses are active, each status will be shown for approximately 800ms with a 200ms black separator between them. The sequence will continue to cycle through all active statuses.
+Emergency has priority over the normal status sequence and is shown as a blinking red LED.
+
+### Emergency ROS API
+
+Emergency state is published at 10 Hz:
+
+| Topic | Type | Description |
+| --- | --- | --- |
+| `emergency/active` | `std_msgs/msg/Bool` | Emergency latch is active. |
+| `emergency/stop_active` | `std_msgs/msg/Bool` | Stop input is active after debounce. |
+| `emergency/lift_active` | `std_msgs/msg/Bool` | Lift emergency is active. |
+| `emergency/tilt_active` | `std_msgs/msg/Bool` | Tilt emergency is active. |
+| `emergency/software_requested` | `std_msgs/msg/Bool` | Emergency was requested by ROS. |
+| `emergency/release_blocked` | `std_msgs/msg/Bool` | Release is blocked by an active physical input. |
+| `emergency/lifted_wheels` | `std_msgs/msg/UInt8` | Number of active lift inputs. |
+
+Commands are accepted on `emergency/command` as `std_msgs/msg/Bool`:
+
+| Value | Meaning |
+| --- | --- |
+| `true` | Request/latch emergency. |
+| `false` | Request emergency release. |
+
+The same command value must be received three times within one second before it is accepted.
+Release requests are ignored while any physical stop/lift/tilt input is active.
+The emergency latch starts active after boot and must be released explicitly.
 
 ## Build
 

src/emergency.cpp

@@ -0,0 +1,190 @@
+#include "emergency.h"
+
+#include <pico/mutex.h>
+
+#include "pins.h"
+
+namespace emergency
+{
+
+namespace
+{
+constexpr unsigned long STOP_DEBOUNCE_MS = 20;
+constexpr unsigned long LIFT_PERIOD_MS = 100;
+constexpr unsigned long TILT_PERIOD_MS = 2500;
+constexpr unsigned long COMMAND_WINDOW_MS = 1000;
+constexpr uint8_t COMMAND_CONFIRMATIONS_REQUIRED = 3;
+
+auto_init_mutex(emergency_mutex);
+
+bool latch_active = true;
+bool stop_active = false;
+bool lift_active = false;
+bool tilt_active = false;
+bool software_requested = false;
+bool release_blocked = false;
+uint8_t lifted_wheels = 0;
+
+bool raw_stop_active = false;
+bool raw_lift_input_active = false;
+
+unsigned long stop_started_ms = 0;
+unsigned long lift_started_ms = 0;
+unsigned long tilt_started_ms = 0;
+
+bool command_pending = false;
+bool pending_command = false;
+uint8_t pending_command_count = 0;
+unsigned long pending_command_started_ms = 0;
+
+bool isActiveLowInput(uint8_t pin)
+{
+  return digitalRead(pin) == LOW;
+}
+
+bool elapsedSinceStart(bool condition,
+                       unsigned long& started_ms,
+                       unsigned long threshold_ms,
+                       unsigned long now)
+{
+  if (!condition)
+  {
+    started_ms = 0;
+    return false;
+  }
+
+  if (started_ms == 0)
+  {
+    started_ms = now;
+    return false;
+  }
+
+  return now - started_ms >= threshold_ms;
+}
+
+bool physicalInputPresent()
+{
+  return raw_stop_active || raw_lift_input_active;
+}
+
+void updateUnlocked()
+{
+  const unsigned long now = millis();
+
+  lifted_wheels = 0;
+  if (isActiveLowInput(PIN_EMERGENCY_1))
+  {
+    lifted_wheels++;
+  }
+  if (isActiveLowInput(PIN_EMERGENCY_2))
+  {
+    lifted_wheels++;
+  }
+
+  raw_lift_input_active = lifted_wheels > 0;
+  raw_stop_active = isActiveLowInput(PIN_EMERGENCY_3) || isActiveLowInput(PIN_EMERGENCY_4);
+
+  stop_active = elapsedSinceStart(raw_stop_active, stop_started_ms, STOP_DEBOUNCE_MS, now);
+  lift_active = elapsedSinceStart(lifted_wheels >= 2, lift_started_ms, LIFT_PERIOD_MS, now);
+  tilt_active = elapsedSinceStart(raw_lift_input_active, tilt_started_ms, TILT_PERIOD_MS, now);
+
+  if (stop_active || lift_active || tilt_active)
+  {
+    latch_active = true;
+  }
+
+  release_blocked = latch_active && physicalInputPresent();
+}
+
+void requestLatch()
+{
+  software_requested = true;
+  latch_active = true;
+}
+
+void requestRelease()
+{
+  updateUnlocked();
+
+  if (physicalInputPresent())
+  {
+    release_blocked = true;
+    return;
+  }
+
+  latch_active = false;
+  software_requested = false;
+  release_blocked = false;
+}
+
+}  // namespace
+
+void init()
+{
+  pinMode(PIN_EMERGENCY_1, INPUT);
+  pinMode(PIN_EMERGENCY_2, INPUT);
+  pinMode(PIN_EMERGENCY_3, INPUT);
+  pinMode(PIN_EMERGENCY_4, INPUT);
+}
+
+void update()
+{
+  mutex_enter_blocking(&emergency_mutex);
+  updateUnlocked();
+  mutex_exit(&emergency_mutex);
+}
+
+void handleCommand(bool latch_requested)
+{
+  mutex_enter_blocking(&emergency_mutex);
+
+  const unsigned long now = millis();
+
+  if (!command_pending || pending_command != latch_requested
+      || now - pending_command_started_ms > COMMAND_WINDOW_MS)
+  {
+    command_pending = true;
+    pending_command = latch_requested;
+    pending_command_count = 1;
+    pending_command_started_ms = now;
+    mutex_exit(&emergency_mutex);
+    return;
+  }
+
+  pending_command_count++;
+  if (pending_command_count < COMMAND_CONFIRMATIONS_REQUIRED)
+  {
+    mutex_exit(&emergency_mutex);
+    return;
+  }
+
+  command_pending = false;
+  pending_command_count = 0;
+
+  if (latch_requested)
+  {
+    requestLatch();
+  }
+  else
+  {
+    requestRelease();
+  }
+
+  mutex_exit(&emergency_mutex);
+}
+
+State getState()
+{
+  mutex_enter_blocking(&emergency_mutex);
+  State state = {latch_active,
+                 stop_active,
+                 lift_active,
+                 tilt_active,
+                 software_requested,
+                 release_blocked,
+                 lifted_wheels};
+  mutex_exit(&emergency_mutex);
+  return state;
+}
+
+}  // namespace emergency

src/emergency.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <Arduino.h>
+
+namespace emergency
+{
+
+struct State
+{
+  bool active;
+  bool stop_active;
+  bool lift_active;
+  bool tilt_active;
+  bool software_requested;
+  bool release_blocked;
+  uint8_t lifted_wheels;
+};
+
+void init();
+void update();
+void handleCommand(bool latch_requested);
+State getState();
+
+}  // namespace emergency

src/led_status.hpp

@@ -13,6 +13,7 @@ enum LedStatusFlag
   LED_STATUS_DISCHARGING = (1 << 2),
   LED_STATUS_BATTERY_LOW = (1 << 3),
   LED_STATUS_IMU_FAILED = (1 << 4),
+  LED_STATUS_EMERGENCY = (1 << 5),
 };
 
 struct StatusColor
@@ -130,6 +131,12 @@ class LedStatus
 
     std::vector<LedStatusFlag> activeStatuses = getActiveStatuses();
 
+    if (getFlag(LED_STATUS_EMERGENCY))
+    {
+      setColor(255, 0, 0, true);
+      return;
+    }
+
     // with no active statuses, turn off the LED
     if (activeStatuses.empty())
     {
@@ -178,6 +185,7 @@ class LedStatus
     statusColorMap.push_back({LED_STATUS_DISCHARGING, {255, 0, 255}});  // Magenta for discharging
     statusColorMap.push_back({LED_STATUS_BATTERY_LOW, {255, 0, 0}});    // Red for battery low
     statusColorMap.push_back({LED_STATUS_IMU_FAILED, {0, 0, 255}});     // Blue for IMU failed
+    statusColorMap.push_back({LED_STATUS_EMERGENCY, {255, 0, 0}});      // Red blink for emergency
   }
 
   std::vector<LedStatusFlag> getActiveStatuses() const