lib/src/main/cpp/driver_test.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
lib/src/main/cpp/driver_test.h | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
lib/src/main/cpp/rtk_module/rtk.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
lib/src/main/cpp/rtk_module/rtk.h | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
lib/src/main/cpp/rtk_platform/platform.cpp | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 | |
lib/src/main/cpp/rtk_platform/platform.h | ●●●●● 补丁 | 查看 | 原始文档 | blame | 历史 |
lib/src/main/cpp/driver_test.cpp
@@ -412,7 +412,7 @@ MA_SendExamStatus(1, 0); } void UpdateRTKInfo(struct rtk_info *s) void UpdateRTKInfo(rtk_info *s) { struct tm test_tm; lib/src/main/cpp/driver_test.h
@@ -6,20 +6,7 @@ #define RTKDRIVERTEST_DRIVER_TEST_H #include "Geometry.h" struct rtk_info { int YY; int MM; int DD; int hh; int mm; int ss; int dss; int qf; double heading; double x; double y; }; #include "rtk_module/rtk.h" #define TIRE_OUTSIDE 0 #define TIRE_INSIDE 1 @@ -54,7 +41,7 @@ void SetSensorCfg(int (*sensor)[2], int sensorNum); void StartDriverExam(int start); void UpdateRTKInfo(struct rtk_info *s); void UpdateRTKInfo(rtk_info *s); void UpdateCarCoord(void); car_model_cache_t *GetCarModelCache(int node); lib/src/main/cpp/rtk_module/rtk.cpp
@@ -38,8 +38,8 @@ static gpsStatus_t gpsStatus; static int tra_hh, tra_mm, tra_ss, tra_dss; struct rtk_info CurrRTKInfo; static time_t recved_gprmc_time = 0, recved_gpgga_time = 0;; static rtk_info CurrRTKInfo; static void CheckPjkParam(void); static void CheckPjkParamTimeout(union sigval sig); @@ -49,10 +49,12 @@ void ConfigRTKModule(void) { // TODO DEBUG("ConfigRTKModule"); memset(&CurrRTKInfo, 0, sizeof(CurrRTKInfo)); tra_hh = tra_mm = tra_ss = tra_dss = 0; memset(&gpsStatus, 0, sizeof(gpsStatus)); gpsStatus.hh = -1; static struct serial_config serialConfig; @@ -228,48 +230,38 @@ { DEBUG("handleGPGGA num = %d", s->nmea_num); if (s->nmea_num >= 10) { gpsStatus.gps_status = str2int(s->nmea_value[5].data, s->nmea_value[5].length); int hh = str2int(s->nmea_value[0].data, 2); int mm = str2int(s->nmea_value[0].data + 2, 2); int ss = str2int(s->nmea_value[0].data + 4, 2); int mss = str2int(s->nmea_value[0].data + 7, 2); double lat1, lat2, lon1, lon2, alt; int qf; str2float(&lat1, s->nmea_value[1].data, 2); str2float(&lat2, s->nmea_value[1].data+2, s->nmea_value[1].length-2); qf = str2int(s->nmea_value[5].data, s->nmea_value[5].length); str2float(&lon1, s->nmea_value[3].data, 3); str2float(&lon2, s->nmea_value[3].data+3, s->nmea_value[3].length-3); if (qf > 0) { str2float(&lat1, s->nmea_value[1].data, 2); str2float(&lat2, s->nmea_value[1].data+2, s->nmea_value[1].length-2); lat1 = lat1 + lat2/60.0; lon1 = lon1 + lon2/60.0; str2float(&lon1, s->nmea_value[3].data, 3); str2float(&lon2, s->nmea_value[3].data+3, s->nmea_value[3].length-3); str2float(&alt, s->nmea_value[8].data, s->nmea_value[8].length); lat1 = lat1 + lat2/60.0; lon1 = lon1 + lon2/60.0; gpsStatus.latitude = lat1; gpsStatus.longitude = lon1; gpsStatus.altitude = alt; gpsStatus.satNum = str2int(s->nmea_value[6].data, s->nmea_value[6].length); str2float(&alt, s->nmea_value[8].data, s->nmea_value[8].length); gpsStatus.gps_status = 1; gpsStatus.latitude = (uint32_t)(lat1 * 1000000); gpsStatus.longitude = (uint32_t)(lon1 * 1000000); // char buff1[32] = {0}; // char buff2[32] = {0}; // char buff3[32] = {0}; // // memcpy(buff1, s->nmea_value[1].data, s->nmea_value[1].length); // memcpy(buff2, s->nmea_value[3].data, s->nmea_value[3].length); // memcpy(buff3, s->nmea_value[8].data, s->nmea_value[8].length); // // DEBUG("%s %s %s: lat = %ld lon = %ld alt = %f", buff1, buff2, buff3, gpsStatus.latitude, gpsStatus.longitude, alt); gpsStatus.altitude = (int) fabs(alt); recved_gpgga_time = AppTimer_GetTickCount(); if (abs(recved_gpgga_time - recved_gprmc_time) < 500) { RequestRtkDownload(gpsStatus.latitude, gpsStatus.longitude, gpsStatus.altitude, gpsStatus.bcd_time, 1); } } else { gpsStatus.gps_status = 0; if (hh == gpsStatus.hh && mm == gpsStatus.mm && ss == gpsStatus.ss && mss == gpsStatus.mss) { // 同步的RMC消息也收集了 PlatformStatusChanged(GPS_UPDATE_EVT, (uint8_t *)&gpsStatus, sizeof(gpsStatus)); } gpsStatus.hh = hh; gpsStatus.mm = mm; gpsStatus.ss = ss; gpsStatus.mss = mss; } } @@ -282,29 +274,32 @@ void handleGPRMC(const struct nmea *s) { DEBUG("handleGPRMC num = %d", s->nmea_num); if (s->nmea_num >= 9) { if (s->nmea_num >= 9) { int hh = str2int(s->nmea_value[0].data, 2); int mm = str2int(s->nmea_value[0].data + 2, 2); int ss = str2int(s->nmea_value[0].data + 4, 2); int mss = str2int(s->nmea_value[0].data + 7, 2); int DD = str2int(s->nmea_value[8].data, 2); int MM = str2int(s->nmea_value[8].data + 2, 2); int YY = str2int(s->nmea_value[8].data + 4, 2); gpsStatus.DD = str2int(s->nmea_value[8].data, 2); gpsStatus.MM = str2int(s->nmea_value[8].data + 2, 2); gpsStatus.YY = str2int(s->nmea_value[8].data + 4, 2); gpsStatus.bcd_time[0] = ((YY/10)<<4) + (YY%10); gpsStatus.bcd_time[1] = ((MM/10)<<4) + (MM%10); gpsStatus.bcd_time[2] = ((DD/10)<<4) + (DD%10); gpsStatus.bcd_time[3] = ((hh/10)<<4) + (hh%10); gpsStatus.bcd_time[4] = ((mm/10)<<4) + (mm%10); gpsStatus.bcd_time[5] = ((ss/10)<<4) + (ss%10); double speed; recved_gprmc_time = AppTimer_GetTickCount(); str2float(&speed, s->nmea_value[6].data, s->nmea_value[6].length); if (abs(recved_gpgga_time - recved_gprmc_time) < 500) { RequestRtkDownload(gpsStatus.latitude, gpsStatus.longitude, gpsStatus.altitude, gpsStatus.bcd_time, 1); gpsStatus.speed = speed * 1.85184; // 节 -> 公里 if (hh == gpsStatus.hh && mm == gpsStatus.mm && ss == gpsStatus.ss && mss == gpsStatus.mss) { // 同步的GGA消息也收集了 PlatformStatusChanged(GPS_UPDATE_EVT, (uint8_t *)&gpsStatus, sizeof(gpsStatus)); } gpsStatus.hh = hh; gpsStatus.mm = mm; gpsStatus.ss = ss; gpsStatus.mss = mss; } } @@ -379,6 +374,7 @@ // } if (CurrRTKInfo.hh == tra_hh && CurrRTKInfo.mm == tra_mm && CurrRTKInfo.ss == tra_ss && CurrRTKInfo.dss == tra_dss) { PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo)); UpdateRTKInfo(&CurrRTKInfo); // up_num++; /*if ((up_num % 5) == 0)*/ { @@ -400,6 +396,7 @@ // CurrRTKInfo.heading = 60; if (CurrRTKInfo.hh == tra_hh && CurrRTKInfo.mm == tra_mm && CurrRTKInfo.ss == tra_ss && CurrRTKInfo.dss == tra_dss) { PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo)); UpdateRTKInfo(&CurrRTKInfo); // up_num++; /*if ((up_num % 5) == 0)*/ { lib/src/main/cpp/rtk_module/rtk.h
@@ -7,12 +7,37 @@ typedef struct { uint16_t gps_status; uint32_t latitude; uint32_t longitude; uint16_t altitude; uint8_t bcd_time[6]; int YY; int MM; int DD; int hh; int mm; int ss; int mss; int satNum; double latitude; double longitude; double altitude; double speed; }gpsStatus_t; typedef struct { int YY; int MM; int DD; int hh; int mm; int ss; int dss; int qf; double heading; double pitch; double roll; double x; double y; }rtk_info; void ConfigRTKModule(void); void FactorySettings(void); void RebootModule(void); lib/src/main/cpp/rtk_platform/platform.cpp
@@ -7,6 +7,7 @@ #include <cstring> #include <pthread.h> #include <semaphore.h> #include <cmath> #include "platform.h" #include "../jni_log.h" #include "../common/net.h" @@ -51,8 +52,6 @@ uint32_t login : 1; uint32_t downloadRtk : 1; } platformStatus; struct platformSocket exceptSocket, currSocket; static uint32_t eventMask; @@ -279,6 +278,39 @@ } MA_RtkPlatformLogin(data[0]); } if (events & GPS_UPDATE_EVT) { DEBUG("GPS_UPDATE_EVT"); const gpsStatus_t *gps = (gpsStatus_t *)data; struct gpsBrief brief; brief.qf = gps->gps_status; brief.latitude = gps->latitude; brief.longitude = gps->longitude; brief.altitude = gps->altitude; brief.speed = gps->speed; brief.sat_num = gps->satNum; sprintf(brief.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + gps->YY, gps->MM, gps->DD, gps->hh, gps->mm, gps->ss, gps->mss); MA_SendGpsBrief(&brief); RequestRtkDownload(gps, 1); } if (events & RTK_UPDATE_EVT) { DEBUG("RTK_UPDATE_EVT"); const rtk_info *rtk = (rtk_info *)data; struct rtkBrief brief; brief.qf = rtk->qf; brief.coord_x = rtk->x; brief.coord_y = rtk->y; brief.heading = rtk->heading; brief.pitch = rtk->pitch; brief.roll = rtk->roll; brief.coord_x_dir = 'N'; brief.coord_y_dir = 'E'; sprintf(brief.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtk->YY, rtk->MM, rtk->DD, rtk->hh, rtk->mm, rtk->ss, rtk->dss); MA_SendRtkBrief(&brief); } } static void *PlatformDataListenThread(void *p) { @@ -446,16 +478,28 @@ requestPlatformSendRtk = true; } void RequestRtkDownload(uint32_t latitude, uint32_t longitude, uint16_t altitude, const uint8_t *bcd_time, uint16_t rtk_pkt_interval) void RequestRtkDownload(const gpsStatus_t *gps, uint16_t rtk_pkt_interval) { if (requestPlatformSendRtk) { DisplayText("请求下载RTK数据"); if (requestPlatformSendRtk && gps->gps_status > 0) { DEBUG("请求下载RTK数据"); requestPlatformSendRtk = false; AppTimer_delete(RequestRtkNoResp); AppTimer_add(RequestRtkNoResp, D_SEC(5)); uint32_t latitude = (uint32_t)(gps->latitude * 1000000); uint32_t longitude = (uint32_t)(gps->longitude * 1000000); int altitude = (int) fabs(gps->altitude); uint8_t bcd_time[6]; bcd_time[0] = ((gps->YY/10)<<4) + (gps->YY%10); bcd_time[1] = ((gps->MM/10)<<4) + (gps->MM%10); bcd_time[2] = ((gps->DD/10)<<4) + (gps->DD%10); bcd_time[3] = ((gps->hh/10)<<4) + (gps->hh%10); bcd_time[4] = ((gps->mm/10)<<4) + (gps->mm%10); bcd_time[5] = ((gps->ss/10)<<4) + (gps->ss%10); SendRTKStart(latitude, longitude, altitude, bcd_time, rtk_pkt_interval); } } lib/src/main/cpp/rtk_platform/platform.h
@@ -5,6 +5,8 @@ #ifndef RTKDRIVERTEST_PLATFORM_H #define RTKDRIVERTEST_PLATFORM_H #include "../rtk_module/rtk.h" #define PLATFORM_CONNECT_EVT 0x0001 #define PLATFORM_DISCONNECT_EVT 0x0002 #define PLATFORM_REGISTER_EVT 0x0004 @@ -35,8 +37,7 @@ void DeviceLoginCallback(uint8_t res); void ReceivedRtk(const uint8_t *data, int length); void RequestRtkDownload(uint32_t latitude, uint32_t longitude, uint16_t altitude, const uint8_t *bcd_time, uint16_t rtk_pkt_interval); void RequestRtkDownload(const gpsStatus_t *gps, uint16_t rtk_pkt_interval); void StopRtkDownload(void); #endif //RTKDRIVERTEST_PLATFORM_H