Added Startup State

Author: gabryelreyes

lib/ConvoyLeader/src/App.cpp

@@ -226,39 +226,20 @@ void App::loop()
     m_systemStateMachine.process();
 
     /* Process periodic tasks. */
-    if (true == m_initialCommandTimer.isTimeout())
+    if ((true == m_initialCommandTimer.isTimeout()) && (true == m_smpServer.isSynced()))
     {
-        if (false == m_receivedMaxMotorSpeed)
-        {
-            Command payload;
-            payload.commandId = RemoteControl::CMD_ID_GET_MAX_SPEED;
-
-            (void)m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &payload, sizeof(payload));
-        }
+        Command* pendingCommand = StartupState::getInstance().getPendingCommand();
 
-        if (false == m_initialPositionSent)
+        if (nullptr != pendingCommand)
         {
-            SettingsHandler& settings = SettingsHandler::getInstance();
-            Command          initialVehicleDataCmd;
-            initialVehicleDataCmd.commandId   = RemoteControl::CMD_ID_SET_INIT_POS;
-            initialVehicleDataCmd.xPos        = settings.getInitialXPosition();
-            initialVehicleDataCmd.yPos        = settings.getInitialYPosition();
-            initialVehicleDataCmd.orientation = settings.getInitialHeading();
-
-            (void)m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &initialVehicleDataCmd,
-                                       sizeof(initialVehicleDataCmd));
-        }
+            if (false == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, pendingCommand, sizeof(Command)))
+            {
+                LOG_WARNING("Failed to send pending command to RU.");
+            }
 
-        if ((false == m_receivedMaxMotorSpeed) || (false == m_initialPositionSent))
-        {
             /* Something is pending. */
             m_initialCommandTimer.restart();
         }
-        else
-        {
-            /* Nothing is pending anymore. */
-            m_initialCommandTimer.stop();
-        }
     }
 
     if (true == m_sendWaypointTimer.isTimeout())
@@ -440,11 +421,11 @@ void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize
         {
             LOG_DEBUG("Max Speed: %d", cmdRsp->maxMotorSpeed);
             application->setMaxMotorSpeed(cmdRsp->maxMotorSpeed);
-            application->notifyMaxMotorSpeedIsReceived();
+            StartupState::getInstance().notifyCommandProcessed();
         }
         else if (RemoteControl::CMD_ID_SET_INIT_POS == cmdRsp->commandId)
         {
-            application->notifyInitialPositionIsSet();
+            StartupState::getInstance().notifyCommandProcessed();
         }
     }
     else

lib/ConvoyLeader/src/App.h

@@ -73,7 +73,6 @@ class App
         m_smpServer(Board::getInstance().getDevice().getStream(), this),
         m_mqttClient(),
         m_v2vClient(m_mqttClient),
