add driver_gps_cookoo_f9k. not complete.

src/driver/driver_gps_cookoo_f9k/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/driver/driver_gps_cookoo_f9k/cookoo_sdk.h

@@ -0,0 +1,492 @@
+/*
+ * Copyright : Cookoo Science, All Rights Reserved ©2022
+ *
+ * cookoo_sdk.h
+ *
+ * Author cookoo
+ */
+#ifndef COOKOO_SDK
+#define COOKOO_SDK
+
+/* COOKOO SDK 设备接口
+*    设备包含:
+*    CAN   * 5
+*        CAN0-CAN1 支持波特率, 支持扩展帧 或者使用linux/can/raw
+*        CAN2-CAN4 固定波特率500k, 不支持设置波特率, 不支持扩展帧
+*    IO    * 8
+         输入口 * 4
+		 输出口 * 4
+*    ADC   * 2
+*
+*    RS232 * 2
+*        封装MCU上的232,
+*        封装ARM上的232设备，可使用本接口，或使用/dev/下的设备符
+*    RS485 * 1
+*        封装ARM上的485设备，可使用本接口，或使用/dev/下的设备符
+*
+*    相机 * 8
+*        封装ARM上的video(/dev/video*)
+*/
+namespace cookoo {
+
+// CAN设备
+class Ck_Can
+{
+public:
+    // COOKOO CAN设备ID*5
+    enum CK_CAN {
+        CK_CAN0 = 0,
+        CK_CAN1,
+        CK_CAN2,
+        CK_CAN3,
+        CK_CAN4,
+    };
+    struct ck_can_frame {
+        uint32_t can_id;  /* 32 bit CAN_ID + EFF/RTR/ERR flags */
+        uint8_t  can_dlc; /* frame payload length in byte (0 .. CAN_MAX_DLEN) */
+        uint8_t  data[8] __attribute__((aligned(8)));
+    };
+
+public:
+    /**
+     * @fn     (Ck_Can)
+     * @brief  
+     * @param  CAN通道号, 波特率
+     * @return None
+     */
+	Ck_Can(CK_CAN can_ch, int rate);
+
+	/**
+     * @fn     (~Ck_Can)
+     * @brief  
+     * @param  None
+     * @return None
+     */
+	~Ck_Can();
+
+public:
+    /**
+    * @fn     (Ck_Can::Read)
+    * @brief  数据读取
+    * @param  data: can frame to read,
+    * @return 读取长度
+    */
+	int Read(ck_can_frame& data);
+
+	/**
+    * @fn     (Ck_Can::Write)
+    * @brief  数据写入
+    * @param  data: can frame to read,
+    * @return 写入长度
+    */
+	int Write(const ck_can_frame& data);
+
+private:
+    // 对象不支持 拷贝/移动/赋值
+	Ck_Can(const Ck_Can&) = delete;
+	Ck_Can(Ck_Can&&) = delete;
+	void operator=(const Ck_Can&) = delete;
+	void operator=(Ck_Can&&) = delete;
+private:
+    void* m_performer = nullptr;
+};
+
+// GPIO+AD
+class Ck_Pin
+{
+public:
+    // GPIO * 8, 输入口*4, 输出口*4
+    enum CK_PIN {
+        CK_PIN_I1,
+        CK_PIN_I2,
+        CK_PIN_I3,
+        CK_PIN_I4,
+        CK_PIN_O1,
+        CK_PIN_O2,
+        CK_PIN_O3,
+        CK_PIN_O4
+    };
+
+    // AD 电压检测口*2
+    enum CK_AD {
+        CK_AD_1,
+        CK_AD_2,
+    };
+
+public:
+	/**
+    * @fn     (Ck_Pin)
+    * @brief
+    * @param  None
+    * @return None
+    */
+    Ck_Pin();
+
+	/**
+    * @fn     (~Ck_Pin)
+    * @brief
+    * @param  None
+    * @return None
+    */
+    ~Ck_Pin();
+
+	/**
+    * 暂不支持
+    * @fn     (Ck_Pin::Set)
+    * @brief  设置管脚-只支持O1~O4设置
+    * @param  idx: GPIO index, value: 1-High, 0-Low
+    * @return true:OK, false:Failed
+    */
+    bool  Set(CK_PIN idx, bool value);
+
+	/**
+    * @fn     (Ck_Pin::Get)
+    * @brief  获取GPIO状态
+    * @param  idx: GPIO index
+    * @return 1:High, 0:Low
+    */
+    bool  Get(CK_PIN idx);
+
+	/**
+    * @fn     (Ck_Pin::Get)
+    * @brief  获取AD电压
+    * @param  idx: AD index
+    * @return 电压值 单位V
+    */
+	float Get(CK_AD ad_id);
+
+    const char* Check();
+
+private:
+    // 对象不支持 拷贝/移动/赋值
+	Ck_Pin(const Ck_Pin&) = delete;
+	Ck_Pin(Ck_Pin&&) = delete;
+	void operator=(const Ck_Pin&) = delete;
+	void operator=(Ck_Pin&&) = delete;
+private:
+    void* m_performer = nullptr;
+};
+
+// 串口
+class Ck_Serial
+{
+public:
+    // 串口设备: RS232 * 2, RS485 * 1
+    enum CK_SERIAL {
+        CK_SERIAL_RS232_1,// ARM
+        CK_SERIAL_RS232_2,// MCU, 固定波特率
+        CK_SERIAL_RS485_0 // ARM
+    };
+
+public:
+    /**
+    * @fn     (Ck_Serial)
+    * @brief  
+    * @param  port: Serial port
+	*         rate: Serial bitrate
+    *               CK_SERIAL_RS232_0, 固定波特率, 此参数无效 
+    * @return None
+    */
+    explicit Ck_Serial(CK_SERIAL port, int rate);
+
+	/**
+    * @fn     (~Ck_Serial)
+    * @brief  
+    * @param  None
+    * @return None
+    */
+    ~Ck_Serial();
+
+public:
+/**
+    * @fn     (Ck_Serial::Read)
+    * @brief  数据读取
+    * @param  data: buffer to read,
+	*         len : max length of data
+    * @return 读取长度
+    */
+	int Read(void* data, int len);
+
+	/**
+    * @fn     (Ck_Serial::Write)
+    * @brief  数据写入
+    * @param  data: buffer to read,
+	*         len : length of data
+    * @return 写入长度
+    */
+	int Write(const void* data, int len);
+
+private:
+    // 对象不支持 拷贝/移动/赋值
+    Ck_Serial(const Ck_Serial&) = delete;
+    Ck_Serial(Ck_Serial&&) = delete;
+    void operator=(const Ck_Serial&) = delete;
+    void operator=(Ck_Serial&&) = delete;
+private:
+    void* m_performer = nullptr;
+};
+
+// 设置激光雷达电源开关
+/**
+* @fn     (Set_Power)
+* @brief  设置激光雷达供电开关
+* @param   ch:通道1-4
+           sw: 开关 0-1
+*/
+void Set_Power(uint8_t ch, bool sw);
+
+// 相机外触发
+/**
+* @fn     (Set_Poe)
+* @brief  设置POE供电开关
+* @param  sw: 开关 0-1
+*/
+void Set_Poe(bool sw);
+/**
+* @fn     (Cam_Trigger_On)
+* @brief  相机外触发开
+* @param  channel: 相机线束通道 0-1
+*         rate   : 帧率
+*         duty   : 占空比
+*/
+void Cam_Trigger_On(uint8_t channel, uint16_t rate, uint8_t duty);
+/**
+* @fn     (Cam_Trigger_Off)
+* @brief  相机外触发关
+* @param  channel: 相机线束通道 0-1
+*/
+void Cam_Trigger_Off(uint8_t channel);
+
+//相机设备
+class Ck_Cam
+{
+public:
+    /**
+    * @st     (Ck_Frame)
+    * @brief  相机图像帧数据
+    * @param  cnt: 帧序号
+    *         size:数据大小
+    *         data:数据
+    *         type:图像格式
+    *              1-YUYV
+    *              2-UYVY
+    */
+    typedef struct _st_ck_frame_
+    {
+        unsigned int cnt;
+        unsigned int size;
+        unsigned char* data;
+        unsigned char type;
+    } Ck_Frame;
+
+public:
+    /**
+    * @fn     (Ck_Cam::Ck_Cam)
+    * @brief  相机设备类构造
+    * @param  dev:设备描述符
+    *         w:分辨率-宽
+    *         h:分辨率-高
+    *         fps:帧率
+    */
+    Ck_Cam(const std::string& dev, int w, int h, int fps);
+
+    /**
+    * @fn     (Ck_Cam::~Ck_Cam)
+    * @brief  相机设备类析构
+    */
+    ~Ck_Cam();
+
+    /**
+    * @fn     (Ck_Cam::Start)
+    * @brief  相机设备对象开始运行
+    */
+    void Start();
+
+    /**
+    * @fn     (Ck_Cam::Stop)
+    * @brief  相机设备对象停止运行
+    */
+    void Stop();
+
+    /**
+    * @fn     (Ck_Cam::GetFrame)
+    * @brief  相机设备获取图像
+    * @return 图像数据       
+    */
+    Ck_Frame GetFrame();
+
+private:
+    std::string m_dev;
+    int m_w;
+    int m_h;
+    int m_fps;
+    void* m_performer = nullptr;
+
+private:
+    // 对象不支持 拷贝/移动/赋值
+    Ck_Cam(const Ck_Cam&) = delete;
+    Ck_Cam(Ck_Cam&&) = delete;
+    void operator=(const Ck_Cam&) = delete;
+    void operator=(Ck_Cam&&) = delete;
+};
+
+class Ck_GPS
+{
+public:
+    static Ck_GPS* GetInstance();
+    static void    DelInstance();
+
+    /**
+    * @fn     (Ck_GPS::Start)
+    * @brief  开始运行
+    * @param  
+    * @return None
+    */
+	void Start();
+
+    /**
+    * @fn     (Ck_GPS::Start)
+    * @brief  停止运行
+    * @param  
+    * @return None
+    */
+	void Stop();
+
+    /**
+    * @fn     (Ck_GPS::Calibrate)
+    * @brief  F9K配置标定参数
+    *         x1, y1, z1 惯导到后轮中心,参数单位为 cm
+    *         x2, y2, z2 惯导到定位天线,参数单位为 cm
+    *         e 量取杆臂值的配置误差, 可填0
+    * @param  
+    * @return None
+    */
+	void Set_Calibrate(
+        float x1, float y1, float z1, 
+        float x2, float y2, float z2, 
+        float e);
+
+    /**
+    * @fn     (Ck_GPS::Save_Calibrate)
+    * @brief  保存标定结果
+    * @return true:保存成功,false:保存失败
+    */
+    bool Save_Calibrate();
+
+    /**
+    * @fn     (Ck_GPS::Set_Ntrip)
+    * @brief  配置nrtip服务器
+    *         不使用RTK可不调用此接口
+    * @param  
+    * @return None
+    */
+	void Set_Ntrip(std::string const& ip, int port,
+      std::string const& user, std::string const& passwd,
+      std::string const& mountpoint);
+
+    /**
+    * @fn     (Ck_GPS::Set_IMU_DATA_OUT)
+    * @brief  设置IMU数据输出
+    * @param  freq 0-100, 0:不输出,其他:输出频率
+    * @return None
+    */
+	void Set_IMU_DATA_OUT(uint8_t freq);
+
+    /**
+    * @fn     (Ck_GPS::Read_GPS)
+    * @brief  定位数据读取 支持NMEA格式:GGA,RMC输出
+    * @param  None
+    * @return 定位数据
+    */
+	std::string Read_GPS();
+
+    /**
+    * @fn     (Ck_GPS::Get_IMU_STATE)
+    * @brief  获取IMU状态
+    * @return  
+    *         bit7-4 Automatic IMU-mount alignment on/off 
+    *           0:automatic alignment is not running, 
+    *           1:automatic alignment is running
+    *         bit3-0 Status of IMU-mount alignment
+    *           0:user-defined/fixed angles are used, 
+    *           1:IMU-mount roll/pitch angles alignment is ongoing, 
+    *           2:IMU-mount roll/pitch/yaw angles alignment is ongoing, 
+    *           3:coarse IMU-mount alignment are used, 
+    *           4:fine IMU-mount alignment are used
+    */
+	uint8_t Get_IMU_STATE();
+
+    /**
+    * @fn     (Ck_GPS::Read_IMU_DATA)
+    * @brief  IMU数据读取
+    * @param  acc_x m/s2
+    * @param  acc_y m/s2
+    * @param  acc_z m/s2
+    * @param  gyr_x m/s2
+    * @param  gyr_y m/s2
+    * @param  gyr_z m/s2
+    * @return none
+    */
+	void Read_IMU_DATA(float& acc_x, float& acc_y, float& acc_z, 
+        float& gyr_x, float& gyr_y, float& gyr_z);
+
+    /**
+    * @fn     (Ck_GPS::Set_SPD_IN_MOD)
+    * @brief  车速输入模式
+    * @param  mod输入模式 
+    *           0-不输入, 
+    *           1-软件接口输入
+    *           2-硬件接口输入
+    * @return none
+    */
+	void Set_SPD_IN_MOD(uint8_t mod);
+
+    /**
+    * @fn     (Ck_GPS::Send_SPD)
+    * @brief  发送车速
+    * @param  spd-车速  精度*1000 单位m/s
+    *           使用Set_SPD_IN_MOD(1)设置软件接口输入,
+    *           再使用此接口传入车速, 输入频率>=10HZ
+    * @return none
+    */
+	void Send_SPD(int spd);
+
+    /**
+    * @fn     (Ck_GPS::Set_NMEA_FREQ)
+    * @brief  设置NMEA输出频率
+    * @param  freq: 输出频率 0, 1, 10, 20, 30 Hz
+    * @return none
+    */
+	void Set_NMEA_FREQ(uint8_t freq);
+
+    /**
+    * @fn     (Ck_GPS::Set_NMEA_MSG)
+    * @brief  设置NMEA报文输出开关
+    * @param  name: "GGA", "RMC"...
+    *         output: 0-关, 1-开
+    * @return none
+    */
+	void Set_NMEA_MSG(const std::string& name, bool output);
+
+    /**
+     * @fn     (~Ck_GPS)
+     * @brief  
+     * @param  None
+     * @return None
+     */
+	~Ck_GPS();
+
+private:
+    Ck_GPS();
+    // 对象不支持 拷贝/移动/赋值
+	Ck_GPS(const Ck_GPS&) = delete;
+	Ck_GPS(Ck_GPS&&) = delete;
+	void operator=(const Ck_GPS&) = delete;
+	void operator=(Ck_GPS&&) = delete;
+private:
+    static Ck_GPS* m_instance;
+};
+
+}
+
+#endif /* COOKOO_SDK */

