Subversion Repositories dashGPS

Rev

Rev 5 | 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 bool
  27. decodePacket (char *linebuff, int linePos, Location *loc);
  28.  
  29. bool
  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.           linePos = 0;
  44.           break;
  45.         case READING:
  46.           if (c == '\r')
  47.             {
  48.               // handle the read code
  49.               bool success = decodePacket (linebuff, linePos, loc);
  50.               lineState = SEARCH;
  51.  
  52.               linePos = 0;
  53.               return success;
  54.             }
  55.           if (linePos < sizeof(linebuff))
  56.             linebuff[linePos++] = c;
  57.           else
  58.             {
  59.               lineState = SEARCH;
  60.               // search for the $  in any unread string
  61.               int i;
  62.  
  63.               for (i = 0; i < linePos; i++)
  64.                 if (linebuff[i] == '$')
  65.                   {
  66.                     int n = i+1;
  67.                     memcpy (linebuff, linebuff + n, linePos - i);
  68.                     linePos = linePos - i;
  69.                     linebuff[linePos++] = c;
  70.                     lineState = READING;
  71.                   }
  72.  
  73.               if (lineState == SEARCH)
  74.                 linePos = 0;
  75.             }
  76.           break;
  77.         }
  78.     }
  79.   return 0;
  80. }
  81.  
  82. static uint8_t
  83. decodeHex (char c)
  84. {
  85.   if (c >= '0' && c <= '9')
  86.     return c - '0';
  87.   c = tolower (c);
  88.   if (c >= 'a' && c <= 'f')
  89.     return c - 'a' + 10;
  90.   return 0;
  91. }
  92.  
  93. // lat/long decoder
  94. static float
  95. decodeLL (char *ptr, int msDigitWeight)
  96. {
  97.   float digitWeight = msDigitWeight;
  98.   float result = 0;
  99.   int i = 0;
  100.   while (1)
  101.     {
  102.       char c = ptr[i++];
  103.       if (c == '.')
  104.         continue;
  105.       if (c >= '0' && c <= '9')
  106.         {
  107.           result += digitWeight * (c - '0');
  108.           if (fabs (digitWeight - 1) < 0.01)
  109.             digitWeight = 1 / 6.0;
  110.           else
  111.             digitWeight = digitWeight / 10;
  112.  
  113.           continue;
  114.         }
  115.       break;
  116.     }
  117.  
  118.   return result;
  119.  
  120. }
  121.  
  122. static float
  123. decodeFP (char *ptr)
  124. {
  125.   return strtof (ptr, NULL);
  126. }
  127.  
  128. bool
  129. decodePacket (char *linebuff, int linePos, Location *loc)
  130. {
  131.   uint8_t checksum = 0;
  132.   for (int i = 0; i < linePos - 3; i++)
  133.     checksum ^= linebuff[i];
  134.   uint8_t givenSum = (decodeHex (linebuff[linePos - 2]) << 4)
  135.       + decodeHex (linebuff[linePos - 1]);
  136.   if (givenSum != checksum)
  137.     return false;
  138.  
  139.   char *fieldPos[20];
  140.   int fieldCnt = 0;
  141.   // split fields
  142.   for (int i = 5; i < linePos - 3; i++)
  143.     {
  144.       if (linebuff[i] == ',')
  145.         {
  146.           fieldPos[fieldCnt++] = linebuff + i + 1;
  147.         }
  148.  
  149.     }
  150.  
  151.   // decode RMC
  152.   if (linebuff[2] == 'R' && linebuff[3] == 'M' && linebuff[4] == 'C')
  153.  
  154.     {
  155.       // decode the fields
  156.       memcpy (loc->time, fieldPos[0], 6);
  157.       loc->valid = *fieldPos[1];
  158.       loc->lat = decodeLL (fieldPos[2], 10);
  159.       loc->ns = *fieldPos[3];
  160.       loc->lon = decodeLL (fieldPos[4], 100);
  161.       loc->ew = *fieldPos[5];
  162.       loc->speed = decodeFP (fieldPos[6]);
  163.       loc->heading = decodeFP (fieldPos[7]);
  164.       loc->good = true;
  165.     }
  166.   return true;
  167. }
  168.