added couple new Channels

Author: Tobias Haeckel

lib/APPRemoteControl/src/App.cpp

@@ -64,6 +64,7 @@ static void App_cmdChannelCallback(const uint8_t* payload, const uint8_t payload
 static void App_motorSpeedSetpointsChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
 static void App_statusChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
 static void App_robotSpeedSetpointChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
+static void App_timeSyncReqChannelCallback(const uint8_t* payload, const uint8_t payloadSize, void* userData);
 
 /******************************************************************************
  * Local Variables
@@ -78,6 +79,7 @@ App::App() :
     m_serialMuxProtChannelIdCurrentVehicleData(0U),
     m_serialMuxProtChannelIdStatus(0U),
     m_serialMuxProtChannelIdLineSensors(0U),
+    m_serialMuxProtChannelIdTimeSyncRsp(0U),
     m_systemStateMachine(),
     m_controlInterval(),
     m_reportTimer(),
@@ -340,6 +342,9 @@ void App::reportVehicleData()
     averageCounts = m_movAvgProximitySensor.write(maxCounts);
 
     odometry.getPosition(xPos, yPos);
+
+    payload.timestamp   = static_cast<int64_t>(millis());
+
     payload.xPos        = xPos;
     payload.yPos        = yPos;
     payload.orientation = odometry.getOrientation();
@@ -361,6 +366,7 @@ bool App::setupSerialMuxProt()
     m_smpServer.subscribeToChannel(MOTOR_SPEED_SETPOINT_CHANNEL_NAME, App_motorSpeedSetpointsChannelCallback);
     m_smpServer.subscribeToChannel(STATUS_CHANNEL_NAME, App_statusChannelCallback);
     m_smpServer.subscribeToChannel(ROBOT_SPEED_SETPOINT_CHANNEL_NAME, App_robotSpeedSetpointChannelCallback);
+    m_smpServer.subscribeToChannel(TIME_SYNC_REQUEST_CHANNEL_NAME, App_timeSyncReqChannelCallback);
 
     /* Channel creation. */
     m_serialMuxProtChannelIdRemoteCtrlRsp =
@@ -369,10 +375,13 @@ bool App::setupSerialMuxProt()
         m_smpServer.createChannel(CURRENT_VEHICLE_DATA_CHANNEL_NAME, CURRENT_VEHICLE_DATA_CHANNEL_DLC);
     m_serialMuxProtChannelIdStatus      = m_smpServer.createChannel(STATUS_CHANNEL_NAME, STATUS_CHANNEL_DLC);
     m_serialMuxProtChannelIdLineSensors = m_smpServer.createChannel(LINE_SENSOR_CHANNEL_NAME, LINE_SENSOR_CHANNEL_DLC);
+    m_serialMuxProtChannelIdTimeSyncRsp =
+        m_smpServer.createChannel(TIME_SYNC_RESPONSE_CHANNEL_NAME, TIME_SYNC_RESPONSE_CHANNEL_DLC);
 
     /* Channels succesfully created? */
     if ((0U != m_serialMuxProtChannelIdCurrentVehicleData) && (0U != m_serialMuxProtChannelIdRemoteCtrlRsp) &&
-        (0U != m_serialMuxProtChannelIdStatus) && (0U != m_serialMuxProtChannelIdLineSensors))
+        (0U != m_serialMuxProtChannelIdStatus) && (0U != m_serialMuxProtChannelIdLineSensors) &&
+        (0U != m_serialMuxProtChannelIdTimeSyncRsp))
     {
         isSuccessful = true;
     }
@@ -483,4 +492,42 @@ void App_robotSpeedSetpointChannelCallback(const uint8_t* payload, const uint8_t
         /* Set the robot speeds. */
         DrivingState::getInstance().setRobotSpeeds(centerSpeed, angularSpeed);
     }
