Report Odometry implemented

lib/ConvoyLeader/App.cpp

@@ -39,6 +39,7 @@
 #include <DifferentialDrive.h>
 #include <Odometry.h>
 #include <Util.h>
+#include <SerialMuxChannels.h>
 
 /******************************************************************************
  * Compiler Switches
@@ -60,9 +61,6 @@
  * Local Variables
  *****************************************************************************/
 
-/* Name of Channel to send Position Data to. */
-const char* App::POSITION_CHANNEL = "POSITION";
-
 /******************************************************************************
  * Public Methods
  *****************************************************************************/
@@ -73,7 +71,13 @@ void App::setup()
     Board::getInstance().init();
     m_systemStateMachine.setState(&StartupState::getInstance());
     m_controlInterval.start(DIFFERENTIAL_DRIVE_CONTROL_PERIOD);
-    m_smpServer.createChannel(POSITION_CHANNEL, POSITION_CHANNEL_DLC);
+    m_serialMuxProtChannelIdOdometry = m_smpServer.createChannel(ODOMETRY_CHANNEL_NAME, ODOMETRY_CHANNEL_DLC);
+
+    /* Channel sucesfully created? */
+    if (0U != m_serialMuxProtChannelIdOdometry)
+    {
+        m_reportOdometryTimer.start(REPORT_ODOMETRY_PERIOD);
+    }
 }
 
 void App::loop()
@@ -95,12 +99,17 @@ void App::loop()
          */
         Odometry::getInstance().process();
 
-        /* Send Position to SerialMuxProt Client */
-        reportPosition();
-
         m_controlInterval.restart();
     }
 
+    if (true == m_reportOdometryTimer.isTimeout())
+    {
+        /* Send Odometry to SerialMuxProt Client */
+        reportOdometry();
+
+        m_reportOdometryTimer.restart();
+    }
+
     m_systemStateMachine.process();
 }
 
@@ -112,22 +121,19 @@ void App::loop()
  * Private Methods
  *****************************************************************************/
 
-void App::reportPosition()
+void App::reportOdometry()
 {
-    int32_t xPos;
-    int32_t yPos;
-    uint8_t outBuf[POSITION_CHANNEL_DLC];
-
-    Odometry::getInstance().getPosition(xPos, yPos);
+    Odometry&    odometry = Odometry::getInstance();
+    OdometryData payload;
+    int32_t      xPos = 0;
+    int32_t      yPos = 0;
 
-    Util::int32ToByteArray(&outBuf[0U], (sizeof(outBuf) - sizeof(int32_t)), xPos);
-    Util::int32ToByteArray(&outBuf[4U], (sizeof(outBuf) - sizeof(int32_t)), yPos);
+    odometry.getPosition(xPos, yPos);
+    payload.xPos        = xPos;
+    payload.yPos        = yPos;
+    payload.orientation = odometry.getOrientation();
 
-    m_smpServer.sendData(POSITION_CHANNEL, outBuf, sizeof(outBuf));
-}
-
-void App::positionCallback(const uint8_t* payload, const uint8_t payloadSize)
-{
+    m_smpServer.sendData(ODOMETRY_CHANNEL_NAME, reinterpret_cast<uint8_t*>(&payload), sizeof(payload));
 }
 
 /******************************************************************************

lib/ConvoyLeader/App.h

@@ -27,7 +27,7 @@
 /**
  * @brief  ConvoyLeader application
  * @author Andreas Merkle <[email protected]>
- * 
+ *
  * @addtogroup Application
  *
  * @{
@@ -60,13 +60,14 @@
 class App
 {
 public:
-
     /**
      * Construct the convoy leader application.
      */
     App() :
+        m_serialMuxProtChannelIdOdometry(0U),
         m_systemStateMachine(),
         m_controlInterval(),
+        m_reportOdometryTimer(),
         m_smpServer(Serial)
     {
     }
@@ -89,25 +90,27 @@ class App
     void loop();
 
 private:
-
     /** Differential drive control period in ms. */
     static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U;
 
-    /** Name of Channel to send Position Data to. */
-    static const char* POSITION_CHANNEL;
-
-    /** DLC of Position Channel */
-    static const uint8_t POSITION_CHANNEL_DLC = 8U;
+    /** Odometry reporting period in ms. */
+    static const uint32_t REPORT_ODOMETRY_PERIOD = 100U;
 
     /** Baudrate for Serial Communication */
     static const uint32_t SERIAL_BAUDRATE = 115200U;
 
+    /** SerialMuxProt Channel id for sending the current odometry. */
+    uint8_t m_serialMuxProtChannelIdOdometry;
+
     /** The system state machine. */
     StateMachine m_systemStateMachine;
 
     /** Timer used for differential drive control processing. */
     SimpleTimer m_controlInterval;
 
+    /** Timer for reporting odometry through SerialMuxProt. */
+    SimpleTimer m_reportOdometryTimer;
+
     /**
      * SerialMuxProt Server Instance
      *
@@ -117,18 +120,13 @@ class App
     SerialMuxProtServer<10U> m_smpServer;
 
     /**
-     * Report the current position of the robot using the Odometry data.
+     * Report the current position and heading of the robot using the Odometry data.
      * Sends data through the SerialMuxProtServer.
      */
-    void reportPosition();
-
-    /**
-     * Callback for incoming data from the Position Channel.
-     * @param[in] payload Byte buffer containing incomming data.
-     * @param[in] payloadSize Number of bytes received.
-     */
-    static void positionCallback(const uint8_t* payload, const uint8_t payloadSize);
+    void reportOdometry();
 
+private:
+    /* An instance shall not be copied or assigned. */
     App(const App& app);
     App& operator=(const App& app);
 };