From f7a18ec4494b9c5c9ef3fd440bbf68ffc6425e18 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 08 十二月 2022 15:40:56 +0800
Subject: [PATCH] 智慧驾培首次提交
---
lib/src/main/cpp/rtk_module/rtk.cpp | 204 +++++++--------------------------------------------
1 files changed, 28 insertions(+), 176 deletions(-)
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index 67a2d35..88adfa5 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/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妯″潡鏀朵笉鍒癎PS鏁版嵁");
GetModuleVersion();
--
Gitblit v1.8.0