gps数据解析和组包发送
#include <malloc.h>
#include <memory.h>
#include <time.h>
#include <stdbool.h>
#include <stdlib.h>
#include <cstdint>
typedef unsigned char UCHAR;
typedef struct GbGpsData{
char isvalid;
long longtitude;
char lonDirect;
long latitude;
char latDirect;
float speed;
float angle;
char year;
char month;
char day;
char hour;
char min;
char sec;
bool isAvailable;
}S_GbGpsData,*PS_GbGpsData;
static PS_GbGpsData g_psLocData = NULL;
#define HEAD_MCU "\x4c\x59"
#define HEAD_LEN 2
#define COUNT_LEN 2
#define MCU_USRDATA_MAXLEN 1000
typedef struct {
UCHAR MCU_Head[HEAD_LEN];
UCHAR MCU_Command;
UCHAR MCU_CommandRsp;
UCHAR MAJOR;
UCHAR MINOR;
UCHAR MCU_DataLen[COUNT_LEN];
UCHAR MCU_User_data[MCU_USRDATA_MAXLEN];
UCHAR MCU_CRC;
}MCU_RECV;
MCU_RECV MCU_UartRecv;
int getComma(int num,char *str)
{
int i,j=0;
int len = strlen(str);
if(len > 100)
{
printf("getComma len = [%d] error",len);
return 0;
}
for(i=0;i<len;i++)
{
if(str[i]==',')
{
j++;
}
if(j==num)
{
return i+1;
}
}
printf("getComma error\n");
return 0;
}
double get_locate(double temp)
{
int m;
double n;
m=(int)temp/100;
n=(temp-m*100)/60;
n=n+m;
return n;
}
double get_double_number(char * s)
{
char buf[64];
int i;
double rev;
i=getComma(1, s);
if(i==0)
{
printf("get_double_number error");
return 0;
}
strncpy(buf,s,i);
buf[i]=0;
rev=atof(buf);
return rev;
}
//2021/3/9校验GPS数据完整性与正确性
int GetGpsBufVerification(char *Gpsbuf)
{
int Verification,j,num = 0;
char str[3];
num = getComma(13,Gpsbuf);
if(num == 0)
{
printf("getComma Is ERROR,num = %d\n",num);
return 0;
}
memset(str,0,sizeof(str));
for(j=0;j<2;j++)
{
str[j] = Gpsbuf[num+j+2];
}
str[j] = '\0';
sscanf(str,"%x\n",&Verification);
return Verification;
}
//2021/3/9校验GPS数据完整性与正确性
int CalculateGpsBufVerification(char *Gpsbuf)
{
int result,i;
int len = strlen(Gpsbuf);
for(result=Gpsbuf[1],i=2;(Gpsbuf[i]!='*') && (i < len);i++)
{
result^=Gpsbuf[i];
}
return result;
}
//DFYY_TEST
int gpsMouleSaveLocationData(PS_GbGpsData pData)
{
if(pData == NULL)
{
return 1;
}
if(pData->isvalid == 'A')
{
g_psLocData->isvalid = pData->isvalid;
g_psLocData->latDirect= pData->latDirect;
g_psLocData->lonDirect = pData->lonDirect;
g_psLocData->latitude = pData->latitude;
g_psLocData->longtitude = pData->longtitude;
g_psLocData->speed = pData->speed;
g_psLocData->angle = pData->angle;
g_psLocData->year = pData->year;
g_psLocData->month = pData->month;
g_psLocData->day = pData->day;
g_psLocData->hour = pData->hour;
g_psLocData->min = pData->min;
g_psLocData->sec = pData->sec;
return 0;
}else if(pData->isvalid == 'V')
{
g_psLocData->isvalid = pData->isvalid;
return 1;
}else{
return 1;
}
}
void handleNmeaData(char * pData, int size)
{
int idx;
int i,j;
int timenum;
char str [3];
char *buf = (char*)malloc(256);
char *pData_Bak = (char*)malloc(1024);
char *GPRMC;
int GPRMC_Len = 0;
PS_GbGpsData pGpsData = NULL;
int ret = 1;
char cmd[256]={0};
struct tm ptime;
if(buf == NULL)
{
return;
}
if(pData_Bak == NULL)
{
return;
}
memset(buf,0,256);
memset(pData_Bak,0,1024);
pGpsData = (PS_GbGpsData)malloc(sizeof(S_GbGpsData));
if(size < 1024)
{
memcpy(pData_Bak,pData,size);
}else{
free(buf);
free(pData_Bak);
free(pGpsData);
return;
}
if(!pGpsData)
{
free(buf);
free(pData_Bak);
return;
}
for(idx = 0;idx < size;idx++)
{
if((pData_Bak[idx] == '\r') || (pData_Bak[idx] == '\0'))
{
pData_Bak[idx] = ' ';
}
}
if((GPRMC = strstr(pData_Bak,"$GNRMC"))!=NULL)
{
GPRMC_Len = strlen(GPRMC);
for(i=0;i<GPRMC_Len;i++)
{
if(GPRMC[i] == '\n')
{
GPRMC[i] = '\0';
break;
}
}
memcpy(buf,GPRMC,i);
//2021/3/3测试验证GPS数据完整性,如果第13个逗号(最后一个逗号)后有数据,则数据完整
if((getComma(13, buf) != 0) && (GetGpsBufVerification(buf) == CalculateGpsBufVerification(buf)))
{
if(buf[getComma(2, buf)] == 'A') //valid value
{
printf("#DFYY_TEST# GPS_BUF_DATA = %s\n",buf);
timenum = getComma(1,buf);
for(j=0;j<2;j++)
{
str[j] = buf[timenum+j];
}
str[j] = '\0';
pGpsData->hour = atof(str);
memset(str,0,3);
for(j=0;j<2;j++)
{
str[j] = buf[timenum+2+j];
}
str[j] = '\0';
pGpsData->min = atof(str);
memset(str,0,3);
for(j=0;j<2;j++)
{
str[j] = buf[timenum+4+j];
}
str[j] = '\0';
pGpsData->sec = atof(str);
pGpsData->isvalid = 'A';
pGpsData->latitude=get_locate(get_double_number(&buf[getComma(3, buf)]))*1000000;
pGpsData->latDirect = buf[getComma(4, buf)];
pGpsData->longtitude=get_locate(get_double_number(&buf[getComma(5, buf)]))*1000000;
pGpsData->lonDirect=buf[getComma(6, buf)];
pGpsData->speed = (float)(get_double_number(&buf[getComma(7, buf)]) * 1.8);
pGpsData->angle = (float)get_double_number(&buf[getComma(8, buf)]);
timenum=getComma(9, buf);
memset(str,0,3);
for(j=0;j<2;j++)
{
str[j] = buf[timenum+j];
}
str[j] = '\0';
pGpsData->day=atof(str);
memset(str,0,3);
for(j=0;j<2;j++)
{
str[j] = buf[timenum+2+j];
}
str[j] = '\0';
pGpsData->month=atof(str);
memset(str,0,3);
for(j=0;j<2;j++)
{
str[j] = buf[timenum+4+j];
}
str[j] = '\0';
pGpsData->year=atof(str);
printf("lat direct = %c,lat = %ld,lon direct = %c,log = %ld, speed = %f, angle = %f,year = %d,month = %d, data = %d,hour = %d, min = %d, sec = %d\n",
pGpsData->latDirect,pGpsData->latitude,pGpsData->lonDirect,pGpsData->longtitude,pGpsData->speed,pGpsData->angle,
pGpsData->year,pGpsData->month,pGpsData->day,pGpsData->hour,pGpsData->min,pGpsData->sec);
}
else{
pGpsData->isvalid='V';
printf("no valid value data pGpsData->isvalid=[%c]\n",pGpsData->isvalid);
}
}
else
{
printf("this gps data is incorrect,the buf is %s,GetVerification is %d,CalculateVerification is %d\n",buf,
GetGpsBufVerification(buf),CalculateGpsBufVerification(buf));
}
}
gpsMouleSaveLocationData(pGpsData);
free(buf);
free(pData_Bak);
free(pGpsData);
}
unsigned int getTimeStamp(void)
{
struct timespec time;
memset(&time, 0, sizeof(struct timespec));
clock_gettime(CLOCK_REALTIME, &time);
return time.tv_sec;
}
unsigned char BCC( UCHAR *upCreatBcc_Data, unsigned int uCreatBcc_Data_len)
{
int bcc=0;
unsigned int i;
for (i = 2; i<uCreatBcc_Data_len; i++)
bcc ^= upCreatBcc_Data[i];
return bcc;
}
void printRawData( const char *head,UCHAR const * data, int len)
{
int i;
char *p;
char *buffer;
int bufferSize = 3 * (len + 1);
buffer = (char*)malloc(bufferSize);
if(!buffer)
{
return;
}
p = buffer;
memset(p, 0, bufferSize);
for(i=0; i<len; i++)
{
sprintf(p, "%02X ", data[i]);
p += 3;
}
printf( "RAW:%s%s\n", head,buffer);
free(buffer);
}
int make_GBframe(UCHAR MCU_Command, UCHAR ack, UCHAR * data, unsigned int data_length)
{
UCHAR *packet =NULL;
unsigned char bcc = 0;
unsigned char length = data_length + 10;
unsigned char ACK_GB=ack;
MCU_UartRecv.MAJOR = 0X02;
MCU_UartRecv.MINOR = 0X02;
int len = 0;
// char encrypt_type=0x01;
if(data != NULL)
{
// printRawData(data, data_length);
}
packet = ( UCHAR* )malloc(length);
if(packet)
{
memset(packet, 0, length);
memcpy(packet, HEAD_MCU, HEAD_LEN);
memcpy(packet+2, &MCU_Command, 1);
memcpy(packet+3, &ACK_GB, 1);
memcpy(packet+4, &MCU_UartRecv.MAJOR, 1);
memcpy(packet+5, &MCU_UartRecv.MINOR, 1);
packet[6] = 0xff&(data_length>>8);
packet[7] = 0xff&(data_length);
if((data != NULL) && (data_length != 0))
{
memcpy(packet+8, data, data_length);
}
bcc = BCC(packet, 8+ data_length);
memcpy(packet+8+data_length, &bcc, 1);
}
len = 50;
printRawData( " ",packet, len);
// if(((g_sUartModuleHandler.fd >= 0)&&( g_module_infor_status.Available_Status==1)) || (g_sUartModuleHandler.otaStatus))
// {
// len = write(g_sUartModuleHandler.fd,packet,data_length + 9);
// if(len>=0)
// {
// printf(" mcu_manager dispatch %d bytes Via UART \n",len);
// printRawData( /*ELOG_LVL_DEBUG,TAG_NONE,*/" ",packet, len);
// }
// else
// printf(" mcu_manager dispatch error Via UART \n");
// //free(packet);
// }
// else
// {
// printf(" mcu_manager MCU unavailable\n");
// }
if (packet)
free(packet);
return len;
}
void onSendGPSTimeout(void *context)
{
printf("it's time to send GPS data\n");
UCHAR data_temp[31]={0};
data_temp[0] = 0x02;
unsigned int time_stamp = getTimeStamp();
struct tm *p;
p=localtime((time_t *)&(time_stamp));
data_temp[1] = p->tm_year-100;
data_temp[2] = p->tm_mon+1;
data_temp[3] = p->tm_mday;
data_temp[4] = p->tm_hour;
data_temp[5] = p->tm_min;
data_temp[6] = p->tm_sec;
if(g_psLocData->isvalid == 'A')
{
data_temp[7] = 0x00;
}else{
data_temp[7] = 0x01;
}
data_temp[8] = 0x02; //定位系统类型GPS
if(g_psLocData->latDirect == 'N')
{
data_temp[9] = 0x00;
}else{
data_temp[9] = 0x01;
}
data_temp[10] = (UCHAR)(((uint32_t)g_psLocData->latitude)>>24);
data_temp[11] = (UCHAR)(((uint32_t)g_psLocData->latitude)>>16);
data_temp[12] = (UCHAR)(((uint32_t)g_psLocData->latitude)>>8);
data_temp[13] = (UCHAR)((uint32_t)g_psLocData->latitude);
if(g_psLocData->lonDirect == 'E')
{
data_temp[14] = 0x00;
}else{
data_temp[14] = 0x01;
}
data_temp[15] = (UCHAR)(((uint32_t)g_psLocData->longtitude)>>24);
data_temp[16] = (UCHAR)(((uint32_t)g_psLocData->longtitude)>>16);
data_temp[17] = (UCHAR)(((uint32_t)g_psLocData->longtitude)>>8);
data_temp[18] = (UCHAR)((uint32_t)g_psLocData->longtitude);
data_temp[19] = (UCHAR)(((uint32_t)g_psLocData->speed)>>8); //速度
data_temp[20] = (UCHAR)((uint32_t)g_psLocData->speed);
data_temp[21] = 0xFF; //海拔
data_temp[22] = 0xFF;
data_temp[23] = (UCHAR)(((uint32_t)g_psLocData->angle)>>8); //方位角
data_temp[24] = (UCHAR)((uint32_t)g_psLocData->angle);
printf("speed is %f,angle is %f\n",g_psLocData->speed,g_psLocData->angle);
data_temp[25] = g_psLocData->year;
data_temp[26] = g_psLocData->month;
data_temp[27] = g_psLocData->day;
data_temp[28] = g_psLocData->hour+8;
data_temp[29] = g_psLocData->min;
data_temp[30] = g_psLocData->sec;
int len = make_GBframe(0xC6, 0xFE, (UCHAR*)data_temp, sizeof(data_temp));
printRawData("GPS data ",data_temp,sizeof(data_temp));
}
int main(){
g_psLocData = (PS_GbGpsData)malloc(sizeof(S_GbGpsData));
memset(g_psLocData, 0x00, sizeof(S_GbGpsData));
char *buf = NULL;
buf = (char*)malloc(4096);
memset(buf,0,4096);
buf = "$GNRMC,101033.00,A,2322.85223,N,11310.35019,E,0.056,,220824,,,A,V*1A\n"
"$GNGGA,101033.00,2322.85223,N,11310.35019,E,1,06,3.86,21.6,M,-6.1,M,,*65\n"
"$GNGSA,A,3,28,32,31,25,12,,,,,,,,4.33,3.86,1.96,";
int len = strlen(buf);
printf("len = %d\n",len);
handleNmeaData(buf,len);
onSendGPSTimeout(NULL);
if (g_psLocData){
free(g_psLocData);
}
return 0;
}
long 转成16进制
#include <cstdlib>
#include <cstring>
#include <cstdio>
typedef unsigned char UCHAR;
void printRawData( const char *head,UCHAR const * data, int len)
{
int i;
char *p;
char *buffer;
int bufferSize = 3 * (len + 1);
buffer = (char*)malloc(bufferSize);
if(!buffer)
{
return;
}
p = buffer;
memset(p, 0, bufferSize);
for(i=0; i<len; i++)
{
sprintf(p, "%02X ", data[i]);
p += 3;
}
printf( "RAW:%s%s\n", head,buffer);
free(buffer);
}
int main(){
long latitude = 23380870;
unsigned char data_temp[4] = {0};
data_temp[0] = (latitude>>24);
data_temp[1] = (latitude>>16);
data_temp[2] = (latitude>>8);
data_temp[3] = (latitude);
printRawData(" ",data_temp,4);
return 0;
}