Added speed reading

chalmersfsd · Jul 4, 2019 · 614bb66 · 614bb66
1 parent 5e59003
commit 614bb66
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 2 deletions.
diff --git a/docker-compose.yml b/docker-compose.yml
@@ -2,6 +2,6 @@ version: '2'
 
 services:
     state-machine:
-        image: chalmersfsd/cfsd-logic-lynx-state-machine:test-delay
+        image: chalmersfsd/cfsd-logic-lynx-state-machine:tag
         network_mode: "host"
         command: "state-machine --cid=130 --freq=30"
diff --git a/src/logic-state-machine.hpp b/src/logic-state-machine.hpp
@@ -218,6 +218,9 @@ class StateMachine {
     // Input from CAN proxy
     const uint16_t m_senderStampAsMission = 1906;
 
+    // Input from SLAM
+    const uint16_t m_senderStampGroundSpeed = 3000;
+
     // Depends on pin value in opendlv-device-stm32-lynx
     const uint16_t m_gpioStampEbsOk = 49;
     const uint16_t m_gpioStampAsms = 115;

diff --git a/src/state-machine.cpp b/src/state-machine.cpp
@@ -44,7 +44,19 @@ int32_t main(int32_t argc, char **argv)
 
         StateMachine stateMachine(od4, VERBOSE);
 
-        // StateMachine stateMachine(od4, VERBOSE);
+        auto onGroundSpeedReading{[&stateMachine, VERBOSE_DATA](cluon::data::Envelope &&envelope) 
+        {
+            uint16_t senderStamp = envelope.senderStamp();
+            if (senderStamp == stateMachine.m_senderStampGroundSpeed) {
+              auto gsr = cluon::extractMessage<opendlv::proxy::GroundSpeedReading>(std::move(envelope));
+              stateMachine.setVehicleSpeed(gsr.groundSpeed());
+
+              if (VERBOSE_DATA) {
+                std::cout << "GroundSpeedReading: " << gsr.groundSpeed() << std::endl;
+              }
+            }
+          }};
+        od4.dataTrigger(opendlv::proxy::GroundSpeedReading::ID(), onGroundSpeedReading);
 
         auto onPressureReading{[&stateMachine, &VERBOSE, VERBOSE_DATA](cluon::data::Envelope &&envelope)
         {