//
|
// 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"
|
|
#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"};
|
|
static gpsStatus_t gpsStatus;
|
|
static rtk_info CurrRTKInfo;
|
|
static void CheckPjkParam(void);
|
static void CheckPjkParamTimeout(union sigval sig);
|
|
static void *UartThread(void *p);
|
|
void ConfigRTKModule(void)
|
{
|
// TODO
|
DEBUG("ConfigRTKModule");
|
|
memset(&CurrRTKInfo, 0, sizeof(CurrRTKInfo));
|
CurrRTKInfo.hh = -1;
|
|
memset(&gpsStatus, 0, sizeof(gpsStatus));
|
gpsStatus.hh = -1;
|
|
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);
|
}
|
|
void FactorySettings(void)
|
{
|
WriteSerialPort(RTK_MODULE_UART, FACTORY, strlen(FACTORY));
|
}
|
|
void RebootModule(void)
|
{
|
WriteSerialPort(RTK_MODULE_UART, REBOOT, strlen(REBOOT));
|
}
|
|
void handleRTKRebootComp(const struct nmea *s)
|
{
|
DEBUG("RTK Reboot complete!!");
|
SetAYFactoryParam(5);
|
}
|
|
void handlePJKParam(const struct nmea *s) {
|
//PJK Parameter: A:6378137.000, 1/F:298.257223563, B0:0.000000deg, L0:120.000000, N0:0.000, E0:500000.000.
|
//PJK Parameter: A:6378137.000, 1/F:298.257223563, B0:29.512453deg, L0:106.455336, N0:0.000, E0:0.000.
|
bool setparam = true;
|
|
const char DP1[] = "A:6378137.000";
|
const char DP2[] = "1/F:298.257223563";
|
const char DP3[] = "B0:29.512453deg";
|
const char DP4[] = "L0:106.455336";
|
const char DP5[] = "N0:0.000";
|
const char DP6[] = "E0:0.000";
|
|
AppTimer_delete(CheckPjkParamTimeout);
|
|
for (int i = 0; i < s->nmea_num; ++i) {
|
char out[64] = {0};
|
|
memcpy(out, s->nmea_value[i].data, s->nmea_value[i].length);
|
|
DEBUG("handlePJKParam = %s", out);
|
}
|
|
if (s->nmea_num != 6) return;
|
|
if (memcmp(s->nmea_value[0].data, DP1, strlen(DP1))) {
|
setparam = true;
|
}
|
if (memcmp(s->nmea_value[1].data, DP2, strlen(DP2))) {
|
setparam = true;
|
}
|
if (memcmp(s->nmea_value[2].data, DP3, strlen(DP3))) {
|
setparam = true;
|
}
|
if (memcmp(s->nmea_value[3].data, DP4, strlen(DP4))) {
|
setparam = true;
|
}
|
if (memcmp(s->nmea_value[4].data, DP5, strlen(DP5))) {
|
setparam = true;
|
}
|
if (memcmp(s->nmea_value[5].data, DP6, strlen(DP6))) {
|
setparam = true;
|
}
|
|
if (setparam) {
|
SetAYFactoryParam(5);
|
}
|
}
|
|
void SetAYFactoryParam(int freq)
|
{
|
WriteSerialPort(RTK_MODULE_UART, UNLOGALL, strlen(UNLOGALL));
|
WriteSerialPort(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);
|
WriteSerialPort(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);
|
WriteSerialPort(RTK_MODULE_UART, cmd, strlen(cmd));
|
}
|
|
WriteSerialPort(RTK_MODULE_UART, AY_PJKPARAM, strlen(AY_PJKPARAM));
|
// WriteSerialPort(RTK_MODULE_UART, SAVECONFIG, strlen(SAVECONFIG));
|
}
|
|
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();
|
}
|
|
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 0
|
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, 3000));
|
// DEBUG("handleUnrecognisedNMEA: %s", buff);
|
}
|
|
void handleGPGGA(const struct nmea *s)
|
{
|
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;
|
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;
|
}
|
}
|
|
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)
|
{
|
WriteSerialPort(RTK_MODULE_UART, INQ_PJK_PARAM, strlen(INQ_PJK_PARAM));
|
|
AppTimer_delete(CheckPjkParamTimeout);
|
AppTimer_add(CheckPjkParamTimeout, D_SEC(3));
|
}
|
|
static void CheckPjkParamTimeout(union sigval sig) {
|
AppTimer_delete(CheckPjkParamTimeout);
|
|
DEBUG("RTK Module failure!!");
|
}
|
