智慧驾培首次提交

yy1717

2022-12-08 f7a18ec4494b9c5c9ef3fd440bbf68ffc6425e18

 lib/src/main/cpp/rtk_module/rtk.cpp

@@ -4,10 +4,10 @@

#include <cstring>
#include <cstdio>
#include <pthread.h>
#include <cmath>
#include <cstdlib>
#include <cctype>
#include <thread>
#include "rtk.h"
#include "parse_gps.h"
#include "../common/serial_port.h"
@@ -20,10 +20,9 @@
#include "../native-lib.h"
#include "virtual_rtk.h"
#include "../mcu/mcu_if.h"
#include "../mcu/ahp.h"

#define DEBUG(fmt, args...)     LOGD("<rtk> <%s>: " fmt, __func__, ##args)

#define RTK_MODULE_UART         UART_0

#define PARSE_BUFF_SIZE         4096

@@ -41,64 +40,17 @@
static gpsStatus_t gpsStatus;
static char rtkModel[32] = {0};

static int (*WriteRtk)(int id, const void *buf, int len);

static rtk_info CurrRTKInfo;
static bool needSetPjk = false;
static int lostCnt;

static void CheckPjkParam(void);
static void CheckPjkParamTimeout(union sigval sig);
static void CheckPjkParamTimeout(apptimer_var_t val);
static int WriteBluetooth(int id, const void *buf, int len);
static void GetModuleVersion(void);

static void VersionTimeout(union sigval sig);
static void GpsDataTimeout(union sigval sig);

static void *UartThread(void *p);

void RtkCommModeSel(int mode)
{
    if (mode == 0) {
        WriteRtk = WriteSerialPort;
    } else {
        WriteRtk = WriteBluetooth;
    }
}

void ConfigRTKModule(bool ayDevice)
{
    // TODO
    DEBUG("ConfigRTKModule");

    memset(&CurrRTKInfo, 0, sizeof(CurrRTKInfo));
    CurrRTKInfo.hh = -1;

    memset(&gpsStatus, 0, sizeof(gpsStatus));
    gpsStatus.hh = -1;

    if (ayDevice) {
        RtkCommModeSel(0);

        static struct serial_config serialConfig;

        strcpy(serialConfig.name, "/dev/ttyHSL0");
        serialConfig.baud = 115200;
        serialConfig.data_bit = 8;
        serialConfig.verify_bit = 'N';
        serialConfig.stop_bit = 1;
        serialConfig.flow_ctrl = 0;

        pthread_t pid;
        pthread_attr_t attr;
        pthread_attr_init(&attr);
        pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);//detached
        pthread_create(&pid, &attr, UartThread, &serialConfig);
    } else {
        RtkCommModeSel(1);
        // 等待蓝牙连接
    }
}
static void VersionTimeout(apptimer_var_t val);
static void GpsDataTimeout(apptimer_var_t val);

// 蓝牙连接后
void ConfigRTKModuleLater(void)
@@ -111,12 +63,12 @@

void FactorySettings(void)
{
    WriteRtk(RTK_MODULE_UART, FACTORY, strlen(FACTORY));
    WriteRtkCommand(FACTORY, strlen(FACTORY));
}

void RebootModule(void)
{
    WriteRtk(RTK_MODULE_UART, REBOOT, strlen(REBOOT));
    WriteRtkCommand(REBOOT, strlen(REBOOT));
}

void handleRTKRebootComp(const struct nmea *s)
@@ -147,8 +99,8 @@

