Sending top motor speeds periodically

Author: gabryelreyes

lib/ConvoyLeader/src/App.cpp

@@ -175,6 +175,7 @@ void App::setup()
             /* Initialize timers. */
             m_sendWaypointTimer.start(SEND_WAYPOINT_TIMER_INTERVAL);
             m_commandTimer.start(SEND_COMMANDS_TIMER_INTERVAL);
+            m_motorSpeedTimer.start(SEND_MOTOR_SPEED_TIMER_INTERVAL);
 
             /* Start with startup state. */
             m_systemStateMachine.setState(&StartupState::getInstance());
@@ -356,15 +357,31 @@ void App::processPeriodicTasks()
         m_commandTimer.restart();
     }
 
-    if (true == m_sendWaypointTimer.isTimeout())
+    if ((true == m_sendWaypointTimer.isTimeout()) && (true == m_mqttClient.isConnected()))
     {
         if (false == m_v2vClient.sendWaypoint(m_latestVehicleData))
         {
             LOG_WARNING("Waypoint could not be sent.");
         }
-        else
+
+        m_sendWaypointTimer.restart();
+    }
+
+    if ((true == m_motorSpeedTimer.isTimeout()) && (true == m_smpServer.isSynced()))
+    {
+        int16_t centerSpeed = 0;
+
+        if (true == DrivingState::getInstance().getTopMotorSpeed(centerSpeed))
         {
-            m_sendWaypointTimer.restart();
+            SpeedData payload;
+            payload.center = centerSpeed;
+
+            if (false == m_smpServer.sendData(m_serialMuxProtChannelIdMotorSpeeds, &payload, sizeof(SpeedData)))
+            {
+                LOG_WARNING("Failed to send motor speeds to RU.");
+            }
+
+            m_motorSpeedTimer.restart();
         }
     }
 }

lib/ConvoyLeader/src/App.h

@@ -72,7 +72,11 @@ class App
         m_smpServer(Board::getInstance().getDevice().getStream(), this),
         m_mqttClient(),
         m_v2vClient(m_mqttClient),
-        m_sendWaypointTimer()
+        m_systemStateMachine(),
+        m_latestVehicleData(),
+        m_sendWaypointTimer(),
+        m_commandTimer(),
+        m_motorSpeedTimer()
     {
     }
 
@@ -115,6 +119,9 @@ class App
     /** Send commands timer interval in ms. */
     static const uint32_t SEND_COMMANDS_TIMER_INTERVAL = 100U;
 
+    /** Send motor speed timer interval in ms. */
+    static const uint32_t SEND_MOTOR_SPEED_TIMER_INTERVAL = 100U;
+
     /** MQTT topic name for birth messages. */
     static const char* TOPIC_NAME_BIRTH;
 
@@ -147,7 +154,9 @@ class App
      */
     V2VClient m_v2vClient;
 
-    /** The system state machine. */
+    /**
+     * The system state machine.
+     */
     StateMachine m_systemStateMachine;
 
     /**
@@ -165,6 +174,11 @@ class App
      */
     SimpleTimer m_commandTimer;
 
+    /**
+     * Timer for sending motor speed to RU.
+     */
+    SimpleTimer m_motorSpeedTimer;
+
 private:
     /**
      * Handler of fatal errors in the Application.