Added driving state.

Replaced LongitudinalController with driving state

Author: gabryelreyes

lib/ConvoyLeader/src/App.cpp

@@ -43,6 +43,7 @@
 #include <Util.h>
 #include "StartupState.h"
 #include "IdleState.h"
+#include "DrivingState.h"
 
 /******************************************************************************
  * Compiler Switches
@@ -171,12 +172,6 @@ void App::setup()
         }
         else
         {
-            /* Setup longitudinal controller. */
-            LongitudinalController::MotorSetpointCallback motorSetpointCallback = [this](const int16_t& topCenterSpeed)
-            { return this->motorSetpointCallback(topCenterSpeed); };
-
-            m_longitudinalController.setMotorSetpointCallback(motorSetpointCallback);
-
             /* Initialize timers. */
             m_sendWaypointTimer.start(SEND_WAYPOINT_TIMER_INTERVAL);
             m_commandTimer.start(SEND_COMMANDS_TIMER_INTERVAL);
@@ -220,62 +215,16 @@ void App::loop()
     /* Process V2V Communication */
     m_v2vClient.process();
 
-    /* Process Longitudinal Controller */
-    m_longitudinalController.process();
-
     /* Process System State Machine */
     m_systemStateMachine.process();
 
     /* Process periodic tasks. */
     processPeriodicTasks();
 }
 
