add clock synchronization feature (NTP)

CMakeLists.txt

@@ -1,26 +1,38 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(lms1xx)
 
-# Build ROS-independent library.
+
 find_package(console_bridge REQUIRED)
-include_directories(include ${console_bridge_INCLUDE_DIRS})
-add_library(LMS1xx src/LMS1xx.cpp)
-target_link_libraries(LMS1xx ${console_bridge_LIBRARIES})
 
 # Regular catkin package follows.
 find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs)
-catkin_package(CATKIN_DEPENDS roscpp)
+catkin_package(
+INCLUDE_DIRS
+    include
+LIBRARIES  
+    lms1xx
+CATKIN_DEPENDS roscpp)
 
 include_directories(include ${catkin_INCLUDE_DIRS})
 add_executable(LMS1xx_node src/LMS1xx_node.cpp)
-target_link_libraries(LMS1xx_node LMS1xx ${catkin_LIBRARIES})
+target_link_libraries(LMS1xx_node lms1xx ${catkin_LIBRARIES})
 
+add_executable(LMS1xx_NTP_diagnostic src/LMS1xx_NTP_diagnostic.cpp)
+target_link_libraries(LMS1xx_NTP_diagnostic lms1xx ${catkin_LIBRARIES})
+# Build ROS-independent library.
+include_directories(include ${console_bridge_INCLUDE_DIRS})
+add_library(lms1xx SHARED src/LMS1xx.cpp)
+target_link_libraries(lms1xx ${console_bridge_LIBRARIES})
 
-install(TARGETS LMS1xx LMS1xx_node
+install(TARGETS lms1xx LMS1xx_node LMS1xx_NTP_diagnostic
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
+install(DIRECTORY include/lms1xx/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE)
 
 install(DIRECTORY meshes launch urdf
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

include/LMS1xx/LMS1xx.h → include/lms1xx/LMS1xx.h

@@ -24,8 +24,8 @@
 #ifndef LMS1XX_H_
 #define LMS1XX_H_
 
-#include <LMS1xx/lms_buffer.h>
-#include <LMS1xx/lms_structs.h>
+#include <lms1xx/lms_buffer.h>
+#include <lms1xx/lms_structs.h>
 #include <string>
 #include <stdint.h>
 
@@ -61,6 +61,15 @@ class LMS1xx
   */
   void connect(std::string host, int port = 2111);
 
+  /*!
+  * @brief get max time offset and delay.
+  */
+  NTPstatus getNTPstatus() const;
+
+  /*!
+  * @brief set NTP settings.
+  */
+  void setNTPsettings(const NTPcfg &cfg);
   /*!
   * @brief Disconnect from LMS1xx device.
   */
@@ -167,11 +176,13 @@ class LMS1xx
   * @brief Receive single scan message.
   * @param data pointer to scanData buffer structure.
   */
-  static void parseScanData(char* buf, scanData* data);
+  void parseScanData(char* buf, scanData* data);
 
+private:
   bool connected_;
   LMSBuffer buffer_;
   int socket_fd_;
+  char* strtok_saveptr;
 };
 
 #endif /* LMS1XX_H_ */

include/LMS1xx/lms_structs.h → include/lms1xx/lms_structs.h

@@ -59,6 +59,67 @@ struct scanCfg
   int stopAngle;
 };
 
+/*!
+* @class NTProleCfg
+* @brief Structure containing NTP role configuration.
+*
+* @author Wojciech Dudek
+*/
+
+struct NTPcfg
+{
+  /*!
+   * @brief NTP role.
+   * None: 0
+   * Client: 1
+   * Server: 2
+   */
+  int NTProle;
+  /*!
+   * @brief Time synchronization interface.
+   * Ethernet: 0
+   * CAN: 1
+   */
+  int timeSyncIfc;
+  /*!
+   * @brief Time server IP address.
+   * [0].[1].[2].[3]
+   */
+  int serverIP[4];
+  /*!
+   * @brief Time zone.
+   * Set values in number of hours relative to GMT, hex specially coded
+   * [GMT + …] -12d … +12d (00h … 18h)
+   */
+  int timeZone;
+  /*!
+   * @brief Update time.
+   * Set values in seconds
+   */
+  int updateTime;
+};
+
+/*!
+* @class NTProleCfg
+* @brief Structure containing NTP status.
+*
+* @author Wojciech Dudek
+*/
+
+struct NTPstatus
+{
+  /*!
+   * @brief Read maximum offset time.
+   * [Seconds as float according to IEEE754]
+   */
+  float maxOffsetNTP;
+  /*!
+   * @brief Delay time.
+   * [Seconds as float according to IEEE754]
+   */
+  float timeDelay;
+};
+
 /*!
 * @class scanDataCfg
 * @brief Structure containing scan data configuration.
@@ -201,6 +262,24 @@ struct scanData
    *
    */
   uint16_t rssi2[1082];
+
+  /*!
+   * @brief Time stamp of the msg - seconds
+   *
+   */
+  time_t msg_sec;
+
+  /*!
+   * @brief Time stamp of the msg - micro seconds
+   *
+   */
+  uint32_t msg_usec;
+
+  /*!
+   * @brief Time since start up - micro seconds
+   *
+   */
+  uint32_t msg_startup_usec;
 };
 
 #endif  // LMS1XX_LMS_STRUCTS_H_

launch/LMS1xx.launch

@@ -1,6 +1,19 @@
 <launch>
   <arg name="host" default="192.168.1.14" />
-  <node pkg="lms1xx" name="lms1xx" type="LMS1xx_node">
-    <param name="host" value="$(arg host)" />
-  </node>
+  <arg name="useNTP" default="false" />
+  <arg name="NTProle" default="1" />
+  <arg name="timeSyncIfc" default="0" />
+  <arg name="serverIP" default="192.168.1.1" />
+  <arg name="timeZone" default="1" />
+  <arg name="updateTime" default="40" />
+
+    <node pkg="lms1xx" name="lms1xx" type="LMS1xx_node">
+       <param name="host" value="$(arg host)" />
+       <param name="useNTP" value="$(arg useNTP)" />
+       <param name="NTProle" value="$(arg NTProle)" />
+       <param name="timeSyncIfc" value="$(arg timeSyncIfc)" />
+       <param name="serverIP" value="$(arg serverIP)" />
+       <param name="timeZone" value="$(arg timeZone)" />
+       <param name="updateTime" value="$(arg updateTime)" />
+    </node>
 </launch>