+}
+
+/**
+ * Receives time sync requests and responds with timestamps.
+ *
+ * @param[in] payload       TimeSyncRequest structure.
+ * @param[in] payloadSize   Size of TimeSyncRequest structure.
+ * @param[in] userData      Instance of App class.
+ */
+void App_timeSyncReqChannelCallback(const uint8_t* payload,
+                                    uint8_t        payloadSize,
+                                    void*          userData)
+{
+    const uint32_t t2 = millis();
+    if ((payload == nullptr) ||
+        (userData == nullptr) ||
+        (payloadSize != TIME_SYNC_REQUEST_CHANNEL_DLC))
+    {
+        return;
+    }
+
+    App* app = static_cast<App*>(userData);
+
+    TimeSyncRequest req;
+    memcpy(&req, payload, sizeof(req));
+
+    app->handleTimeSyncRequest(req, t2);
+}
+
+void App::handleTimeSyncRequest(const TimeSyncRequest& req, const uint32_t t2)
+{
+    TimeSyncResponse rsp{req.seq, req.t1_ms, t2, millis()};
+
+    if (m_serialMuxProtChannelIdTimeSyncRsp == 0U)
+    {
+        return;
+    }
+    (void)m_smpServer.sendData(m_serialMuxProtChannelIdTimeSyncRsp, &rsp, sizeof(rsp));
 }

lib/APPRemoteControl/src/App.h

@@ -97,6 +97,13 @@ class App
      */
     void systemStatusCallback(SMPChannelPayload::Status status);
 
+    /**
+     * Handle a Time Sync request and send response via SerialMuxProt.
+     *
+     * @param[in] req Time sync request payload.
+     */
+    void handleTimeSyncRequest(const TimeSyncRequest& req, uint32_t t2);
+
 private:
     /** Differential drive control period in ms. */
     static const uint32_t DIFFERENTIAL_DRIVE_CONTROL_PERIOD = 5U;
@@ -133,6 +140,9 @@ class App
     /** SerialMuxProt Channel id for sending line sensors data. */
     uint8_t m_serialMuxProtChannelIdLineSensors;
 
+    /** SerialMuxProt Channel id for sending time sync responses. */
+    uint8_t m_serialMuxProtChannelIdTimeSyncRsp;
+
     /** The system state machine. */
     StateMachine m_systemStateMachine;
 

lib/APPRemoteControl/src/SerialMuxChannels.h

@@ -88,6 +88,18 @@
 /** DLC of Line Sensor Channel */
 #define LINE_SENSOR_CHANNEL_DLC (sizeof(LineSensorData))
 
+/** Name of the Channel to receive Time Sync Request from the DCS. */
+#define TIME_SYNC_REQUEST_CHANNEL_NAME "TIME_SYNC_REQ"
+
+/** DLC of Time Sync Request Channel */
+#define TIME_SYNC_REQUEST_CHANNEL_DLC (sizeof(TimeSyncRequest))
+
+/** Name of the Channel to send Time Sync Response to the DCS. */
+#define TIME_SYNC_RESPONSE_CHANNEL_NAME "TIME_SYNC_RSP"
+
+/** DLC of Time Sync Response Channel */
+#define TIME_SYNC_RESPONSE_CHANNEL_DLC (sizeof(TimeSyncResponse))
+
 /******************************************************************************
  * Types and Classes
  *****************************************************************************/
@@ -196,6 +208,7 @@ typedef struct _RobotSpeed
 /** Struct of the "Current Vehicle Data" channel payload. */
 typedef struct _VehicleData
 {
+    int64_t                  timestamp;   /**< Timestamp [ms]. */
     int32_t                  xPos;        /**< X position [mm]. */
     int32_t                  yPos;        /**< Y position [mm]. */
     int32_t                  orientation; /**< Orientation [mrad]. */
@@ -217,8 +230,24 @@ typedef struct _LineSensorData
     uint16_t lineSensorData[5U]; /**< Line sensor data [digits] normalized to max 1000 digits. */
 } __attribute__((packed)) LineSensorData;
 
+/** Struct of the "Time Sync Request" channel payload. */
+typedef struct _TimeSyncRequest
+{
+    uint32_t seq;  /**< Sequence number to match request/response. */
+    uint32_t t1_ms;/**< Timestamp at sender when request left [ms]. */
+} __attribute__((packed)) TimeSyncRequest;
+
+/** Struct of the "Time Sync Response" channel payload. */
+typedef struct _TimeSyncResponse
+{
+    uint32_t seq;   /**< Sequence number to match request/response. */
+    uint32_t t1_ms; /**< Echo of request timestamp [ms]. */
+    uint32_t t2_ms; /**< Timestamp at receiver when request arrived [ms]. */
+    uint32_t t3_ms; /**< Timestamp at receiver when response sent [ms]. */
+} __attribute__((packed)) TimeSyncResponse;
+
 /******************************************************************************
  * Functions
  *****************************************************************************/
 
-#endif /* SERIAL_MUX_CHANNELS_H_ */
+#endif /* SERIAL_MUX_CHANNELS_H_ */