Dynamically register input drivers

Author: rovo89

robots/include/worx_robot.hpp

@@ -2,9 +2,14 @@
 #define WORX_ROBOT_HPP
 
 #include <drivers/charger/bq_2576/bq_2576.hpp>
+#include <drivers/input/gpio_input_driver.hpp>
+#include <drivers/input/worx_input_driver.hpp>
+#include <services.hpp>
 
 #include "robot.hpp"
 
+using namespace xbot::driver::input;
+
 class WorxRobot : public MowerRobot {
  public:
   void InitPlatform() override;
@@ -29,6 +34,8 @@ class WorxRobot : public MowerRobot {
 
  private:
   BQ2576 charger_{};
+  GpioInputDriver gpio_driver_{input_service};
+  WorxInputDriver worx_driver_{input_service};
 };
 
 #endif  // WORX_ROBOT_HPP

robots/src/worx_robot.cpp

@@ -7,6 +7,8 @@ void WorxRobot::InitPlatform() {
   InitMotors();
   charger_.setI2C(&I2CD1);
   power_service.SetDriver(&charger_);
+  input_service.RegisterInputDriver("gpio", &gpio_driver_);
+  input_service.RegisterInputDriver("worx", &worx_driver_);
 }
 
 bool WorxRobot::IsHardwareSupported() {

src/services/input_service/input_service.cpp

@@ -4,6 +4,7 @@
 
 #include <xbot-service/Lock.hpp>
 
+#include "../../drivers/input/gpio_input_driver.hpp"
 #include "../../globals.hpp"
 #include "../../json_stream.hpp"
 
@@ -121,7 +122,8 @@ void InputService::OnStop() {
 void InputService::OnLoop(uint32_t, uint32_t) {
   eventmask_t events = chEvtGetAndClearEvents(Events::ids_to_mask({Events::GPIO_TRIGGERED}));
   if (events & EVENT_MASK(Events::GPIO_TRIGGERED)) {
-    gpio_driver_.tick();
+    auto* gpio_driver = static_cast<GpioInputDriver*>(drivers_.find("gpio")->second);
+    gpio_driver->tick();
   }
 }
 

src/services/input_service/input_service.hpp

@@ -7,8 +7,7 @@
 
 #include <InputServiceBase.hpp>
 
-#include "drivers/input/gpio_input_driver.hpp"
-#include "drivers/input/worx_input_driver.hpp"
+#include "../../drivers/input/input_driver.hpp"
 
 using namespace xbot::driver::input;
 using namespace xbot::service;
@@ -23,6 +22,10 @@ class InputService : public InputServiceBase {
 
   mutex_t mutex_;
 
+  void RegisterInputDriver(const char* id, InputDriver* driver) {
+    drivers_.emplace(id, driver);
+  }
+
   const etl::ivector<Input*>& GetAllInputs() const {
     return all_inputs_;
   }
@@ -34,13 +37,7 @@ class InputService : public InputServiceBase {
   void OnInputChanged(Input& input);
 
  private:
-  GpioInputDriver gpio_driver_{*this};
-  WorxInputDriver worx_driver_{*this};
-
-  const etl::flat_map<etl::string<4>, InputDriver*, 2> drivers_ = {
-      {"gpio", &gpio_driver_},
-      {"worx", &worx_driver_},
-  };
+  etl::flat_map<etl::string<4>, InputDriver*, 2> drivers_;
 
   // Must not have more than 64 inputs due to the size of various bitmasks.
   etl::vector<Input*, 30> all_inputs_;