-        m_initialPositionSent(false),
         m_longitudinalController(),
         m_sendWaypointTimer()
     {
@@ -132,22 +131,6 @@ class App
      */
     void setLastFollowerFeedback(const Waypoint& feedback);
 
-    /**
-     * Notify initial position is set.
-     */
-    void notifyInitialPositionIsSet()
-    {
-        m_initialPositionSent = true;
-    }
-
-    /**
-     * Notify max motor speed is received.
-     */
-    void notifyMaxMotorSpeedIsReceived()
-    {
-        m_receivedMaxMotorSpeed = true;
-    }
-
 private:
     /** Minimum battery level in percent. */
     static const uint8_t MIN_BATTERY_LEVEL = 10U;
@@ -190,16 +173,6 @@ class App
     /** The system state machine. */
     StateMachine m_systemStateMachine;
 
-    /**
-     * Flag for setting initial position through SMP.
-     */
-    bool m_initialPositionSent;
-
-    /**
-     * Flag for received the max motor speed through SMP.
-     */
-    bool m_receivedMaxMotorSpeed;
-
     /**
      * Longitudinal controller.
      */

lib/ConvoyLeader/src/StartupState.cpp

@@ -34,6 +34,8 @@
  *****************************************************************************/
 
 #include "StartupState.h"
+#include "RemoteControl.h"
+#include <SettingsHandler.h>
 
 /******************************************************************************
  * Compiler Switches
@@ -61,17 +63,79 @@
 
 void StartupState::entry()
 {
-    /* Nothing to do */
+    m_isActive = true;
 }
 
 void StartupState::process(StateMachine& sm)
 {
-    /* Nothing to do */
+
+    SettingsHandler& settings = SettingsHandler::getInstance();
+
+    switch (m_pendingCommandCounter)
+    {
+    case CMD_GET_MAX_SPEED:
+        if (nullptr == m_pendingCommand)
+        {
+            /* Create new pending commmand. */
+            m_pendingCommand = new Command{RemoteControl::CMD_ID_GET_MAX_SPEED};
+        }
+        else
+        {
+            /* Command is still pending. */
+        }
+        break;
+
+    case CMD_SET_INIT_POS:
+        if (nullptr == m_pendingCommand)
+        {
+            /* Create new pending commmand. */
+            m_pendingCommand              = new Command;
+            m_pendingCommand->commandId   = RemoteControl::CMD_ID_SET_INIT_POS;
+            m_pendingCommand->xPos        = settings.getInitialXPosition();
+            m_pendingCommand->yPos        = settings.getInitialYPosition();
+            m_pendingCommand->orientation = settings.getInitialHeading();
+        }
+        else
+        {
+            /* Command is still pending. */
+        }
+        break;
+
+    case CMD_NONE:
+        /* All commands processed. Switch to idle state. */
+        // sm.setState(&IdleState::getInstance());
+        break;
+
+    default:
+        break;
+    }
 }
 
 void StartupState::exit()
 {
-    /* Nothing to do */
+    /* Ensure the pending command is deleted. */
+    if (nullptr != m_pendingCommand)
+    {
+        delete m_pendingCommand;
+        m_pendingCommand = nullptr;
+    }
+
+    m_isActive = false;
+}
+
+Command* StartupState::getPendingCommand()
+{
+    return (true == m_isActive) ? m_pendingCommand : nullptr;
+}
+
+void StartupState::notifyCommandProcessed()
+{
+    if (nullptr != m_pendingCommand)
+    {
+        delete m_pendingCommand;
+        m_pendingCommand = nullptr;
+        m_pendingCommandCounter++;
+    }
 }
 
 /******************************************************************************

lib/ConvoyLeader/src/StartupState.h

@@ -44,6 +44,9 @@
  *****************************************************************************/
 
 #include <IState.h>
+#include <StateMachine.h>
+#include "SerialMuxChannels.h"
+#include "SimpleTimer.hpp"
 
 /******************************************************************************
  * Macros
@@ -57,6 +60,14 @@
 class StartupState : public IState
 {
 public:
+    /** Commands to send and process in the Startup State. */
+    enum StartupCommands : uint8_t
+    {
+        CMD_GET_MAX_SPEED = 0, /**< Get maximum motor speed. */
+        CMD_SET_INIT_POS,      /**< Set initial position. */
+        CMD_NONE               /**< No pending command. Required to signal the end of the enum. */
+    };
+
     /**
      * Get state instance.
      *
@@ -88,12 +99,33 @@ class StartupState : public IState
      */
     void exit() final;
 
+    /**
+     * Get pending command. If there is no pending command or the state is not active, it will return nullptr.
+     *
+     * @return Pending command or nullptr.
+     */
+    Command* getPendingCommand();
+
+    /**
+     * Notify the state, that the pending command is successfully processed by RU.
+     */
+    void notifyCommandProcessed();
+
 protected:
 private:
+    /** Flag: State is active. */
+    bool m_isActive;
+
+    /** Pending command. */
+    Command* m_pendingCommand;
+
+    /** Pending command counter. */
+    uint8_t m_pendingCommandCounter;
+
     /**
      * Default constructor.
      */
-    StartupState()
+    StartupState() : IState(), m_isActive(false), m_pendingCommand(nullptr), m_pendingCommandCounter(CMD_GET_MAX_SPEED)
     {
     }