-void App::currentVehicleChannelCallback(const VehicleData& vehicleData)
-{
-    m_latestVehicleData = Waypoint(vehicleData.xPos, vehicleData.yPos, vehicleData.orientation, vehicleData.left,
-                                   vehicleData.right, vehicleData.center);
-
-    // latestVehicleData.debugPrint();
-
-    m_longitudinalController.calculateTopMotorSpeed(m_latestVehicleData);
-}
-
-bool App::motorSetpointCallback(const int16_t topCenterSpeed)
+void App::setLatestVehicleData(const Waypoint& waypoint)
 {
-    SpeedData payload;
-    payload.center = topCenterSpeed;
-
-    return m_smpServer.sendData(m_serialMuxProtChannelIdMotorSpeeds, &payload, sizeof(payload));
-}
-
-void App::release()
-{
-    Command payload;
-    payload.commandId = RemoteControl::CMD_ID_START_DRIVING;
-
-    /* Release robot. */
-    if (false == m_longitudinalController.release())
-    {
-        LOG_WARNING("Robot could not be released.");
-    }
-    else if (false == m_smpServer.sendData(m_serialMuxProtChannelIdRemoteCtrl, &payload, sizeof(payload)))
-    {
-        LOG_WARNING("Failed to send release command to RU.");
-    }
-    else
-    {
-        LOG_INFO("Robot released.");
-    }
-}
-
-void App::setMaxMotorSpeed(const int16_t maxMotorSpeed)
-{
-    m_longitudinalController.setMaxMotorSpeed(maxMotorSpeed);
-}
-
-void App::setLastFollowerFeedback(const Waypoint& feedback)
-{
-    m_longitudinalController.setLastFollowerFeedback(feedback);
+    m_latestVehicleData = waypoint;
 }
 
 /******************************************************************************
@@ -329,20 +278,19 @@ bool App::setupMqttClient()
     }
     else
     {
-        isSuccessful =
-            m_mqttClient.subscribe(TOPIC_NAME_RELEASE, true, [this](const String& payload) { this->release(); });
-
-        isSuccessful &= m_mqttClient.subscribe("platoons/0/vehicles/0/feedback", false,
-                                               [this](const String& payload)
-                                               {
-                                                   Waypoint* waypoint = Waypoint::deserialize(payload);
-
-                                                   if (nullptr != waypoint)
-                                                   {
-                                                       this->setLastFollowerFeedback(*waypoint);
-                                                       delete waypoint;
-                                                   }
-                                               });
+        isSuccessful = m_mqttClient.subscribe(
+            TOPIC_NAME_RELEASE, true, [this](const String& payload) { IdleState::getInstance().requestRelease(); });
+
+        isSuccessful &=
+            m_mqttClient.subscribe("platoons/0/vehicles/0/feedback", false,
+                                   [this](const String& payload)
+                                   {
+                                       Waypoint*   waypoint = Waypoint::deserialize(payload);
+                                       VehicleData feedback{waypoint->xPos, waypoint->yPos,  waypoint->orientation,
+                                                            waypoint->left, waypoint->right, waypoint->center};
+
+                                       DrivingState::getInstance().setLastFollowerFeedback(feedback);
+                                   });
     }
 
     return isSuccessful;
@@ -441,7 +389,7 @@ void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize
         else if (RemoteControl::CMD_ID_GET_MAX_SPEED == cmdRsp->commandId)
         {
             LOG_DEBUG("Max Speed: %d", cmdRsp->maxMotorSpeed);
-            application->setMaxMotorSpeed(cmdRsp->maxMotorSpeed);
+            DrivingState::getInstance().setMaxMotorSpeed(cmdRsp->maxMotorSpeed);
             StartupState::getInstance().notifyCommandProcessed();
         }
         else if (RemoteControl::CMD_ID_SET_INIT_POS == cmdRsp->commandId)
@@ -477,7 +425,11 @@ void App_currentVehicleChannelCallback(const uint8_t* payload, const uint8_t pay
     {
         const VehicleData* currentVehicleData = reinterpret_cast<const VehicleData*>(payload);
         App*               application        = reinterpret_cast<App*>(userData);
-        application->currentVehicleChannelCallback(*currentVehicleData);
+        Waypoint dataAsWaypoint(currentVehicleData->xPos, currentVehicleData->yPos, currentVehicleData->orientation,
+                                currentVehicleData->left, currentVehicleData->right, currentVehicleData->center);
+
+        application->setLatestVehicleData(dataAsWaypoint);
+        DrivingState::getInstance().setVehicleData(*currentVehicleData);
     }
     else
     {

lib/ConvoyLeader/src/App.h

@@ -50,7 +50,6 @@
 #include <StateMachine.h>
 #include <V2VClient.h>
 #include "SerialMuxChannels.h"
-#include "LongitudinalController.h"
 
 /******************************************************************************
  * Macros
@@ -73,7 +72,6 @@ class App
         m_smpServer(Board::getInstance().getDevice().getStream(), this),
         m_mqttClient(),
         m_v2vClient(m_mqttClient),
-        m_longitudinalController(),
         m_sendWaypointTimer()
     {
     }
@@ -96,40 +94,11 @@ class App
     void loop();
 
     /**
-     * Callback for the current vehicle data.
+     * Set latest vehicle data from RU.
      *
-     * @param[in] vehicleData Current vehicle data.
+     * @param[in] waypoint  Latest vehicle data from RU.
      */
-    void currentVehicleChannelCallback(const VehicleData& vehicleData);
-
-    /**
-     * Motor setpoint callback.
-     * Called in order to send the motor speeds using SerialMuxProt to the robot.
-     *
-     * @param[in] topCenterSpeed  Center motor speed [steps/s].
-     *
-     * @return If the motor speed was sent successfully, returns true. Otherwise, false.
-     */
-    bool motorSetpointCallback(const int16_t topCenterSpeed);
-
-    /**
-     * Release robot and start driving.
-     */
-    void release();
-
-    /**
-     * Set max motor speed.
-     *
-     * @param[in] maxMotorSpeed    Max motor speed [steps/s].
-     */
-    void setMaxMotorSpeed(const int16_t maxMotorSpeed);
-
-    /**
-     * Set incoming feedback from last follower.
-     *
-     * @param[in] feedback Feedback from last follower.
-     */
-    void setLastFollowerFeedback(const Waypoint& feedback);
+    void setLatestVehicleData(const Waypoint& waypoint);
 
 private:
     /** Minimum battery level in percent. */
