From fc285f889efb0e1bdf116d6fcd21c658cb84d7b2 Mon Sep 17 00:00:00 2001
From: gabryelreyes <gabryel.reyes@newtec.de>
Date: Tue, 16 Jan 2024 15:03:18 +0100
Subject: [PATCH] Added current and initial vehicle data channels

---
 lib/RemoteControl/src/App.cpp | 42 +++++++++++++++++++++++++++++++++++
 lib/RemoteControl/src/App.h   | 16 +++++++++----
 2 files changed, 54 insertions(+), 4 deletions(-)

diff --git a/lib/RemoteControl/src/App.cpp b/lib/RemoteControl/src/App.cpp
index 0d6f07e7..08876cf6 100644
--- a/lib/RemoteControl/src/App.cpp
+++ b/lib/RemoteControl/src/App.cpp
@@ -62,6 +62,7 @@
 
 static void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
 static void App_lineSensorChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
+static void App_currentVehicleChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
 
 /******************************************************************************
  * Local Variables
@@ -161,6 +162,9 @@ void App::setup()
                 m_smpServer.createChannel(SPEED_SETPOINT_CHANNEL_NAME, SPEED_SETPOINT_CHANNEL_DLC);
             m_smpServer.subscribeToChannel(COMMAND_RESPONSE_CHANNEL_NAME, App_cmdRspChannelCallback);
             m_smpServer.subscribeToChannel(LINE_SENSOR_CHANNEL_NAME, App_lineSensorChannelCallback);
+            m_smpServer.subscribeToChannel(CURRENT_VEHICLE_DATA_CHANNEL_NAME, App_currentVehicleChannelCallback);
+            m_serialMuxProtChannelInitialVehicleData =
+                m_smpServer.createChannel(INITIAL_VEHICLE_DATA_CHANNEL_NAME, INITIAL_VEHICLE_DATA_CHANNEL_DLC);
 
             if (false == m_mqttClient.init())
             {
@@ -223,6 +227,18 @@ void App::loop()
 
     /* Process SerialMuxProt. */
     m_smpServer.process(millis());
+
+    if (false == m_initialDataSent)
+    {
+        VehicleData initialVehicleData = {.xPos = 0, .yPos = 0, .orientation = 0, .left = 0, .right = 0, .center = 0};
+
+        if (true == m_smpServer.sendData(m_serialMuxProtChannelInitialVehicleData, &initialVehicleData,
+                                         sizeof(initialVehicleData)))
+        {
+            LOG_DEBUG("Initial vehicle data sent.");
+            m_initialDataSent = true;
+        }
+    }
 }
 
 /******************************************************************************
@@ -363,3 +379,29 @@ void App_lineSensorChannelCallback(const uint8_t* payload, const uint8_t payload
     UTIL_NOT_USED(payloadSize);
     UTIL_NOT_USED(userData);
 }
+
+/**
+ * Receives current position and heading of the robot over SerialMuxProt channel.
+ *
+ * @param[in] payload       Current vehicle data. Two coordinates, one orientation and two motor speeds.
+ * @param[in] payloadSize   Size of two coordinates, one orientation and two motor speeds.
+ * @param[in] userData      Instance of App class.
+ */
+void App_currentVehicleChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData)
+{
+    UTIL_NOT_USED(userData);
+
+    if ((nullptr != payload) && (CURRENT_VEHICLE_DATA_CHANNEL_DLC == payloadSize))
+    {
+        const VehicleData* currentVehicleData = reinterpret_cast<const VehicleData*>(payload);
+
+        LOG_DEBUG("X: %d Y: %d Heading: %d Left: %d Right: %d Center: %d", currentVehicleData->xPos,
+                  currentVehicleData->yPos, currentVehicleData->orientation, currentVehicleData->left,
+                  currentVehicleData->right, currentVehicleData->center);
+    }
+    else
+    {
+        LOG_WARNING("%s: Invalid payload size. Expected: %u Received: %u", CURRENT_VEHICLE_DATA_CHANNEL_NAME,
+                    CURRENT_VEHICLE_DATA_CHANNEL_DLC, payloadSize);
+    }
+}
\ No newline at end of file
diff --git a/lib/RemoteControl/src/App.h b/lib/RemoteControl/src/App.h
index e9f7c198..01834550 100644
--- a/lib/RemoteControl/src/App.h
+++ b/lib/RemoteControl/src/App.h
@@ -65,10 +65,12 @@ class App
      * Construct the Remote Control application.
      */
     App() :
-        m_smpServer(Board::getInstance().getDevice().getStream()),
+        m_smpServer(Board::getInstance().getDevice().getStream(), this),
         m_serialMuxProtChannelIdRemoteCtrl(0U),
         m_serialMuxProtChannelIdMotorSpeeds(0U),
-        m_mqttClient()
+        m_serialMuxProtChannelInitialVehicleData(0U),
+        m_mqttClient(),
+        m_initialDataSent(false)
     {
     }
 
@@ -102,12 +104,15 @@ class App
     /** MQTT topic name for receiving motor speeds. */
     static const char* TOPIC_NAME_MOTOR_SPEEDS;
 
-    /** SerialMuxProt Channel id sending remote control commands. */
+    /** SerialMuxProt Channel id for sending remote control commands. */
     uint8_t m_serialMuxProtChannelIdRemoteCtrl;
 
-    /** SerialMuxProt Channel id sending sending motor speeds. */
+    /** SerialMuxProt Channel id for sending motor speeds. */
     uint8_t m_serialMuxProtChannelIdMotorSpeeds;
 
+    /** SerialMuxProt Channel id for sending initial position data. */
+    uint8_t m_serialMuxProtChannelInitialVehicleData;
+
     /**
      * SerialMuxProt Server Instance
      *
@@ -120,6 +125,9 @@ class App
      */
     MqttClient m_mqttClient;
 
+    /** Flag for setting initial data through SMP. */
+    bool m_initialDataSent;
+
 private:
     /**
      * Handler of fatal errors in the Application.