Move ownership of inputs to the service

Author: rovo89

src/drivers/input/gpio_input_driver.cpp

@@ -11,11 +11,10 @@ namespace xbot::driver::input {
 
 bool GpioInputDriver::OnInputConfigValue(lwjson_stream_parser_t* jsp, const char* key, lwjson_stream_type_t type,
                                          Input& input) {
-  auto& gpio_input = static_cast<GpioInput&>(input);
   if (strcmp(key, "line") == 0) {
     JsonExpectType(STRING);
-    gpio_input.line = GetIoLineByName(jsp->data.str.buff);
-    if (gpio_input.line == PAL_NOLINE) {
+    input.gpio.line = GetIoLineByName(jsp->data.str.buff);
+    if (input.gpio.line == PAL_NOLINE) {
       ULOG_ERROR("Unknown GPIO line \"%s\"", jsp->data.str.buff);
       return false;
     }
@@ -41,23 +40,23 @@ static void LineCallback(void*) {
 }
 
 bool GpioInputDriver::OnStart() {
-  for (const auto& input : inputs_) {
-    palSetLineMode(input.line, PAL_MODE_INPUT);
-    palSetLineCallback(input.line, LineCallback, nullptr);
-    palEnableLineEvent(input.line, PAL_EVENT_MODE_BOTH_EDGES);
+  for (const auto& input : Inputs()) {
+    palSetLineMode(input.gpio.line, PAL_MODE_INPUT);
+    palSetLineCallback(input.gpio.line, LineCallback, nullptr);
+    palEnableLineEvent(input.gpio.line, PAL_EVENT_MODE_BOTH_EDGES);
   }
   return true;
 }
 
 void GpioInputDriver::OnStop() {
-  for (const auto& input : inputs_) {
-    palDisableLineEvent(input.line);
+  for (const auto& input : Inputs()) {
+    palDisableLineEvent(input.gpio.line);
   }
 }
 
 void GpioInputDriver::tick() {
-  for (auto& input : inputs_) {
-    input.Update(palReadLine(input.line) == PAL_HIGH);
+  for (auto& input : Inputs()) {
+    input.Update(palReadLine(input.gpio.line) == PAL_HIGH);
   }
 }
 

src/drivers/input/gpio_input_driver.hpp

@@ -12,25 +12,13 @@ class GpioInputDriver : public InputDriver {
   using InputDriver::InputDriver;
 
  public:
-  Input& AddInput() override {
-    return inputs_.emplace_back();
-  }
-  void ClearInputs() override {
-    inputs_.clear();
-  }
   bool OnInputConfigValue(lwjson_stream_parser_t* jsp, const char* key, lwjson_stream_type_t type,
                           Input& input) override;
   bool OnStart() override;
   void OnStop() override;
   void tick();
 
  private:
-  struct GpioInput : public Input {
-    ioline_t line = PAL_NOLINE;
-  };
-
-  etl::vector<GpioInput, 4> inputs_;
-
   ServiceSchedule tick_schedule_{input_service, 1'000'000,
                                  XBOT_FUNCTION_FOR_METHOD(GpioInputDriver, &GpioInputDriver::tick, this)};
 };

src/drivers/input/input_driver.cpp

@@ -22,4 +22,20 @@ bool Input::Update(bool new_active) {
   return false;
 }
 
+void InputDriver::AddInput(Input* input) {
+  if (inputs_head_ == nullptr) {
+    inputs_head_ = input;
+  } else {
+    Input* current = inputs_head_;
+    while (current->next_for_driver_ != nullptr) {
+      current = current->next_for_driver_;
+    }
+    current->next_for_driver_ = input;
+  }
+}
+
+void InputDriver::ClearInputs() {
+  inputs_head_ = nullptr;
+}
+
 }  // namespace xbot::driver::input

src/drivers/input/input_driver.hpp

@@ -3,6 +3,7 @@
 
 #include <etl/atomic.h>
 #include <etl/string.h>
+#include <hal.h>
 #include <lwjson/lwjson.h>
 
 #include <xbot-service/portable/system.hpp>
@@ -17,6 +18,16 @@ struct Input {
   uint16_t emergency_reason = 0;
   uint16_t emergency_delay_ms = 0;
 
+  union {
+    struct {
+      ioline_t line;
+    } gpio;
+
+    struct {
+      uint8_t bit;
+    } worx;
+  };
+
   // State
   bool IsActive() const {
     return active;
@@ -31,20 +42,59 @@ struct Input {
  private:
   etl::atomic<bool> active = false;
   uint32_t active_since = 0;
+  Input* next_for_driver_ = nullptr;
+
+  friend class InputDriver;
+  friend struct InputIterable;
+};
+
+struct InputIterable {
+  Input* head;
+
+  struct Iterator {
+    Input* ptr;
+
+    Input& operator*() const {
+      return *ptr;
+    }
+
+    Iterator& operator++() {
+      ptr = ptr->next_for_driver_;
+      return *this;
+    }
+
+    bool operator!=(const Iterator& other) const {
+      return ptr != other.ptr;
+    }
+  };
+
+  Iterator begin() const {
+    return Iterator{head};
+  }
+
+  Iterator end() const {
+    return Iterator{nullptr};
+  }
 };
 
 class InputDriver {
  public:
   virtual ~InputDriver() = default;
   explicit InputDriver() = default;
-  virtual Input& AddInput() = 0;
-  virtual void ClearInputs() = 0;
+  void AddInput(Input* input);
+  void ClearInputs();
   virtual bool OnInputConfigValue(lwjson_stream_parser_t* jsp, const char* key, lwjson_stream_type_t type,
                                   Input& input) = 0;
   virtual bool OnStart() {
     return true;
   };
   virtual void OnStop(){};
+
+ protected:
+  Input* inputs_head_ = nullptr;
+  InputIterable Inputs() {
+    return InputIterable{inputs_head_};
+  }
 };
 }  // namespace xbot::driver::input
 

src/drivers/input/simulated_input_driver.cpp

@@ -18,7 +18,7 @@ bool SimulatedInputDriver::OnInputConfigValue(lwjson_stream_parser_t* jsp, const
 }
 
 void SimulatedInputDriver::SetActiveInputs(uint64_t active_inputs_mask) {
-  for (auto& input : inputs_) {
+  for (auto& input : Inputs()) {
     input.Update(IS_BIT_SET(active_inputs_mask, input.idx));
   }
 }

src/drivers/input/simulated_input_driver.hpp

@@ -10,18 +10,9 @@ class SimulatedInputDriver : public InputDriver {
   using InputDriver::InputDriver;
 
  public:
-  Input& AddInput() override {
-    return inputs_.emplace_back();
-  }
-  void ClearInputs() override {
-    inputs_.clear();
-  }
   bool OnInputConfigValue(lwjson_stream_parser_t* jsp, const char* key, lwjson_stream_type_t type,
                           Input& input) override;
   void SetActiveInputs(uint64_t active_inputs_mask);
-
- private:
-  etl::vector<Input, 64> inputs_;
 };
 }  // namespace xbot::driver::input
 

src/drivers/input/worx_input_driver.cpp

@@ -27,7 +27,6 @@ static const etl::flat_map<etl::string<13>, uint8_t, 8> INPUT_BITS = {
 
 bool WorxInputDriver::OnInputConfigValue(lwjson_stream_parser_t* jsp, const char* key, lwjson_stream_type_t type,
                                          Input& input) {
-  auto& worx_input = static_cast<WorxInput&>(input);
   if (strcmp(key, "id") == 0) {
     JsonExpectType(STRING);
     decltype(INPUT_BITS)::key_type input_id{jsp->data.str.buff};
@@ -36,7 +35,7 @@ bool WorxInputDriver::OnInputConfigValue(lwjson_stream_parser_t* jsp, const char
       ULOG_ERROR("Unknown Worx input ID \"%s\"", input_id.c_str());
       return false;
     }
-    worx_input.bit = bit_it->second;
+    input.worx.bit = bit_it->second;
     return true;
   }
   ULOG_ERROR("Unknown attribute \"%s\"", key);
@@ -52,8 +51,8 @@ bool WorxInputDriver::OnStart() {
 void WorxInputDriver::tick() {
   KeypadResponse keypad{};
   if (ReadKeypad(keypad)) {
-    for (auto& input : inputs_) {
-      input.Update(IS_BIT_SET(keypad.keys_and_halls, input.bit));
+    for (auto& input : Inputs()) {
+      input.Update(IS_BIT_SET(keypad.keys_and_halls, input.worx.bit));
     }
   }
 }

src/drivers/input/worx_input_driver.hpp

@@ -13,12 +13,6 @@ class WorxInputDriver : public InputDriver {
   using InputDriver::InputDriver;
 
  public:
-  Input& AddInput() override {
-    return inputs_.emplace_back();
-  }
-  void ClearInputs() override {
-    inputs_.clear();
-  }
   bool OnInputConfigValue(lwjson_stream_parser_t* jsp, const char* key, lwjson_stream_type_t type,
                           Input& input) override;
   bool OnStart() override;
@@ -33,14 +27,8 @@ class WorxInputDriver : public InputDriver {
   };
 #pragma pack(pop)
 
-  struct WorxInput : public Input {
-    uint8_t bit = 0;
-  };
-
   I2CDriver* i2c_driver_ = nullptr;
 
-  etl::vector<WorxInput, 16> inputs_;
-
   bool ReadKeypad(KeypadResponse& response) const;
 
   void tick();

src/services/input_service/input_service.cpp

@@ -68,11 +68,11 @@ bool InputService::InputConfigsJsonCallback(lwjson_stream_parser_t* jsp, lwjson_
           ULOG_ERROR("Too many inputs (max. %d)", all_inputs_.max_size());
           return false;
         }
-        data->current_input = &data->driver->AddInput();
+        data->current_input = &all_inputs_.emplace_back();
         data->current_input->idx = data->next_idx++;
+        data->driver->AddInput(data->current_input);
       } else {
         // TODO: Give driver a chance to check completeness of the input?
-        all_inputs_.push_back(data->current_input);
         data->current_input = nullptr;
       }
       break;
@@ -166,8 +166,8 @@ uint32_t InputService::OnLoop(uint32_t, uint32_t) {
 void InputService::SendStatus() {
   uint64_t active_inputs_mask = 0;
   for (auto& input : all_inputs_) {
-    if (input->IsActive()) {
-      active_inputs_mask |= 1 << input->idx;
+    if (input.IsActive()) {
+      active_inputs_mask |= 1 << input.idx;
     }
   }
 
@@ -214,9 +214,9 @@ etl::pair<uint16_t, uint32_t> InputService::GetEmergencyReasons(uint32_t now) {
   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)) {
-      reasons |= input->emergency_reason;
+    if (input.emergency_reason == 0 || !input.IsActive()) continue;
+    if (TimeoutReached(input.ActiveDuration(now), input.emergency_delay_ms * 1'000, block_time)) {
+      reasons |= input.emergency_reason;
     }
   }
 

src/services/input_service/input_service.hpp

@@ -41,7 +41,7 @@ class InputService : public InputServiceBase {
   etl::flat_map<etl::string<10>, InputDriver*, 3> drivers_;
 
   // Must not have more than 64 inputs due to the size of various bitmasks.
-  etl::vector<Input*, 30> all_inputs_;
+  etl::vector<Input, 30> all_inputs_;
 
   etl::atomic<uint8_t> num_active_lift_{0};
   Input lift_input_;