@@ -176,11 +145,6 @@ class App
     /** The system state machine. */
     StateMachine m_systemStateMachine;
 
-    /**
-     * Longitudinal controller.
-     */
-    LongitudinalController m_longitudinalController;
-
     /**
      * Latest vehicle data from RU.
      */

lib/ConvoyLeader/src/LongitudinalController.cpp lib/ConvoyLeader/src/DrivingState.cpp

@@ -25,16 +25,15 @@
     DESCRIPTION
 *******************************************************************************/
 /**
- * @brief  Leader Longitudinal Controller.
+ * @brief  Driving state.
  * @author Gabryel Reyes <gabryelrdiaz@gmail.com>
  */
 
 /******************************************************************************
  * Includes
  *****************************************************************************/
 
-#include "LongitudinalController.h"
-#include <Logging.h>
+#include "DrivingState.h"
 
 /******************************************************************************
  * Compiler Switches
@@ -60,72 +59,55 @@
  * Public Methods
  *****************************************************************************/
 
-LongitudinalController::LongitudinalController() :
-    m_maxMotorSpeed(0),
-    m_state(STATE_STARTUP),
-    m_lastFollowerFeedback(),
-    m_motorSetpointCallback(nullptr)
+void DrivingState::entry()
 {
+    m_isActive = true;
 }
 
-LongitudinalController::~LongitudinalController()
+void DrivingState::process(StateMachine& sm)
 {
+    /* Check if the state is active. */
+    if (false == m_isActive)
+    {
+        m_topMotorSpeed = 0;
+    }
+    else
+    {
+        /* Check limits. */
+
+        /* Check follower feedback. Calculate platoon length and react accordingly */
+
+        /* Calculate top motor speed. */
+        m_topMotorSpeed = m_maxMotorSpeed;
+    }
 }
 
-void LongitudinalController::process()
+void DrivingState::exit()
 {
-    switch (m_state)
-    {
-    case STATE_STARTUP:
-        /* Max speed must be positive. */
-        if (0 < m_maxMotorSpeed)
-        {
-            m_state = STATE_READY;
-        }
-
-        break;
-
-    case STATE_READY:
-        /* Wait for external release. */
-        break;
-
-    case STATE_DRIVING:
-        /* Allow top motor speed calculation. */
-        break;
-
-    case STATE_SAFE:
-        /* Stop the vehicle. Sent continously to prevent overwriting by other modules. */
-        if (nullptr != m_motorSetpointCallback)
-        {
-            m_motorSetpointCallback(0);
-        }
-
-        break;
-
-    default:
-        /* Should never happen. */
-        m_state = STATE_SAFE;
-        break;
-    }
+    m_isActive = false;
 }
 
-void LongitudinalController::calculateTopMotorSpeed(const Waypoint& currentVehicleData)
+void DrivingState::setMaxMotorSpeed(int16_t maxSpeed)
 {
-    if (STATE_DRIVING == m_state)
-    {
-        int16_t topMotorSpeed = 0;
+    m_maxMotorSpeed = maxSpeed;
+}
 
-        /* TODO: Check follower feedback. Calculate platoon length and react accordingly. */
+bool DrivingState::getTopMotorSpeed(int16_t& topMotorSpeed) const
+{
+    topMotorSpeed = m_topMotorSpeed;
 
-        /* Calculate top motor speed. */
-        topMotorSpeed = m_maxMotorSpeed;
+    /* Only valid if the state is active. */
+    return m_isActive;
+}
 
-        /* Send top motor speed. */
-        if (nullptr != m_motorSetpointCallback)
-        {
-            m_motorSetpointCallback(topMotorSpeed);
-        }
-    }
+void DrivingState::setVehicleData(const VehicleData& vehicleData)
+{
+    m_vehicleData = vehicleData;
+}
+
+void DrivingState::setLastFollowerFeedback(const VehicleData& feedback)
+{
+    m_followerFeedback = feedback;
 }
 
 /******************************************************************************