void SetAYFactoryParam(int freq)
{
    WriteRtk(RTK_MODULE_UART, UNLOGALL, strlen(UNLOGALL));
    WriteRtk(RTK_MODULE_UART, IFCOM2, strlen(IFCOM2));
    WriteRtkCommand(UNLOGALL, strlen(UNLOGALL));
    WriteRtkCommand(IFCOM2, strlen(IFCOM2));

    if (freq == 0)
        freq = 5;
@@ -156,13 +108,13 @@
    for (int i = 0; i < sizeof(PJKITEMS)/ sizeof(PJKITEMS[0]); ++i) {
        char cmd[64];
        sprintf(cmd, "log com1 %s ontime %0.1f\r\n", PJKITEMS[i], 1.0/(double)freq);
        WriteRtk(RTK_MODULE_UART, cmd, strlen(cmd));
        WriteRtkCommand(cmd, strlen(cmd));
    }

    for (int i = 0; i <  sizeof(GPSITEMS)/ sizeof(GPSITEMS[0]); ++i) {
        char cmd[64];
        sprintf(cmd, "log com1 %s ontime %0.1f\r\n", GPSITEMS[i], 1.0/(double)freq);
        WriteRtk(RTK_MODULE_UART, cmd, strlen(cmd));
        WriteRtkCommand(cmd, strlen(cmd));
    }

//    WriteSerialPort(RTK_MODULE_UART, AY_PJKPARAM, strlen(AY_PJKPARAM));
@@ -175,7 +127,7 @@

    sprintf(buff, "set pjkpara 6378137 298.257223563 0 %d 0 500000\r\n", centLon);

    WriteRtk(RTK_MODULE_UART, buff, strlen(buff));
    WriteRtkCommand(buff, strlen(buff));

    DEBUG("%s", buff);
}
@@ -185,79 +137,10 @@
    data = gpsStatus;
}

static void *UartThread(void *p) {
    struct serial_config *cfg = (struct serial_config *) p;

    int res = InitSerialPort(RTK_MODULE_UART, cfg->baud, cfg->data_bit, cfg->verify_bit, cfg->stop_bit, cfg->flow_ctrl);
    DEBUG("Serial %s open %d", cfg->name, res);
    uint8_t RxBuf[PARSE_BUFF_SIZE];
    int RxBufLen = 0;

    if (res == 0) {
//        CheckPjkParam();
        GetModuleVersion();
    }

    while (res == 0) {
        int ul = ReadSerialPort(RTK_MODULE_UART, (uint8_t *)RxBuf + RxBufLen, sizeof(RxBuf) - RxBufLen);
        RxBufLen += ul;

        /*{
            static char buffd[16384];

            buffd[0] = 0;
            int i = 0;
            for (i = 0; i < ul; i++) {
                if ((i % 32) == 0) {
                    sprintf(buffd + strlen(buffd), "\n");
                }
                sprintf(buffd + strlen(buffd), "%02X ", RxBuf[i]);
                if (strlen(buffd) > 800) {
                    DEBUG("%s <- %s...", "UART", buffd);
                    buffd[0] = 0;
                }
            }
            if (strlen(buffd) > 0)
                DEBUG("%s <- %s", "UART", buffd);
        }*/

        if (RxBufLen > 0) {
#if 1
            if (VirtualIsConnected()) {     //PC模拟用时
                static bool first = false;
                if (!first) {
                    first = true;

                    uint8_t buff[33] = "emulator";
                    buff[32] = 1;
                    PlatformStatusChanged(RTK_STATUS_EVT, buff, 33);
                }
                RxBufLen = 0;
            } else {
                const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
                if (ptr != RxBuf) {
                    memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
                    RxBufLen -= ptr - RxBuf;
                } else if (RxBufLen == PARSE_BUFF_SIZE) {        //填满了，且没有一个\r，都抛弃
                    DEBUG("Parse GPS error");
                    RxBufLen = 0;
                }
            }
#else
            RxBufLen = 0;           //PC模拟用时
#endif
        }
    }
    if (res == 0) {
        UninitSerialPort(RTK_MODULE_UART);
    }
    pthread_exit(NULL);
}

void handleUnrecognisedNMEA(const uint8_t *data, uint16_t length) {
    char buff[4096] = {0};
    memcpy(buff, data, MIN(length, 4000));
//    DEBUG("handleUnrecognisedNMEA: %s", buff);
    DEBUG("handleUnrecognisedNMEA: %s", buff);

    if (length >= 100) {
        string cs(buff);
@@ -280,12 +163,20 @@
    }
}

static bool bbs = false;

