Add remote control

include/ControlSystem.hpp

@@ -2,6 +2,9 @@
 
 #include <ofa/modules/wheels/steered-wheel/OST23/control/Receive.hpp>
 #include <ofa/modules/wheels/steered-wheel/OST23/control/Send.hpp>
+#include <ofa/modules/interfaces/remote-control/sbus/control/SBusSerial.hpp>
+#include <ofa/modules/interfaces/remote-control/sbus/control/Rc_frsky_TaranisX7.hpp>
+#include <ofa/modules/interfaces/remote-control/sbus/control/RcActivityTracker.hpp>
 #include <config/Constants.hpp>
 #include <config/RoverConfig.hpp>
 #include <eeros/control/Constant.hpp>
@@ -13,20 +16,30 @@ namespace rover {
 namespace examplerover {
 
 namespace wheels = ofa::modules::wheels::steered_wheel::ost23;
+namespace rc = ofa::modules::interfaces::remote_control::sbus;
 
 class ControlSystem {
  public:
   ControlSystem(CANopen canBus, config::RoverConfig& config)
       : wheelsReceive(canBus, config.wheels),
         wheelsSend{canBus, config.wheels, config::PERIOD},
+        sbusSerial{config.rc},
+        rc{config.rc},
+        tracker{config::PERIOD, 20, 1, 0.005},
         td("wheels", config::PERIOD, true) {
 
     wheelsReceive.setName("wheel receive");
     wheelsSend.setName("wheel send");
+    sbusSerial.setName("sbus serial receiver");
+    rc.setName("remote control");
+    tracker.setName("activity tracker");
     twist.setName("twist");
     twist.setValue({0.1, 0, 0, 0, 0, 0});
     td.addBlock(wheelsSend);
     td.addBlock(wheelsReceive);
+    td.addBlock(sbusSerial);
+    td.addBlock(rc);
+    td.addBlock(tracker);
     td.addBlock(twist);
     wheelsSend.getIn().connect(twist.getOut());
     wheelsSend.getInAngle().connect(wheelsReceive.getOutAngle());
@@ -36,6 +49,9 @@ class ControlSystem {
 
   wheels::Receive<config::NUMBER_OF_WHEELS> wheelsReceive;
   wheels::Send<config::NUMBER_OF_WHEELS> wheelsSend;
+  rc::SBusSerial sbusSerial;
+  rc::Rc_frsky_TaranisX7 rc;
+  rc::RcActivityTracker tracker;
   Constant<eeros::math::Matrix<6,1>> twist;
   TimeDomain td;
 };

include/SafetyProperties.hpp

@@ -36,18 +36,15 @@ class RoverSafetyProperties : public safe::SafetyProperties {
     criticalOutputs = {};
     setEntryLevel(boot);
 
-    off.setLevelAction([&](SafetyContext* ctx) {
-      eeros::Executor::instance().stop();
-    });
+    off.setLevelAction(
+        [&](SafetyContext* ctx) { eeros::Executor::instance().stop(); });
 
     boot.setLevelAction([&](SafetyContext* ctx) {
       cs.td.start();
       ctx->triggerEvent(startInit);
     });
 
-    exitFunction = [this](SafetyContext* ctx) {
-      ctx->triggerEvent(ctrl_C);
-    };
+    exitFunction = [this](SafetyContext* ctx) { ctx->triggerEvent(ctrl_C); };
   }
 
   using Level = safe::SafetyLevel;

include/Sequencer.hpp

 #pragma once
 
-#include <eeros/sequencer/Sequencer.hpp>
 #include <eeros/sequencer/Sequence.hpp>
+#include <eeros/sequencer/Sequencer.hpp>
 #include <eeros/sequencer/Step.hpp>
 #include <eeros/sequencer/Wait.hpp>
-
 #include <ofa/modules/wheels/steered-wheel/OST23/sequence/Configure.hpp>
-#include <ofa/modules/wheels/steered-wheel/OST23/sequence/Enable.hpp>
 #include <ofa/modules/wheels/steered-wheel/OST23/sequence/Disable.hpp>
+#include <ofa/modules/wheels/steered-wheel/OST23/sequence/Enable.hpp>
+// #include
+// <ofa/modules/interfaces/remote-control/sbus/control/RcActivityTracker.hpp>
 
 #include <ControlSystem.hpp>
 #include <SafetyProperties.hpp>
@@ -24,44 +25,49 @@ namespace wheels = ofa::modules::wheels::steered_wheel::ost23;
 
 class MainSequence : public seq::Sequence {
  public:
-  MainSequence(std::string name, seq::Sequencer& seq, ControlSystem& cs, safe::SafetySystem& ss,
-               RoverSafetyProperties& sp, config::RoverConfig& config, CANopen canBus)
-      : Sequence(name, seq), cs(cs), ss(ss), sp(sp),
+  MainSequence(std::string name, seq::Sequencer& seq, ControlSystem& cs,
+               safe::SafetySystem& ss, RoverSafetyProperties& sp,
+               config::RoverConfig& config, CANopen canBus)
+      : Sequence(name, seq),
+        cs(cs),
+        ss(ss),
+        sp(sp),
         initWheels("wheel init", this, config.wheels, canBus),
         enableWheels("wheel enable", this, config.wheels, canBus),
         disableWheels("wheel disable", this, config.wheels, canBus),
-        wait("wait", this) { }
+        wait("wait", this) {}
   int action() {
-    while(seq::Sequencer::running) {
+    while (seq::Sequencer::running) {
+      wait(3);
       try {
-      if (ss.getCurrentLevel() == sp.stopping) {
-        cs.td.disableBlocks();
-        disableWheels();
-        ss.triggerEvent(sp.shutDown);
-       } else if (ss.getCurrentLevel() == sp.initializing) {
+        if (ss.getCurrentLevel() == sp.stopping) {
+          cs.td.disableBlocks();
+          disableWheels();
+          ss.triggerEvent(sp.shutDown);
+        } else if (ss.getCurrentLevel() == sp.initializing) {
           int res = initWheels();
           log.info() << "initWheels returned " << res;
           if (res == 0)
             ss.triggerEvent(sp.doIdle);
           else
             raise(SIGINT);
-        } else if(ss.getCurrentLevel() == sp.idle) {
-        wait(2);
-        ss.triggerEvent(sp.startEnable);
-      } else if(ss.getCurrentLevel() == sp.disabling) {
-        cs.td.disableBlocks();
-        disableWheels();
-        ss.triggerEvent(sp.doIdle);
-      } else if(ss.getCurrentLevel() == sp.enabling) {
-        enableWheels();
-        cs.wheelsSend.setCtrl(0xf);
-        cs.td.enableBlocks();
-        ss.triggerEvent(sp.startRunning);
-      } else if(ss.getCurrentLevel() == sp.running) {
-//         if (rc->stop())
-//           ss.triggerEvent(sp.stopRunning);
-      }
-      wait(0.5);
+        } else if (ss.getCurrentLevel() == sp.idle) {
+          if (cs.tracker.getOut().getSignal().getValue())
+            ss.triggerEvent(sp.startEnable);
+        } else if (ss.getCurrentLevel() == sp.disabling) {
+          cs.td.disableBlocks();
+          disableWheels();
+          ss.triggerEvent(sp.doIdle);
+        } else if (ss.getCurrentLevel() == sp.enabling) {
+          enableWheels();
+          cs.wheelsSend.setCtrl(0xf);
+          cs.td.enableBlocks();
+          ss.triggerEvent(sp.startRunning);
+        } else if (ss.getCurrentLevel() == sp.running) {
+          if (!cs.tracker.getOut().getSignal().getValue())
+            ss.triggerEvent(sp.stopRunning);
+        }
+        wait(0.5);
       } catch (eeros::Fault) {
         ss.triggerEvent(sp.CAN_fault);
       }

include/config/RoverConfig.hpp

-
 #pragma once
 
 #include <ucl++.h>
 
-#include <ofa/modules/wheels/steered-wheel/OST23/config/Config_OST23.hpp>
+#include <ofa/modules/wheels/steered-wheel/OST23/config/Config.hpp>
+#include <ofa/modules/interfaces/remote-control/sbus/config/Config.hpp>
 
 namespace ofa {
 namespace rover {
 namespace examplerover {
 namespace config {
+
 namespace wheel = ofa::modules::wheels::steered_wheel::ost23;
+namespace rc = ofa::modules::interfaces::remote_control::sbus;
+
+/**
+ * Reads the global configuration file for this rover.
+ * The file must include all the necessary properties for this rover.
+ */
 class RoverConfig {
  public:
   bool readJSON(std::string path) {
     std::string error;
-    auto json = ucl::Ucl::parse_from_file_strategy(path,
-                                                   UCL_DUPLICATE_ERROR, error);
+    auto json =
+        ucl::Ucl::parse_from_file_strategy(path, UCL_DUPLICATE_ERROR, error);
     if (!json) return false;
     eeros::tf::TF_Tree::instance().initJSON(json);
     for (auto& wheel : json["driveModules"]) wheels.emplace_back(wheel);
+    rc = json["remoteControl"];
     return true;
   }
 
   wheel::Config::WheelConfig wheels;
+  rc::Config rc;
 };
+
 }  // namespace config
 }  // namespace examplerover
 }  // namespace rover