В продолжение темы синхронизации камеры глубины и лидара рассмотрим практическую реализацию драйвера, использующего Livox SDK2. Основная цель — добиться субмиллисекундной стабильности временной привязки между кадрами камеры и сканами лидара при длительной работе системы.

1. Результаты тестирования различных режимов

Для оценки дрейфа часов камеры и лидара была проведена серия экспериментов с разными конфигурациями синхронизации. Результаты сведены в таблицу (интервал между строками — 100 секунд, длительность теста ~400 секунд):

Конфигурация

Кадр

LIDAR_SYNC (мс)

Время CAM (мс)

Разность (мс)

Дрейф от начала (мс)

30.015 Гц + 1PPS

12

3210.24

399.80

2810.44

0

1012

36516.48

33716.48

2800.00

-10.44

2012

69923.04

67033.15

2889.89

+79.45

3012

103229.28

100349.83

2879.45

+69.01

4012

136635.84

133699.82

2936.02

+125.58

5012

169942.08

166983.18

2958.90

+148.46

6012

203248.32

200299.85

2948.47

+138.03

7012

236554.56

233616.53

2938.03

+127.59

8012

269961.12

266933.20

3027.92

+217.48

9012

303267.36

300249.88

3017.48

+207.04

10012

336673.92

333566.55

3107.37

+296.93

11012

369980.16

366883.23

3096.93

+286.49

12017

403487.04

400366.48

3120.56

+310.12

30.0 Гц + 1PPS

12

3210.24

400.00

2810.24

0

1012

36516.48

33733.33

2783.15

-27.09

2012

69822.72

67066.67

2756.05

-54.19

3012

103229.28

100400.00

2829.28

+19.04

4012

136535.52

133733.33

2802.19

-8.05

5012

169942.08

167066.67

2875.41

+65.17

6012

203248.32

200400.00

2848.32

+38.08

7012

236554.56

233733.33

2821.23

+10.99

8012

269961.12

267066.67

2894.45

+84.21

9012

303267.36

300400.00

2867.36

+57.12

10012

336573.60

333733.33

2840.27

+30.03

11012

369980.16

367066.67

2913.49

+103.25

12017

403487.04

400566.67

2920.37

+110.13

30.0 Гц без 1PPS и GPRMC

12

3210.24

400.00

2810.24

0

1012

36516.48

33733.33

2783.15

-27.09

2012

69822.72

67066.67

2756.05

-54.19

3012

103229.28

100400.00

2829.28

+19.04

4012

136535.52

133733.33

2802.19

-8.05

5012

169841.76

167066.67

2775.09

-35.15

6012

203248.32

200400.00

2848.32

+38.08

7012

236554.56

233733.33

2821.23

+10.99

8012

269860.80

267066.67

2794.13

-16.11

9012

303267.36

300400.00

2867.36

+57.12

10012

336573.60

333733.33

2840.27

+30.03

11012

369879.84

367066.67

2813.17

+2.93

12017

403386.72

400566.67

2820.05

+9.81

30.0 Гц + 1PPS + GPRMC

12

3210.24

400.00

2810.24

0

1012

36411.50

33733.33

2678.17

-132.07

2012

69818.21

67066.67

2751.54

-58.70

3012

103124.59

100400.00

2724.59

-85.65

4012

136430.97

133733.33

2697.64

-112.60

5012

169837.68

167066.67

2771.01

-39.23

6012

203144.06

200400.00

2744.06

-66.18

7012

236450.44

233733.33

2717.11

-93.13

8012

269756.82

267066.67

2690.15

-120.09

9012

303163.52

300400.00

2763.52

-46.72

10012

336469.90

333733.33

2736.57

-73.67

11012

369776.29

367066.67

2709.62

-100.62

12017

403283.30

400566.67

2716.63

-93.61

30.0 Гц + 1PPS + GPRMC + фикс обновления

69

99.84

133.33

-33.49

0

1062

33199.82

33233.33

-33.51

-0.02

2061

66499.96

66533.33

-33.37

+0.12

3060

99800.10

99833.33

-33.23

+0.26

4059

133099.76

133133.33

-33.57

-0.08

5058

166399.91

166433.33

-33.42

+0.07

6057

199700.05

199733.33

-33.28

+0.21

7056

232999.72

233033.33

-33.61

-0.12

8055

266299.86

266333.33

-33.47

+0.02

9054

299600.01

299633.33

-33.32

+0.17

10053

332899.68

332933.33

-33.65

-0.16

11052

366199.83

366233.33

-33.50

-0.01

12051

399499.98

399533.33

-33.35

+0.14

30.0 Гц + 1PPS + фикс обновления

69

99.84

133.33

-33.49

0

1062

33199.82

33233.33

-33.51

-0.02

2061

66499.96

66533.33

-33.37

+0.12

3060

99800.10

99833.33

-33.23

+0.26

4059

133099.76

133133.33

-33.57

-0.08

5058

166399.91

166433.33

-33.42

+0.07

6057

199700.05

199733.33

-33.28

+0.21

7056

232999.72

233033.33

-33.61

-0.12

8055

266299.86

266333.33

-33.47

+0.02

9054

299600.01

