Subversion Repositories dashGPS

Rev

Rev 4 | Blame | Last modification | View Log | Download | RSS feed

  1. /*
  2.  * nmea.c
  3.  *
  4.  *  Created on: 6 Sep 2020
  5.  *      Author: mike
  6.  */
  7.  
  8. #include <math.h>
  9. #include <stdint.h>
  10. #include <stdlib.h>
  11.  
  12. #include "nmea.h"
  13.  
  14. #include "libSerial/serial.h"
  15.  
  16. char linebuff[80];
  17. unsigned linePos = 0;
  18.  
  19. typedef enum
  20. {
  21.   SEARCH, READING
  22. } NmeaState_t;
  23.  
  24. NmeaState_t lineState = SEARCH;
  25.  
  26. static uint8_t
  27. decodePacket (char *linebuff, int linePos, Location *loc);
  28.  
  29. uint8_t
  30. updateLocation (Location *loc)
  31. {
  32.   while (1)
  33.     {
  34.       if (!SerialCharsReceived (&uc1))
  35.         return false; // nothing to read, return immediately
  36.  
  37.       char c = GetCharSerial (&uc1);
  38.       switch (lineState)
  39.         {
  40.         case SEARCH:
  41.           if (c == '$')
  42.             lineState = READING;
  43.           break;
  44.         case READING:
  45.           if (c == '\r')
  46.             {
  47.               // handle the read code
  48.               uint8_t success = decodePacket (linebuff, linePos, loc);
  49.               lineState = SEARCH;
  50.  
  51.               linePos = 0;
  52.               return success;
  53.             }
  54.           if (linePos < sizeof(linebuff))
  55.             linebuff[linePos++] = c;
  56.           else
  57.             {
  58.               linePos = 0;
  59.               lineState = SEARCH;
  60.             }
  61.           break;
  62.         }
  63.     }
  64.   return 0;
  65. }
  66.  
  67. static uint8_t
  68. decodeHex (char c)
  69. {
  70.   if (c >= '0' && c <= '9')
  71.     return c - '0';
  72.   c = tolower (c);
  73.   if (c >= 'a' && c <= 'f')
  74.     return c - 'a' + 10;
  75.   return 0;
  76. }
  77.  
  78. // lat/long decoder
  79. static float
  80. decodeLL (char *ptr, int msDigitWeight)
  81. {
  82.   float digitWeight = msDigitWeight;
  83.   float result = 0;
  84.   int i = 0;
  85.   while (1)
  86.     {
  87.       char c = ptr[i++];
  88.       if (c == '.')
  89.         continue;
  90.       if (c >= '0' && c <= '9')
  91.         {
  92.           result += digitWeight * (c - '0');
  93.           if (fabs (digitWeight - 1) < 0.01)
  94.             digitWeight = 1 / 6.0;
  95.           else
  96.             digitWeight = digitWeight / 10;
  97.  
  98.           continue;
  99.         }
  100.       break;
  101.     }
  102.  
  103.   return result;
  104.  
  105. }
  106.  
  107. static float decodeFP(char * ptr)
  108. {
  109.   return strtof(ptr,NULL);
  110. }
  111.  
  112. uint8_t
  113. decodePacket (char *linebuff, int linePos, Location *loc)
  114. {
  115.   uint8_t checksum = 0;
  116.   for (int i = 0; i < linePos - 3; i++)
  117.     checksum ^= linebuff[i];
  118.   uint8_t givenSum = (decodeHex (linebuff[linePos - 2]) << 4)
  119.       + decodeHex (linebuff[linePos - 1]);
  120.   if (givenSum != checksum)
  121.     return 0;
  122.  
  123.   char *fieldPos[20];
  124.   int fieldCnt = 0;
  125.   // split fields
  126.   for (int i = 5; i < linePos - 3; i++)
  127.     {
  128.       if (linebuff[i] == ',')
  129.         {
  130.           fieldPos[fieldCnt++] = linebuff + i + 1;
  131.         }
  132.  
  133.     }
  134.  
  135.   if (linebuff[2] == 'R' && linebuff[3] == 'M' && linebuff[4] == 'C')
  136.  
  137.     {
  138.       // decode the fields
  139.       loc->valid = *fieldPos[1];
  140.       loc->lat = decodeLL (fieldPos[2], 10);
  141.       loc->ns = *fieldPos[3];
  142.       loc->lon = decodeLL (fieldPos[4], 100);
  143.       loc->ew = *fieldPos[5];
  144.       loc->speed =  decodeFP (fieldPos[6]);
  145.       loc->heading = decodeFP (fieldPos[7]);
  146.  
  147.     }
  148. return linePos;
  149. }
  150.