Merge pull request #43 from BlueAndi/SensorFusion/dev

Send End Line Status via SerialMuxProt

lib/APPSensorFusion/App.cpp

@@ -76,6 +76,9 @@ void App::setup()
 
     /* Providing Sensor data */
     m_smpChannelIdSensorData = m_smpServer.createChannel(SENSORDATA_CHANNEL_NAME, SENSORDATA_CHANNEL_DLC);
+
+    /* Sending End Line Detection signal */
+    m_smpChannelIdEndLine = m_smpServer.createChannel(ENDLINE_CHANNEL_NAME, ENDLINE_CHANNEL_DLC);
 }
 
 void App::loop()
@@ -107,6 +110,12 @@ void App::loop()
     {
         m_sendSensorsDataInterval.restart();
         sendSensorData();
+
+        /* Send End line detection signal if the application is currently in the Driving state. */
+        if (&DrivingState::getInstance() == m_systemStateMachine.getState())
+        {
+            sendEndLineDetectionSignal();
+        }
     }
     m_systemStateMachine.process();
 }
@@ -159,6 +168,21 @@ void App::sendSensorData() const
     (void)m_smpServer.sendData(m_smpChannelIdSensorData, reinterpret_cast<uint8_t*>(&payload), sizeof(payload));
 }
 
+void App::sendEndLineDetectionSignal()
+{
+    DrivingState::LineStatus lineStatus = DrivingState::getInstance().getLineStatus();
+
+    /* Send only if a new End Line has been detected. */
+    if ((DrivingState::LINE_STATUS_FIND_END_LINE == m_lastLineDetectionStatus) &&
+        (DrivingState::LINE_STATUS_END_LINE_DETECTED == lineStatus))
+    {
+        EndLineFlag payload = {.isEndLineDetected = true};
+        (void)m_smpServer.sendData(m_smpChannelIdEndLine, reinterpret_cast<uint8_t*>(&payload), sizeof(payload));
+    }
+
+    m_lastLineDetectionStatus = lineStatus;
+}
+
 /******************************************************************************
  * External Functions
  *****************************************************************************/

lib/APPSensorFusion/App.h

@@ -48,6 +48,7 @@
 #include <Arduino.h>
 #include <SerialMuxProtServer.hpp>
 #include "SerialMuxChannels.h"
+#include "DrivingState.h"
 
 /******************************************************************************
  * Macros
@@ -66,9 +67,11 @@ class App
      */
     App() :
         m_smpChannelIdSensorData(0U),
+        m_smpChannelIdEndLine(0U),
         m_systemStateMachine(),
         m_controlInterval(),
         m_sendSensorsDataInterval(),
+        m_lastLineDetectionStatus(DrivingState::LINE_STATUS_FIND_START_LINE),
         m_smpServer(Serial)
     {
     }
@@ -100,9 +103,12 @@ class App
     /** Baudrate for Serial Communication */
     static const uint32_t SERIAL_BAUDRATE = 115200U;
 
-    /** Channel id sending sensor data used for sensor fusion. */
+    /** Channel id for sending sensor data used for sensor fusion. */
     uint8_t m_smpChannelIdSensorData;
 
+    /** Channel id for sending End Line Detection signal. */
+    uint8_t m_smpChannelIdEndLine;
+
     /** The system state machine. */
     StateMachine m_systemStateMachine;
 
@@ -112,6 +118,9 @@ class App
     /** Timer used for sending data periodically. */
     SimpleTimer m_sendSensorsDataInterval;
 
+    /** End Line Status of the previous iteration */
+    DrivingState::LineStatus m_lastLineDetectionStatus;
+
     /**
      * SerialMuxProt Server Instance
      *
@@ -124,6 +133,12 @@ class App
      */
     void sendSensorData() const;
 
+    /**
+     * Send the End Line Detection Flag as a EndLineFlag struct via SerialMuxProt.
+     * The Signal will only be sent if the a new End Line has been detected.
+     */
+    void sendEndLineDetectionSignal();
+
     /* Not allowed. */
     App(const App& app);            /**< Copy construction of an instance. */
     App& operator=(const App& app); /**< Assignment of an instance. */

lib/APPSensorFusion/DrivingState.cpp

