gps.c 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414
  1. #include "gps.h"
  2. #include "stdio.h"
  3. #include "stdarg.h"
  4. #include "string.h"
  5. #include "math.h"
  6. //从buf里面得到第cx个逗号所在的位置
  7. //返回值:0~0XFE,代表逗号所在位置的偏移.
  8. // 0XFF,代表不存在第cx个逗号
  9. uint8_t NMEA_Comma_Pos(uint8_t *buf,uint8_t cx)
  10. {
  11. uint8_t *p=buf;
  12. while(cx)
  13. {
  14. if(*buf=='*'||*buf<' '||*buf>'z')return 0XFF;//遇到'*'或者非法字符,则不存在第cx个逗号
  15. if(*buf==',')cx--;
  16. buf++;
  17. }
  18. return buf-p;
  19. }
  20. //m^n函数
  21. //返回值:m^n次方.
  22. uint32_t NMEA_Pow(uint8_t m,uint8_t n)
  23. {
  24. uint32_t result=1;
  25. while(n--)result*=m;
  26. return result;
  27. }
  28. //str转换为数字,以','或者'*'结束
  29. //buf:数字存储区
  30. //dx:小数点位数,返回给调用函数
  31. //返回值:转换后的数值
  32. int NMEA_Str2num(uint8_t *buf,uint8_t*dx)
  33. {
  34. uint8_t *p=buf;
  35. uint32_t ires=0,fres=0;
  36. uint8_t ilen=0,flen=0,i;
  37. uint8_t mask=0;
  38. int res;
  39. while(1) //得到整数和小数的长度
  40. {
  41. if(*p=='-'){mask|=0X02;p++;}//是负数
  42. if(*p==','||(*p=='*'))break;//遇到结束了
  43. if(*p=='.'){mask|=0X01;p++;}//遇到小数点了
  44. else if(*p>'9'||(*p<'0')) //有非法字符
  45. {
  46. ilen=0;
  47. flen=0;
  48. break;
  49. }
  50. if(mask&0X01)flen++;
  51. else ilen++;
  52. p++;
  53. }
  54. if(mask&0X02)buf++; //去掉负号
  55. for(i=0;i<ilen;i++) //得到整数部分数据
  56. {
  57. ires+=NMEA_Pow(10,ilen-1-i)*(buf[i]-'0');
  58. }
  59. if(flen>5)flen=5; //最多取5位小数
  60. *dx=flen; //小数点位数
  61. for(i=0;i<flen;i++) //得到小数部分数据
  62. {
  63. fres+=NMEA_Pow(10,flen-1-i)*(buf[ilen+1+i]-'0');
  64. }
  65. res=ires*NMEA_Pow(10,flen)+fres;
  66. if(mask&0X02)res=-res;
  67. return res;
  68. }
  69. //分析GPGSV信息
  70. //gpsx:nmea信息结构体
  71. //buf:接收到的GPS数据缓冲区首地址
  72. void NMEA_GPGSV_Analysis(nmea_msg *gpsx,uint8_t *buf)
  73. {
  74. uint8_t *p,*p1,dx;
  75. uint8_t len,i,j,slx=0;
  76. uint8_t posx;
  77. p=buf;
  78. p1=(uint8_t*)strstr((const char *)p,"$GNGSV");
  79. len=p1[7]-'0'; //得到GPGSV的条数
  80. posx=NMEA_Comma_Pos(p1,3); //得到可见卫星总数
  81. if(posx!=0XFF)gpsx->svnum=NMEA_Str2num(p1+posx,&dx);
  82. for(i=0;i<len;i++)
  83. {
  84. p1=(uint8_t*)strstr((const char *)p,"$GNGSV");
  85. for(j=0;j<4;j++)
  86. {
  87. posx=NMEA_Comma_Pos(p1,4+j*4);
  88. if(posx!=0XFF)gpsx->slmsg[slx].num=NMEA_Str2num(p1+posx,&dx); //得到卫星编号
  89. else break;
  90. posx=NMEA_Comma_Pos(p1,5+j*4);
  91. if(posx!=0XFF)gpsx->slmsg[slx].eledeg=NMEA_Str2num(p1+posx,&dx);//得到卫星仰角
  92. else break;
  93. posx=NMEA_Comma_Pos(p1,6+j*4);
  94. if(posx!=0XFF)gpsx->slmsg[slx].azideg=NMEA_Str2num(p1+posx,&dx);//得到卫星方位角
  95. else break;
  96. posx=NMEA_Comma_Pos(p1,7+j*4);
  97. if(posx!=0XFF)gpsx->slmsg[slx].sn=NMEA_Str2num(p1+posx,&dx); //得到卫星信噪比
  98. else break;
  99. slx++;
  100. }
  101. p=p1+1;//切换到下一个GPGSV信息
  102. }
  103. }
  104. //分析GPGGA信息
  105. //gpsx:nmea信息结构体
  106. //buf:接收到的GPS数据缓冲区首地址
  107. void NMEA_GPGGA_Analysis(nmea_msg *gpsx,uint8_t *buf)
  108. {
  109. uint8_t *p1,dx;
  110. uint8_t posx;
  111. p1=(uint8_t*)strstr((const char *)buf,"$GNGGA");
  112. posx=NMEA_Comma_Pos(p1,6); //得到GPS状态
  113. if(posx!=0XFF)gpsx->gpssta=NMEA_Str2num(p1+posx,&dx);
  114. posx=NMEA_Comma_Pos(p1,7); //得到用于定位的卫星数
  115. if(posx!=0XFF)gpsx->posslnum=NMEA_Str2num(p1+posx,&dx);
  116. posx=NMEA_Comma_Pos(p1,9); //得到海拔高度
  117. if(posx!=0XFF)gpsx->altitude=NMEA_Str2num(p1+posx,&dx);
  118. uint32_t temp;
  119. float rs;
  120. posx=NMEA_Comma_Pos(p1,2); //得到纬度
  121. if(posx!=0XFF)
  122. {
  123. temp=NMEA_Str2num(p1+posx,&dx);
  124. gpsx->latitude=temp/NMEA_Pow(10,dx+2); //得到°
  125. rs=temp%NMEA_Pow(10,dx+2); //得到'
  126. gpsx->latitude=gpsx->latitude*NMEA_Pow(10,5)+(rs*NMEA_Pow(10,5-dx))/60;//转换为°
  127. }
  128. posx=NMEA_Comma_Pos(p1,3); //南纬还是北纬
  129. if(posx!=0XFF)gpsx->nshemi=*(p1+posx);
  130. posx=NMEA_Comma_Pos(p1,4); //得到经度
  131. if(posx!=0XFF)
  132. {
  133. temp=NMEA_Str2num(p1+posx,&dx);
  134. gpsx->longitude=temp/NMEA_Pow(10,dx+2); //得到°
  135. rs=temp%NMEA_Pow(10,dx+2); //得到'
  136. gpsx->longitude=gpsx->longitude*NMEA_Pow(10,5)+(rs*NMEA_Pow(10,5-dx))/60;//转换为°
  137. }
  138. posx=NMEA_Comma_Pos(p1,5); //东经还是西经
  139. if(posx!=0XFF)gpsx->ewhemi=*(p1+posx);
  140. // posx=NMEA_Comma_Pos(p1,1); //得到UTC时间
  141. // if(posx!=0XFF)
  142. // {
  143. // temp=NMEA_Str2num(p1+posx,&dx); //得到UTC时间
  144. // gpsx->utc.date=temp/10000;
  145. // gpsx->utc.month=(temp/100)%100;
  146. // gpsx->utc.year=2000+temp%100;
  147. // }
  148. }
  149. //分析GPGSA信息
  150. //gpsx:nmea信息结构体
  151. //buf:接收到的GPS数据缓冲区首地址
  152. void NMEA_GPGSA_Analysis(nmea_msg *gpsx,uint8_t *buf)
  153. {
  154. uint8_t *p1,dx;
  155. uint8_t posx;
  156. uint8_t i;
  157. p1=(uint8_t*)strstr((const char *)buf,"$GNGSA");
  158. posx=NMEA_Comma_Pos(p1,2); //得到定位类型
  159. if(posx!=0XFF)gpsx->fixmode=NMEA_Str2num(p1+posx,&dx);
  160. for(i=0;i<12;i++) //得到定位卫星编号
  161. {
  162. posx=NMEA_Comma_Pos(p1,3+i);
  163. if(posx!=0XFF)gpsx->possl[i]=NMEA_Str2num(p1+posx,&dx);
  164. else break;
  165. }
  166. posx=NMEA_Comma_Pos(p1,15); //得到PDOP位置精度因子
  167. if(posx!=0XFF)gpsx->pdop=NMEA_Str2num(p1+posx,&dx);
  168. posx=NMEA_Comma_Pos(p1,16); //得到HDOP位置精度因子
  169. if(posx!=0XFF)gpsx->hdop=NMEA_Str2num(p1+posx,&dx);
  170. posx=NMEA_Comma_Pos(p1,17); //得到VDOP位置精度因子
  171. if(posx!=0XFF)gpsx->vdop=NMEA_Str2num(p1+posx,&dx);
  172. }
  173. //分析GPRMC信息
  174. //gpsx:nmea信息结构体
  175. //buf:接收到的GPS数据缓冲区首地址
  176. void NMEA_GPRMC_Analysis(nmea_msg *gpsx,uint8_t *buf)
  177. {
  178. uint8_t *p1,dx;
  179. uint8_t posx;
  180. uint32_t temp;
  181. float rs;
  182. p1=(uint8_t*)strstr((const char *)buf,"GNRMC");//"$GPRMC",经常有&和GPRMC分开的情况,故只判断GPRMC.
  183. posx=NMEA_Comma_Pos(p1,1); //得到UTC时间
  184. if(posx!=0XFF)
  185. {
  186. temp=NMEA_Str2num(p1+posx,&dx)/NMEA_Pow(10,dx); //得到UTC时间,去掉ms
  187. gpsx->utc.hour=temp/10000;
  188. gpsx->utc.min=(temp/100)%100;
  189. gpsx->utc.sec=temp%100;
  190. }
  191. // posx=NMEA_Comma_Pos(p1,3); //得到纬度
  192. // if(posx!=0XFF)
  193. // {
  194. // temp=NMEA_Str2num(p1+posx,&dx);
  195. // gpsx->latitude=temp/NMEA_Pow(10,dx+2); //得到°
  196. // rs=temp%NMEA_Pow(10,dx+2); //得到'
  197. // gpsx->latitude=gpsx->latitude*NMEA_Pow(10,5)+(rs*NMEA_Pow(10,5-dx))/60;//转换为°
  198. // }
  199. // posx=NMEA_Comma_Pos(p1,4); //南纬还是北纬
  200. // if(posx!=0XFF)gpsx->nshemi=*(p1+posx);
  201. // posx=NMEA_Comma_Pos(p1,5); //得到经度
  202. // if(posx!=0XFF)
  203. // {
  204. // temp=NMEA_Str2num(p1+posx,&dx);
  205. // gpsx->longitude=temp/NMEA_Pow(10,dx+2); //得到°
  206. // rs=temp%NMEA_Pow(10,dx+2); //得到'
  207. // gpsx->longitude=gpsx->longitude*NMEA_Pow(10,5)+(rs*NMEA_Pow(10,5-dx))/60;//转换为°
  208. // }
  209. // posx=NMEA_Comma_Pos(p1,6); //东经还是西经
  210. // if(posx!=0XFF)gpsx->ewhemi=*(p1+posx);
  211. posx=NMEA_Comma_Pos(p1,9); //得到UTC日期
  212. if(posx!=0XFF)
  213. {
  214. temp=NMEA_Str2num(p1+posx,&dx); //得到UTC日期
  215. gpsx->utc.date=temp/10000;
  216. gpsx->utc.month=(temp/100)%100;
  217. gpsx->utc.year=2000+temp%100;
  218. }
  219. }
  220. //分析GPVTG信息
  221. //gpsx:nmea信息结构体
  222. //buf:接收到的GPS数据缓冲区首地址
  223. void NMEA_GPVTG_Analysis(nmea_msg *gpsx,uint8_t *buf)
  224. {
  225. uint8_t *p1,dx;
  226. uint8_t posx;
  227. p1=(uint8_t*)strstr((const char *)buf,"$GNVTG");
  228. posx=NMEA_Comma_Pos(p1,7); //得到地面速率
  229. if(posx!=0XFF)
  230. {
  231. gpsx->speed=NMEA_Str2num(p1+posx,&dx);
  232. if(dx<3)gpsx->speed*=NMEA_Pow(10,3-dx); //确保扩大1000倍
  233. }
  234. }
  235. //提取NMEA-0183信息
  236. //gpsx:nmea信息结构体
  237. //buf:接收到的GPS数据缓冲区首地址
  238. void GPS_Analysis(nmea_msg *gpsx,uint8_t *buf)
  239. {
  240. // NMEA_GPGSV_Analysis(gpsx,buf); //GPGSV解析
  241. NMEA_GPGGA_Analysis(gpsx,buf); //GPGGA解析
  242. // NMEA_GPGSA_Analysis(gpsx,buf); //GPGSA解析
  243. NMEA_GPRMC_Analysis(gpsx,buf); //GPRMC解析
  244. // NMEA_GPVTG_Analysis(gpsx,buf); //GPVTG解析
  245. }
  246. //GPS校验和计算
  247. //buf:数据缓存区首地址
  248. //len:数据长度
  249. //cka,ckb:两个校验结果.
  250. void Ublox_CheckSum(uint8_t *buf,uint16_t len,uint8_t* cka,uint8_t*ckb)
  251. {
  252. uint16_t i;
  253. *cka=0;*ckb=0;
  254. for(i=0;i<len;i++)
  255. {
  256. *cka=*cka+buf[i];
  257. *ckb=*ckb+*cka;
  258. }
  259. }
  260. ///////////////////////////////////////////UBLOX 配置代码/////////////////////////////////////
  261. ////检查CFG配置执行情况
  262. ////返回值:0,ACK成功
  263. //// 1,接收超时错误
  264. //// 2,没有找到同步字符
  265. //// 3,接收到NACK应答
  266. //uint8_t Ublox_Cfg_Ack_Check(void)
  267. //{
  268. // uint16_t len=0,i;
  269. // uint8_t rval=0;
  270. // while((USART2_RX_STA&0X8000)==0 && len<100)//等待接收到应答
  271. // {
  272. // len++;
  273. // delay_ms(5);
  274. // }
  275. // if(len<250) //超时错误.
  276. // {
  277. // len=USART2_RX_STA&0X7FFF; //此次接收到的数据长度
  278. // for(i=0;i<len;i++)if(USART2_RX_BUF[i]==0XB5)break;//查找同步字符 0XB5
  279. // if(i==len)rval=2; //没有找到同步字符
  280. // else if(USART2_RX_BUF[i+3]==0X00)rval=3;//接收到NACK应答
  281. // else rval=0; //接收到ACK应答
  282. // }else rval=1; //接收超时错误
  283. // USART2_RX_STA=0; //清除接收
  284. // return rval;
  285. //}
  286. ////配置保存
  287. ////将当前配置保存在外部EEPROM里面
  288. ////返回值:0,执行成功;1,执行失败.
  289. //uint8_t Ublox_Cfg_Cfg_Save(void)
  290. //{
  291. // uint8_t i;
  292. // _ublox_cfg_cfg *cfg_cfg=(_ublox_cfg_cfg *)USART2_TX_BUF;
  293. // cfg_cfg->header=0X62B5; //cfg header
  294. // cfg_cfg->id=0X0906; //cfg cfg id
  295. // cfg_cfg->dlength=13; //数据区长度为13个字节.
  296. // cfg_cfg->clearmask=0; //清除掩码为0
  297. // cfg_cfg->savemask=0XFFFF; //保存掩码为0XFFFF
  298. // cfg_cfg->loadmask=0; //加载掩码为0
  299. // cfg_cfg->devicemask=4; //保存在EEPROM里面
  300. // Ublox_CheckSum((uint8_t*)(&cfg_cfg->id),sizeof(_ublox_cfg_cfg)-4,&cfg_cfg->cka,&cfg_cfg->ckb);
  301. // while(DMA1_Channel7->CNDTR!=0); //等待通道7传输完成
  302. // UART_DMA_Enable(DMA1_Channel7,sizeof(_ublox_cfg_cfg)); //通过dma发送出去
  303. // for(i=0;i<6;i++)if(Ublox_Cfg_Ack_Check()==0)break; //EEPROM写入需要比较久时间,所以连续判断多次
  304. // return i==6?1:0;
  305. //}
  306. ////配置NMEA输出信息格式
  307. ////msgid:要操作的NMEA消息条目,具体见下面的参数表
  308. //// 00,GPGGA;01,GPGLL;02,GPGSA;
  309. //// 03,GPGSV;04,GPRMC;05,GPVTG;
  310. //// 06,GPGRS;07,GPGST;08,GPZDA;
  311. //// 09,GPGBS;0A,GPDTM;0D,GPGNS;
  312. ////uart1set:0,输出关闭;1,输出开启.
  313. ////返回值:0,执行成功;其他,执行失败.
  314. //uint8_t Ublox_Cfg_Msg(uint8_t msgid,uint8_t uart1set)
  315. //{
  316. // _ublox_cfg_msg *cfg_msg=(_ublox_cfg_msg *)USART2_TX_BUF;
  317. // cfg_msg->header=0X62B5; //cfg header
  318. // cfg_msg->id=0X0106; //cfg msg id
  319. // cfg_msg->dlength=8; //数据区长度为8个字节.
  320. // cfg_msg->msgclass=0XF0; //NMEA消息
  321. // cfg_msg->msgid=msgid; //要操作的NMEA消息条目
  322. // cfg_msg->iicset=1; //默认开启
  323. // cfg_msg->uart1set=uart1set; //开关设置
  324. // cfg_msg->uart2set=1; //默认开启
  325. // cfg_msg->usbset=1; //默认开启
  326. // cfg_msg->spiset=1; //默认开启
  327. // cfg_msg->ncset=1; //默认开启
  328. // Ublox_CheckSum((uint8_t*)(&cfg_msg->id),sizeof(_ublox_cfg_msg)-4,&cfg_msg->cka,&cfg_msg->ckb);
  329. // while(DMA1_Channel7->CNDTR!=0); //等待通道7传输完成
  330. // UART_DMA_Enable(DMA1_Channel7,sizeof(_ublox_cfg_msg)); //通过dma发送出去
  331. // return Ublox_Cfg_Ack_Check();
  332. //}
  333. ////配置NMEA输出信息格式
  334. ////baudrate:波特率,4800/9600/19200/38400/57600/115200/230400
  335. ////返回值:0,执行成功;其他,执行失败(这里不会返回0了)
  336. //uint8_t Ublox_Cfg_Prt(u32 baudrate)
  337. //{
  338. // _ublox_cfg_prt *cfg_prt=(_ublox_cfg_prt *)USART2_TX_BUF;
  339. // cfg_prt->header=0X62B5; //cfg header
  340. // cfg_prt->id=0X0006; //cfg prt id
  341. // cfg_prt->dlength=20; //数据区长度为20个字节.
  342. // cfg_prt->portid=1; //操作串口1
  343. // cfg_prt->reserved=0; //保留字节,设置为0
  344. // cfg_prt->txready=0; //TX Ready设置为0
  345. // cfg_prt->mode=0X08D0; //8位,1个停止位,无校验位
  346. // cfg_prt->baudrate=baudrate; //波特率设置
  347. // cfg_prt->inprotomask=0X0007;//0+1+2
  348. // cfg_prt->outprotomask=0X0007;//0+1+2
  349. // cfg_prt->reserved4=0; //保留字节,设置为0
  350. // cfg_prt->reserved5=0; //保留字节,设置为0
  351. // Ublox_CheckSum((uint8_t*)(&cfg_prt->id),sizeof(_ublox_cfg_prt)-4,&cfg_prt->cka,&cfg_prt->ckb);
  352. // while(DMA1_Channel7->CNDTR!=0); //等待通道7传输完成
  353. // UART_DMA_Enable(DMA1_Channel7,sizeof(_ublox_cfg_prt)); //通过dma发送出去
  354. // delay_ms(200); //等待发送完成
  355. // USART2_Init(36,baudrate); //重新初始化串口2
  356. // return Ublox_Cfg_Ack_Check();//这里不会反回0,因为UBLOX发回来的应答在串口重新初始化的时候已经被丢弃了.
  357. //}
  358. ////配置UBLOX NEO-6的时钟脉冲输出
  359. ////interval:脉冲间隔(us)
  360. ////length:脉冲宽度(us)
  361. ////status:脉冲配置:1,高电平有效;0,关闭;-1,低电平有效.
  362. ////返回值:0,发送成功;其他,发送失败.
  363. //uint8_t Ublox_Cfg_Tp(u32 interval,u32 length,signed char status)
  364. //{
  365. // _ublox_cfg_tp *cfg_tp=(_ublox_cfg_tp *)USART2_TX_BUF;
  366. // cfg_tp->header=0X62B5; //cfg header
  367. // cfg_tp->id=0X0706; //cfg tp id
  368. // cfg_tp->dlength=20; //数据区长度为20个字节.
  369. // cfg_tp->interval=interval; //脉冲间隔,us
  370. // cfg_tp->length=length; //脉冲宽度,us
  371. // cfg_tp->status=status; //时钟脉冲配置
  372. // cfg_tp->timeref=0; //参考UTC 时间
  373. // cfg_tp->flags=0; //flags为0
  374. // cfg_tp->reserved=0; //保留位为0
  375. // cfg_tp->antdelay=820; //天线延时为820ns
  376. // cfg_tp->rfdelay=0; //RF延时为0ns
  377. // cfg_tp->userdelay=0; //用户延时为0ns
  378. // Ublox_CheckSum((uint8_t*)(&cfg_tp->id),sizeof(_ublox_cfg_tp)-4,&cfg_tp->cka,&cfg_tp->ckb);
  379. // while(DMA1_Channel7->CNDTR!=0); //等待通道7传输完成
  380. // UART_DMA_Enable(DMA1_Channel7,sizeof(_ublox_cfg_tp)); //通过dma发送出去
  381. // return Ublox_Cfg_Ack_Check();
  382. //}
  383. ////配置UBLOX NEO-6的更新速率
  384. ////measrate:测量时间间隔,单位为ms,最少不能小于200ms(5Hz)
  385. ////reftime:参考时间,0=UTC Time;1=GPS Time(一般设置为1)
  386. ////返回值:0,发送成功;其他,发送失败.
  387. //uint8_t Ublox_Cfg_Rate(uint16_t measrate,uint8_t reftime)
  388. //{
  389. // _ublox_cfg_rate *cfg_rate=(_ublox_cfg_rate *)USART2_TX_BUF;
  390. // if(measrate<200)return 1; //小于200ms,直接退出
  391. // cfg_rate->header=0X62B5; //cfg header
  392. // cfg_rate->id=0X0806; //cfg rate id
  393. // cfg_rate->dlength=6; //数据区长度为6个字节.
  394. // cfg_rate->measrate=measrate;//脉冲间隔,us
  395. // cfg_rate->navrate=1; //导航速率(周期),固定为1
  396. // cfg_rate->timeref=reftime; //参考时间为GPS时间
  397. // Ublox_CheckSum((uint8_t*)(&cfg_rate->id),sizeof(_ublox_cfg_rate)-4,&cfg_rate->cka,&cfg_rate->ckb);
  398. // while(DMA1_Channel7->CNDTR!=0); //等待通道7传输完成
  399. // UART_DMA_Enable(DMA1_Channel7,sizeof(_ublox_cfg_rate));//通过dma发送出去
  400. // return Ublox_Cfg_Ack_Check();
  401. //}