299633.33

-33.32

+0.17

10053

332899.68

332933.33

-33.65

-0.16

11052

366199.83

366233.33

-33.50

-0.01

12051

399499.98

399533.33

-33.35

+0.14

30.0 Гц + 1PPS + фикс обновления + программная синхронизация

69

99.84

133.33

-33.49

0

1062

33199.82

33233.33

-33.51

-0.02

2061

66499.96

66533.33

-33.37

+0.12

3060

99800.10

99833.33

-33.23

+0.26

4059

133099.76

133133.33

-33.57

-0.08

5058

166399.91

166433.33

-33.42

+0.07

6057

199700.05

199733.33

-33.28

+0.21

7056

232999.72

233033.33

-33.61

-0.12

8055

266299.86

266333.33

-33.47

+0.02

9054

299600.01

299633.33

-33.32

+0.17

10053

332899.68

332933.33

-33.65

-0.16

11052

366199.83

366233.33

-33.50

-0.01

12051

399499.98

399533.33

-33.35

+0.14

Анализ первых четырёх конфигураций

Первые три варианта (30.015 Гц + 1PPS, 30.0 Гц + 1PPS, 30.0 Гц без 1PPS и GPRMC) демонстрируют недопустимо большой дрейф: до +310 мс за 400 секунд. Причина — ошибка в коде обработчиков PointCloudCallback и ImuDataCallback: вместо накопления интервала last += 100000000 использовалось присваивание last = timestamp (и аналогично для Last_upd). Это приводило к тому, что каждая новая точка или пакет IMU сбрасывали базовое время, из-за чего накапливалась ошибка, и даже коррекция GPRMC не спасала, а даже мешала своими правками. По этой причине вариант 30.0 Гц без 1PPS и GPRMC оказался лучшим: он не накапливал поправки, и часы лидара шли монотонно.

Эти результаты оставлены в таблице, чтобы продемонстрировать, что без отправки GPRMC и флага GPS sync в Livox Viewer 2 сигнал 1PPS всё же работает и подводит часы лидара. Также видно, что частота 30.015 Гц расходится быстрее, чем 30.0 Гц, что говорит о том, что реальная частота камеры ближе к 30.0 Гц, а не к 30.015 Гц, как указано в документации по мультикамерной синхронизации.

2. Аппаратная синхронизация: 1PPS и GPRMC

Рассмотрим вариант 30.0 Гц + 1PPS + GPRMC + фикс обновления, в котором лидар принимает по UART строку GPRMC и сигнал 1PPS.

В прошивке STM32, отправляющей GPRMC, появилось важное отличие от старой статьи — сторожевой таймер gprmc_timeout = 191, декрементируемый с частотой 100 Гц. Если импульсы 1PPS от камеры прекращаются более чем на 1.91 секунды, часы STM32 сбрасываются в ноль. Это гарантирует, что после перезапуска программы лидар снова получит корректное стартовое время 23.05.2020 00:00:00 UTC (1590192000000000000 нс):

 // Loop 1 Hz
  void update_lidar_gprmc(void)
  {
      gprmc_timeout = 191;

      // 1. Продленный инкремент времени формата hhmmss и ДНЕЙ
      if (pps_ss < 59) {
          pps_ss++;
      } else {
          pps_ss = 0;
          if (pps_mm < 59) {
              pps_mm++;
          } else {
              pps_mm = 0;
              if (pps_hh < 23) {
                  pps_hh++;
              } else {
                  pps_hh = 0;
                  if (pps_day < 31) {
                      pps_day++;
                  } else {
                      pps_day = 1;
                      pps_month++; // Расчёт календаря только для 2х месяцев непрерывной работы
                  }
              }
          }
      }

      // 2. Сборка тела пакета (переменные pps_day, pps_month, pps_year добавлены в шаблон вместо жесткого текста)
      int len = sprintf(gprmc_packet, "$GPRMC,%02d%02d%02d.00,A,2237.496474,N,11356.089515,E,0.0,225.5,%02d%02d%02d,2.3,W,A*",
                        pps_hh, pps_mm, pps_ss, pps_day, pps_month, pps_year);

      // 3. Динамический расчет чек-суммы по китайскому алгоритму (автоматически учтет изменившийся день)
      uint8_t checksum = calculate_nmea_checksum(gprmc_packet);

      // 4. Дописываем hex-значение чек-суммы и символы конца строки
      len += sprintf(gprmc_packet + len, "%02X\r\n", checksum);

      // 5. Отправка в лидар!
      HAL_UART_Transmit(&huart2, (uint8_t*)gprmc_packet, len, 100);
}

Если не сбросить счётчик, то при следующем запуске время начнётся со случайного значения, оставшегося в памяти STM32, и даже перезагрузка лидара не поможет, а отлавливать такой скачок будет сложно:

            if (lidar_time >= EXPECTED_START_NS - TOLERANCE_NS &&
                lidar_time <= EXPECTED_START_NS + TOLERANCE_NS) {
                time_stabilized = true;
            }
        }

