From 9bf5fb912295ab6c21a431e5f01983d553289b72 Mon Sep 17 00:00:00 2001
From: gabryelreyes <gabryel.reyes@newtec.de>
Date: Fri, 27 Oct 2023 16:23:12 +0200
Subject: [PATCH] Added motor speed setpoints

---
 lib/ConvoyLeader/App.cpp             | 22 +++++++++++++++++++++-
 lib/ConvoyLeader/SerialMuxChannels.h |  6 ++++++
 2 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/lib/ConvoyLeader/App.cpp b/lib/ConvoyLeader/App.cpp
index 39371538..f0c51971 100644
--- a/lib/ConvoyLeader/App.cpp
+++ b/lib/ConvoyLeader/App.cpp
@@ -57,6 +57,8 @@
  * Prototypes
  *****************************************************************************/
 
+static void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize);
+
 /******************************************************************************
  * Local Variables
  *****************************************************************************/
@@ -71,8 +73,11 @@ void App::setup()
     Board::getInstance().init();
     m_systemStateMachine.setState(&StartupState::getInstance());
     m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD);
+
+    /* Setup SerialMuxProt Channels. */
     m_serialMuxProtChannelIdOdometry = m_smpServer.createChannel(ODOMETRY_CHANNEL_NAME, ODOMETRY_CHANNEL_DLC);
     m_serialMuxProtChannelIdSpeed    = m_smpServer.createChannel(SPEED_CHANNEL_NAME, SPEED_CHANNEL_DLC);
+    m_smpServer.subscribeToChannel(SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedSetpointsChannelCallback);
 
     /* Channel sucesfully created? */
     if ((0U != m_serialMuxProtChannelIdOdometry) && (0U != m_serialMuxProtChannelIdSpeed))
@@ -154,4 +159,19 @@ void App::reportSpeed()
 
 /******************************************************************************
  * Local Functions
- *****************************************************************************/
\ No newline at end of file
+ *****************************************************************************/
+
+/**
+ * Receives motor speed setpoints over SerialMuxProt channel.
+ *
+ * @param[in] payload       Motor speed left/right
+ * @param[in] payloadSize   Size of twice motor speeds
+ */
+void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize)
+{
+    if ((nullptr != payload) && (SPEED_SETPOINT_CHANNEL_DLC == payloadSize))
+    {
+        const SpeedData* motorSpeedData = reinterpret_cast<const SpeedData*>(payload);
+        DifferentialDrive::getInstance().setLinearSpeed(motorSpeedData->left, motorSpeedData->right);
+    }
+}
\ No newline at end of file
diff --git a/lib/ConvoyLeader/SerialMuxChannels.h b/lib/ConvoyLeader/SerialMuxChannels.h
index 43563838..9ade8921 100644
--- a/lib/ConvoyLeader/SerialMuxChannels.h
+++ b/lib/ConvoyLeader/SerialMuxChannels.h
@@ -55,6 +55,12 @@ SOFTWARE.
 /** DLC of Speedometer Channel */
 #define SPEED_CHANNEL_DLC (sizeof(SpeedData))
 
+/** Name of Channel to send Motor Speed Setpoints to. */
+#define SPEED_SETPOINT_CHANNEL_NAME "SPEED_SET"
+
+/** DLC of Speedometer Channel */
+#define SPEED_SETPOINT_CHANNEL_DLC (sizeof(SpeedData))
+
 /******************************************************************************
  * Types and Classes
  *****************************************************************************/