@@ -182,25 +182,12 @@ void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorVa
                 m_lineStatus = LINE_STATUS_START_LINE_DETECTED;
 
                 Sound::playBeep();
-
-                /* Measure the lap time and use as start point the detected start line. */
-                m_lapTime.start(0);
             }
             /* End line detected */
             else if (LINE_STATUS_FIND_END_LINE == m_lineStatus)
             {
-                DifferentialDrive& diffDrive = DifferentialDrive::getInstance();
-
-                /* Stop motors immediately. Don't move this to a later position,
-                 * as this would extend the driven length.
-                 */
-                diffDrive.setLinearSpeed(0, 0);
-
-                Sound::playBeep();
-                m_trackStatus = TRACK_STATUS_FINISHED;
-
-                /* Calculate lap time and show it*/
-                ReadyState::getInstance().setLapTime(m_lapTime.getCurrentDuration());
+                m_lineStatus = LINE_STATUS_END_LINE_DETECTED;
+                /* Don't switch to the Ready State and do nothing. */
             }
             else
             {
@@ -213,7 +200,8 @@ void DrivingState::processOnTrack(int16_t position, const uint16_t* lineSensorVa
         }
         else
         {
-            ;
+            /* Be able to switch back to LINE_STATUS_FIND_END_LINE */
+            m_lineStatus = LINE_STATUS_FIND_END_LINE;
         }
 
         if (TRACK_STATUS_FINISHED != m_trackStatus)

lib/APPSensorFusion/DrivingState.h

@@ -92,18 +92,29 @@ class DrivingState : public IState
      */
     void exit() final;
 
-protected:
-private:
     /**
      * The line detection status.
      */
     enum LineStatus
     {
         LINE_STATUS_FIND_START_LINE = 0, /**< Find the start line. */
         LINE_STATUS_START_LINE_DETECTED, /**< Start line detected. */
-        LINE_STATUS_FIND_END_LINE        /**< Find the end line. */
+        LINE_STATUS_FIND_END_LINE,       /**< Find the end line. */
+        LINE_STATUS_END_LINE_DETECTED    /**< End Line detected. */
     };
 
+    /**
+     * Indicates if there is currently an End line detected.
+     *
+     * @return current Line Status as a LineStatus enum
+     */
+    DrivingState::LineStatus getLineStatus()
+    {
+        return m_lineStatus;
+    }
+
+protected:
+private:
     /**
      * The track status.
      */
@@ -124,7 +135,6 @@ class DrivingState : public IState
     static const uint32_t PID_PROCESS_PERIOD = 10;
 
     SimpleTimer            m_observationTimer; /**< Observation timer to observe the max. time per challenge. */
-    SimpleTimer            m_lapTime;          /**< Timer used to calculate the lap time. */
     SimpleTimer            m_pidProcessTime;   /**< Timer used for periodically PID processing. */
     PIDController<int16_t> m_pidCtrl;          /**< PID controller, used for driving. */
     int16_t                m_topSpeed;         /**< Top speed in [steps/s]. It might be lower or equal to the max. speed! */
@@ -138,7 +148,6 @@ class DrivingState : public IState
      */
     DrivingState() :
         m_observationTimer(),
-        m_lapTime(),
         m_pidProcessTime(),
         m_pidCtrl(),
         m_topSpeed(0),

lib/APPSensorFusion/SerialMuxChannels.h

@@ -49,6 +49,12 @@
 /** DLC of Sensordata Channel */
 #define SENSORDATA_CHANNEL_DLC (sizeof(SensorData))
 
+/** Name of Channel to End Line Detected Signal. */
+#define ENDLINE_CHANNEL_NAME "END_LINE"
+
+/** DLC of End Line Channel */
+#define ENDLINE_CHANNEL_DLC (sizeof(EndLineFlag))
+
 /** Maximum number of SerialMuxProt Channels. */
 #define MAX_CHANNELS (10U)
 
@@ -99,6 +105,13 @@ typedef struct _SensorData
     int16_t turnRate;
 } __attribute__((packed)) SensorData;
 
+/** Struct of the End Line Detection payload. */
+typedef struct _EndLineFlag
+{
+    /** Indicates if the End Line has been detected. */
+    bool isEndLineDetected;
+} __attribute__((packed)) EndLineFlag;
+
 /******************************************************************************
  * Functions
  *****************************************************************************/