开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 嵌入式/单片机编程 > uCOS > 查看源码
gps.c
资源名称：STM32-uCOS-II_UAV.rar　[点击查看]
上传用户：yj_qqy
上传日期：2017-01-28
资源大小：2911k
文件大小：27k
源码类别：

uCOS

开发平台：

C/C++

gps.c：源码内容
  2. #define  GPS_GLOBALS
  3. #include <includes.h>
  5. #if GPS_MODULE == DEF_ENABLED
  6. #define MAXFIELD 25 // maximum field length
  8. static  OS_STK     GPS_TaskStk[GPS_TASK_STK_SIZE];
  9. static  OS_EVENT  *GPS_Sem;
  11. NP_STATE m_nState; // Current state protocol parser is in
  12. INT8U m_btChecksum; // Calculated NMEA sentence checksum
  13. INT8U m_btReceivedChecksum; // Received NMEA sentence checksum (if exists)
  14. INT16U m_wIndex; // Index used for command and data
  15. INT8U m_pCommand[NP_MAX_CMD_LEN]; // NMEA command
  16. INT8U m_pData[NP_MAX_DATA_LEN]; // NMEA data
  18. static  void    GPS_InitOS(void);
  19. static  void    GPS_Task(void *p_arg);
  21. /*
  22. *********************************************************************************************************
  23. *                                       INITIALIZE GPS
  24. *********************************************************************************************************
  25. */
  27. void  GPS_Init (void)
  28. {
  29. m_nState = NP_STATE_SOM;
  30. m_dwCommandCount = 0;
  31. GPDataReset();
  33.     GPS_InitOS();
  34.     GPS_InitTarget();                            /* Initialize target specific code                 */
  35. }
  37. /*
  38. *********************************************************************************************************
  39. *                             INITIALIZE THE uC/OS-View TASK AND OS OBJECTS
  40. *********************************************************************************************************
  41. */
  43. static  void  GPS_InitOS (void)
  44. {
  45. #if OS_TASK_NAME_SIZE > 7 || OS_EVENT_NAME_SIZE > 7
  46.     INT8U  err;
  47. #endif
  49.     GPS_Sem = OSSemCreate(0);
  50. #if OS_EVENT_NAME_SIZE > 17
  51.     OSEventNameSet(GPS_Sem, (INT8U *)"GPS Signal", &err);
  52. #else
  53. #if OS_EVENT_NAME_SIZE > 7
  54.     OSEventNameSet(GPS_Sem, (INT8U *)"GPS", &err);
  55. #endif
  56. #endif
  58.     (void)OSTaskCreateExt(GPS_Task,
  59.                           (void *)0,                                   /* No arguments passed to OSView_Task() */
  60.                           &GPS_TaskStk[GPS_TASK_STK_SIZE - 1],  /* Set Top-Of-Stack                     */
  61.                           GPS_TASK_PRIO,                           /* Lowest priority level                */
  62.                           GPS_TASK_PRIO,
  63.                           &GPS_TaskStk[0],                          /* Set Bottom-Of-Stack                  */
  64.                           GPS_TASK_STK_SIZE,
  65.                           (void *)0,                                   /* No TCB extension                     */
  66.                           OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR);  /* Enable stack checking + clear stack  */
  68. #if OS_TASK_NAME_SIZE > 10
  69.     OSTaskNameSet(GPS_TASK_PRIO, (INT8U *)"GPS Task", &err);
  70. #else
  71. #if OS_TASK_NAME_SIZE > 7
  72.     OSTaskNameSet(GPS_TASK_PRIO, (INT8U *)"GPS", &err);
  73. #endif
  74. #endif
  75. }
  77. /*
  78. *********************************************************************************************************
  79. *                                           GPS TASK
  80. *********************************************************************************************************
  81. */
  83. static  void  GPS_Task (void *p_arg)
  84. {
  85.     //INT8U      err;
  86.     //INT8U      s[100];
  88. while(1) {
  89. //(void)OSSemPend(GPS_Sem, 0, &err);
  90. OSTimeDlyHMSM(0, 0, 0, 500);
  91. //sprintf((char *)s, "n时:%2d 分:%2d 秒:%2d 纬度:%f 经度:%f 高度:%fn", GPGGAData.Hour, GPGGAData.Minute, GPGGAData.Second, GPGGAData.Latitude, GPGGAData.Longitude, GPGGAData.Altitude);
  92. //OSView_TxStr(s, 1);
  94. }
  95. }
  97. /*
  98. *********************************************************************************************************
  99. *                                              Rx Handler
  100. *
  101. * Description: This routine is called from the Rx interrupt service handler.
  102. *********************************************************************************************************
  103. */
  105. void  GPS_RxHandler (INT8U rx_data)
  106. {
  107. ProcessNMEA(rx_data);
  108. }
  111. /*
  112. *********************************************************************************************************
  113. *                                               Tx Handler
  114. *
  115. * Description: This routine is called from the transmitter buffer empty interrupt service handler.
  116. *              It will send out the next byte in the buffer.
  117. *
  118. * Returns:     none
  119. *********************************************************************************************************
  120. */
  122. void  GPS_TxHandler (void)
  123. {
  124. }
  126. ///////////////////////////////////////////////////////////////////////////////
  127. // Reset: Reset all NMEA data to start-up default values.
  128. ///////////////////////////////////////////////////////////////////////////////
  129. void GPDataReset(void)
  130. {
  131. int i;
  133. //
  134. // GPGGA Data
  135. //
  136. GPGGAData.Hour = 0; //
  137. GPGGAData.Minute = 0; //
  138. GPGGAData.Second = 0; //
  139. GPGGAData.Latitude = 0.0; // < 0 = South, > 0 = North 
  140. GPGGAData.Longitude = 0.0; // < 0 = West, > 0 = East
  141. GPGGAData.GPSQuality = 0; // 0 = fix not available, 1 = GPS sps mode, 2 = Differential GPS, SPS mode, fix valid, 3 = GPS PPS mode, fix valid
  142. GPGGAData.NumOfSatsInUse = 0; //
  143. GPGGAData.HDOP = 0.0; //
  144. GPGGAData.Altitude = 0.0; // Altitude: mean-sea-level (geoid) meters
  145. GPGGAData.Count = 0; //
  146. GPGGAData.OldVSpeedSeconds = 0; //
  147. GPGGAData.OldVSpeedAlt = 0.0; //
  148. GPGGAData.VertSpeed = 0.0; //
  150. //
  151. // GPGSA
  152. //
  153. GPGSAData.Mode = 'M'; // M = manual, A = automatic 2D/3D
  154. GPGSAData.FixMode = 1; // 1 = fix not available, 2 = 2D, 3 = 3D
  155. for(i = 0; i < NP_MAX_CHAN; i++)
  156. {
  157. GPGSAData.SatsInSolution[i] = 0; // ID of sats in solution
  158. }
  159. GPGSAData.PDOP = 0.0; //
  160. GPGSAData.HDOP = 0.0; //
  161. GPGSAData.VDOP = 0.0; //
  162. GPGSAData.Count = 0; //
  164. //
  165. // GPGSV
  166. //
  167. GPGSVData.TotalNumOfMsg = 0; //
  168. GPGSVData.TotalNumSatsInView = 0; //
  169. for(i = 0; i < NP_MAX_CHAN; i++)
  170. {
  171. GPGSVData.SatInfo[i].Azimuth = 0;
  172. GPGSVData.SatInfo[i].Elevation = 0;
  173. GPGSVData.SatInfo[i].PRN = 0;
  174. GPGSVData.SatInfo[i].SignalQuality = 0;
  175. GPGSVData.SatInfo[i].UsedInSolution = FALSE;
  176. }
  177. GPGSVData.Count = 0;
  179. //
  180. // GPRMB
  181. //
  182. GPRMBData.DataStatus = 'V'; // A = data valid, V = navigation receiver warning
  183. GPRMBData.CrosstrackError = 0.0; // nautical miles
  184. GPRMBData.DirectionToSteer = '?'; // L/R
  185. GPRMBData.OriginWaypoint[0] = ''; // Origin Waypoint ID
  186. GPRMBData.DestWaypoint[0] = ''; // Destination waypoint ID
  187. GPRMBData.DestLatitude = 0.0; // destination waypoint latitude
  188. GPRMBData.DestLongitude = 0.0; // destination waypoint longitude
  189. GPRMBData.RangeToDest = 0.0; // Range to destination nautical mi
  190. GPRMBData.BearingToDest = 0.0; // Bearing to destination, degrees true
  191. GPRMBData.DestClosingVelocity = 0.0; // Destination closing velocity, knots
  192. GPRMBData.ArrivalStatus = 'V'; // A = arrival circle entered, V = not entered
  193. GPRMBData.Count = 0; //
  195. //
  196. // GPRMC
  197. //
  198. GPRMCData.Hour = 0; //
  199. GPRMCData.Minute = 0; //
  200. GPRMCData.Second = 0; //
  201. GPRMCData.DataValid = 'V'; // A = Data valid, V = navigation rx warning
  202. GPRMCData.Latitude = 0.0; // current latitude
  203. GPRMCData.Longitude = 0.0; // current longitude
  204. GPRMCData.GroundSpeed = 0.0; // speed over ground, knots
  205. GPRMCData.Course = 0.0; // course over ground, degrees true
  206. GPRMCData.Day = 1; //
  207. GPRMCData.Month = 1; //
  208. GPRMCData.Year = 2000; //
  209. GPRMCData.MagVar = 0.0; // magnitic variation, degrees East(+)/West(-)
  210. GPRMCData.Count = 0; //
  212. //
  213. // GPZDA
  214. //
  215. GPZDAData.Hour = 0; //
  216. GPZDAData.Minute = 0; //
  217. GPZDAData.Second = 0; //
  218. GPZDAData.Day = 1; // 1 - 31
  219. GPZDAData.Month = 1; // 1 - 12
  220. GPZDAData.Year = 2000; //
  221. GPZDAData.LocalZoneHour = 0; // 0 to +/- 13
  222. GPZDAData.LocalZoneMinute = 0; // 0 - 59
  223. GPZDAData.Count = 0; //
  224. }
  226. ///////////////////////////////////////////////////////////////////////////////
  227. // ProcessNMEA: This method is the main state machine which processes individual
  228. // bytes from the buffer and parses a NMEA sentence. A typical
  229. // sentence is constructed as:
  230. //
  231. // $CMD,DDDD,DDDD,....DD*CS<CR><LF>
  232. //
  233. // Where:
  234. // '$' HEX 24 Start of sentence
  235. // 'CMD' Address/NMEA command
  236. // ',DDDD' Zero or more data fields
  237. // '*CS' Checksum field
  238. // <CR><LF> Hex 0d 0A End of sentence
  239. //
  240. // When a valid sentence is received, this function sends the
  241. // NMEA command and data to the ProcessCommand method for
  242. // individual field parsing.
  243. //
  244. // NOTE:
  245. //
  246. ///////////////////////////////////////////////////////////////////////////////
  247. void ProcessNMEA(INT8U btData)
  248. {
  249. switch(m_nState)
  250. {
  251. ///////////////////////////////////////////////////////////////////////
  252. // Search for start of message '$'
  253. case NP_STATE_SOM :
  254. if(btData == '$')
  255. {
  256. m_btChecksum = 0; // reset checksum
  257. m_wIndex = 0; // reset index
  258. m_nState = NP_STATE_CMD;
  259. }
  260. break;
  262. ///////////////////////////////////////////////////////////////////////
  263. // Retrieve command (NMEA Address)
  264. case NP_STATE_CMD :
  265. if(btData != ',' && btData != '*')
  266. {
  267. m_pCommand[m_wIndex++] = btData;
  268. m_btChecksum ^= btData;
  270. // Check for command overflow
  271. if(m_wIndex >= NP_MAX_CMD_LEN)
  272. {
  273. m_nState = NP_STATE_SOM;
  274. }
  275. }
  276. else
  277. {
  278. m_pCommand[m_wIndex] = ''; // terminate command
  279. m_btChecksum ^= btData;
  280. m_wIndex = 0;
  281. m_nState = NP_STATE_DATA; // goto get data state
  282. }
  283. break;
  285. ///////////////////////////////////////////////////////////////////////
  286. // Store data and check for end of sentence or checksum flag
  287. case NP_STATE_DATA :
  288. if(btData == '*') // checksum flag?
  289. {
  290. m_pData[m_wIndex] = '';
  291. m_nState = NP_STATE_CHECKSUM_1;
  292. }
  293. else // no checksum flag, store data
  294. {
  295. //
  296. // Check for end of sentence with no checksum
  297. //
  298. if(btData == 'r')
  299. {
  300. m_pData[m_wIndex] = '';
  301. ProcessCommand(m_pCommand, m_pData);
  302. m_nState = NP_STATE_SOM;
  303. return;
  304. }
  306. //
  307. // Store data and calculate checksum
  308. //
  309. m_btChecksum ^= btData;
  310. m_pData[m_wIndex] = btData;
  311. if(++m_wIndex >= NP_MAX_DATA_LEN) // Check for buffer overflow
  312. {
  313. m_nState = NP_STATE_SOM;
  314. }
  315. }
  316. break;
  318. ///////////////////////////////////////////////////////////////////////
  319. case NP_STATE_CHECKSUM_1 :
  320. if( (btData - '0') <= 9)
  321. {
  322. m_btReceivedChecksum = (btData - '0') << 4;
  323. }
  324. else
  325. {
  326. m_btReceivedChecksum = (btData - 'A' + 10) << 4;
  327. }
  329. m_nState = NP_STATE_CHECKSUM_2;
  331. break;
  333. ///////////////////////////////////////////////////////////////////////
  334. case NP_STATE_CHECKSUM_2 :
  335. if( (btData - '0') <= 9)
  336. {
  337. m_btReceivedChecksum |= (btData - '0');
  338. }
  339. else
  340. {
  341. m_btReceivedChecksum |= (btData - 'A' + 10);
  342. }
  344. if(m_btChecksum == m_btReceivedChecksum)
  345. {
  346. ProcessCommand(m_pCommand, m_pData);
  347. }
  349. m_nState = NP_STATE_SOM;
  350. break;
  352. ///////////////////////////////////////////////////////////////////////
  353. default : m_nState = NP_STATE_SOM;
  354. }
  355. }
  357. ///////////////////////////////////////////////////////////////////////////////
  358. // Process NMEA sentence - Use the NMEA address (*pCommand) and call the
  359. // appropriate sentense data prossor.
  360. ///////////////////////////////////////////////////////////////////////////////
  361. BOOLEAN ProcessCommand(INT8U *pCommand, INT8U *pData)
  362. {
  363. //
  364. // GPGGA
  365. //
  366. if( strcmp((char *)pCommand, "GPGGA") == NULL )
  367. {
  368. ProcessGPGGA(pData);
  369. OSSemPost(GPS_Sem);
  370. }
  372. //
  373. // GPGSA
  374. //
  375. else if( strcmp((char *)pCommand, "GPGSA") == NULL )
  376. {
  377. ProcessGPGSA(pData);
  378. }
  380. //
  381. // GPGSV
  382. //
  383. else if( strcmp((char *)pCommand, "GPGSV") == NULL )
  384. {
  385. ProcessGPGSV(pData);
  386. }
  388. //
  389. // GPRMB
  390. //
  391. else if( strcmp((char *)pCommand, "GPRMB") == NULL )
  392. {
  393. ProcessGPRMB(pData);
  394. }
  396. //
  397. // GPRMC
  398. //
  399. else if( strcmp((char *)pCommand, "GPRMC") == NULL )
  400. {
  401. ProcessGPRMC(pData);
  402. }
  404. //
  405. // GPZDA
  406. //
  407. else if( strcmp((char *)pCommand, "GPZDA") == NULL )
  408. {
  409. ProcessGPZDA(pData);
  410. }
  412. m_dwCommandCount++;
  413. return TRUE;
  414. }
  416. ///////////////////////////////////////////////////////////////////////////////
  417. // Check to see if supplied satellite ID is used in the GPS solution.
  418. // Retruned: BOOL - TRUE if satellate ID is used in solution
  419. // FALSE if not used in solution.
  420. ///////////////////////////////////////////////////////////////////////////////
  421. BOOLEAN IsSatUsedInSolution(INT16U wSatID)
  422. {
  423. INT32S i;
  424. if(wSatID == 0) return FALSE;
  425. for(i = 0; i < 12; i++)
  426. {
  427. if(wSatID == GPGSAData.SatsInSolution[i])
  428. {
  429. return TRUE;
  430. }
  431. }
  433. return FALSE;
  434. }
  437. ///////////////////////////////////////////////////////////////////////////////
  438. // Name: GetField
  439. //
  440. // Description: This function will get the specified field in a NMEA string.
  441. //
  442. // Entry: INT8U *pData - Pointer to NMEA string
  443. // INT8U *pField - pointer to returned field
  444. // INT32S nfieldNum - Field offset to get
  445. // INT32S nMaxFieldLen - Maximum of bytes pFiled can handle
  446. ///////////////////////////////////////////////////////////////////////////////
  447. BOOLEAN GetField(INT8U *pData, INT8U *pField, INT32S nFieldNum, INT32S nMaxFieldLen)
  448. {
  449. INT32S i, i2, nField;
  450. //
  451. // Validate params
  452. //
  453. if(pData == NULL || pField == NULL || nMaxFieldLen <= 0)
  454. {
  455. return FALSE;
  456. }
  458. //
  459. // Go to the beginning of the selected field
  460. //
  461. i = 0;
  462. nField = 0;
  463. while(nField != nFieldNum && pData[i])
  464. {
  465. if(pData[i] == ',')
  466. {
  467. nField++;
  468. }
  470. i++;
  472. if(pData[i] == NULL)
  473. {
  474. pField[0] = '';
  475. return FALSE;
  476. }
  477. }
  479. if(pData[i] == ',' || pData[i] == '*')
  480. {
  481. pField[0] = '';
  482. return FALSE;
  483. }
  485. //
  486. // copy field from pData to Field
  487. //
  488. i2 = 0;
  489. while(pData[i] != ',' && pData[i] != '*' && pData[i])
  490. {
  491. pField[i2] = pData[i];
  492. i2++; i++;
  494. //
  495. // check if field is too big to fit on passed parameter. If it is,
  496. // crop returned field to its max length.
  497. //
  498. if(i2 >= nMaxFieldLen)
  499. {
  500. i2 = nMaxFieldLen-1;
  501. break;
  502. }
  503. }
  504. pField[i2] = '';
  506. return TRUE;
  507. }
  509. ///////////////////////////////////////////////////////////////////////////////
  510. ///////////////////////////////////////////////////////////////////////////////
  511. void ProcessGPZDA(INT8U *pData)
  512. {
  513. char  pBuff[10];
  514. INT8U  pField[MAXFIELD];
  516. //
  517. // Time
  518. //
  519. if(GetField(pData, pField, 0, MAXFIELD))
  520. {
  521. // Hour
  522. pBuff[0] = pField[0];
  523. pBuff[1] = pField[1];
  524. pBuff[2] = '';
  525. GPZDAData.Hour = atoi(pBuff);
  527. // minute
  528. pBuff[0] = pField[2];
  529. pBuff[1] = pField[3];
  530. pBuff[2] = '';
  531. GPZDAData.Minute = atoi(pBuff);
  533. // Second
  534. pBuff[0] = pField[4];
  535. pBuff[1] = pField[5];
  536. pBuff[2] = '';
  537. GPZDAData.Second = atoi(pBuff);
  538. }
  540. //
  541. // Day
  542. //
  543. if(GetField(pData, pField, 1, MAXFIELD))
  544. {
  545. GPZDAData.Day = atoi((char *)pField);
  546. }
  547. else
  548. {
  549. GPZDAData.Day = 1;
  550. }
  552. //
  553. // Month
  554. //
  555. if(GetField(pData, pField, 2, MAXFIELD))
  556. {
  557. GPZDAData.Month = atoi((char *)pField);
  558. }
  559. else
  560. {
  561. GPZDAData.Month = 1;
  562. }
  564. //
  565. // Year
  566. //
  567. if(GetField(pData, pField, 3, MAXFIELD))
  568. {
  569. GPZDAData.Year = atoi((char *)pField);
  570. }
  571. else
  572. {
  573. GPZDAData.Year = 1;
  574. }
  576. //
  577. // Local zone hour
  578. //
  579. if(GetField(pData, pField, 4, MAXFIELD))
  580. {
  581. GPZDAData.LocalZoneHour = atoi((char *)pField);
  582. }
  583. else
  584. {
  585. GPZDAData.LocalZoneHour = 0;
  586. }
  588. //
  589. // Local zone hour
  590. //
  591. if(GetField(pData, pField, 5, MAXFIELD))
  592. {
  593. GPZDAData.LocalZoneMinute = atoi((char *)pField);
  594. }
  595. else
  596. {
  597. GPZDAData.LocalZoneMinute = 0;
  598. }
  600. GPZDAData.Count++;
  601. }
  603. ///////////////////////////////////////////////////////////////////////////////
  604. ///////////////////////////////////////////////////////////////////////////////
  605. void ProcessGPRMC(INT8U *pData)
  606. {
  607. char pBuff[10];
  608. INT8U pField[MAXFIELD];
  610. //
  611. // Time
  612. //
  613. if(GetField(pData, pField, 0, MAXFIELD))
  614. {
  615. // Hour
  616. pBuff[0] = pField[0];
  617. pBuff[1] = pField[1];
  618. pBuff[2] = '';
  619. GPRMCData.Hour = atoi(pBuff);
  621. // minute
  622. pBuff[0] = pField[2];
  623. pBuff[1] = pField[3];
  624. pBuff[2] = '';
  625. GPRMCData.Minute = atoi(pBuff);
  627. // Second
  628. pBuff[0] = pField[4];
  629. pBuff[1] = pField[5];
  630. pBuff[2] = '';
  631. GPRMCData.Second = atoi(pBuff);
  632. }
  634. //
  635. // Data valid
  636. //
  637. if(GetField(pData, pField, 1, MAXFIELD))
  638. {
  639. GPRMCData.DataValid = pField[0];
  640. }
  641. else
  642. {
  643. GPRMCData.DataValid = 'V';
  644. }
  646. //
  647. // latitude
  648. //
  649. if(GetField(pData, pField, 2, MAXFIELD))
  650. {
  651. GPRMCData.Latitude = atof((char *)pField+2) / 60.0;
  652. pField[2] = '';
  653. GPRMCData.Latitude += atof((char *)pField);
  655. }
  656. if(GetField(pData, pField, 3, MAXFIELD))
  657. {
  658. if(pField[0] == 'S')
  659. {
  660. GPRMCData.Latitude = -GPRMCData.Latitude;
  661. }
  662. }
  664. //
  665. // Longitude
  666. //
  667. if(GetField(pData, pField, 4, MAXFIELD))
  668. {
  669. GPRMCData.Longitude = atof((char *)pField+3) / 60.0;
  670. pField[3] = '';
  671. GPRMCData.Longitude += atof((char *)pField);
  672. }
  673. if(GetField(pData, pField, 5, MAXFIELD))
  674. {
  675. if(pField[0] == 'W')
  676. {
  677. GPRMCData.Longitude = -GPRMCData.Longitude;
  678. }
  679. }
  681. //
  682. // Ground speed
  683. //
  684. if(GetField(pData, pField, 6, MAXFIELD))
  685. {
  686. GPRMCData.GroundSpeed = atof((char *)pField);
  687. }
  688. else
  689. {
  690. GPRMCData.GroundSpeed = 0.0;
  691. }
  693. //
  694. // course over ground, degrees true
  695. //
  696. if(GetField(pData, pField, 7, MAXFIELD))
  697. {
  698. GPRMCData.Course = atof((char *)pField);
  699. }
  700. else
  701. {
  702. GPRMCData.Course = 0.0;
  703. }
  705. //
  706. // Date
  707. //
  708. if(GetField(pData, pField, 8, MAXFIELD))
  709. {
  710. // Day
  711. pBuff[0] = pField[0];
  712. pBuff[1] = pField[1];
  713. pBuff[2] = '';
  714. GPRMCData.Day = atoi(pBuff);
  716. // Month
  717. pBuff[0] = pField[2];
  718. pBuff[1] = pField[3];
  719. pBuff[2] = '';
  720. GPRMCData.Month = atoi(pBuff);
  722. // Year (Only two digits. I wonder why?)
  723. pBuff[0] = pField[4];
  724. pBuff[1] = pField[5];
  725. pBuff[2] = '';
  726. GPRMCData.Year = atoi(pBuff);
  727. GPRMCData.Year += 2000; // make 4 digit date -- What assumptions should be made here?
  728. }
  730. //
  731. // course over ground, degrees true
  732. //
  733. if(GetField(pData, pField, 9, MAXFIELD))
  734. {
  735. GPRMCData.MagVar = atof((char *)pField);
  736. }
  737. else
  738. {
  739. GPRMCData.MagVar = 0.0;
  740. }
  741. if(GetField(pData, pField, 10, MAXFIELD))
  742. {
  743. if(pField[0] == 'W')
  744. {
  745. GPRMCData.MagVar = -GPRMCData.MagVar;
  746. }
  747. }
  749. GPRMCData.Count++;
  750. }
  752. ///////////////////////////////////////////////////////////////////////////////
  753. ///////////////////////////////////////////////////////////////////////////////
  754. void ProcessGPRMB(INT8U *pData)
  755. {
  756. INT8U pField[MAXFIELD];
  758. //
  759. // Data status
  760. //
  761. if(GetField(pData, pField, 0, MAXFIELD))
  762. {
  763. GPRMBData.DataStatus = pField[0];
  764. }
  765. else
  766. {
  767. GPRMBData.DataStatus = 'V';
  768. }
  770. //
  771. // Cross track error
  772. //
  773. if(GetField(pData, pField, 1, MAXFIELD))
  774. {
  775. GPRMBData.CrosstrackError = atof((char *)pField);
  776. }
  777. else
  778. {
  779. GPRMBData.CrosstrackError = 0.0;
  780. }
  782. //
  783. // Direction to steer
  784. //
  785. if(GetField(pData, pField, 2, MAXFIELD))
  786. {
  787. GPRMBData.DirectionToSteer = pField[0];
  788. }
  789. else
  790. {
  791. GPRMBData.DirectionToSteer = '?';
  792. }
  794. //
  795. // Orgin waypoint ID
  796. //
  797. if(GetField(pData, pField, 3, MAXFIELD))
  798. {
  799. strcpy(GPRMBData.OriginWaypoint, (char *)pField);
  800. }
  801. else
  802. {
  803. GPRMBData.OriginWaypoint[0] = '';
  804. }
  806. //
  807. // Destination waypoint ID
  808. //
  809. if(GetField(pData, pField, 4, MAXFIELD))
  810. {
  811. strcpy(GPRMBData.DestWaypoint, (char *)pField);
  812. }
  813. else
  814. {
  815. GPRMBData.DestWaypoint[0] = '';
  816. }
  818. //
  819. // Destination latitude
  820. //
  821. if(GetField(pData, pField, 5, MAXFIELD))
  822. {
  823. GPRMBData.DestLatitude = atof((char *)pField+2) / 60.0;
  824. pField[2] = '';
  825. GPRMBData.DestLatitude += atof((char *)pField);
  827. }
  828. if(GetField(pData, pField, 6, MAXFIELD))
  829. {
  830. if(pField[0] == 'S')
  831. {
  832. GPRMBData.DestLatitude = -GPRMBData.DestLatitude;
  833. }
  834. }
  836. //
  837. // Destination Longitude
  838. //
  839. if(GetField(pData, pField, 7, MAXFIELD))
  840. {
  841. GPRMBData.DestLongitude = atof((char *)pField+3) / 60.0;
  842. pField[3] = '';
  843. GPRMBData.DestLongitude += atof((char *)pField);
  844. }
  845. if(GetField(pData, pField, 8, MAXFIELD))
  846. {
  847. if(pField[0] == 'W')
  848. {
  849. GPRMBData.DestLongitude = -GPRMBData.DestLongitude;
  850. }
  851. }
  853. //
  854. // Range to destination nautical mi
  855. //
  856. if(GetField(pData, pField, 9, MAXFIELD))
  857. {
  858. GPRMBData.RangeToDest = atof((char *)pField);
  859. }
  860. else
  861. {
  862. GPRMBData.CrosstrackError = 0.0;
  863. }
  865. //
  866. // Bearing to destination degrees true
  867. //
  868. if(GetField(pData, pField, 10, MAXFIELD))
  869. {
  870. GPRMBData.BearingToDest = atof((char *)pField);
  871. }
  872. else
  873. {
  874. GPRMBData.BearingToDest = 0.0;
  875. }
  877. //
  878. // Closing velocity
  879. //
  880. if(GetField(pData, pField, 11, MAXFIELD))
  881. {
  882. GPRMBData.DestClosingVelocity = atof((char *)pField);
  883. }
  884. else
  885. {
  886. GPRMBData.DestClosingVelocity = 0.0;
  887. }
  889. //
  890. // Arrival status
  891. //
  892. if(GetField(pData, pField, 12, MAXFIELD))
  893. {
  894. GPRMBData.ArrivalStatus = pField[0];
  895. }
  896. else
  897. {
  898. GPRMBData.DestClosingVelocity = 'V';
  899. }
  901. GPRMBData.Count++;
  902. }
  904.  ///////////////////////////////////////////////////////////////////////////////
  905. ///////////////////////////////////////////////////////////////////////////////
  906. void ProcessGPGSV(INT8U *pData)
  907. {
  908. INT32S i, nTotalNumOfMsg = 0, nMsgNum = 0;
  909. INT8U pField[MAXFIELD];
  911. //
  912. // Total number of messages
  913. //
  914. if(GetField(pData, pField, 0, MAXFIELD))
  915. {
  916. nTotalNumOfMsg = atoi((char *)pField);
  918. //
  919. // Make sure that the nTotalNumOfMsg is valid. This is used to
  920. // calculate indexes into an array. I've seen corrept NMEA strings
  921. // with no checksum set this to large values.
  922. //
  923. if(nTotalNumOfMsg > 9 || nTotalNumOfMsg < 0) return; 
  924. }
  925. if(nTotalNumOfMsg < 1 || nTotalNumOfMsg*4 >= NP_MAX_CHAN)
  926. {
  927. return;
  928. }
  930. //
  931. // message number
  932. //
  933. if(GetField(pData, pField, 1, MAXFIELD))
  934. {
  935. nMsgNum = atoi((char *)pField);
  937. //
  938. // Make sure that the message number is valid. This is used to
  939. // calculate indexes into an array
  940. //
  941. if(nMsgNum > 9 || nMsgNum < 0) return; 
  942. }
  944. //
  945. // Total satellites in view
  946. //
  947. if(GetField(pData, pField, 2, MAXFIELD))
  948. {
  949. GPGSVData.TotalNumSatsInView = atoi((char *)pField);
  950. }
  952. //
  953. // Satelite data
  954. //
  955. for(i = 0; i < 4; i++)
  956. {
  957. // Satellite ID
  958. if(GetField(pData, pField, 3 + 4*i, MAXFIELD))
  959. {
  960. GPGSVData.SatInfo[i+(nMsgNum-1)*4].PRN = atoi((char *)pField);
  961. }
  962. else
  963. {
  964. GPGSVData.SatInfo[i+(nMsgNum-1)*4].PRN = 0;
  965. }
  967. // Elevarion
  968. if(GetField(pData, pField, 4 + 4*i, MAXFIELD))
  969. {
  970. GPGSVData.SatInfo[i+(nMsgNum-1)*4].Elevation = atoi((char *)pField);
  971. }
  972. else
  973. {
  974. GPGSVData.SatInfo[i+(nMsgNum-1)*4].Elevation = 0;
  975. }
  977. // Azimuth
  978. if(GetField(pData, pField, 5 + 4*i, MAXFIELD))
  979. {
  980. GPGSVData.SatInfo[i+(nMsgNum-1)*4].Azimuth = atoi((char *)pField);
  981. }
  982. else
  983. {
  984. GPGSVData.SatInfo[i+(nMsgNum-1)*4].Azimuth = 0;
  985. }
  987. // SNR
  988. if(GetField(pData, pField, 6 + 4*i, MAXFIELD))
  989. {
  990. GPGSVData.SatInfo[i+(nMsgNum-1)*4].SignalQuality = atoi((char *)pField);
  991. }
  992. else
  993. {
  994. GPGSVData.SatInfo[i+(nMsgNum-1)*4].SignalQuality = 0;
  995. }
  997. //
  998. // Update "used in solution" (m_bUsedInSolution) flag. This is base
  999. // on the GSA message and is an added convenience for post processing
  1000. //
  1001. GPGSVData.SatInfo[i+(nMsgNum-1)*4].UsedInSolution = IsSatUsedInSolution(GPGSVData.SatInfo[i+(nMsgNum-1)*4].PRN);
  1002. }
  1004. GPGSVData.Count++;
  1005. }
  1007. ///////////////////////////////////////////////////////////////////////////////
  1008. ///////////////////////////////////////////////////////////////////////////////
  1009. void ProcessGPGSA(INT8U *pData)
  1010. {
  1011. INT8U pField[MAXFIELD];
  1012. char pBuff[10];
  1013. INT32S i;
  1015. //
  1016. // Mode
  1017. //
  1018. if(GetField(pData, pField, 0, MAXFIELD))
  1019. {
  1020. GPGSAData.Mode = pField[0];
  1021. }
  1023. //
  1024. // Fix Mode
  1025. //
  1026. if(GetField(pData, pField, 1, MAXFIELD))
  1027. {
  1028. GPGSAData.FixMode = pField[0] - '0';
  1029. }
  1031. //
  1032. // Active satellites
  1033. //
  1034. for(i = 0; i < 12; i++)
  1035. {
  1036. if(GetField(pData, pField, 2 + i, MAXFIELD))
  1037. {
  1038. pBuff[0] = pField[0];
  1039. pBuff[1] = pField[1];
  1040. pBuff[2] = '';
  1041. GPGSAData.SatsInSolution[i] = atoi(pBuff);
  1042. }
  1043. else
  1044. {
  1045. GPGSAData.SatsInSolution[i] = 0;
  1046. }
  1047. }
  1049. //
  1050. // PDOP
  1051. //
  1052. if(GetField(pData, pField, 14, MAXFIELD))
  1053. {
  1054. GPGSAData.PDOP = atof((char *)pField);
  1055. }
  1056. else
  1057. {
  1058. GPGSAData.PDOP = 0.0;
  1059. }
  1061. //
  1062. // HDOP
  1063. //
  1064. if(GetField(pData, pField, 15, MAXFIELD))
  1065. {
  1066. GPGSAData.HDOP = atof((char *)pField);
  1067. }
  1068. else
  1069. {
  1070. GPGSAData.HDOP = 0.0;
  1071. }
  1073. //
  1074. // VDOP
  1075. //
  1076. if(GetField(pData, pField, 16, MAXFIELD))
  1077. {
  1078. GPGSAData.VDOP = atof((char *)pField);
  1079. }
  1080. else
  1081. {
  1082. GPGSAData.VDOP = 0.0;
  1083. }
  1085. GPGSAData.Count++;
  1086. }
  1088. ///////////////////////////////////////////////////////////////////////////////
  1089. ///////////////////////////////////////////////////////////////////////////////
  1090. void ProcessGPGGA(INT8U *pData)
  1091. {
  1092. INT8U pField[MAXFIELD];
  1093. char pBuff[10];
  1094. INT32S nSeconds;
  1096. //
  1097. // Time
  1098. //
  1099. if(GetField(pData, pField, 0, MAXFIELD))
  1100. {
  1101. // Hour
  1102. pBuff[0] = pField[0];
  1103. pBuff[1] = pField[1];
  1104. pBuff[2] = '';
  1105. GPGGAData.Hour = atoi(pBuff);
  1107. // minute
  1108. pBuff[0] = pField[2];
  1109. pBuff[1] = pField[3];
  1110. pBuff[2] = '';
  1111. GPGGAData.Minute = atoi(pBuff);
  1113. // Second
  1114. pBuff[0] = pField[4];
  1115. pBuff[1] = pField[5];
  1116. pBuff[2] = '';
  1117. GPGGAData.Second = atoi(pBuff);
  1118. }
  1120. //
  1121. // Latitude
  1122. //
  1123. if(GetField(pData, pField, 1, MAXFIELD))
  1124. {
  1125. GPGGAData.Latitude = atof((char *)pField+2) / 60.0;
  1126. pField[2] = '';
  1127. GPGGAData.Latitude += atof((char *)pField);
  1129. }
  1130. if(GetField(pData, pField, 2, MAXFIELD))
  1131. {
  1132. if(pField[0] == 'S')
  1133. {
  1134. GPGGAData.Latitude = -GPGGAData.Latitude;
  1135. }
  1136. }
  1138. //
  1139. // Longitude
  1140. //
  1141. if(GetField(pData, pField, 3, MAXFIELD))
  1142. {
  1143. GPGGAData.Longitude = atof((char *)pField+3) / 60.0;
  1144. pField[3] = '';
  1145. GPGGAData.Longitude += atof((char *)pField);
  1146. }
  1147. if(GetField(pData, pField, 4, MAXFIELD))
  1148. {
  1149. if(pField[0] == 'W')
  1150. {
  1151. GPGGAData.Longitude = -GPGGAData.Longitude;
  1152. }
  1153. }
  1155. //
  1156. // GPS quality
  1157. //
  1158. if(GetField(pData, pField, 5, MAXFIELD))
  1159. {
  1160. GPGGAData.GPSQuality = pField[0] - '0';
  1161. }
  1163. //
  1164. // Satellites in use
  1165. //
  1166. if(GetField(pData, pField, 6, MAXFIELD))
  1167. {
  1168. pBuff[0] = pField[0];
  1169. pBuff[1] = pField[1];
  1170. pBuff[2] = '';
  1171. GPGGAData.NumOfSatsInUse = atoi(pBuff);
  1172. }
  1174. //
  1175. // HDOP
  1176. //
  1177. if(GetField(pData, pField, 7, MAXFIELD))
  1178. {
  1179. GPGGAData.HDOP = atof((char *)pField);
  1180. }
  1182. //
  1183. // Altitude
  1184. //
  1185. if(GetField(pData, pField, 8, MAXFIELD))
  1186. {
  1187. GPGGAData.Altitude = atof((char *)pField);
  1188. }
  1190. //
  1191. // Durive vertical speed (bonus)
  1192. //
  1193. nSeconds = (INT32S)GPGGAData.Minute * 60 + (INT32S)GPGGAData.Second;
  1194. if(nSeconds > GPGGAData.OldVSpeedSeconds)
  1195. {
  1196. double dDiff = (double)(GPGGAData.OldVSpeedSeconds-nSeconds);
  1197. double dVal = dDiff/60.0;
  1198. if(dVal != 0.0)
  1199. {
  1200. GPGGAData.VertSpeed = (GPGGAData.OldVSpeedAlt - GPGGAData.Altitude) / dVal;
  1201. }
  1202. }
  1203. GPGGAData.OldVSpeedAlt = GPGGAData.Altitude;
  1204. GPGGAData.OldVSpeedSeconds = nSeconds;
  1206. GPGGAData.Count++;
  1207. }
  1208. #endif