Merge pull request #69 from BlueAndi/feature/set_external_position

Initial Position

lib/APPRemoteControl/App.cpp

@@ -62,6 +62,7 @@
 
 static void App_cmdChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
 static void App_motorSpeedsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
+static void App_initialDataChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
 
 /******************************************************************************
  * Local Variables
@@ -94,6 +95,10 @@ void App::setup()
     /* Providing line sensor data */
     m_smpChannelIdLineSensors = m_smpServer.createChannel(LINE_SENSOR_CHANNEL_NAME, LINE_SENSOR_CHANNEL_DLC);
 
+    /* Receive initial vehicle data. */
+    m_smpServer.subscribeToChannel(INITIAL_VEHICLE_DATA_CHANNEL_DLC_CHANNEL_NAME, App_initialDataChannelCallback);
+
+    /* Providing current vehicle data. */
     m_smpChannelIdCurrentVehicleData =
         m_smpServer.createChannel(CURRENT_VEHICLE_DATA_CHANNEL_DLC_CHANNEL_NAME, CURRENT_VEHICLE_DATA_CHANNEL_DLC);
 
@@ -172,16 +177,17 @@ void App::loop()
 
 void App::sendRemoteControlResponses()
 {
-    RemoteCtrlState::RspId remoteControlRspId = RemoteCtrlState::getInstance().getCmdRsp();
+    CommandResponse remoteControlRspId = RemoteCtrlState::getInstance().getCmdRsp();
 
     /* Send only on change. */
-    if (remoteControlRspId != m_lastRemoteControlRspId)
+    if ((remoteControlRspId.responseId != m_lastRemoteControlRspId.responseId) ||
+        (remoteControlRspId.commandId != m_lastRemoteControlRspId.commandId))
     {
-        const CommandResponse* payload = reinterpret_cast<const CommandResponse*>(&remoteControlRspId);
-
-        (void)m_smpServer.sendData(m_smpChannelIdRemoteCtrlRsp, reinterpret_cast<uint8_t*>(&payload), sizeof(payload));
-
-        m_lastRemoteControlRspId = remoteControlRspId;
+        if (true == m_smpServer.sendData(m_smpChannelIdRemoteCtrlRsp, reinterpret_cast<uint8_t*>(&remoteControlRspId),
+                                         sizeof(remoteControlRspId)))
+        {
+            m_lastRemoteControlRspId = remoteControlRspId;
+        }
     }
 }
 
@@ -268,3 +274,25 @@ static void App_motorSpeedsChannelCallback(const uint8_t* payload, const uint8_t
         DifferentialDrive::getInstance().setLinearSpeed(motorSpeedData->left, motorSpeedData->right);
     }
 }
+
+/**
+ * Receives initial data over SerialMuxProt channel.
+ *
+ * @param[in] payload       Initial vehicle data. Position coordinates, orientation, and motor speeds
+ * @param[in] payloadSize   Size of VehicleData struct.
+ * @param[in] userData      User data
+ */
+static void App_initialDataChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData)
+{
+    (void)userData;
+    if ((nullptr != payload) && (INITIAL_VEHICLE_DATA_CHANNEL_DLC == payloadSize) && (true == gIsRemoteCtrlActive))
+    {
+        const VehicleData* vehicleData = reinterpret_cast<const VehicleData*>(payload);
+        Odometry&          odometry    = Odometry::getInstance();
+
+        odometry.clearPosition();
+        odometry.clearMileage();
+        odometry.setPosition(vehicleData->xPos, vehicleData->yPos);
+        odometry.setOrientation(vehicleData->orientation);
+    }
+}

lib/APPRemoteControl/App.h

@@ -73,7 +73,7 @@ class App
         m_smpChannelIdRemoteCtrlRsp(0U),
         m_smpChannelIdLineSensors(0U),
         m_smpChannelIdCurrentVehicleData(0U),
-        m_lastRemoteControlRspId(RemoteCtrlState::RSP_ID_OK)
+        m_lastRemoteControlRspId{RemoteCtrlState::CMD_ID_IDLE, RemoteCtrlState::RSP_ID_OK, 0}
     {
     }
 
@@ -136,7 +136,7 @@ class App
     uint8_t m_smpChannelIdCurrentVehicleData;
 
     /** Last remote control response id */
-    RemoteCtrlState::RspId m_lastRemoteControlRspId;
+    CommandResponse m_lastRemoteControlRspId;
 
     /* Not allowed. */
     App(const App& app);            /**< Copy construction of an instance. */

lib/APPRemoteControl/RemoteCtrlState.cpp

