lidar-sync-mimics-gps icon indicating copy to clipboard operation
lidar-sync-mimics-gps copied to clipboard

Open-Source LiDAR Time Synchronization System by Mimicking GPS-clock

Open-Source LiDAR Time Synchronization System by Mimicking GNSS-clock1

The LiDAR-to-IMU time synchronized system mimicing a GNSS-supplied clock interface by a microcontroller-powered platform that provides 1 microsecond synchronization precision.

To clone the repository with proper submodules utilize --recurse-submodules argument:

git clone --recurse-submodules https://github.com/MobileRoboticsSkoltech/lidar-sync-mimics-gps.git

System requirements

The project was tested using:

  • Ubuntu 18.06
  • ROS Melodic Morenia
  • STM32CubeIDE for Linux

Hardware

Lidar Velodyne VLP-16
MCU-platform with built-in programmer STM32F4DISCOVERY
IMU MPU-9150
UART-to-USB stick any, we used stick based on CP2102 stone
Signal inverter any, see Note about MCU to Lidar data signal inverter below

The STM32 MCU-platform is chosen as it meets all the requirements described in the paper. The IMU is fed by external MCU reference clock for data rate stability.

Firmware

The MCU firmware aimed on IMU-data gathering and LiDAR-to-IMU time synchronization. It is designed using STM32CubeIDE for Linux. The main source code is placed in main.c while the other setups are stored in firmware.ioc and setup through the IDE GUI. It means there is no need to edit other source files except two mentioned.

IMU data format

The MCU outputs IMU data as are plain ASCII strings that can be used independently on ROS and may be parsed by another data recording/processing API. These strings contain timestamp (minutes, seconds, subseconds (1/25600000 of second) and IMU data (3D-angular velocity, temperature, 3D-acceleration) with format:

"i0%02x %02x %04x %04x %04x %04x %04x %04x %04x %04x %04x %04x\n"
 \/\__/ \__/ \__/ \_______/ \____________/ \__/ \____________/
 | hour mins secs  subsecs     3D gyro     temp     3D acc
IMU0 identifier for compatibility with other projects

Software

The software consists of ROS drivers for handling Lidar and IMU data and precise timestamping. Lidar ROS driver is based on common ROS package with our patch for hardware timestamping by mimicking GNSS-clock.
IMU ROS driver is developed from scratch and produces sensor_msgs/Imu messages.

Wiring

MCU pin Name Role
PB6 I2C1_SCL I2C SCL line for IMU module data transfer
PB7 I2C1_SDA I2C SDA line for IMU module data transfer
PH0 RCC_OSC_IN Crystal resonator connection, already connected to MCU by default
PH1 RCC_OSC_OUT Crystal resonator connection, already connected to MCU by default
PA0 TIM5_CH1 Output compare signal, Not used
PA6 TIM3_CH1 Reference clock source for IMU
PE9 TIM1_CH1 PPS signal output
PD12 GPIO_OUT Debugging LED. IMU interrupt came
PD13 GPIO_OUT Debugging LED. Toggling in the main loop (`while`)
PD14 GPIO_OUT Debugging LED. Not used
PD15 GPIO_OUT Debugging LED. Not used
PC9 GPIO_EXTI9 Interrupt input pin from IMU module. IMU trigger this pin when new data sample is ready
PC10 UART4_TX UART Transmit line to PC through UART-to-USB stick
PC11 UART4_RX UART Receive line from PC through UART-to-USB stick, not used
PC12 UART5_TX UART Transmit MCU clock line to Lidar through signal inverter (see note below)
PD2 UART5_RX UART Receive line from Lidar, not used

TIM5 (RTC) and TIM1(PPS signal generator, NMEA message transmission trigger) now share three tasks explained in the article. For optimization, only single timer can be utilized for all this tasks. However, to keep compatibility with current uasge of the firmware in ongoing projects, we do not plan to update it.

Wiring result schematically should look as shown in the figure:

Note about MCU to Lidar data signal inverter

Page 43 of section 7.4.3 Timing and Polarity Requirements VLP-16 User Manual states the following:

The serial connection for the NMEA message follows the RS232 standard. The interface is capable of handling voltages between ±15 VDC.

  • Low voltages are marks and represent a logical 1.
  • High voltages are spaces and represent a logical 0.

The serial line idle state (MARK) is a low voltage indicating a logical 1. When the start bit is asserted, the positive voltage will be asserted representing a logical 0.

This means that the polarity of the input UART signal must be inverse: "1" is "Low", "0" is "High". For that case some UART transmitters can provide such a signal polarity inversion. However, UART transmitter of STM32F4 MCU cannot. To solve the issue, separate signal invertor can be utilized that simply converts UART signal to the inverse one:

HIGH---    ┌─────────┐    ┌───────┐                   ───┐         ┌────┐       ┌──────
           │         │    │       │         ---->        │         │    │       │
LOW---- ───┘         └────┘       └──────                └─────────┘    └───────┘

Single Schmitt-trigger inverter SN74LVC1G14 is a good choice for that purpose, we used it in a board.
Googling of schmitt inverter, 74HC14 can help.

In case of questions

Any question — raise an issue, please.

1 :

@misc{faizullin2021opensource,
  title={Open-Source LiDAR Time Synchronization System by Mimicking GNSS-clock}, 
  author={Marsel Faizullin and Anastasiia Kornilova and Gonzalo Ferrer},
  year={2021},
  eprint={2107.02625},
  archivePrefix={arXiv},
  primaryClass={cs.RO}
}