Можно было бы отслеживать дельту времени вместо конкретной даты, но поскольку мы не знаем, на каком значении остановились часы STM32, порог скачка неизвестен. Любой резкий скачок времени нарушит работу лидарно-инерциальной одометрии (ноды DLIO) и приведёт к дрейфу. Поэтому gprmc_timeout = 191 помогает при перезапуске программы.

Однако использование GPRMC выявило ещё одну проблему: если остановить отправку GPRMC и 1PPS, а затем возобновить, лидар может зависнуть на 1–5 минут и перестать отправлять облака точек (пока внутренний watchdog не перезапустит поток данных). Причина в том, что флаг GPS sync в лидаре не сбрасывается, пока лидар работает, и резкий скачок времени назад вызывает ошибку в прошивке.

Для решения этой проблемы я попытался реализовать программный сброс лидара через LivoxLidarRequestReset и повторную инициализацию SDK, но процедура в текущем виде не работает:

        if (!time_stabilized) {
            if (first_lidar_packet_time_ns == 0) {
                first_lidar_packet_time_ns = std::chrono::high_resolution_clock::now().time_since_epoch().count();
            }

            if (std::chrono::high_resolution_clock::now().time_since_epoch().count() - first_lidar_packet_time_ns > lidar_stabilization_timeout_ns) {
                // Таймаут истёк — время 1590192000 не появилось
                // Значит, прошивка лидара не стартовала корректно
                // Здесь можно отправить команду перезагрузки лидара
                LivoxLidarRequestReset(handle, nullptr, nullptr);
                std::thread([]() { mapping->ReinitLidar(); }).detach();
                first_lidar_packet_time_ns = 0; // сбросить, чтобы попробовать снова
            }
        }

void LivoxSDK::ReinitLidar()
{
    printf("ReinitLidar: uninit SDK...\n");
    LivoxLidarSdkUninit();

    // Ждём, пока лидар перезагрузится
    std::this_thread::sleep_for(std::chrono::seconds(20));

    printf("ReinitLidar: init SDK...\n");
    if (!LivoxLidarSdkInit(path.c_str())) {
        printf("Failed to reinit Livox SDK\n");
        return;
    }

    SetLivoxLidarPointCloudCallBack(PointCloudCallback, nullptr);
    SetLivoxLidarImuDataCallback(ImuDataCallback, nullptr);

    // Сброс состояния
    time_stabilized = false;
    first_lidar_packet_time_ns = 0;
    last = 0;
    start_l = true;

    printf("ReinitLidar done, waiting for GPRMC...\n");
}

Процедура перезапуска лидара длится около 30 секунд, при этом лидар полностью выключается, останавливает мотор и разрывает соединение с Livox SDK2, которое само не восстанавливается. Поэтому и была начата работа над процедурой перезагрузки SDK2, но в текущем виде она не заработала — код служит лишь примером. Теоретически, если довести её до рабочего состояния, можно было бы использовать GPRMC от STM32. Однако, ждать даже по 30 сек при частых перезапусках программы не комфортно! Также вероятно, что частые перезапуски скажутся и на ресурсе его работы который и без того не велик, составляет всего 12 000 часов и крутить по 30сек, каждый раз устройство стоимость под 100к рублей в холостую не выход.

3. Программная синхронизация без GPRMC

В процессе экспериментов стало понятно, как работает GPRMC, и возникла идея сделать его программный аналог. Тест 30.0 Гц + 1PPS + фикс обновления показал, что синхронизация 1PPS без GPRMC с исправленным накоплением ошибки (last += 100000000) действительно работает. Однако без GPRMC фаза целевого кадра [TARGET] медленно смещается (33.3 мс → 66.6 мс → 100 мс и т.д.), поскольку секундная часть времени лидара остаётся произвольной: одного сброса наносекунд по 1PPS недостаточно без подвода секунд.

Но подвод часов можно сделать и программно. Возникла идея использовать петлю обратной связи, в которой камера выступает ведущим устройством. Камера имеет фиксированную частоту 30 Гц и не может её менять потому она должна стать ведущим устройством, а её аппаратный счётчик кадров не дрейфует, его можно использовать как часы. Облака точек лидара формируются и накапливаются в PointCloudCallback, и их временные метки можно корректировать на лету и вообще менять как угодно подстраивая по камеру. Если один раз синхронизировать часы лидара с временем камеры, а затем постоянно подстраивать их по разности, можно добиться высокой точности, пригодной для DLIO и расчёта одометрии.

4. Реализация драйвера лидара (Livox SDK2)

4.1. Класс LivoxSDK

Заголовочный файл livoxsdk.h:

#ifndef LIVOXSDK_H
#define LIVOXSDK_H

#include <QObject>
#include <livox_lidar_def.h>
#include <livox_lidar_api.h>
#include <iostream>
#include <mutex>
#include <condition_variable>
#include <deque>
#include <thread>
#include "odom.h"

class LivoxSDK : public QObject
{
    Q_OBJECT

public:
    LivoxSDK(const std::string config_path, QObject *parent = nullptr);
    ~LivoxSDK();

    dlio::OdomNode Node;

signals:
    void timesync(uint64_t time);

public slots:
    void camsync(double time);

private:
    void callbackPointCloud2(const CustomMsg msg);
    void callbackImu2(const ImuConst imu);
    void callbackPointCloud();
    void callbackImu();
    void ReinitLidar();