void handleGPGGA(const struct nmea *s)
{
    static uint32_t qfCnt = 0;

//    DEBUG("handleGPGGA num = %d", s->nmea_num);
    if (s->nmea_num >= 10) {

        if (!bbs) {
            bbs = true;
            RebootModule();
        }

        lostCnt = 0;
        AppTimer_delete(GpsDataTimeout);
        AppTimer_add(GpsDataTimeout, D_SEC(5));
@@ -325,6 +216,7 @@

        // 计算中央子午线
        int qf = str2int(s->nmea_value[5].data, s->nmea_value[5].length);

        if (qf > 0) {
            qfCnt++;
            if (needSetPjk && qfCnt >= 3) {
@@ -430,34 +322,6 @@
    str2float(&CurrRTKInfo.y, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&CurrRTKInfo.x, s->nmea_value[4].data, s->nmea_value[4].length);

    static double sbx = -12;
    static double sby = 27;

//    CurrRTKInfo.x = (-2.8984 - 4.913)/2;
//    CurrRTKInfo.y = (31.6962 + 29.4974)/2;

    sbx += 0.01;
    sby += 0.01;

//    const double by1 = 28.013;
//    const double bx1 = -11.9669;
//
//    const double by2 = 29.3232;
//    const double bx2 = -9.5057;
//
//    static double xx = -10.9669, yy = 28.013;
//
//    CurrRTKInfo.y = yy;
//    CurrRTKInfo.x = xx;
//
//    if (forwardx) {
//        xx += 0.02;
//        yy += 0.02 * (by2 - by1) / (bx2 - bx1);
//    } else {
//        xx -= 0.02;
//        yy -= 0.02 * (by2 - by1) / (bx2 - bx1);
//    }

    if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
//        UpdateRTKInfo(&CurrRTKInfo);
@@ -484,13 +348,6 @@
    str2float(&CurrRTKInfo.pitch, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&CurrRTKInfo.roll, s->nmea_value[3].data, s->nmea_value[3].length);

//    CurrRTKInfo.heading = 60;

    static double deg = 0;
//    CurrRTKInfo.heading = deg;
    deg += 2.0;
    if (deg >= 360) deg = 0;

    if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
//        UpdateRTKInfo(&CurrRTKInfo);
@@ -507,15 +364,13 @@

static void CheckPjkParam(void)
{
    WriteRtk(RTK_MODULE_UART, INQ_PJK_PARAM, strlen(INQ_PJK_PARAM));
    WriteRtkCommand(INQ_PJK_PARAM, strlen(INQ_PJK_PARAM));
    DEBUG("获取PJK参数...");
    AppTimer_delete(CheckPjkParamTimeout);
    AppTimer_add(CheckPjkParamTimeout, D_SEC(3));
}

static void CheckPjkParamTimeout(union sigval sig) {
    AppTimer_delete(CheckPjkParamTimeout);

static void CheckPjkParamTimeout(apptimer_var_t val) {
    DEBUG("获取PJK参数超时");

    uint8_t buff[33];
@@ -529,7 +384,7 @@
static int WriteBluetooth(int id, const void *buf, int len)
{
//    SendToBluetooth((uint8_t *)buf, len);
    SendMcuCommand(0x000B, (uint8_t *)buf, len);
//    SendMcuCommand(0x000B, (uint8_t *)buf, len);
    return len;
}

@@ -537,14 +392,13 @@
{
    AppTimer_delete(VersionTimeout);
    AppTimer_add(VersionTimeout, D_SEC(3));
    WriteRtk(RTK_MODULE_UART, CMD_VERSION, strlen(CMD_VERSION));
    WriteRtkCommand(CMD_VERSION, strlen(CMD_VERSION));

    DEBUG("获取版本...");
}

static void VersionTimeout(union sigval sig)
static void VersionTimeout(apptimer_var_t val)
{
    AppTimer_delete(VersionTimeout);
    DEBUG("版本获取超时");
    GetModuleVersion();

@@ -554,10 +408,8 @@
    PlatformStatusChanged(RTK_STATUS_EVT, buff, 33);
}

static void GpsDataTimeout(union sigval sig)
static void GpsDataTimeout(apptimer_var_t val)
{
    AppTimer_delete(GpsDataTimeout);

    if (++lostCnt >= 3) {
        DEBUG("RTK模块收不到GPS数据");
        GetModuleVersion();