From 7c78792105661dde317184504062ccc486360997 Mon Sep 17 00:00:00 2001
From: gabryelreyes <gabryel.reyes@newtec.de>
Date: Fri, 27 Oct 2023 15:22:39 +0200
Subject: [PATCH] Reporting Motor Speeds

---
 lib/ConvoyLeader/App.cpp             | 22 +++++++++++++++++-----
 lib/ConvoyLeader/App.h               | 20 +++++++++++++++-----
 lib/ConvoyLeader/SerialMuxChannels.h | 15 ++++++++++++++-
 3 files changed, 46 insertions(+), 11 deletions(-)

diff --git a/lib/ConvoyLeader/App.cpp b/lib/ConvoyLeader/App.cpp
index 69d2aa2f..fa0be505 100644
--- a/lib/ConvoyLeader/App.cpp
+++ b/lib/ConvoyLeader/App.cpp
@@ -72,11 +72,12 @@ void App::setup()
     m_systemStateMachine.setState(&StartupState::getInstance());
     m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD);
     m_serialMuxProtChannelIdOdometry = m_smpServer.createChannel(ODOMETRY_CHANNEL_NAME, ODOMETRY_CHANNEL_DLC);
+    m_serialMuxProtChannelIdSpeed    = m_smpServer.createChannel(SPEED_CHANNEL_NAME, SPEED_CHANNEL_DLC);
 
     /* Channel sucesfully created? */
     if (0U != m_serialMuxProtChannelIdOdometry)
     {
-        m_reportOdometryTimer.start(REPORT_ODOMETRY_PERIOD);
+        m_reportTimer.start(REPORTING_PERIOD);
     }
 }
 
@@ -102,12 +103,13 @@ void App::loop()
         m_controlInterval.restart();
     }
 
-    if (true == m_reportOdometryTimer.isTimeout())
+    if (true == m_reportTimer.isTimeout())
     {
-        /* Send Odometry to SerialMuxProt Client */
+        /* Send current data to SerialMuxProt Client */
         reportOdometry();
+        reportSpeed();
 
-        m_reportOdometryTimer.restart();
+        m_reportTimer.restart();
     }
 
     m_systemStateMachine.process();
@@ -133,7 +135,17 @@ void App::reportOdometry()
     payload.yPos        = yPos;
     payload.orientation = odometry.getOrientation();
 
-    m_smpServer.sendData(ODOMETRY_CHANNEL_NAME, reinterpret_cast<uint8_t*>(&payload), sizeof(payload));
+    m_smpServer.sendData(m_serialMuxProtChannelIdOdometry, reinterpret_cast<uint8_t*>(&payload), sizeof(payload));
+}
+
+void App::reportSpeed()
+{
+    Speedometer& speedometer = Speedometer::getInstance();
+    SpeedData    payload;
+    payload.left  = speedometer.getLinearSpeedLeft();
+    payload.right = speedometer.getLinearSpeedRight();
+
+    m_smpServer.sendData(m_serialMuxProtChannelIdSpeed, reinterpret_cast<uint8_t*>(&payload), sizeof(payload));
 }
 
 /******************************************************************************
diff --git a/lib/ConvoyLeader/App.h b/lib/ConvoyLeader/App.h
index c4d42502..a090358d 100644
--- a/lib/ConvoyLeader/App.h
+++ b/lib/ConvoyLeader/App.h
@@ -65,9 +65,10 @@ class App
      */
     App() :
         m_serialMuxProtChannelIdOdometry(0U),
+        m_serialMuxProtChannelIdSpeed(0U),
         m_systemStateMachine(),
         m_controlInterval(),
-        m_reportOdometryTimer(),
+        m_reportTimer(),
         m_smpServer(Serial)
     {
     }
@@ -93,8 +94,8 @@ class App
     /** Differential drive control period in ms. */
     static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U;
 
-    /** Odometry reporting period in ms. */
-    static const uint32_t REPORT_ODOMETRY_PERIOD = 100U;
+    /** Current data reporting period in ms. */
+    static const uint32_t REPORTING_PERIOD = 100U;
 
     /** Baudrate for Serial Communication */
     static const uint32_t SERIAL_BAUDRATE = 115200U;
@@ -102,14 +103,17 @@ class App
     /** SerialMuxProt Channel id for sending the current odometry. */
     uint8_t m_serialMuxProtChannelIdOdometry;
 
+    /** SerialMuxProt Channel id for sending the current speed. */
+    uint8_t m_serialMuxProtChannelIdSpeed;
+
     /** The system state machine. */
     StateMachine m_systemStateMachine;
 
     /** Timer used for differential drive control processing. */
     SimpleTimer m_controlInterval;
 
-    /** Timer for reporting odometry through SerialMuxProt. */
-    SimpleTimer m_reportOdometryTimer;
+    /** Timer for reporting current data through SerialMuxProt. */
+    SimpleTimer m_reportTimer;
 
     /**
      * SerialMuxProt Server Instance
@@ -125,6 +129,12 @@ class App
      */
     void reportOdometry();
 
+    /**
+     * Report the current motor speeds of the robot using the Speedometer data.
+     * Sends data through the SerialMuxProtServer.
+     */
+    void reportSpeed();
+
 private:
     /* An instance shall not be copied or assigned. */
     App(const App& app);
diff --git a/lib/ConvoyLeader/SerialMuxChannels.h b/lib/ConvoyLeader/SerialMuxChannels.h
index 765acf56..43563838 100644
--- a/lib/ConvoyLeader/SerialMuxChannels.h
+++ b/lib/ConvoyLeader/SerialMuxChannels.h
@@ -47,7 +47,13 @@ SOFTWARE.
 #define ODOMETRY_CHANNEL_NAME "ODOMETRY"
 
 /** DLC of Odometry Channel */
-#define ODOMETRY_CHANNEL_DLC (12U)
+#define ODOMETRY_CHANNEL_DLC (sizeof(OdometryData))
+
+/** Name of Channel to send Speedometer Data to. */
+#define SPEED_CHANNEL_NAME "SPEED"
+
+/** DLC of Speedometer Channel */
+#define SPEED_CHANNEL_DLC (sizeof(SpeedData))
 
 /******************************************************************************
  * Types and Classes
@@ -61,6 +67,13 @@ typedef struct _OdometryData
     int32_t orientation;
 } __attribute__((packed)) OdometryData;
 
+/** Struct of the "Speed" channel payload. */
+typedef struct _SpeedData
+{
+    int16_t left;
+    int16_t right;
+} __attribute__((packed)) SpeedData;
+
 /******************************************************************************
  * Functions
  *****************************************************************************/