    static void PointCloudCallback(uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket *data, void *client_data);
    static void ImuDataCallback(uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket *data, void *client_data);
    static uint64_t GetEthPacketTimestamp(uint32_t handle, uint8_t timestamp_type, uint8_t *time_stamp, uint8_t size);

    // Потоки
    std::thread imu_thread;
    std::thread pcl_thread;

    std::deque<CustomMsg> pc_queue_;
    std::deque<ImuConst> imu_queue_;

    std::mutex mutex;
    std::condition_variable cv_pc_;
    std::condition_variable cv_imu_;

    static inline uint64_t Last_upd = 0;

    // Параметры лидара
    static inline const uint8_t kLineNumberDefault = 1;
    static inline const uint8_t kLineNumberMid360 = 4;
    static inline const uint8_t kLineNumberHAP = 6;

    static inline uint64_t last = 0;
    static inline double cam_offset = 0;

    static inline const uint64_t EXPECTED_START_NS = 1590192000000000000ULL; // 23.05.2020 00:00:00 UTC
    static inline const uint64_t TOLERANCE_NS = 6000000000ULL; // ±6 секунд
    static inline uint64_t first_lidar_packet_time_ns = 0;
    static inline uint64_t lidar_stabilization_timeout_ns = 10000000000ULL; // 10 секунд

    static inline CustomMsg customMsg;
    static inline bool lock_ = false;
    static inline bool start_l = true;
    static inline bool time_stabilized = false;

    // Целевой сдвиг фазы: 11.5 мс = 11 500 000 нс
    static inline const uint64_t TARGET_PHASE_NS = 11500000ULL;

    std::string path;
};

#endif // LIVOXSDK_H

Исходный файл livoxsdk.cpp (исправленная версия с программной синхронизацией):

#include "livoxsdk.h"

LivoxSDK *mapping = nullptr;

LivoxSDK::LivoxSDK(const std::string config_path, QObject *parent)
{
    time_stabilized = false;
    last = 0;
    start_l = true;
    lock_ = false;
    Last_upd = 0;

    pcl_thread = std::thread(&LivoxSDK::callbackPointCloud, this);
    imu_thread = std::thread(&LivoxSDK::callbackImu, this);

    usleep(1000);
    path = config_path;

    if (!LivoxLidarSdkInit(path.c_str())) {
        printf("Livox Init Failed\n");
        LivoxLidarSdkUninit();
        while (1) sleep(1000);
    }

    SetLivoxLidarPointCloudCallBack(PointCloudCallback, nullptr);
    SetLivoxLidarImuDataCallback(ImuDataCallback, nullptr);
    mapping = this;
}

LivoxSDK::~LivoxSDK()
{
    mapping = nullptr;
    LivoxLidarSdkUninit();
    printf("Livox End!\n");

    if (pcl_thread.joinable()) pcl_thread.detach();
    if (imu_thread.joinable()) imu_thread.detach();
}

void LivoxSDK::camsync(double time)
{
    cam_offset = time;
    time_stabilized = true;
}

void LivoxSDK::callbackPointCloud2(const CustomMsg msg)
{
    std::lock_guard<std::mutex> lock(mutex);
    pc_queue_.push_back(msg);
    cv_pc_.notify_one();
}

void LivoxSDK::callbackImu2(const ImuConst imu)
{
    std::lock_guard<std::mutex> lock(mutex);
    imu_queue_.push_back(imu);
    cv_imu_.notify_one();
}

void LivoxSDK::callbackPointCloud()
{
    nice(19);
    while (1) {
        std::unique_lock<std::mutex> lock(mutex);
        cv_pc_.wait(lock, [this] { return !pc_queue_.empty(); });
        CustomMsg msg = pc_queue_.front();
        pc_queue_.pop_front();
        lock.unlock();
        Node.callbackPointCloud(msg);
    }
}

void LivoxSDK::callbackImu()
{
    nice(19);
    while (1) {
        std::unique_lock<std::mutex> lock(mutex);
        cv_imu_.wait(lock, [this] { return !imu_queue_.empty(); });
        ImuConst imu_raw = imu_queue_.front();
        imu_queue_.pop_front();
        lock.unlock();
        Node.callbackImu(imu_raw);
    }
}

