Handle failure to send data over the serial Mux protocol

BlueAndi · Aug 5, 2024 · 8a75912 · 8a75912
1 parent c11acae
commit 8a75912
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 32 deletions.
diff --git a/lib/APPReinforcementLearning/src/App.cpp b/lib/APPReinforcementLearning/src/App.cpp
@@ -42,6 +42,7 @@
 #include <Odometry.h>
 #include "ReadyState.h"
 #include <Logging.h>
+#include "TrainingState.h"
 
 /******************************************************************************
  * Compiler Switches
@@ -123,8 +124,7 @@ void App::loop()
             payload = {SMPChannelPayload::Status::DONE};
         }
 
-        /* Ignoring return value, as error handling is not available. */
-        (void)m_smpServer.sendData(m_serialMuxProtChannelIdStatus, &payload, sizeof(payload));
+        m_data_sent = m_smpServer.sendData(m_serialMuxProtChannelIdStatus, &payload, sizeof(payload));
 
         m_statusTimer.restart();
     }
@@ -133,7 +133,23 @@ void App::loop()
     if (true == m_sendLineSensorsDataInterval.isTimeout() &&
         (&DrivingState::getInstance() == m_systemStateMachine.getState()))
     {
-        sendLineSensorsData();
+        ILineSensors&   lineSensors      = Board::getInstance().getLineSensors();
+        uint8_t         maxLineSensors   = lineSensors.getNumLineSensors();
+        const uint16_t* lineSensorValues = lineSensors.getSensorValues();
+        uint8_t         lineSensorIdx    = 0U;
+        LineSensorData  payload;
+
+        if (LINE_SENSOR_CHANNEL_DLC == (maxLineSensors * sizeof(uint16_t)))
+        {
+            while (maxLineSensors > lineSensorIdx)
+            {
+                payload.lineSensorData[lineSensorIdx] = lineSensorValues[lineSensorIdx];
+
+                ++lineSensorIdx;
+            }
+        }
+
+        m_data_sent = m_smpServer.sendData(m_serialMuxProtChannelIdLineSensors, &payload, sizeof(payload));
 
         m_sendLineSensorsDataInterval.restart();
     }
@@ -148,13 +164,19 @@ void App::loop()
             SMPChannelPayload::Mode payload =
                 (1 == mode_options) ? SMPChannelPayload::Mode::DRIVING_MODE : SMPChannelPayload::Mode::TRAINING_MODE;
 
-            /* Ignoring return value, as error handling is not available. */
-            (void)m_smpServer.sendData(m_serialMuxProtChannelIdMode, &payload, sizeof(payload));
+            m_data_sent = m_smpServer.sendData(m_serialMuxProtChannelIdMode, &payload, sizeof(payload));
 
             m_modeSelectionSent = true;
         }
     }
 
+    if (false == m_data_sent)
+    {
+        /* Failed to send data to the supervisor. Go to error state. */
+        ErrorState::getInstance().setErrorMsg("DSF");
+        m_systemStateMachine.setState(&ErrorState::getInstance());
+    }
+
     m_smpServer.process(millis());
 
     m_systemStateMachine.process();
@@ -174,6 +196,13 @@ void App::handleRemoteCommand(const Command& cmd)
         m_modeSelectionSent = false;
         break;
 
+    case SMPChannelPayload::CmdId::CMD_ID_SET_TRAINING_STATE:
+        Odometry::getInstance().clearPosition();
+        Odometry::getInstance().clearMileage();
+
+        m_systemStateMachine.setState(&TrainingState::getInstance());
+        break;
+
     default:
         /* Nothing to do. */
         break;
@@ -188,28 +217,6 @@ void App::handleRemoteCommand(const Command& cmd)
  * Private Methods
  *****************************************************************************/
 
-void App::sendLineSensorsData() const
-{
-    ILineSensors&   lineSensors      = Board::getInstance().getLineSensors();
-    uint8_t         maxLineSensors   = lineSensors.getNumLineSensors();
-    const uint16_t* lineSensorValues = lineSensors.getSensorValues();
-    uint8_t         lineSensorIdx    = 0U;
-    LineSensorData  payload;
-
-    if (LINE_SENSOR_CHANNEL_DLC == (maxLineSensors * sizeof(uint16_t)))
-    {
-        while (maxLineSensors > lineSensorIdx)
-        {
-            payload.lineSensorData[lineSensorIdx] = lineSensorValues[lineSensorIdx];
-
-            ++lineSensorIdx;
-        }
-    }
-
-    /* Ignoring return value, as error handling is not available. */
-    (void)m_smpServer.sendData(m_serialMuxProtChannelIdLineSensors, &payload, sizeof(payload));
-}
-
 bool App::setupSerialMuxProt()
 {
     bool isSuccessful = false;

diff --git a/lib/APPReinforcementLearning/src/App.h b/lib/APPReinforcementLearning/src/App.h
@@ -73,6 +73,7 @@ class App
         m_statusTimer(),
         m_sendLineSensorsDataInterval(),
         m_modeSelectionSent(false),
+        m_data_sent(true),
         m_smpServer(Serial, this)
     {
     }
@@ -141,18 +142,16 @@ class App
     /** Ensue that the mode is only sent once */
     bool m_modeSelectionSent;
 
+    /** check if the data has been sent */
+    bool m_data_sent;
+
     /**
      * Setup the SerialMuxProt channels.
      *
      * @return If successful returns true, otherwise false.
      */
     bool setupSerialMuxProt();
 
-    /**
-     * Send line sensors data via SerialMuxProt.
-     */
-    void sendLineSensorsData() const;
-
     /**
      * Copy construction of an instance.
      * Not allowed.