| | |
| | | #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) |
| | | |
| | |
| | | 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); |
| | |
| | | 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) { |
| | |
| | | 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) |