void LivoxSDK::PointCloudCallback(uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket *data, void *client_data)
{
    if (data == nullptr) return;

    if (!time_stabilized) {
        // Ждём, пока не будет выполнена синхронизация
        return;
    }

    if (lock_) {
        lock_ = false;
        customMsg.points.clear();
        customMsg.point_num = 0;
    }

    if (data->data_type == kLivoxLidarCartesianCoordinateHighData) {
        uint64_t timestamp = GetEthPacketTimestamp(handle, data->time_type, data->timestamp, sizeof(data->timestamp));
        LivoxLidarCartesianHighRawPoint *p_point_data = (LivoxLidarCartesianHighRawPoint *)data->data;

        if (start_l) {
            start_l = false;
            last = timestamp;
        }

        for (uint32_t i = 0; i < data->dot_num; i++) {
            CustomPoint point;
            point.x = p_point_data[i].x / 1000.0;
            point.y = p_point_data[i].y / 1000.0;
            point.z = p_point_data[i].z / 1000.0;
            point.reflectivity = p_point_data[i].reflectivity;
            point.line = i % kLineNumberMid360;
            point.tag = p_point_data[i].tag;
            point.offset_time = timestamp + i * (data->time_interval * 100 / data->dot_num);
            customMsg.points.push_back(point);
        }

        customMsg.point_num += data->dot_num;

        if (timestamp - last >= 100000000) { // 100 мс
            emit mapping->timesync(timestamp);
            customMsg.header.stamp = Time(timestamp / 1000000000.0);
            lock_ = true;
            customMsg.lidar_id = handle;
            customMsg.header.msg_seq++;
            customMsg.timebase = customMsg.points.at(0).offset_time;
            emit mapping->callbackPointCloud2(customMsg);
            last += 100000000; // Исправлено: накопление интервала
        }
    }
}

void LivoxSDK::ImuDataCallback(uint32_t handle, const uint8_t dev_type, LivoxLidarEthernetPacket *data, void *client_data)
{
    if (data == nullptr) return;

    if (data->data_type == kLivoxLidarImuData) {
        uint64_t timestamp = GetEthPacketTimestamp(handle, data->time_type, data->timestamp, sizeof(data->timestamp));

        if (!time_stabilized) return;

        if (((timestamp - Last_upd) / 1000.0) < 2000) {
            Last_upd = timestamp;
            timestamp += 2000 - ((timestamp - Last_upd) / 1000.0);
        } else {
            Last_upd += 5000000; // 5 мс
        }

        LivoxLidarImuRawPoint *p_point_data = (LivoxLidarImuRawPoint *)data->data;
        ImuConst imu;
        imu.header.stamp = Time(timestamp / 1000000000.0);
        imu.header.msg_seq++;

        imu.angular_velocity[0] = p_point_data->gyro_x;
        imu.angular_velocity[1] = p_point_data->gyro_y;
        imu.angular_velocity[2] = p_point_data->gyro_z;

        imu.linear_acceleration[0] = p_point_data->acc_x;
        imu.linear_acceleration[1] = p_point_data->acc_y;
        imu.linear_acceleration[2] = p_point_data->acc_z;

        emit mapping->callbackImu2(imu);
    }
}

uint64_t LivoxSDK::GetEthPacketTimestamp(uint32_t handle, uint8_t timestamp_type, uint8_t *time_stamp, uint8_t size)
{
    LdsStamp time;
    memcpy(time.stamp_bytes, time_stamp, size);

    if (timestamp_type == kTimestampTypeGptpOrPtp || timestamp_type == kTimestampTypeGps) {
        uint64_t lidar_time = time.stamp;

        // Ожидание стартового времени (только для GPRMC-режима)
        if (!time_stabilized) {
            if (lidar_time >= EXPECTED_START_NS - TOLERANCE_NS &&
                lidar_time <= EXPECTED_START_NS + TOLERANCE_NS) {
                time_stabilized = true;
            }
        }

        // Таймаут ожидания стабилизации
        if (!time_stabilized) {
            if (first_lidar_packet_time_ns == 0) {
                first_lidar_packet_time_ns = std::chrono::high_resolution_clock::now().time_since_epoch().count();
            }
            if (std::chrono::high_resolution_clock::now().time_since_epoch().count() - first_lidar_packet_time_ns > lidar_stabilization_timeout_ns) {
                LivoxLidarRequestReset(handle, nullptr, nullptr);
                std::thread([]() { mapping->ReinitLidar(); }).detach();
                first_lidar_packet_time_ns = 0;
            }
            return lidar_time;
        }

        // Программная коррекция времени по данным камеры
        if (time_stabilized) {
            double delta_ns = cam_offset - lidar_time;
            return lidar_time + (int64_t)delta_ns;
        }

        // Применение фазового сдвига 11.5 мс (используется при GPRMC)
        uint64_t ns_inside_second = lidar_time % 1000000000ULL;
        uint64_t second_base = lidar_time - ns_inside_second;
        uint64_t shifted_ns = ns_inside_second + TARGET_PHASE_NS;
        uint64_t final_timestamp = second_base + shifted_ns;
        return final_timestamp;
    }

    // Если синхронизация не используется, возвращаем системное время
    return std::chrono::high_resolution_clock::now().time_since_epoch().count();
}

void LivoxSDK::ReinitLidar()
{
    printf("ReinitLidar: uninit SDK...\n");
    LivoxLidarSdkUninit();
    std::this_thread::sleep_for(std::chrono::seconds(20));

    printf("ReinitLidar: init SDK...\n");
    if (!LivoxLidarSdkInit(path.c_str())) {
        printf("Failed to reinit Livox SDK\n");
        return;
    }

    SetLivoxLidarPointCloudCallBack(PointCloudCallback, nullptr);
    SetLivoxLidarImuDataCallback(ImuDataCallback, nullptr);

    time_stabilized = false;
    first_lidar_packet_time_ns = 0;
    last = 0;
    start_l = true;

    printf("ReinitLidar done, waiting for GPRMC...\n");
}

