//
|
// Created by YY on 2019/12/23.
|
//
|
|
#include <cstring>
|
#include <cstdio>
|
#include <cmath>
|
#include <cstdlib>
|
#include <cctype>
|
#include <thread>
|
#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"
|
#include "../mcu/ahp.h"
|
|
#define DEBUG(fmt, args...) LOGD("<rtk> <%s>: " fmt, __func__, ##args)
|
|
#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 rtk_info CurrRTKInfo;
|
static bool needSetPjk = false;
|
static int lostCnt;
|
|
static void CheckPjkParam(void);
|
static void CheckPjkParamTimeout(apptimer_var_t val);
|
static int WriteBluetooth(int id, const void *buf, int len);
|
static void GetModuleVersion(void);
|
|
static void VersionTimeout(apptimer_var_t val);
|
static void GpsDataTimeout(apptimer_var_t val);
|
|
// 蓝牙连接后
|
void ConfigRTKModuleLater(void)
|
{
|
AppTimer_delete(GpsDataTimeout);
|
AppTimer_delete(VersionTimeout);
|
AppTimer_delete(CheckPjkParamTimeout);
|
AppTimer_add(CheckPjkParamTimeout, D_SEC(1));
|
}
|
|
void FactorySettings(void)
|
{
|
WriteRtkCommand(FACTORY, strlen(FACTORY));
|
}
|
|
void RebootModule(void)
|
{
|
WriteRtkCommand(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)
|
{
|
WriteRtkCommand(UNLOGALL, strlen(UNLOGALL));
|
WriteRtkCommand(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);
|
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);
|
WriteRtkCommand(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);
|
|
WriteRtkCommand(buff, strlen(buff));
|
|
DEBUG("%s", buff);
|
}
|
|
void GetGpsStatus(gpsStatus_t &data)
|
{
|
data = gpsStatus;
|
}
|
|
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();
|
}
|
}
|
}
|
|
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));
|
|
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);
|
|
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);
|
|
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)
|
{
|
WriteRtkCommand(INQ_PJK_PARAM, strlen(INQ_PJK_PARAM));
|
DEBUG("获取PJK参数...");
|
AppTimer_delete(CheckPjkParamTimeout);
|
AppTimer_add(CheckPjkParamTimeout, D_SEC(3));
|
}
|
|
static void CheckPjkParamTimeout(apptimer_var_t val) {
|
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));
|
WriteRtkCommand(CMD_VERSION, strlen(CMD_VERSION));
|
|
DEBUG("获取版本...");
|
}
|
|
static void VersionTimeout(apptimer_var_t val)
|
{
|
DEBUG("版本获取超时");
|
GetModuleVersion();
|
|
uint8_t buff[33];
|
memcpy(buff, rtkModel, 32);
|
buff[32] = 0;
|
PlatformStatusChanged(RTK_STATUS_EVT, buff, 33);
|
}
|
|
static void GpsDataTimeout(apptimer_var_t val)
|
{
|
if (++lostCnt >= 3) {
|
DEBUG("RTK模块收不到GPS数据");
|
GetModuleVersion();
|
} else {
|
AppTimer_add(GpsDataTimeout, D_SEC(5));
|
}
|
}
|
