Add signal conditioning for remote control

CMakeLists.txt

@@ -11,6 +11,7 @@ set(VERBOSE True)
 ofa_declare_dependency(NAME OST23 CATEGORY modules/wheels/steered-wheel VERSION main)
 # ofa_declare_dependency(NAME linux_gpio CATEGORY modules/interfaces/gpio VERSION main)
 ofa_declare_dependency(NAME sbus CATEGORY modules/interfaces/remote-control VERSION main)
+ofa_declare_dependency(NAME signalconditioner CATEGORY modules/interfaces/remote-control VERSION main)
 
 ofa_FetchContent(EEROS core/eeros http://github.com/eeros-project/eeros-framework v1.4.2)
 
@@ -20,10 +21,11 @@ ofa_FetchContent(canopenlib core/canopen
 ofa_ModuleToTarget(wheels "modules/wheels/steered-wheel" "OST23")
 # ofa_ModuleToTarget(gpio "modules/interfaces/gpio" "linux_gpio")
 ofa_ModuleToTarget(remote_control "modules/interfaces/remote-control" "sbus")
+ofa_ModuleToTarget(sig_cond "modules/interfaces/remote-control" "signalconditioner")
 
 add_executable(${PROJECT_NAME}_rover src/main.cpp)
 target_include_directories(${PROJECT_NAME}_rover PRIVATE ${PROJECT_SOURCE_DIR}/include)
-target_link_libraries(${PROJECT_NAME}_rover PRIVATE EEROS::eeros ${wheels} ${remote_control} canopenlib::canopen)
+target_link_libraries(${PROJECT_NAME}_rover PRIVATE EEROS::eeros ${wheels} ${remote_control} ${sig_cond} canopenlib::canopen)
 
 include(OfaJSON)
 ofa_RegisterConfigDependency(CHILD_MODULE ${wheels} N 2)

config/config_template.json

 {
+    "rover" : {
+              "maxLinVel": 0.8,
+              "maxLinAcc": 0.5,
+              "maxLinDec": 2.0,
+              "maxAngVel": 0.5,
+              "maxAngAcc": 0.5,
+              "maxAngDec": 1.0
+    },
 
     "tfList": [{
             "name":"robotRef",

include/ControlSystem.hpp

@@ -5,6 +5,7 @@
 #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 <ofa/modules/interfaces/remote-control/signal-conditioner/control/TwistVector.hpp>
 #include <config/Constants.hpp>
 #include <config/RoverConfig.hpp>
 #include <eeros/control/Constant.hpp>
@@ -17,6 +18,7 @@ namespace examplerover {
 
 namespace wheels = ofa::modules::wheels::steered_wheel::ost23;
 namespace rc = ofa::modules::interfaces::remote_control::sbus;
+namespace sc = ofa::modules::interfaces::remote_control::signal_conditioner;
 
 class ControlSystem {
  public:
@@ -26,24 +28,42 @@ class ControlSystem {
         sbusSerial{config.rc},
         rc{config.rc},
         tracker{config::PERIOD, 20, 1, 0.005},
+        twistRC(genConfig),
         td("wheels", config::PERIOD, true) {
 
+    // pass general configuration settings for rover to remote control signal conditioner
+    genConfig.maxLinVel = config.getMaxLinVelocity();
+    genConfig.maxLinAcc = config.getMaxLinAcceleration();
+    genConfig.maxLinDec = config.getMaxLinDeceleration();
+    genConfig.maxAngVel = config.getMaxAngVelocity();
+    genConfig.maxAngAcc = config.getMaxAngAcceleration();
+    genConfig.maxAngDec = config.getMaxAngDeceleration();
+
     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});
+    tracker.setName("rc activity tracker");
+    twistConst.setName("constant twist");
+    twistRC.setName("rc twist");
+
+    twistConst.setValue({0.1, 0, 0, 0, 0, 0});
+
+    tracker.getIn().connect(rc.getOut());
+    twistRC.getIn().connect(rc.getOut());
+    wheelsSend.getIn().connect(twistConst.getOut());
+//     wheelsSend.getIn().connect(twistRC.getOut());
+    wheelsSend.getInAngle().connect(wheelsReceive.getOutAngle());
+    wheelsSend.getInVelocity().connect(wheelsReceive.getOutVelocity());
+
     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());
-    wheelsSend.getInVelocity().connect(wheelsReceive.getOutVelocity());
+    td.addBlock(twistConst);
+    td.addBlock(twistRC);
+
     eeros::Executor::instance().add(td);
   }
 
@@ -52,7 +72,9 @@ class ControlSystem {
   rc::SBusSerial sbusSerial;
   rc::Rc_frsky_TaranisX7 rc;
   rc::RcActivityTracker tracker;
-  Constant<eeros::math::Matrix<6,1>> twist;
+  Constant<eeros::math::Matrix<6,1>> twistConst;
+  sc::TwistVector twistRC;
+  sc::GeneralConfig genConfig;
   TimeDomain td;
 };
 

include/config/RoverConfig.hpp

@@ -27,11 +27,44 @@ class RoverConfig {
     eeros::tf::TF_Tree::instance().initJSON(json);
     for (auto& wheel : json["driveModules"]) wheels.emplace_back(wheel);
     rc = json["remoteControl"];
+    auto general = json["rover"];
+    maxLinVel = general["maxLinVel"].number_value();
+    maxLinAcc = general["maxLinAcc"].number_value();
+    maxLinDec = general["maxLinDec"].number_value();
+    maxAngVel = general["maxAngVel"].number_value();
+    maxAngAcc = general["maxAngAcc"].number_value();
+    maxAngDec = general["maxAngDec"].number_value();
     return true;
   }
 
+  void printValues() {
+    eeros::logger::Logger log = eeros::logger::Logger::getLogger('J');
+    log.info() << "rover: ";
+    log.info() << "  maxLinVel: " << maxLinVel;
+    log.info() << "  maxLinAcc: " << maxLinAcc;
+    log.info() << "  maxLinDec: " << maxLinDec;
+    log.info() << "  maxAngVel: " << maxAngVel;
+    log.info() << "  maxAngAcc: " << maxAngAcc;
+    log.info() << "  maxAngDec: " << maxAngDec;
+  }
+
+  double getMaxLinVelocity() const { return maxLinVel; }
+  double getMaxLinAcceleration() const { return maxLinAcc; }
+  double getMaxLinDeceleration() const { return maxLinDec; }
+  double getMaxAngVelocity() const { return maxAngVel; }
+  double getMaxAngAcceleration() const { return maxAngAcc; }
+  double getMaxAngDeceleration() const { return maxAngDec; }
+
   wheel::Config::WheelConfig wheels;
   rc::Config rc;
+
+ private:
+   double maxLinVel;
+   double maxLinAcc;
+   double maxLinDec;
+   double maxAngVel;
+   double maxAngAcc;
+   double maxAngDec;
 };
 
 }  // namespace config

src/main.cpp

@@ -32,6 +32,10 @@ int main(int argc, char* argv[]) {
 
   auto config = config::RoverConfig{};
   config.readJSON(argv[1]);
+  config.rc.printValues();
+  for (auto& w : config.wheels) w.printValues();
+  config.printValues();
+
   CANopen canBus{std::make_shared<CAN::CANSocket>(config::CAN_INTERFACE)};
   debug::CanDebugger canErrors(config::CAN_INTERFACE, log);
 
@@ -48,7 +52,7 @@ int main(int argc, char* argv[]) {
 
   eeros::task::Lambda l{[&]() {
     log.info() << "safety level: " << ss.getCurrentLevel()
-               << ", set twist: " << cs.twist.getValue() << ", local twist: "
+               << ", set twist: " << cs.twistConst.getValue() << ", local twist: "
                << cs.wheelsReceive.getOut().getSignal().getValue();
     log.info() << "velocities: "
                << cs.wheelsReceive.getOutVelocity().getSignal().getValue()