//
|
// Created by YY on 2019/12/23.
|
//
|
|
#include <cstring>
|
#include <cstdio>
|
#include <pthread.h>
|
#include <cmath>
|
#include <cstdlib>
|
#include <cctype>
|
#include "rtk.h"
|
#include "parse_gps.h"
|
#include "../common/serial_port.h"
|
#include "../jni_log.h"
|
#include "../driver_test.h"
|
#include "../utils/num.h"
|
#include "../defs.h"
|
#include "../common/apptimer.h"
|
#include "../rtk_platform/platform.h"
|
#include "../native-lib.h"
|
#include "virtual_rtk.h"
|
#include "../mcu/mcu_if.h"
|
|
#define DEBUG(fmt, args...) LOGD("<rtk> <%s>: " fmt, __func__, ##args)
|
|
#define RTK_MODULE_UART UART_0
|
|
#define PARSE_BUFF_SIZE 4096
|
|
const static char FACTORY[] = "FRESET\r\n";
|
const static char REBOOT[] = "RESET\r\n";
|
const static char INQ_PJK_PARAM[] = "LOG PJKPARA\r\n";
|
const static char AY_PJKPARAM[] = "set pjkpara 6378137 298.257223563 29.51245330924 106.4553361945 0 0\r\n";
|
const static char UNLOGALL[] = "unlogall\r\n";
|
const static char IFCOM2[] = "interfacemode com2 auto auto on\r\n";
|
const static char SAVECONFIG[] = "saveconfig\r\n";
|
const static char *PJKITEMS[] = {"gptra", "ptnlpjk"};
|
const static char *GPSITEMS[] = {"gpgga", "gprmc", "gpvtg"};
|
const char CMD_VERSION[] = "log version\r\n";
|
|
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 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);
|
// 等待蓝牙连接
|
}
|
}
|
|
// 蓝牙连接后
|
void ConfigRTKModuleLater(void)
|
{
|
AppTimer_delete(GpsDataTimeout);
|
AppTimer_delete(VersionTimeout);
|
AppTimer_delete(CheckPjkParamTimeout);
|
AppTimer_add(CheckPjkParamTimeout, D_SEC(1));
|
}
|
|
void FactorySettings(void)
|
{
|
WriteRtk(RTK_MODULE_UART, FACTORY, strlen(FACTORY));
|
}
|
|
void RebootModule(void)
|
{
|
WriteRtk(RTK_MODULE_UART, REBOOT, strlen(REBOOT));
|
}
|
|
void handleRTKRebootComp(const struct nmea *s)
|
{
|
DEBUG("RTK Reboot complete!!");
|
// CheckPjkParam();
|
GetModuleVersion();
|
}
|
|
void handlePJKParam(const struct nmea *s) {
|
DEBUG("handlePJKParam");
|
|
AppTimer_delete(CheckPjkParamTimeout);
|
|
SetAYFactoryParam(5);
|
needSetPjk = true;
|
|
lostCnt = 0;
|
AppTimer_delete(GpsDataTimeout);
|
AppTimer_add(GpsDataTimeout, D_SEC(5));
|
|
uint8_t buff[33];
|
|
memcpy(buff, rtkModel, 32);
|
buff[32] = 1;
|
PlatformStatusChanged(RTK_STATUS_EVT, buff, 33);
|
}
|
|
void SetAYFactoryParam(int freq)
|
{
|
WriteRtk(RTK_MODULE_UART, UNLOGALL, strlen(UNLOGALL));
|
WriteRtk(RTK_MODULE_UART, IFCOM2, strlen(IFCOM2));
|
|
if (freq == 0)
|
freq = 5;
|
|
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));
|
}
|
|
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));
|
}
|
|
// WriteSerialPort(RTK_MODULE_UART, AY_PJKPARAM, strlen(AY_PJKPARAM));
|
// WriteSerialPort(RTK_MODULE_UART, SAVECONFIG, strlen(SAVECONFIG));
|
}
|
|
void SetPjkPara(int centLon)
|
{
|
char buff[64];
|
|
sprintf(buff, "set pjkpara 6378137 298.257223563 0 %d 0 500000\r\n", centLon);
|
|
WriteRtk(RTK_MODULE_UART, buff, strlen(buff));
|
|
DEBUG("%s", buff);
|
}
|
|
void GetGpsStatus(gpsStatus_t &data)
|
{
|
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);
|
|
if (length >= 100) {
|
string cs(buff);
|
|
if (cs.find("K708") != string::npos) {
|
// 最初的标准基站模块
|
DEBUG("K708 模块");
|
strcpy(rtkModel, "K708");
|
|
AppTimer_delete(VersionTimeout);
|
CheckPjkParam();
|
} else if (cs.find("K726") != string::npos) {
|
// 移动站模块，也可以用做基站功能
|
DEBUG("K726 模块");
|
strcpy(rtkModel, "K726");
|
|
AppTimer_delete(VersionTimeout);
|
CheckPjkParam();
|
}
|
}
|
}
|
|
void handleGPGGA(const struct nmea *s)
|
{
|
static uint32_t qfCnt = 0;
|
|
// DEBUG("handleGPGGA num = %d", s->nmea_num);
|
if (s->nmea_num >= 10) {
|
lostCnt = 0;
|
AppTimer_delete(GpsDataTimeout);
|
AppTimer_add(GpsDataTimeout, D_SEC(5));
|
|
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;
|
str2float(&lat1, s->nmea_value[1].data, 2);
|
str2float(&lat2, s->nmea_value[1].data+2, s->nmea_value[1].length-2);
|
|
str2float(&lon1, s->nmea_value[3].data, 3);
|
str2float(&lon2, s->nmea_value[3].data+3, s->nmea_value[3].length-3);
|
|
lat1 = lat1 + lat2/60.0;
|
lon1 = lon1 + lon2/60.0;
|
|
str2float(&alt, s->nmea_value[8].data, s->nmea_value[8].length);
|
|
gpsStatus.latitude = lat1;
|
gpsStatus.longitude = lon1;
|
gpsStatus.altitude = alt;
|
gpsStatus.satNum = str2int(s->nmea_value[6].data, s->nmea_value[6].length);
|
|
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;
|
|
// 计算中央子午线
|
int qf = str2int(s->nmea_value[5].data, s->nmea_value[5].length);
|
if (qf > 0) {
|
qfCnt++;
|
if (needSetPjk && qfCnt >= 3) {
|
needSetPjk = false;
|
|
DEBUG("set pjkpara 原始经度 %f", lon1);
|
SetPjkPara(((int) (lon1 / 3.0 + 0.5)) * 3);
|
}
|
} else {
|
qfCnt = 0;
|
}
|
}
|
}
|
|
void handleGPGSA(const struct nmea *s) {
|
}
|
|
void handleGPGSV(const struct nmea *s) {
|
}
|
|
void handleGPRMC(const struct nmea *s)
|
{
|
// DEBUG("handleGPRMC num = %d", s->nmea_num);
|
|
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);
|
|
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);
|
|
double speed;
|
|
str2float(&speed, s->nmea_value[6].data, s->nmea_value[6].length);
|
|
gpsStatus.speed = speed * 1.85184; // 节 -> 公里
|
|
double trackTure;
|
str2float(&trackTure, s->nmea_value[7].data, s->nmea_value[7].length);
|
gpsStatus.trackTure = trackTure;
|
|
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;
|
}
|
}
|
|
const char *GPS_SOL_OK = "SOL_COMPUTED";
|
const char *GPS_SOL_OBS = "INSUFFICIENT_OBS";
|
const char *GPS_SOL_CS = "COLD_START";
|
|
const char *GPS_POS_NONE = "NONE";
|
const char *GPS_POS_FIX = "FIXEDPOS";
|
const char *GPS_POS_SIG = "SINGLE";
|
const char *GPS_POS_PDIFF = "PSRDIFF";
|
const char *GPS_POS_NARF = "NARROW_FLOAT";
|
const char *GPS_POS_WIDI = "WIDE_INT";
|
const char *GPS_POS_NARI = "NARROW_INT";
|
const char *GPS_POS_SWIDL = "SUPER WIDE-LANE";
|
|
void handleBESTPOSA(const struct nmea *s) {
|
DEBUG("handleBESTPOSA num = %d", s->nmea_num);
|
double lat, lon;
|
|
if (memcmp(s->nmea_value[0].data, GPS_SOL_OK, s->nmea_value[0].length)) {
|
str2float(&lat, s->nmea_value[2].data, s->nmea_value[2].length);
|
str2float(&lon, s->nmea_value[3].data, s->nmea_value[3].length);
|
}
|
}
|
|
void handleGPVTG(const struct nmea *s) {
|
}
|
|
void handleGPGLL(const struct nmea *s) {
|
}
|
|
void handleGPZDA(const struct nmea *s) {
|
}
|
|
void handlePJK(const struct nmea *s) {
|
// DEBUG("handlePJK num = %d", s->nmea_num);
|
|
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 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);
|
|
CurrRTKInfo.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);
|
|
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);
|
// up_num++;
|
/*if ((up_num % 5) == 0)*/ {
|
// NewMgrEvent(DRIVER_UPDATE_EVT);
|
}
|
}
|
CurrRTKInfo.hh = hh;
|
CurrRTKInfo.mm = mm;
|
CurrRTKInfo.ss = ss;
|
CurrRTKInfo.dss = dss;
|
}
|
|
void handleGPTRA(const struct nmea *s) {
|
// DEBUG("handleGPTRA num = %d", s->nmea_num);
|
|
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 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);
|
|
// 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);
|
// up_num++;
|
/*if ((up_num % 5) == 0)*/ {
|
// NewMgrEvent(DRIVER_UPDATE_EVT);
|
}
|
}
|
CurrRTKInfo.hh = hh;
|
CurrRTKInfo.mm = mm;
|
CurrRTKInfo.ss = ss;
|
CurrRTKInfo.dss = dss;
|
}
|
|
static void CheckPjkParam(void)
|
{
|
WriteRtk(RTK_MODULE_UART, 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);
|
|
DEBUG("获取PJK参数超时");
|
|
uint8_t buff[33];
|
memcpy(buff, rtkModel, 32);
|
buff[32] = 0;
|
PlatformStatusChanged(RTK_STATUS_EVT, buff, 33);
|
|
GetModuleVersion();
|
}
|
|
static int WriteBluetooth(int id, const void *buf, int len)
|
{
|
// SendToBluetooth((uint8_t *)buf, len);
|
SendMcuCommand(0x000B, (uint8_t *)buf, len);
|
return len;
|
}
|
|
static void GetModuleVersion(void)
|
{
|
AppTimer_delete(VersionTimeout);
|
AppTimer_add(VersionTimeout, D_SEC(3));
|
WriteRtk(RTK_MODULE_UART, CMD_VERSION, strlen(CMD_VERSION));
|
|
DEBUG("获取版本...");
|
}
|
|
static void VersionTimeout(union sigval sig)
|
{
|
AppTimer_delete(VersionTimeout);
|
DEBUG("版本获取超时");
|
GetModuleVersion();
|
|
uint8_t buff[33];
|
memcpy(buff, rtkModel, 32);
|
buff[32] = 0;
|
PlatformStatusChanged(RTK_STATUS_EVT, buff, 33);
|
}
|
|
static void GpsDataTimeout(union sigval sig)
|
{
|
AppTimer_delete(GpsDataTimeout);
|
|
if (++lostCnt >= 3) {
|
DEBUG("RTK模块收不到GPS数据");
|
GetModuleVersion();
|
} else {
|
AppTimer_add(GpsDataTimeout, D_SEC(5));
|
}
|
}
|
