Check for incoming status.

Author: gabryelreyes

lib/ConvoyLeader/src/App.cpp

@@ -67,6 +67,7 @@
 
 static void App_cmdRspChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
 static void App_currentVehicleChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
+static void App_statusChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
 
 /******************************************************************************
  * Local Variables
@@ -234,6 +235,16 @@ void App::setErrorState()
     m_systemStateMachine.setState(&ErrorState::getInstance());
 }
 
+void App::systemStatusCallback(SMPChannelPayload::Status status)
+{
+    if (SMPChannelPayload::STATUS_FLAG_ERROR == status)
+    {
+        setErrorState();
+    }
+
+    m_statusTimeoutTimer.restart();
+}
+
 /******************************************************************************
  * Protected Methods
  *****************************************************************************/
@@ -407,6 +418,17 @@ void App::processPeriodicTasks()
 
         m_statusTimer.restart();
     }
+
+    if ((false == m_statusTimeoutTimer.isTimerRunning()) && (true == m_smpServer.isSynced()))
+    {
+        /* Start status timeout timer once SMP is synced the first time. */
+        m_statusTimeoutTimer.start(STATUS_TIMEOUT_TIMER_INTERVAL);
+    }
+    else if (true == m_statusTimeoutTimer.isTimeout())
+    {
+        /* Not receiving status from RU. Go to error state. */
+        setErrorState();
+    }
 }
 
 /******************************************************************************
@@ -480,3 +502,20 @@ void App_currentVehicleChannelCallback(const uint8_t* payload, const uint8_t pay
                     CURRENT_VEHICLE_DATA_CHANNEL_DLC, payloadSize);
     }
 }
+
+/**
+ * Receives current status of the RU over SerialMuxProt channel.
+ *
+ * @param[in] payload       Status of the RU.
+ * @param[in] payloadSize   Size of the Status Flag
+ * @param[in] userData      Instance of App class.
+ */
+void App_statusChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData)
+{
+    if ((nullptr != payload) && (STATUS_CHANNEL_DLC == payloadSize) && (nullptr != userData))
+    {
+        const Status* currentStatus = reinterpret_cast<const Status*>(payload);
+        App*          application   = reinterpret_cast<App*>(userData);
+        application->systemStatusCallback(currentStatus->status);
+    }
+}

lib/ConvoyLeader/src/App.h

@@ -78,7 +78,8 @@ class App
         m_sendWaypointTimer(),
         m_commandTimer(),
         m_motorSpeedTimer(),
-        m_statusTimer()
+        m_statusTimer(),
+        m_statusTimeoutTimer()
     {
     }
 
@@ -111,6 +112,13 @@ class App
      */
     void setErrorState();
 
+    /**
+     * System Status callback.
+     *
+     * @param[in] status    System status
+     */
+    void systemStatusCallback(SMPChannelPayload::Status status);
+
 private:
     /** Minimum battery level in percent. */
     static const uint8_t MIN_BATTERY_LEVEL = 10U;
@@ -127,6 +135,9 @@ class App
     /** Send status timer interval in ms. */
     static const uint32_t SEND_STATUS_TIMER_INTERVAL = 1000U;
 
+    /** Status timeout timer interval in ms. */
+    static const uint32_t STATUS_TIMEOUT_TIMER_INTERVAL = 2U * SEND_STATUS_TIMER_INTERVAL;
+
     /** MQTT topic name for birth messages. */
     static const char* TOPIC_NAME_BIRTH;
 
@@ -192,6 +203,11 @@ class App
      */
     SimpleTimer m_statusTimer;
 
+    /**
+     * Timer for timeout of system status of RU.
+     */
+    SimpleTimer m_statusTimeoutTimer;
+
 private:
     /**
      * Handler of fatal errors in the Application.