科目二修改

yy1717

2023-03-31 4bd08f0355b6b2cf3c027202d5ad301b4e182953

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

@@ -21,6 +21,7 @@
#include "virtual_rtk.h"
#include "../mcu/mcu_if.h"
#include "../mcu/ahp.h"
#include "../utils/xconvert.h"

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

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

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

// 组合PJK和TRA的消息内容
static struct {
    int year;
    int month;
    int day;
    int hour;
    int min;
    int sec;
    int millisec;
    int qf;
    double heading;
    double pitch;
    double roll;
    double x;
    double y;
} xy_temp;

static void CheckPjkParam(void);
static void CheckPjkParamTimeout(apptimer_var_t val);
@@ -312,28 +329,31 @@
    int ss = str2int(s->nmea_value[0].data + 4, 2);
    int dss = str2int(s->nmea_value[0].data + 7, 2);

    CurrRTKInfo.MM = str2int(s->nmea_value[1].data, 2);
    CurrRTKInfo.DD = str2int(s->nmea_value[1].data + 2, 2);
    CurrRTKInfo.YY = str2int(s->nmea_value[1].data + 4, 2);
    xy_temp.month = str2int(s->nmea_value[1].data, 2);
    xy_temp.day = str2int(s->nmea_value[1].data + 2, 2);
    xy_temp.year = str2int(s->nmea_value[1].data + 4, 2);

    CurrRTKInfo.qf = str2int(s->nmea_value[6].data, s->nmea_value[6].length);
    xy_temp.qf = str2int(s->nmea_value[6].data, s->nmea_value[6].length);

    // NOTE: RTK模块是以南北向为X轴，西东向为Y轴，我们交换下，以符合一般逻辑
    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);
    str2float(&xy_temp.y, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&xy_temp.x, s->nmea_value[4].data, s->nmea_value[4].length);

    if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
//        UpdateRTKInfo(&CurrRTKInfo);
//        up_num++;
        /*if ((up_num % 5) == 0)*/ {
//            NewMgrEvent(DRIVER_UPDATE_EVT);
        }
    if (xy_temp.hour == hh && xy_temp.min == mm && xy_temp.sec == ss && xy_temp.millisec == dss) {
        rtk_info_t rtk;
        rtk.qf = xy_temp.qf;
        rtk.utc_time = static_cast<int64_t>(TimeMakeComposite(2000 + xy_temp.year, xy_temp.month, xy_temp.day, xy_temp.hour, xy_temp.min, xy_temp.sec))*static_cast<int64_t>(1000) + static_cast<int64_t>(dss*10);
        rtk.x = xy_temp.x;
        rtk.y = xy_temp.y;
        rtk.heading = xy_temp.heading;
        rtk.pitch = xy_temp.pitch;
        rtk.roll = xy_temp.roll;
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&rtk, sizeof(rtk));
    }
    CurrRTKInfo.hh = hh;
    CurrRTKInfo.mm = mm;
    CurrRTKInfo.ss = ss;
    CurrRTKInfo.dss = dss;
    xy_temp.hour = hh;
    xy_temp.min = mm;
    xy_temp.sec = ss;
    xy_temp.millisec = dss;
}

void handleGPTRA(const struct nmea *s) {
@@ -344,22 +364,25 @@
    int ss = str2int(s->nmea_value[0].data + 4, 2);
    int dss = str2int(s->nmea_value[0].data + 7, 2);

    str2float(&CurrRTKInfo.heading, s->nmea_value[1].data, s->nmea_value[1].length);
    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);
    str2float(&xy_temp.heading, s->nmea_value[1].data, s->nmea_value[1].length);
    str2float(&xy_temp.pitch, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&xy_temp.roll, s->nmea_value[3].data, s->nmea_value[3].length);

    if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
//        UpdateRTKInfo(&CurrRTKInfo);
//        up_num++;
        /*if ((up_num % 5) == 0)*/ {
//            NewMgrEvent(DRIVER_UPDATE_EVT);
        }
    if (xy_temp.hour == hh && xy_temp.min == mm && xy_temp.sec == ss && xy_temp.millisec == dss) {
        rtk_info_t rtk;
        rtk.qf = xy_temp.qf;
        rtk.utc_time = static_cast<int64_t>(TimeMakeComposite(2000 + xy_temp.year, xy_temp.month, xy_temp.day, xy_temp.hour, xy_temp.min, xy_temp.sec))*static_cast<int64_t>(1000) + static_cast<int64_t>(dss*10);
        rtk.x = xy_temp.x;
        rtk.y = xy_temp.y;
        rtk.heading = xy_temp.heading;
        rtk.pitch = xy_temp.pitch;
        rtk.roll = xy_temp.roll;
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&rtk, sizeof(rtk));
    }
    CurrRTKInfo.hh = hh;
    CurrRTKInfo.mm = mm;
    CurrRTKInfo.ss = ss;
    CurrRTKInfo.dss = dss;
    xy_temp.hour = hh;
    xy_temp.min = mm;
    xy_temp.sec = ss;
    xy_temp.millisec = dss;
}

static void CheckPjkParam(void)