gps.c
#include "gps.h"
#include "gpio.h"
#include "usart.h"
#include "stdio.h"
#include "stdarg.h"
#include "string.h"
#include "math.h"
const unsigned int BAUD_id[9]={4800,9600,19200,38400,57600,115200,230400,460800,921600};//模块支持波特率数组
//从buf里面得到第cx个逗号所在的位置
//返回值:0~0XFE,代表逗号所在位置的偏移.
// 0XFF,代表不存在第cx个逗号
unsigned char NMEA_Comma_Pos(unsigned char *buf,unsigned char cx)
{
unsigned char *p=buf;
while(cx)
{
if(*buf=='*'||*buf<' '||*buf>'z')return 0XFF;//遇到'*'或者非法字符,则不存在第cx个逗号
if(*buf==',')cx--;
buf++;
}
return buf-p;
}
//m^n函数
//返回值:m^n次方.
unsigned int NMEA_Pow(unsigned char m,unsigned char n)
{
unsigned int result=1;
while(n--)result*=m;
return result;
}
//str转换为数字,以','或者'*'结束
//buf:数字存储区
//dx:小数点位数,返回给调用函数
//返回值:转换后的数值
int NMEA_Str2num(unsigned char *buf,unsigned char*dx)
{
unsigned char *p=buf;
unsigned int ires=0,fres=0;
unsigned char ilen=0,flen=0,i;
unsigned char mask=0;
int res;
while(1) //得到整数和小数的长度
{
if(*p=='-'){mask|=0X02;p++;}//是负数
if(*p==','||(*p=='*'))break;//遇到结束了
if(*p=='.'){mask|=0X01;p++;}//遇到小数点了
else if(*p>'9'||(*p<'0')) //有非法字符
{
ilen=0;
flen=0;
break;
}
if(mask&0X01)flen++;
else ilen++;
p++;
}
if(mask&0X02)buf++; //去掉负号
for(i=0;i<ilen;i++) //得到整数部分数据
{
ires+=NMEA_Pow(10,ilen-1-i)*(buf[i]-'0');
}
if(flen>5)flen=5; //最多取5位小数
*dx=flen; //小数点位数
for(i=0;i<flen;i++) //得到小数部分数据
{
fres+=NMEA_Pow(10,flen-1-i)*(buf[ilen+1+i]-'0');
}
res=ires*NMEA_Pow(10,flen)+fres;
if(mask&0X02)res=-res;
return res;
}
//分析GPGSV信息
//gpsx:nmea信息结构体
//buf:接收到的GPS数据缓冲区首地址
void NMEA_GPGSV_Analysis(nmea_msg *gpsx,unsigned char *buf)
{
unsigned char *p,*p1,dx;
unsigned char len,i,j,slx=0;
unsigned char posx;
p=buf;
p1=(unsigned char*)strstr((const char *)p,"$GPGSV");
len=p1[7]-'0'; //得到GPGSV的条数
posx=NMEA_Comma_Pos(p1,3); //得到可见卫星总数
if(posx!=0XFF)gpsx->svnum=NMEA_Str2num(p1+posx,&dx);
for(i=0;i<len;i++)
{
p1=(unsigned char*)strstr((const char *)p,"$GPGSV");
for(j=0;j<4;j++)
{
posx=NMEA_Comma_Pos(p1,4+j*4);
if(posx!=0XFF)gpsx->slmsg[slx].num=NMEA_Str2num(p1+posx,&dx); //得到卫星编号
else break;
posx=NMEA_Comma_Pos(p1,5+j*4);
if(posx!=0XFF)gpsx->slmsg[slx].eledeg=NMEA_Str2num(p1+posx,&dx);//得到卫星仰角
else break;
posx=NMEA_Comma_Pos(p1,6+j*4);
if(posx!=0XFF)gpsx->slmsg[slx].azideg=NMEA_Str2num(p1+posx,&dx);//得到卫星方位角
else break;
posx=NMEA_Comma_Pos(p1,7+j*4);
if(posx!=0XFF)gpsx->slmsg[slx].sn=NMEA_Str2num(p1+posx,&dx); //得到卫星信噪比
else break;
slx++;
}
p=p1+1;//切换到下一个GPGSV信息
}
}
//分析BDGSV信息
//gpsx:nmea信息结构体
//buf:接收到的北斗数据缓冲区首地址
void NMEA_BDGSV_Analysis(nmea_msg *gpsx,unsigned char *buf)
{
unsigned char *p,*p1,dx;
unsigned char len,i,j,slx=0;
unsigned char posx;
p=buf;
p1=(unsigned char*)strstr((const char *)p,"$BDGSV");
len=p1[7]-'0'; //得到BDGSV的条数
posx=NMEA_Comma_Pos(p1,3); //得到可见北斗卫星总数
if(posx!=0XFF)gpsx->beidou_svnum=NMEA_Str2num(p1+posx,&dx);
for(i=0;i<len;i++)
{
p1=(unsigned char*)strstr((const char *)p,"$BDGSV");
for(j=0;j<4;j++)
{
posx=NMEA_Comma_Pos(p1,4+j*4);
if(posx!=0XFF)gpsx->beidou_slmsg[slx].beidou_num=NMEA_Str2num(p1+posx,&dx); //得到卫星编号
else break;
posx=NMEA_Comma_Pos(p1,5+j*4);
if(posx!=0XFF)gpsx->beidou_slmsg[slx].beidou_eledeg=NMEA_Str2num(p1+posx,&dx);//得到卫星仰角
else break;
posx=NMEA_Comma_Pos(p1,6+j*4);
if(posx!=0XFF)gpsx->beidou_slmsg[slx].beidou_azideg=NMEA_Str2num(p1+posx,&dx);//得到卫星方位角
else break;
posx=NMEA_Comma_Pos(p1,7+j*4);
if(posx!=0XFF)gpsx->beidou_slmsg[slx].beidou_sn=NMEA_Str2num(p1+posx,&dx); //得到卫星信噪比
else break;
slx++;
}
p=p1+1;//切换到下一个BDGSV信息
}
}
//分析GNGGA信息
//gpsx:nmea信息结构体
//buf:接收到的GPS/北斗数据缓冲区首地址
void NMEA_GNGGA_Analysis(nmea_msg *gpsx,unsigned char *buf)
{
unsigned char *p1,dx;
unsigned char posx;
p1=(unsigned char*)strstr((const char *)buf,"$GNGGA");
posx=NMEA_Comma_Pos(p1,6); //得到GPS状态
if(posx!=0XFF)gpsx->gpssta=NMEA_Str2num(p1+posx,&dx);
posx=NMEA_Comma_Pos(p1,7); //得到用于定位的卫星数
if(posx!=0XFF)gpsx->posslnum=NMEA_Str2num(p1+posx,&dx);
posx=NMEA_Comma_Pos(p1,9); //得到海拔高度
if(posx!=0XFF)gpsx->altitude=NMEA_Str2num(p1+posx,&dx);
}
//分析GNGSA信息
//gpsx:nmea信息结构体
//buf:接收到的GPS/北斗数据缓冲区首地址
void NMEA_GNGSA_Analysis(nmea_msg *gpsx,unsigned char *buf)
{
unsigned char *p1,dx;
unsigned char posx;
unsigned char i;
p1=(unsigned char*)strstr((const char *)buf,"$GNGSA");
posx=NMEA_Comma_Pos(p1,2); //得到定位类型
if(posx!=0XFF)gpsx->fixmode=NMEA_Str2num(p1+posx,&dx);
for(i=0;i<12;i++) //得到定位卫星编号
{
posx=NMEA_Comma_Pos(p1,3+i);
if(posx!=0XFF)gpsx->possl[i]=NMEA_Str2num(p1+posx,&dx);
else break;
}
posx=NMEA_Comma_Pos(p1,15); //得到PDOP位置精度因子
if(posx!=0XFF)gpsx->pdop=NMEA_Str2num(p1+posx,&dx);
posx=NMEA_Comma_Pos(p1,16); //得到HDOP位置精度因子
if(posx!=0XFF)gpsx->hdop=NMEA_Str2num(p1+posx,&dx);
posx=NMEA_Comma_Pos(p1,17); //得到VDOP位置精度因子
if(posx!=0XFF)gpsx->vdop=NMEA_Str2num(p1+posx,&dx);
}
//分析GNRMC信息
//gpsx:nmea信息结构体
//buf:接收到的GPS/北斗数据缓冲区首地址
void NMEA_GNRMC_Analysis(nmea_msg *gpsx,unsigned char *buf)
{
unsigned char *p1,dx;
unsigned char posx;
unsigned int temp;
float rs;
p1=(unsigned char*)strstr((const char *)buf,"GNRMC");//"$GNRMC",经常有&和GNRMC分开的情况,故只判断GPRMC.
posx=NMEA_Comma_Pos(p1,1); //得到UTC时间
if(posx!=0XFF)
{
temp=NMEA_Str2num(p1+posx,&dx)/NMEA_Pow(10,dx); //得到UTC时间,去掉ms
gpsx->utc.hour=(temp/10000)+8;
gpsx->utc.min=(temp/100)%100;
gpsx->utc.sec=temp%100;
}
posx=NMEA_Comma_Pos(p1,3); //得到纬度
if(posx!=0XFF)
{
temp=NMEA_Str2num(p1+posx,&dx);
gpsx->latitude=temp/NMEA_Pow(10,dx+2); //得到°
rs=temp%NMEA_Pow(10,dx+2); //得到'
gpsx->latitude=gpsx->latitude*NMEA_Pow(10,5)+(rs*NMEA_Pow(10,5-dx))/60;//转换为°
}
posx=NMEA_Comma_Pos(p1,4); //南纬还是北纬
if(posx!=0XFF)gpsx->nshemi=*(p1+posx);
posx=NMEA_Comma_Pos(p1,5); //得到经度
if(posx!=0XFF)
{
temp=NMEA_Str2num(p1+posx,&dx);
gpsx->longitude=temp/NMEA_Pow(10,dx+2); //得到°
rs=temp%NMEA_Pow(10,dx+2); //得到'
gpsx->longitude=gpsx->longitude*NMEA_Pow(10,5)+(rs*NMEA_Pow(10,5-dx))/60;//转换为°
}
posx=NMEA_Comma_Pos(p1,6); //东经还是西经
if(posx!=0XFF)gpsx->ewhemi=*(p1+posx);
posx=NMEA_Comma_Pos(p1,9); //得到UTC日期
if(posx!=0XFF)
{
temp=NMEA_Str2num(p1+posx,&dx); //得到UTC日期
gpsx->utc.date=temp/10000;
gpsx->utc.month=(temp/100)%100;
gpsx->utc.year=2000+temp%100;
}
}
//分析GNVTG信息
//gpsx:nmea信息结构体
//buf:接收到的GPS/北斗数据缓冲区首地址
void NMEA_GNVTG_Analysis(nmea_msg *gpsx,unsigned char *buf)
{
unsigned char *p1,dx;
unsigned char posx;
p1=(unsigned char*)strstr((const char *)buf,"$GNVTG");
posx=NMEA_Comma_Pos(p1,7); //得到地面速率
if(posx!=0XFF)
{
gpsx->speed=NMEA_Str2num(p1+posx,&dx);
if(dx<3)gpsx->speed*=NMEA_Pow(10,3-dx); //确保扩大1000倍
}
}
//提取NMEA-0183信息
//gpsx:nmea信息结构体
//buf:接收到的GPS/北斗数据缓冲区首地址
void GPS_Analysis(nmea_msg *gpsx,unsigned char *buf)
{
NMEA_GPGSV_Analysis(gpsx,buf); //GPGSV解析
NMEA_BDGSV_Analysis(gpsx,buf); //BDGSV解析
NMEA_GNGGA_Analysis(gpsx,buf); //GNGGA解析
NMEA_GNGSA_Analysis(gpsx,buf); //GNGSA解析
NMEA_GNRMC_Analysis(gpsx,buf); //GNRMC解析
NMEA_GNVTG_Analysis(gpsx,buf); //GNVTG解析
}
///SkyTraq 配置代码/
检查CFG配置执行情况
返回值:0,ACK成功
1,接收超时错误
2,没有找到同步字符
3,接收到NACK应答
unsigned char SkyTra_Cfg_Ack_Check(void)
{
unsigned short len=0,i;
unsigned char rval=0;
while((USART3_RX_STA&0X8000)==0 && len<100)//等待接收到应答
{
len++;
HAL_Delay(5);
}
if(len<100) //超时错误.
{
len=USART3_RX_STA&0X7FFF; //此次接收到的数据长度
for(i=0;i<len;i++)
{
if(USART3_RX_BUF[i]==0X83)break;
else if(USART3_RX_BUF[i]==0X84)
{
rval=3;
break;
}
}
if(i==len)rval=2; //没有找到同步字符
}else rval=1; //接收超时错误
USART3_RX_STA=0; //清除接收
return rval;
}
//配置SkyTra_GPS/北斗模块波特率
//baud_id:0~8,对应波特率,4800/9600/19200/38400/57600/115200/230400/460800/921600
//返回值:0,执行成功;其他,执行失败(这里不会返回0了)
unsigned char SkyTra_Cfg_Prt(unsigned char baud_id)
{
SkyTra_baudrate *cfg_prt=(SkyTra_baudrate *)USART3_TX_BUF;
cfg_prt->sos=0XA1A0; //引导序列(小端模式)
cfg_prt->PL=0X0400; //有效数据长度(小端模式)
cfg_prt->id=0X05; //配置波特率的ID
cfg_prt->com_port=0X00; //操作串口1
cfg_prt->Baud_id=baud_id; 波特率对应编号
cfg_prt->Attributes=1; //保存到SRAM&FLASH
cfg_prt->CS=cfg_prt->id^cfg_prt->com_port^cfg_prt->Baud_id^cfg_prt->Attributes;
cfg_prt->end=0X0A0D; //发送结束符(小端模式)
SkyTra_Send_Date((unsigned char*)cfg_prt,sizeof(SkyTra_baudrate));//发送数据给SkyTra
HAL_Delay(200); //等待发送完成
return SkyTra_Cfg_Ack_Check();
}
//配置SkyTra_GPS/北斗模块的时钟脉冲宽度
//width:脉冲宽度1~100000(us)
//返回值:0,发送成功;其他,发送失败.
unsigned char SkyTra_Cfg_Tp(unsigned int width)
{
unsigned int temp=width;
SkyTra_pps_width *cfg_tp=(SkyTra_pps_width *)USART3_TX_BUF;
temp=(width>>24)|((width>>8)&0X0000FF00)|((width<<8)&0X00FF0000)|((width<<24)&0XFF000000);//小端模式
cfg_tp->sos=0XA1A0; //cfg header(小端模式)
cfg_tp->PL=0X0700; //有效数据长度(小端模式)
cfg_tp->id=0X65 ; //cfg tp id
cfg_tp->Sub_ID=0X01; //数据区长度为20个字节.
cfg_tp->width=temp; //脉冲宽度,us
cfg_tp->Attributes=0X01; //保存到SRAM&FLASH
cfg_tp->CS=cfg_tp->id^cfg_tp->Sub_ID^(cfg_tp->width>>24)^(cfg_tp->width>>16)&0XFF^(cfg_tp->width>>8)&0XFF^cfg_tp->width&0XFF^cfg_tp->Attributes;
cfg_tp->end=0X0A0D; //发送结束符(小端模式)
SkyTra_Send_Date((unsigned char*)cfg_tp,sizeof(SkyTra_pps_width));//发送数据给SkyTraF8-BD
return SkyTra_Cfg_Ack_Check();
}
//配置SkyTraF8-BD的更新速率
//Frep:(取值范围:1,2,4,5,8,10,20)测量时间间隔,单位为Hz,最大不能大于20Hz
//返回值:0,发送成功;其他,发送失败.
unsigned char SkyTra_Cfg_Rate(unsigned char Frep)
{
SkyTra_PosRate *cfg_rate=(SkyTra_PosRate *)USART3_TX_BUF;
cfg_rate->sos=0XA1A0; //cfg header(小端模式)
cfg_rate->PL=0X0300; //有效数据长度(小端模式)
cfg_rate->id=0X0E; //cfg rate id
cfg_rate->rate=Frep; //更新速率
cfg_rate->Attributes=0X01; //保存到SRAM&FLASH .
cfg_rate->CS=cfg_rate->id^cfg_rate->rate^cfg_rate->Attributes; //校验值
cfg_rate->end=0X0A0D; //发送结束符(小端模式)
SkyTra_Send_Date((unsigned char*)cfg_rate,sizeof(SkyTra_PosRate));//发送数据给SkyTraF8-BD
return SkyTra_Cfg_Ack_Check();
}
//发送一批数据给SkyTraF8-BD,这里通过串口3发送
//dbuf:数据缓存首地址
//len:要发送的字节数
void SkyTra_Send_Date(unsigned char* dbuf,unsigned short len)
{
unsigned short j;
for(j=0;j<len;j++)//循环发送数据
{
while((USART3->ISR&0X40)==0);//循环发送,直到发送完毕
USART3->TDR=dbuf[j];
}
}
|