src/driver/driver_gps_cookoo_f9k/driver_gps_cookoo_f9k.pro

@@ -0,0 +1,21 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# You can make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        main.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+LIBS += -L$$PWD -lcookoosdk
+
+HEADERS += \
+    cookoo_sdk.h

src/driver/driver_gps_cookoo_f9k/main.cpp

@@ -0,0 +1,452 @@
+/*
+ * Copyright : Cookoo Science, All Rights Reserved ©2022
+ *
+ * cookoo_cam.cpp
+ *
+ * Author cookoo
+ * 本代码展示cookoo-接口+F9K的使用
+ */
+#include <stdio.h>
+#include <signal.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <stddef.h>
+#include <thread>
+#include <chrono>
+#include <iostream>
+#include "cookoo_sdk.h"
+
+using namespace cookoo;
+
+static const char* str_imu_auto_sw[] =
+{
+    "\033[41mOFF\033[0m",
+    "\033[42mON\033[0m"
+};
+
+static const char* str_imu_status[] =
+{
+    "\033[41mOFF\033[0m",
+    "\033[43mINITIALIZING\033[0m",
+    "\033[43mINITIALIZING\033[0m",
+    "\033[44mCOARSE\033[0m",
+    "\033[42mFINE\033[0m"
+};
+
+volatile bool sStop = false;
+static void sig_handler(int sign)
+{
+    switch (sign) {
+    case SIGINT:
+    case SIGTERM:
+        sStop = 1;
+        break;
+
+    case SIGALRM:
+        break;
+
+    default:
+        break;
+    }
+}
+
+// UNX Domain Socket 封装
+// #define RECV_USE_SELECT
+struct SENSSION
+{
+    void* addr;
+    int   len;
+};
+
+class Uds
+{
+public:
+    Uds();
+    ~Uds();
+
+public:
+    int Bind(const std::string& path);
+    void Close();
+    SENSSION AddSenssion(const std::string& path);
+    void DelSenssion(SENSSION& senssion);
+    int Recv(void* data, int len);
+    int Send(const SENSSION& senssion, const void* data, int len);
+
+private:
+    int m_local_fd = -1;
+    std::string m_path;
+};
+
+Uds::Uds()
+{
+}
+
+Uds::~Uds()
+{
+    Close();
+}
+
+int Uds::Bind(const std::string& path)
+{
+    if (m_local_fd > 0)
+    {
+        perror("alrady binded!");
+        return -1;
+    }
+
+    m_local_fd = socket(AF_UNIX, SOCK_DGRAM | SOCK_NONBLOCK, 0);
+    if (m_local_fd < 0)
+    {
+        perror("socket failed");
+        return -2;
+    }
+
+    struct sockaddr_un local;
+    memset(&local, 0, sizeof(local));
+    local.sun_family = AF_UNIX;
+
+    if (strlen(path.c_str()) > sizeof(local.sun_path) - 1)
+    {
+        perror("Server socket path too long");
+        close(m_local_fd);
+        return -3;
+    }
+
+    //服务器先删除了与该地址匹配的路径名，以防出现这个路径名已经存在的情况
+    char cmd[256] = {0};
+    snprintf(cmd, sizeof(cmd), "rm -rf %s > /dev/null 2>&1", path.c_str());
+    system(cmd);
+    strcpy(local.sun_path, path.c_str());
+    // delete socket file if such already exists
+    unlink(path.c_str());
+    int reuse = 1;
+    if ( setsockopt(m_local_fd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)) < 0 )
+    {
+        close(m_local_fd);
+        perror("setsockopt failed");
+        return -4;
+    }
+
+    // bind socket to address in UNIX domain
+    if ( bind(m_local_fd, (struct sockaddr*)&local, sizeof(struct sockaddr_un)) < 0 )
+    {
+        close(m_local_fd);
+        perror("error: bind failed");
+        return -5;
+    }
+
+    snprintf(cmd, sizeof(cmd), "chmod 777 %s > /dev/null 2>&1", path.c_str());
+    system(cmd);
+    m_path = path;
+
+    return 0;
+}
+
+void Uds::Close()
+{
+    unlink(m_path.c_str());
+    if (m_local_fd > 0)
+    {
+        close(m_local_fd);
+        m_local_fd = -1;
+    }
+}
+
+SENSSION Uds::AddSenssion(const std::string& path)
+{
+    SENSSION ret;
+    struct sockaddr_un* addr = new struct sockaddr_un;
+    memset(addr, 0, sizeof(struct sockaddr_un));
+    addr->sun_family = AF_UNIX;
+    strcpy(addr->sun_path, path.c_str());
+
+    ret.addr = addr;
+    ret.len  = sizeof(struct sockaddr_un);
+
+    return ret;
+}
+
+void Uds::DelSenssion(SENSSION& senssion)
+{
+    if (senssion.addr)
+    {
+        struct sockaddr_un* tmp = (struct sockaddr_un*)senssion.addr;
+        delete tmp;
+        senssion.addr = nullptr;
+    }
+    senssion.len = 0;
+}
+
+int Uds::Recv(void* data, int len)
+#ifdef RECV_USE_SELECT
+{
+    if (!data || len < 1)
+    {
+        return -1;
+    }
+
+    int ret = -1;
+    struct timeval tv;
+    fd_set readfds;
+
+    tv.tv_sec  = 0;
+    tv.tv_usec = 100;
+
+    FD_ZERO(&readfds);
+    FD_SET(m_local_fd, &readfds);
+    int nSelRet = select(m_local_fd + 1, &readfds, NULL, NULL, &tv);
+    if (nSelRet < 0 )
+    {
+        printf("Error select\n");
+    }
+    else if (nSelRet == 0)
+    {
+    }
+    else
+    {
+        if( FD_ISSET(m_local_fd, &readfds) )
+        {
+            ret = recvfrom(m_local_fd, data, len, 0, 0, 0);
+        }
+    }
+
+    return ret;
+}
+#else
+{
+    int ret = -1;
+    if (data && len > 0)
+    {
+        ret = recvfrom(m_local_fd, data, len, 0, 0, 0);
+    }
+    return ret;
+}
+#endif
+
+int Uds::Send(const SENSSION& senssion, const void* data, int len)
+{
+    int ret = -1;
+    if (senssion.addr && senssion.len > 0 && data && len > 0)
+    {
+        ret = sendto(m_local_fd, data, len, 0, (struct sockaddr*)senssion.addr, senssion.len);
+    }
+
+    return ret;
+}
+
+static float vv6Speed = 0;// 车速 km/h
+// demo程序以测试车-VV6为例从底盘CAN上获取车速
+// VV6测试使用，其他车型不适用
+void speed_vv6()
+{
+    cookoo::Ck_Can can0(cookoo::Ck_Can::CK_CAN(0), 500000);
+    cookoo::Ck_Can::ck_can_frame can_recv = {0};
+    uint8_t DIR;
+
+    while (!sStop)
+    {
+        //VV6读车速--方向
+        if ( can0.Read(can_recv) > 0 )
+        {
+            if(can_recv.can_id == 0x240)
+            {
+                //车速 km/h
+                vv6Speed = (((can_recv.data[7] & 0xF8) >> 3) | (can_recv.data[6] << 5)) * 0.05625f;
+                // 倒车
+                DIR = can_recv.data[1] & 0x03;
+                if (0x02 == DIR)
+                {
+                    vv6Speed = -vv6Speed;
+                }
+            }
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+}
+//通过OBD获得车速
+int obdSpeed = 0;
+void speed_obd()
+{
+    cookoo::Ck_Can can0(cookoo::Ck_Can::CK_CAN(1), 500000);
+    cookoo::Ck_Can::ck_can_frame can_send = {0};
+    cookoo::Ck_Can::ck_can_frame can_recv = {0};
+    can_send.can_id  = 0x07DF;
+    can_send.can_dlc = 8;
+    can_send.data[0] = 0x02;
+    can_send.data[1] = 0x01;
+    can_send.data[2] = 0x0D;
+    can_send.data[3] = 0x00;
+    can_send.data[4] = 0x00;
+    can_send.data[5] = 0x00;
+    can_send.data[6] = 0x00;
+    can_send.data[7] = 0x00;
+
+    uint32_t cnt = 0;
+    while (!sStop)
+    {
+        //发送OBD查询车速
+        if (++cnt % 50 == 0)
+        {
+            can0.Write(can_send);
+        }
+        //读车速
+        if ( can0.Read(can_recv) > 0 )
+        {
+            if(can_recv.can_id == 0x07E8)
+            {
+                //车速 km/h
+                obdSpeed = can_recv.data[3];
+            }
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+}
+
+static inline void PrintDetail()
+{
+    printf("********************************************************************\n");
+    printf("     APP Name  : COOKOO-GPS(F9K)-DEMO\n");
+    printf("     Custom    : COOKOO\n");
+    printf("     Version   : 0.9.11\n");
+    printf("     Compile   : %s-%s\n", __DATE__, __TIME__);
+    printf("     Copyright : Cookoo Science, All Rights Reserved ©2023\n");
+    printf("*********************************************************************\n");
+}
+
+static inline void print_usage(const char* str)
+{
+    printf("=====================================================================\n");
+    printf("Usage:\n");
+    printf("  sudo %s  RUN_MOD  SPEED_MOD  [CALI_PARAM]\n", str);
+    printf("    RUN_MOD  : 1-正常模式, 2-标定模式\n");
+    printf("    SPEED_MOD: 0-不输入; 1-软件接口输入车速; 2-硬件输入脉冲\n");
+    printf("    CALI_PARAM:安装测量距离参数, 6个值, 单位cm, RUN_MOD为2时需要输入\n");
+    printf(" 例:\n");
+    printf("  正常模式+无车速 sudo %s  1  0\n", str);
+    printf("  标定模式+无车速 sudo %s  2  0  0  0  50  0  0  95\n", str);
+    printf("=====================================================================\n");
+    printf(" 步骤:\n");
+    printf(" 1. 运行本程序,输入RUN_MOD为2,进行标定\n");
+    printf(" 2. 标定根据SPEED_MOD不同分为3个模式\n");
+    printf(" 3. 运行程序标定, 以软件输入车速为例：运行程序输入 2 1 0 0 50 0 0 95\n");
+    printf(" 4. 跑车标定，左右转弯，转弯时车速尽量大一些\n");
+    printf(" 5. 查看输出状态(IMU AUTO Alignment, Alignment Status 已高亮变色),\n");
+    printf("      %s-标定中, %s-粗略标定, %s-精确标定\n", str_imu_status[1], str_imu_status[3], str_imu_status[4]);
+    printf(" 6. Ctrl+C 停止程序, 程序会自动保存当前标定结果\n");
+    printf(" 7. 停止程序后,等待30秒,下电重启机器\n");
+    printf(" 8. 上电后,再次运行程序, 本示例采用软件车速模式, 输入 1 1 运行\n");
+    printf(" 9. 跑车采集数据, 数据自动按照[GPS-{MOD}-{时间}.txt]的形式命名\n");
+    printf("10. 过程中可使用UI程序实时查看车辆行驶轨迹\n");
+    printf("11. 若车速一种车速模式测试完毕,需要测试另一个车速模式,重复步骤3-10即可\n");
+    printf("    若不需要改变车速模式,并且安装位置没有变化,重复步骤8-10即可\n");
+    printf("        标定: 2 0 或 2 1 或 2 2\n");
+    printf("        采集: 1 0 或 1 1 或 1 2\n");
+    printf("        \033[31m!注意:标定时用的哪个车速模式，采集时与之相对应\033[0m\n");
+    printf("=====================================================================\n");
+    printf("硬件输入脉冲时,需要工装配合\n");
+    printf("软件输入车速时,需要接VV6底盘CAN或OBD CAN\n");
+    printf("详细接口使用参考源码cookoo_gps.cpp\n");
+    printf("\033[31m注意!RUN_MOD输入2会清除原标定结果且不可恢复,需要重新跑车标定\033[0m\n");
+    printf("=====================================================================\n");
+}
+
+static inline std::string GetTimeStr()
+{
+    struct timeval tv;
+    gettimeofday(&tv, nullptr);
+    struct tm now_time;
+    localtime_r (&tv.tv_sec, &now_time);
+    char dateTime[30];
+    strftime(dateTime, 30, "%Y_%m_%d_%H%M%S", &now_time);
+    char str[64];
+    snprintf(str, sizeof(str),"%s_%03lu", dateTime, tv.tv_usec / 1000);
+
+    return str;
+}
+
+static inline uint64_t GetTimeU64()
+{
+    struct timeval tv;
+    gettimeofday(&tv, nullptr);
+    return (tv.tv_sec * 1000 + tv.tv_usec / 1000);
+}
+
+int main(int argc, char *argv[])
+{
+
+
+
+
+    std::string ip = "116.63.46.168";
+    int port = 8002;
+    std::string user = "63ae1792";
+    std::string passwd = "97786";
+    std::string mountpoint = "RTCM32GGB";
+
+    cookoo::Ck_GPS::GetInstance()->Set_Ntrip(ip, port, user, passwd, mountpoint);
+
+
+    // IMU数据输出,0-不输出
+    cookoo::Ck_GPS::GetInstance()->Set_IMU_DATA_OUT(1);
+
+    // 设置车速输入模式,0-不输入; 1-软件接口输入车速; 2-硬件输入脉冲
+    cookoo::Ck_GPS::GetInstance()->Set_SPD_IN_MOD(0);
+
+    // 设置NMEA输出频率30Hz
+    cookoo::Ck_GPS::GetInstance()->Set_NMEA_FREQ(30);
+    // 设置NMEA输出报文 GGA RMC ...
+    cookoo::Ck_GPS::GetInstance()->Set_NMEA_MSG("GGA", true);
+    cookoo::Ck_GPS::GetInstance()->Set_NMEA_MSG("RMC", true);
+
+    // 开始运行
+    cookoo::Ck_GPS::GetInstance()->Start();
+
+
+    int speed = 0;
+    char* uds_send_data = new char[4096];
+    // 保存的文件名
+
+    uint64_t last_time = 0;
+    uint64_t curr_time = 0;
+    uint8_t  imu_stat = 0;
+    // 读取数据
+    while (!sStop)
+    {
+        // 车速VV6底盘或者OBD
+        speed = vv6Speed;
+        // 读取GPS NMEA数据
+        std::string gps_str = cookoo::Ck_GPS::GetInstance()->Read_GPS();
+        if ( gps_str.length() )
+        {
+
+            printf("%s", gps_str.data());
+        }
+
+
+        // 获取IMU状态, 1秒1次
+        curr_time = GetTimeU64();
+        if (curr_time >= last_time + 1000)
+        {
+            last_time = curr_time;
+            imu_stat = cookoo::Ck_GPS::GetInstance()->Get_IMU_STATE();
+            // IMU状态 高4位-AUTO 自动标定开关, 低4位 3-COARSE, 4-FINE
+            printf("IMU AUTO Alignment: %s, Alignment Status: %s\n",
+                str_imu_auto_sw[imu_stat >> 4 & 0x01],
+                str_imu_status[imu_stat & 0x07]);
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+
+
+    cookoo::Ck_GPS::GetInstance()->Stop();
+    cookoo::Ck_GPS::DelInstance();
+
+    return 0;
+}