@@ -109,6 +109,11 @@ void RemoteCtrlState::process(StateMachine& sm)
         finishCommand(RSP_ID_OK);
         break;
 
+    case CMD_ID_GET_MAX_SPEED:
+        m_cmdRsp.maxMotorSpeed = DifferentialDrive::getInstance().getMaxMotorSpeed();
+        finishCommand(RSP_ID_OK);
+        break;
+
     default:
         break;
     }

lib/APPRemoteControl/RemoteCtrlState.h

@@ -45,6 +45,7 @@
  *****************************************************************************/
 #include <stdint.h>
 #include <IState.h>
+#include "SerialMuxChannels.h"
 
 /******************************************************************************
  * Macros
@@ -64,7 +65,8 @@ class RemoteCtrlState : public IState
         CMD_ID_IDLE = 0,                /**< Nothing to do. */
         CMD_ID_START_LINE_SENSOR_CALIB, /**< Start line sensor calibration. */
         CMD_ID_START_MOTOR_SPEED_CALIB, /**< Start motor speed calibration. */
-        CMD_ID_REINIT_BOARD             /**< Re-initialize the board. Required for webots simulation. */
+        CMD_ID_REINIT_BOARD,            /**< Re-initialize the board. Required for webots simulation. */
+        CMD_ID_GET_MAX_SPEED,           /**< Get maximum speed. */
 
     } CmdId;
 
@@ -118,8 +120,8 @@ class RemoteCtrlState : public IState
     {
         if (CMD_ID_IDLE == m_cmdId)
         {
-            m_cmdId = cmdId;
-            m_rspId = RSP_ID_PENDING;
+            m_cmdId             = cmdId;
+            m_cmdRsp.responseId = RSP_ID_PENDING;
         }
     }
 
@@ -128,20 +130,20 @@ class RemoteCtrlState : public IState
      *
      * @return Command response
      */
-    RspId getCmdRsp() const
+    CommandResponse getCmdRsp()
     {
-        return m_rspId;
+        return m_cmdRsp;
     }
 
 protected:
 private:
-    CmdId m_cmdId; /**< Current pending command. */
-    RspId m_rspId; /**< Current command response. */
+    CmdId           m_cmdId;  /**< Current pending command. */
+    CommandResponse m_cmdRsp; /**< Command response */
 
     /**
      * Default constructor.
      */
-    RemoteCtrlState() : m_cmdId(CMD_ID_IDLE), m_rspId(RSP_ID_OK)
+    RemoteCtrlState() : m_cmdId(CMD_ID_IDLE), m_cmdRsp()
     {
     }
 
@@ -162,8 +164,9 @@ class RemoteCtrlState : public IState
      */
     void finishCommand(RspId rsp)
     {
-        m_rspId = rsp;
-        m_cmdId = CMD_ID_IDLE;
+        m_cmdRsp.commandId  = m_cmdId;
+        m_cmdRsp.responseId = rsp;
+        m_cmdId             = CMD_ID_IDLE;
     }
 };
 

lib/APPRemoteControl/SerialMuxChannels.h

@@ -75,6 +75,12 @@
 /** DLC of Current Vehicle Data Channel */
 #define CURRENT_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData))
 
+/** Name of Channel to send Initial Vehicle Data to. */
+#define INITIAL_VEHICLE_DATA_CHANNEL_DLC_CHANNEL_NAME "INIT_DATA"
+
+/** DLC of Initial Vehicle Data Channel */
+#define INITIAL_VEHICLE_DATA_CHANNEL_DLC (sizeof(VehicleData))
+
 /******************************************************************************
  * Types and Classes
  *****************************************************************************/
@@ -88,7 +94,14 @@ typedef struct _Command
 /** Struct of the "Command Response" channel payload. */
 typedef struct _CommandResponse
 {
-    uint8_t response; /**< Response to the command */
+    uint8_t commandId;  /**< Command ID */
+    uint8_t responseId; /**< Response to the command */
+
+    /** Response Payload. */
+    union
+    {
+        int16_t maxMotorSpeed; /**< Max speed. */
+    };
 } __attribute__((packed)) CommandResponse;
 
 /** Struct of the "Speed" channel payload. */

lib/Service/Odometry.h

@@ -126,6 +126,19 @@ class Odometry
         posY = m_posY;
     }
 
+    /**
+     * Set absolute position in coordinate system.
+     * The x- and y-axis unit is mm.
+     *
+     * @param[in] posX x-coordinate
+     * @param[in] posY y-coordinate
+     */
+    void setPosition(int32_t posX, int32_t posY)
+    {
+        m_posX = posX;
+        m_posY = posY;
+    }
+
     /**
      * Set the orientation.
      * Use this to align the Y axis to north.