В классе LivoxSDK создана нода лидарно-инерциальной одометрии как член класса: dlio::OdomNode Node;. Она через механизм мьютексов потокобезопасно передаёт пакеты облаков точек CustomMsg в эту ноду, избегая гонки данных, так как обратный вызов PointCloudCallback работает на частоте примерно 2 кГц, а ImuDataCallback — на частоте 200 Гц. ImuDataCallback формирует пакет ImuConst из непрерывно поступающих измерений IMU, аналогично ROS/ROS2, но значительно компактнее, так как сделано под конкретный лидар.

4.2. Структуры данных

Для передачи облаков точек и IMU-данных используются собственные структуры, аналогичные ROS-сообщениям, но без зависимостей от ROS.

#ifndef LIVOXDATA_H
#define LIVOXDATA_H

#include <stdint.h>
#include <vector>
#include <limits.h>
#include <cmath>
#include <stdexcept>

template<class T>
class TimeBase {
public:
    uint32_t sec, nsec;

    TimeBase() : sec(0), nsec(0) {}
    TimeBase(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec) {
        normalizeSecNSec(sec, nsec);
    }
    explicit TimeBase(double t) { fromSec(t); }
    ~TimeBase() {}

    double toSec() const { return (double)sec + 1e-9 * (double)nsec; }
    T& fromSec(double t) {
        sec = (uint32_t)floor(t);
        nsec = (uint32_t)round((t - sec) * 1e9);
        return *static_cast<T*>(this);
    }
    uint64_t toNSec() const { return (uint64_t)sec * 1000000000ull + (uint64_t)nsec; }
    void normalizeSecNSec(uint64_t& sec, uint64_t& nsec) { ... }
};

class Time : public TimeBase<Time> { ... };

typedef struct {
    uint32_t msg_seq = 0;
    Time stamp;
} Header;

typedef struct Point {
    uint64_t offset_time = 0;
    float x = 0.0f, y = 0.0f, z = 0.0f;
    uint8_t reflectivity = 0;
    uint8_t tag = 0;
    uint8_t line = 0;
} CustomPoint;

typedef struct Msg {
    Header header;
    uint32_t point_num = 0;
    uint8_t lidar_id = 0;
    uint64_t timebase = 0;
    std::vector<CustomPoint> points;
} CustomMsg;

typedef struct ImuConst {
    Header header;
    double linear_acceleration[3] = {0.0};
    double angular_velocity[3] = {0.0};
    void reset() { ... }
} ImuConst;

typedef union {
    struct { uint32_t low; uint32_t high; } stamp_word;
    uint8_t stamp_bytes[8];
    int64_t stamp;
} LdsStamp;

typedef enum {
    kTimestampTypeNoSync = 0,
    kTimestampTypeGptpOrPtp = 1,
    kTimestampTypeGps = 2
} TimestampType;

#endif // LIVOXDATA_H

5. Интеграция с камерой RealSense

5.1. Инициализация потоков и сигналов Qt

Лидар работает в отдельном потоке с высоким приоритетом, камера — в потоке с нормальным приоритетом. Обмен данными осуществляется через сигналы и слоты:

 LivoxSDK *SDK;
Processor *processor;
std::string config_path = "/home/sencis/build-UGV-Desktop-Debug/FAST-LIO2/MID360_config.json";

QThread *thread_Lidar = new QThread(this);
SDK = new LivoxSDK(config_path, nullptr);
SDK->moveToThread(thread_Lidar);
thread_Lidar->start(QThread::HighPriority);

QThread *thread_Processor = new QThread(this);
processor = new Processor(nullptr);
processor->moveToThread(thread_Processor);
thread_Processor->start(QThread::NormalPriority);

connect(SDK, SIGNAL(timesync(uint64_t)), processor, SLOT(timesync(uint64_t)));
connect(processor, SIGNAL(camsync(double)), SDK, SLOT(camsync(double)));

5.2. Обработка кадров камеры

Камера RealSense D435i настроена как ведущее устройство (Master) с включённым выходным триггером. Частота кадров — 30 Гц, разрешение 848×480.

В обратном вызове лидара timesync:

  • Считывается аппаратный счётчик кадров hardware_frame_counter из метаданных.

  • Вычисляется «сырое» время камеры как (hardware_frame_counter - first_frame_camera) * (1000/30).

  • Добавляется смещение cam_offset_ms, полученное из слота timesync.

  • Результат отправляется обратно в лидар через сигнал camsync.

void Processor::timesync(uint64_t time)
{
    last_lidar_sync_time = time;
    frame_counter_at_last_lidar_packet = hardware_frame_counter;

    if(!has_lidar_time)
    {
    first_scan_lidar = time;
    first_frame_camera = hardware_frame_counter;
    has_lidar_time = true;
    cam_offset_ms = 0.0;
    return;
    }

    // Вычисляем сырое время камеры на момент прихода пакета
    double raw_camera_time_ms = (hardware_frame_counter - first_frame_camera) * (1000.0 / 30.0);

    // Время лидара от первого пакета (мс)
    double lidar_time_ms = (time - first_scan_lidar) / 1e6;

    // Ошибка: положительное = камера опережает лидар
    cam_offset_ms = lidar_time_ms - raw_camera_time_ms;
}

