Trigger emergency even for briefly active inputs

This is especially relevant for the injected key presses, which predate
the activation and immediately deactivate the input. The emergency check
won't run between these two, so we need a flag for a pending emergency.

Author: rovo89

src/drivers/input/input_driver.cpp

@@ -14,14 +14,25 @@ bool Input::Update(bool new_active, uint32_t predate) {
     const uint32_t now = xbot::service::system::getTimeMicros() - predate;
     if (new_active) {
       active_since = now;
+      input_service.OnInputChanged(*this, true, 0);
+    } else {
+      const uint32_t duration = ActiveDuration(now);
+      if (emergency_reason != 0 && duration >= emergency_delay_ms * 1'000) {
+        emergency_pending = true;
+      }
+      input_service.OnInputChanged(*this, false, duration);
     }
-    input_service.OnInputChanged(*this, new_active, ActiveDuration(now));
     chEvtBroadcastFlags(&mower_events, MowerEvents::INPUTS_CHANGED);
     return true;
   }
   return false;
 }
 
+bool Input::GetAndClearPendingEmergency() {
+  bool only_if_pending = true;
+  return emergency_pending.compare_exchange_strong(only_if_pending, false);
+}
+
 void Input::InjectPress(bool long_press) {
   InjectPress(input_service.GetPressDelay(long_press));
 }

src/drivers/input/input_driver.hpp

@@ -34,6 +34,7 @@ struct Input {
   }
 
   bool Update(bool new_active, uint32_t predate = 0);
+  bool GetAndClearPendingEmergency();
 
   void InjectPress(bool long_press = false);
   void InjectPress(uint32_t duration);
@@ -43,9 +44,10 @@ struct Input {
   }
 
  private:
-  etl::atomic<bool> active = false;
-  uint32_t active_since = 0;
-  Input* next_for_driver_ = nullptr;
+  etl::atomic<bool> active{false};
+  etl::atomic<bool> emergency_pending{false};
+  uint32_t active_since{0};
+  Input* next_for_driver_{nullptr};
 
   friend class InputDriver;
   friend struct InputIterable;

src/services/input_service/input_service.cpp

@@ -223,10 +223,13 @@ etl::pair<uint16_t, uint32_t> InputService::GetEmergencyReasons(uint32_t now) {
   Lock lk(&mutex_);
   uint16_t reasons = 0;
   uint32_t block_time = UINT32_MAX;
-  for (const auto& input : all_inputs_) {
-    // TODO: What if the input was triggered so briefly that we couldn't observe it?
-    if (input.emergency_reason == 0 || !input.IsActive()) continue;
-    if (TimeoutReached(input.ActiveDuration(now), input.emergency_delay_ms * 1'000, block_time)) {
+  for (auto& input : all_inputs_) {
+    if (input.emergency_reason == 0) continue;
+    if (input.GetAndClearPendingEmergency()) {
+      reasons |= input.emergency_reason;
+      block_time = 0;
+    } else if (input.IsActive() &&
+               TimeoutReached(input.ActiveDuration(now), input.emergency_delay_ms * 1'000, block_time)) {
       reasons |= input.emergency_reason;
     }
   }