From 8a75912b3e5201e260fdc95e8685dd483200a109 Mon Sep 17 00:00:00 2001
From: Akram <Akram.Bzeouich@Newtec.de>
Date: Mon, 5 Aug 2024 22:28:13 +0200
Subject: [PATCH] Handle failure to send data over the serial Mux protocol

---
 lib/APPReinforcementLearning/src/App.cpp | 61 +++++++++++++-----------
 lib/APPReinforcementLearning/src/App.h   |  9 ++--
 2 files changed, 38 insertions(+), 32 deletions(-)

diff --git a/lib/APPReinforcementLearning/src/App.cpp b/lib/APPReinforcementLearning/src/App.cpp
index 2f469d32..623ff636 100644
--- 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
index 8615bfa0..4d73fc4a 100644
--- 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,6 +142,9 @@ 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.
      *
@@ -148,11 +152,6 @@ class App
      */
     bool setupSerialMuxProt();
 
-    /**
-     * Send line sensors data via SerialMuxProt.
-     */
-    void sendLineSensorsData() const;
-
     /**
      * Copy construction of an instance.
      * Not allowed.