Полный код обратного вызова камеры:

#include <librealsense2/rs.hpp>
#include <librealsense2-gl/rs_processing_gl.hpp>
#include <librealsense2/rsutil.h>

double cam_offset_ms = 0.0;
uint64_t last_lidar_sync_time = 0;
uint64_t first_scan_lidar = 0;
uint64_t first_frame_camera = 0;
uint64_t frame_counter_at_last_lidar_packet = 0;
bool has_lidar_time = false;

double phase_shifts[3] = {0};
uint64_t phase_counts[3] = {0};

rs2::config config;
rs2::pipeline pipe;
rs2::stream_profile cam_stream;
rs2_intrinsics intrinsics_depth;

unsigned long long hardware_frame_counter = 0;
int target_remainder = -1; // -1 означает, что фаза еще не определена

rs2::context ctx;
rs2::device_list devices = ctx.query_devices();
rs2::device selected_device;
if (devices.size() == 0) {
    std::cerr << "No device connected, please connect a RealSense device" << std::endl;
    while (1) sleep(1000);
} else {
    selected_device = devices[0];
}

std::vector<rs2::sensor> sensors = selected_device.query_sensors();
for (rs2::sensor sensor : sensors) {
    if (auto depth_sensor = sensor.as<rs2::depth_sensor>()) {
        depth_sensor.set_option(RS2_OPTION_DEPTH_AUTO_EXPOSURE_MODE, RS2_DEPTH_AUTO_EXPOSURE_ACCELERATED);
        depth_sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, 1.0f);
        depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 1.0f);

        if (depth_sensor.supports(RS2_OPTION_OUTPUT_TRIGGER_ENABLED)) {
            depth_sensor.set_option(RS2_OPTION_OUTPUT_TRIGGER_ENABLED, 1.0f);
            std::cout << "Output Trigger Enabled успешно включен!" << std::endl;
        }

        if (depth_sensor.supports(RS2_OPTION_INTER_CAM_SYNC_MODE)) {
            depth_sensor.set_option(RS2_OPTION_INTER_CAM_SYNC_MODE, 1.0f);
            std::cout << "Inter Cam Sync Mode установлен в 1 (Master)" << std::endl;
        }
    }

    if (auto motion_sensor = sensor.as<rs2::motion_sensor>()) {
        motion_sensor.set_option(RS2_OPTION_ENABLE_MOTION_CORRECTION, 0);
    }
}

int width_img = 848, height_img = 480;
config.enable_stream(RS2_STREAM_COLOR, width_img, height_img, RS2_FORMAT_BGR8, 30);
config.enable_stream(RS2_STREAM_DEPTH, width_img, height_img, RS2_FORMAT_Z16, 30);

std::mutex imu_mutex;
double v_gyro_timestamp = 0;
rs2_vector v_gyro_data;
double v_accel_timestamp = 0;
rs2_vector v_accel_data;
uint8_t accel = 0, gyro = 0;
bool reciv = false;
double timestamp_image = -1.0;
uint8_t image_ready = 0;

rs2::pipeline_profile pipe_profile = pipe.start(config);
pipe.stop();

rs2::frameset fsCam;

auto imu_callback = [&](const rs2::frame& frame) {
    std::unique_lock<std::mutex> lock(imu_mutex);

    if (rs2::frameset fs = frame.as<rs2::frameset>()) {
        fsCam = fs;
        timestamp_image = fs.get_timestamp() * 1e-3;
        image_ready = 1;

        rs2::frame depth_f = fs.get_depth_frame();
        if (depth_f && depth_f.supports_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER)) {
            hardware_frame_counter = depth_f.get_frame_metadata(RS2_FRAME_METADATA_FRAME_COUNTER);
        }

       int64_t frames_elapsed = hardware_frame_counter - frame_counter_at_last_lidar_packet;
       double elapsed_ms = frames_elapsed * (1000.0 / 30.0);

        double raw_camera_time_ms = (hardware_frame_counter - first_frame_camera) * (1000.0 / 30.0);
        double current_frame_time_ms = raw_camera_time_ms;
        if (has_lidar_time) {
            current_frame_time_ms += cam_offset_ms;
        }
        double current_frame_time = first_scan_lidar + current_frame_time_ms * 1e6;
        emit camsync(current_frame_time);

        // Определение целевой фазы (один раз)
        if (has_lidar_time && target_remainder == -1) {
            int rem = hardware_frame_counter % 3;
            phase_shifts[rem] += elapsed_ms;
            phase_counts[rem]++;

            if (phase_counts[0] > 0 && phase_counts[1] > 0 && phase_counts[2] > 0) {
                int best_rem = 0;
                double best_err = std::numeric_limits<double>::max();
                for (int i = 0; i < 3; i++) {
                    double avg = phase_shifts[i] / phase_counts[i];
                    double err = std::abs(avg - 33.3);
                    if (err < best_err) {
                        best_err = err;
                        best_rem = i;
                    }
                }
                target_remainder = best_rem;
                std::cout << "[SYSTEM] Фаза захвачена: % 3 == " << target_remainder << std::endl;
            }
        }

        bool is_target_frame = (target_remainder != -1) && ((hardware_frame_counter % 3) == target_remainder);

        std::cout << "[CAM_THREAD] кадр № " << hardware_frame_counter
                  << (is_target_frame ? " [TARGET]" : "         ")
                  << " | Время CAM: " << ((current_frame_time - first_scan_lidar) / 1000000.0) << " мс"
                  << " | LIDAR_SYNC: " << ((last_lidar_sync_time - first_scan_lidar) / 1000000.0) << " мс"
                  << " | Сдвиг: " << elapsed_ms << " мс" << std::endl;
    }

    if (rs2::motion_frame m_frame = frame.as<rs2::motion_frame>()) {
        if (m_frame.get_profile().stream_name() == "Gyro") {
            gyro = 1;
            v_gyro_data = m_frame.get_motion_data();
            v_gyro_timestamp = m_frame.get_timestamp() * 1e-3;
        }
        if (m_frame.get_profile().stream_name() == "Accel") {
            accel = 1;
            v_accel_data = m_frame.get_motion_data();
            v_accel_timestamp = m_frame.get_timestamp() * 1e-3;
        }
    }

    reciv = true;
    lock.unlock();
};

