数据解析，肯定是要知道数据格式的：
数据格式参考：（前人已经总结的比较齐全了）
星网宇达的XW-GI类产品，GPS解析到的协议是GPFPD：GI定位定姿消息集。其数据格式如下所示：
最高输出速率：100HZ
举例：
配置接收机的输出格式为：（使用cutecom接收，输出频率为5Hz）
[13:29:49:216] $GPFPD,1356,20.60,0.000,0.000,0.000,0.000000000,0.000000000,0.000,0.000,0.000,0.000,0.000,0,0,0*72␍␊
[13:29:49:225] $GPRMC,000006.60,V,0000.0000000,S,00000.0000000,W,0.000,0.0,010106,0.0,E,N*24␍␊
[13:29:49:230] #HEADINGA,COM1,0,0,FINESTEERING,1356,20.600,00000000,0000,0036;SOL_COMPUTED,NONE,0.000000000,0.000000000,0.000000000,0,0.000,0.000,"0000",0,0,0,0,00,00,30,17*3CB347CB␍␊
[13:29:49:416] $GPFPD,1356,20.80,0.000,0.000,0.000,0.000000000,0.000000000,0.000,0.000,0.000,0.000,0.000,0,0,0*7C␍␊
[13:29:49:425] $GPRMC,000006.80,V,0000.0000000,S,00000.0000000,W,0.000,0.0,010106,0.0,E,N*2A␍␊
[13:29:49:430] #HEADINGA,COM1,0,0,FINESTEERING,1356,20.800,00000000,0000,0036;SOL_COMPUTED,NONE,0.000000000,0.000000000,0.000000000,0,0.000,0.000,"0000",0,0,0,0,00,00,30,17*1A63E506␍
使用ROS编写节点进行解析，思路为：
1，配置串口，与接收机的波特率等设置保持一致；配置节点循环频率为5Hz，频率相同会简单很多；
2，打开串口，接收数据；
3，数据分割为三个部分，GPFPD/GPRMC/HEADINGA；
4，使用sscanf函数进行解析（也可以使用C++的分割函数，本人更熟悉C语言。。）
5，存放在结构体中，打印显示。
现实结果为：（terminal显示）
gpfpd=
$GPFPD,1356,13110.00,0.000,0.000,0.000,0.000000000,0.000000000,0.000,0.000,0.000,0.000,0.000,0,0,0*44
gprmc=
$GPRMC,033816.00,V,0000.0000000,S,00000.0000000,W,0.000,0.0,010106,0.0,E,N*2B
headingA=
#HEADINGA,COM1,0,0,FINESTEERING,1356,13110.000,00000000,0000,0036;SOL_COMPUTED,NONE,0.000000000,0.000000000,0.000000000,0,0.000,0.000,"0000",0,0,0,0,00,00,30,17*22C246A7
*************GPS_data*****************
utc_time:033816.00
status:V
latitude:S 0.000000000
longtitude:W 0.000000000
speed:0.000000
azimuth_angle:0.000000
utc_data:010106
isRTKstate:N
*************GPFDP_data*****************
GPSWeek:1356
GPSTime:13110.00
Heading:0.000000
Pitch:0.000000
Roll:0.000000
Latitude: 0.000000000
Longitude: 0.000000000
Altitude: 0.000000000
[Ve Vn Vu]=[0.000000 0.000000 0.000000]
Baseline =0.000000
Status:0
*************HEADINGA_DATA*****************
cal_state=SOL_COMPUTED
pos_type=NONE
baseline=0.000000
HeadingA=0.000000
Pitch=0.000000
（以上为显示，下面上代码）
头文件gps.h
#ifndef __GPS_H__
#define __GPS_H__
#include<stdio.h>
#include<stdlib.h>
#include<unistd.h>
#include<string.h>
#define GPS_LEN 512
#define BUF_SIZE 16
typedef unsigned int u32;
typedef unsigned short u16;
typedef unsigned char u8;
typedef struct __gnrmc__
{
u32 time;
char pos_state;
float latitude;
float longitude;
float speed;
float direction;
u32 date;
float declination;
char dd;
char mode;
}GNRMC;
int gps_analyse(char *buff, GNRMC *gps_date);
int print_gps(GNRMC *gps_date);
// define GPS_INFO
typedef struct gps_info
{
char utc_time[BUF_SIZE];
char status;
double latitude_value;
char latitude;
double longtitude_value;
char longtitude;
float speed;
float azimuth_angle;
char utc_data[BUF_SIZE];
float magnetic_declination;
char magnetic_declination_dir;
char isRTKstate[4];
}GPS_INFO;
int gprmc_analyse(char *buff,GPS_INFO *gprmc_data);
int gprmc_print(GPS_INFO *gprmc_data);
//***********define GPFDP_info**************
typedef struct gpfpd_info
{
unsigned int GPSWeek; //自1980-1-6至当前的星期数
char GPSTime[BUF_SIZE]; //本周00：00：00至当前的秒数
float Heading; //偏航角
float Pitch; //俯仰角
float Roll; //滚转角
double Latitude; //纬度 deg
double Longitude; //经度 deg
float Altitude; //高度 m
float Ve; //东 速度 m/s
float Vn; //北 速度 m/s
float Vu; //天 速度 m/s
float Baseline; //基线长度
int NSV1; //天线1卫星数
int NSV2; //天线2卫星数
char Status[4]; //系统状态
}GPFPD_INFO;
int gpfdp_analyse(char *buff,GPFPD_INFO *gpfdp_data);
int gpdfp_print(GPFPD_INFO *gpfdp_data);
//***********define headingA_info**************
typedef struct headingA_info
{
char cal_state[32]; //SOL_COMPUTED 完全解算 INSUFFICIENT_OBS 观测量不足 COLD_START 冷启动，尚未完全解算
char pos_type[16]; //NONE 未解算
/*FIXEDPOS 已设置固定坐标
SINGLE 单点解定位
PSRDIFF 伪距差分解定位
NARROW_FLOAT 浮点解
WIDE_INT 宽带固定解
NARROE_INT 窄带固定解
SUPER WIDE_LINE 超宽带解 */
float baseline; //基线长度
float HeadingA; //方位角
float Pitch; //俯仰角
}HEADINGA_INFO;
int headingA_analyse(char *buff,HEADINGA_INFO *headinga_data);
int headingA_print(HEADINGA_INFO *headinga_data);
#endif
gps.cpp
#include "/home/ling/catkin_ws/src/serialdemo/include/serialdemo/gps.h"
//************************get-gnrmc-data***************************
int gps_analyse (char *buff,GNRMC *gps_data)
{
char *ptr = NULL;
if(gps_data == NULL)
{
return -1;
}
if(strlen(buff)<10)
{
return -1;
}
//如果buff字符串中包含字符"$GNRMC"则将$GNRMC的地址赋值给ptr
if(NULL==(ptr=strstr(buff,"$GNRMC")))
{
return -1;
}
sscanf(ptr,"$GNRMC,%d.000,%c,%f,N,%f,E,%f,%f,%d,,,%c*",&(gps_data->time),&(gps_data->pos_state),&(gps_data->latitude),&(gps_data->longitude),&(gps_data->speed),&(gps_data->direction),&(gps_data->date),&(gps_data->mode));
//sscanf函数为从字符串输入，上面这句话的意思是将ptr内存单元的值作为输入分别输入到后面的结构体成员
return 0;
}
int print_gps (GNRMC *gps_data)
{
printf("===========================================================\n");
printf("== GPS状态位 : %c [A:有效状态 V:无效状态] \n",gps_data->pos_state);
printf("== GPS模式位 : %c [A:自主定位 D:差分定位] \n", gps_data->mode);
printf("== 日期 : 20%02d-%02d-%02d \n",gps_data->date%100,(gps_data->date%10000)/100,gps_data->date/10000);
printf("== 时间 : %02d:%02d:%02d \n",(gps_data->time/10000+8)%24,(gps_data->time%10000)/100,gps_data->time%100);
printf("== 纬度 : 北纬:%d度%d分%d秒 \n", ((int)gps_data->latitude) / 100, (int)(gps_data->latitude - ((int)gps_data->latitude / 100 * 100)), (int)(((gps_data->latitude - ((int)gps_data->latitude / 100 * 100)) - ((int)gps_data->latitude - ((int)gps_data->latitude / 100 * 100))) * 60.0));
printf("== 经度 : 东经:%d度%d分%d秒 \n", ((int)gps_data->longitude) / 100, (int)(gps_data->longitude - ((int)gps_data->longitude / 100 * 100)), (int)(((gps_data->longitude - ((int)gps_data->longitude / 100 * 100)) - ((int)gps_data->longitude - ((int)gps_data->longitude / 100 * 100))) * 60.0));
printf("== 速度 : %.3f m/s \n",gps_data->speed);
printf("============================================================\n");
return 0;
}
//***************************get gprmc data**************************
int gprmc_analyse(char *buff,GPS_INFO *gprmc_data)
{
char *wellhandled_string;
size_t i=0,readcnt=0;
if(strlen(buff)<32)
{
// length of frame is 72,if datalength is too short,return.
return -1;
}
if ((wellhandled_string = strstr(buff,"$GPRMC")) != NULL)
{
for (i=0; i<strlen(wellhandled_string); i++)
{
if (wellhandled_string[i] == '\n')
{
wellhandled_string[i] = '\0'; //replace ‘/n’ with null
}
}
}
readcnt=sscanf(wellhandled_string,"$GPRMC,%[^,],%c,%lf,%c,%lf,%c,%f,%f,%[^,],%*f,%*c,%[^*]",
gprmc_data->utc_time,\
&(gprmc_data->status),&(gprmc_data->latitude_value),&(gprmc_data->latitude),\
&(gprmc_data->longtitude_value),&(gprmc_data->longtitude),&(gprmc_data->speed),\
&(gprmc_data->azimuth_angle),\
gprmc_data->utc_data,gprmc_data->isRTKstate);
//printf("readcnt=%d\n",readcnt);
return 0;
}
int gprmc_print(GPS_INFO *gprmc_data)
{
printf("*************GPS_data*****************\n");
printf("utc_time:%s\n",gprmc_data->utc_time);
printf("status:%c\n",gprmc_data->status);
printf("latitude:%c ",gprmc_data->latitude);
printf("%12.9lf\n",gprmc_data->latitude_value);
printf("longtitude:%c ",gprmc_data->longtitude);
printf("%12.9lf\n",gprmc_data->longtitude_value);
printf("speed:%f\n",gprmc_data->speed);
printf("azimuth_angle:%f\n",gprmc_data->azimuth_angle);
printf("utc_data:%s\n",gprmc_data->utc_data);
//printf("magnetic_declination:%f %c\n",gprmc_data->magnetic_declination,gprmc_data->magnetic_declination_dir);
printf("isRTKstate:%c\n",gprmc_data->isRTKstate[0]);
}
//***************************get gpfdp data**************************
int gpfdp_analyse(char *buff,GPFPD_INFO *gpfdp_data)
{
char *wellhandled_temp;
size_t i=0,reti=0;
if(strlen(buff)<32)
{
return -1;// length of frame is 98,if datalength is too short,return.
}
if ((wellhandled_temp = strstr(buff,"$GPFPD")) != NULL)
{
for (i=0; i<strlen(wellhandled_temp); i++)
{
if (wellhandled_temp[i] == '\n')
{
wellhandled_temp[i] = '\0';
}
}
}
//printf("wellhandled_temp=%s\n\n",wellhandled_temp);
reti=sscanf(wellhandled_temp,"$GPFPD,%d,%[^,],%f,%f,%f,%lf,%lf,%f,%f,%f,%f,%f,%i,%i,%[^*]",
&(gpfdp_data->GPSWeek),gpfdp_data->GPSTime,&(gpfdp_data->Heading),&(gpfdp_data->Pitch),&(gpfdp_data->Roll), \
&(gpfdp_data->Latitude),&(gpfdp_data->Longitude),&(gpfdp_data->Altitude),\
&(gpfdp_data->Ve),&(gpfdp_data->Vn),&(gpfdp_data->Vu),\
&(gpfdp_data->Baseline),&(gpfdp_data->NSV1),&(gpfdp_data->NSV2),\
gpfdp_data->Status);
//printf("reti=%d\n",reti);
//wellhandled_temp=$GPFPD,1356,9013.40,0.000,0.000,0.000,0.000000000,0.000000000,0.000,0.000,0.000,0.000,0.000,0,0,0*79
return 0;
}
int gpdfp_print(GPFPD_INFO *gpfdp_data)
{
printf("*************GPFDP_data*****************\n");
printf("GPSWeek:%d\n",gpfdp_data->GPSWeek);
printf("GPSTime:%s\n",gpfdp_data->GPSTime);
printf("Heading:%f\n",gpfdp_data->Heading);
printf("Pitch:%f\n",gpfdp_data->Pitch);
printf("Roll:%f\n",gpfdp_data->Roll);
printf("Latitude:%12.9lf\n",gpfdp_data->Latitude);
printf("Longitude:%12.9lf\n",gpfdp_data->Longitude);
printf("Altitude:%12.9lf\n",gpfdp_data->Altitude);
printf("[Ve Vn Vu]=[%f %f %f]\n",gpfdp_data->Ve,gpfdp_data->Vn,gpfdp_data->Vu);
printf("Baseline =%f\n",gpfdp_data->Baseline);
printf("Status:%s\n",gpfdp_data->Status);
}
//***************************get headingA data**************************
int headingA_analyse(char *buff,HEADINGA_INFO *headinga_data)
{
char *headingA_temp;
size_t i=0,reti=0;
if(strlen(buff)<100)
{
return -1;
}
if ((headingA_temp = strstr(buff,";")) != NULL)
{
for (i=0; i<strlen(headingA_temp); i++)
{
if (headingA_temp[i] == '\n')
{
headingA_temp[i] = '\0';
}
}
}
//printf("headingA_temp=%s\n\n",headingA_temp);
reti=sscanf(headingA_temp,";%[^,],%[^,],%f,%f,%f",
headinga_data->cal_state,headinga_data->pos_type,&(headinga_data->baseline),\
&(headinga_data->HeadingA),&(headinga_data->Pitch));
//printf("reti=%d\n",reti);
}
int headingA_print(HEADINGA_INFO *headinga_data)
{
printf("*************HEADINGA_DATA*****************\n");
printf("cal_state=%s\n",headinga_data->cal_state);
printf("pos_type=%s\n",headinga_data->pos_type);
printf("baseline=%f\n",headinga_data->baseline);
printf("HeadingA=%f\n",headinga_data->HeadingA);
printf("Pitch=%f\n",headinga_data->Pitch);
}
主节点：
serial_port.cpp
//serial_port.cpp
#include <ros/ros.h>
#include <serial/serial.h>
#include <iostream>
#include "/home/ling/catkin_ws/src/serialdemo/include/serialdemo/gps.h"
GPS_INFO gps_info;
GPS_INFO *rmc_info=&gps_info;
GPFPD_INFO gpfdp_info;
GPFPD_INFO *gpfdp_pdata=&gpfdp_info;
HEADINGA_INFO headingA_info;
HEADINGA_INFO *headingA_pdata=&headingA_info;
char buff[2048];
char GPFPD_buf[1024];
char GPRMC_buf[1024];
char HEADINGA_buf[1024];
int main(int argc, char** argv)
{
ros::init(argc, argv, "serial_port");
//创建句柄（虽然后面没用到这个句柄，但如果不创建，运行时进程会出错）
ros::NodeHandle n;
//创建一个serial类
serial::Serial sp;
//创建timeout
serial::Timeout to = serial::Timeout::simpleTimeout(1000);
//设置要打开的串口名称
sp.setPort("/dev/ttyUSB0");
//sp.setPort("/dev/ttyS0");
//设置串口通信的波特率
sp.setBaudrate(115200);
//串口设置timeout
sp.setTimeout(to);
try
{
//打开串口
sp.open();
}
catch(serial::IOException& e)
{
ROS_ERROR_STREAM("Unable to open port.");
return -1;
}
//判断串口是否打开成功
if(sp.isOpen())
{
ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
}
else
{
return -1;
}
ros::Rate loop_rate(5);
while(ros::ok())
{
//获取缓冲区内的字节数
size_t n = sp.available();
if(n!=0)
{
uint8_t buffer[2048];
//读出数据
n = sp.read(buffer, n);
for(int i=0; i<n; i++)
{
//16进制的方式打印到屏幕
//std::cout << std::hex << (buffer[i] & 0xff) << " ";
buff[i]=buffer[i];
//printf("%c",buff[i]);
if(buffer[i]=='$'&&buffer[i+1]=='G'&&buffer[i+2]=='P'&&buffer[i+3]=='F'&&buffer[i+4]=='P' && buffer[i+5]=='D')
{
for(int j=0;j<102 && buffer[i+j] != '\n';j++)
{
GPFPD_buf[j]=buffer[i+j];
}
printf("gpfpd=\n%s\n",GPFPD_buf);
}
if(buffer[i]=='$'&&buffer[i+1]=='G'&&buffer[i+2]=='P'&&buffer[i+3]=='R'&&buffer[i+4]=='M' && buffer[i+5]=='C')
{
for(int j=0;j<100 && buffer[i+j] != '\n';j++)
{
GPRMC_buf[j]=buffer[i+j];
}
printf("gprmc=\n%s\n",GPRMC_buf);
}
if(buffer[i]=='#'&&buffer[i+1]=='H'&&buffer[i+2]=='E'&&buffer[i+3]=='A'&&buffer[i+4]=='D' && buffer[i+5]=='I')
{
for(int j=0;j<300 && buffer[i+j] != '\n';j++)
{
HEADINGA_buf[j]=buffer[i+j];
}
printf("headingA=\n%s\n",HEADINGA_buf);
}
}
std::cout << std::endl;
//jiexie gps
gprmc_analyse(GPRMC_buf,rmc_info);
gprmc_print(rmc_info);
gpfdp_analyse(GPFPD_buf,gpfdp_pdata);
gpdfp_print(gpfdp_pdata);
headingA_analyse(HEADINGA_buf,headingA_pdata);
headingA_print(headingA_pdata);
//把数据发送回去
//sp.write(buffer, n);
}
memset(buff,'\0',sizeof(buff));
loop_rate.sleep();
}
//关闭串口
sp.close();
return 0;
}
越来越发现使用ROS可以很方便的连接起来各种传感器，彼此之间通信特别方便。以上过程中，遇到的问题有：linux中的串口配置，ros的seria功能包是使用，ros如何使用自己定义的头文件和源文件，sscanf函数的具体使用等。遇到问题欢迎留言交流～～