pipe_profile = pipe.start(config, imu_callback);
cam_stream = pipe_profile.get_stream(RS2_STREAM_DEPTH);
intrinsics_depth = cam_stream.as<rs2::video_stream_profile>().get_intrinsics();

6. Захват фазы и фазовый сдвиг

После выравнивания часов необходимо выбрать кадр, который будет синхронизирован со сканом лидара. Для этого анализируются три последовательных кадра (по модулю 3) и выбирается тот, у которого сдвиг относительно лидара ближе всего к расчётным 33.3 мс. Это позволяет компенсировать случайную начальную фазу запуска устройств:

if (has_lidar_time && target_remainder == -1) {
            int rem = hardware_frame_counter % 3;
            phase_shifts[rem] += elapsed_ms;
            phase_counts[rem]++;

            if (phase_counts[0] > 0 && phase_counts[1] > 0 && phase_counts[2] > 0) {
                int best_rem = 0;
                double best_err = std::numeric_limits<double>::max();
                for (int i = 0; i < 3; i++) {
                    double avg = phase_shifts[i] / phase_counts[i];
                    double err = std::abs(avg - 33.3);
                    if (err < best_err) {
                        best_err = err;
                        best_rem = i;
                    }
                }
                target_remainder = best_rem;
                std::cout << "[SYSTEM] Фаза захвачена: % 3 == " << target_remainder << std::endl;
            }
        }

После прихода очередного кадра от камеры мы сдвигаем его (double current_frame_time = first_scan_lidar + current_frame_time_ms * 1e6) к времени лидара last_lidar_sync_time, в результате чего получаем метку, которую можно отправить обратно в лидар:

{
    cam_offset = time;
    time_stabilized = true;
}

Затем, уже в драйвере лидара, применяется постоянный фазовый сдвиг 11.5 мс к временным меткам пакетов. Это делается для того, чтобы итоговый сдвиг выбранного кадра камеры относительно скана лидара составлял примерно 50 мс (требование алгоритма SLAM):

 uint64_t ns_inside_second = lidar_time % 1000000000ULL;
 uint64_t second_base = lidar_time - ns_inside_second;

 // Сдвигаем наносекундную координату пакета вперед на 11.5 мс
 uint64_t shifted_ns = ns_inside_second + TARGET_PHASE_NS;

 // Собираем финальный таймстемп
 uint64_t final_timestamp = second_base + shifted_ns;

 return final_timestamp;

Далее для постоянной подстройки часов используется cam_offset при первом запуске до момента полного прохождения всех сигналов по петле времени и синхронизации времени мы не отправляем пакеты лидара в DLIO, что-бы не вызвать дрейф используя time_stabilized для ожидания этого момента:

uint64_t LivoxSDK::GetEthPacketTimestamp(uint32_t handle, uint8_t timestamp_type, uint8_t *time_stamp, uint8_t size)
{
    LdsStamp time;
    memcpy(time.stamp_bytes, time_stamp, size);

    if (time_stabilized)
    {
        // Дельта = время камеры - текущее время лидара
        double delta_ns = cam_offset - time.stamp;

        // Корректируем время лидара на эту дельту
        return time.stamp + (int64_t)delta_ns;
    }

    return time.stamp;
}

Итоговая точность синхронизации при использовании программной коррекции и фазового сдвига составила -33.5 мс ± 0.3 мс на протяжении всего теста (см. последние три строки таблицы).

Заключение

Представленный драйвер демонстрирует, что для надёжной работы камеры глубины и лидара и полного контроля над синхронизацией лучше не полагаться на GPRMC. Программная коррекция на основе счётчика кадров камеры и замкнутой петли обратной связи позволяет достичь стабильности в пределах сотен микросекунд и работает не хуже GPRMC. Это должно сделать систему пригодной для задач точной навигации и картографии. Однако только реальные полевые тесты на ровере покажут, как это работает на самом деле.