Subversion Repositories libNMEA

Rev

Rev 2 | Go to most recent revision | Blame | Compare with Previous | 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. #include <memory.h>
  12.  
  13. #include "libNMEA/nmea.h"
  14.  
  15. #include "libSerial/serial.h"
  16.  
  17. char linebuff[80];
  18. unsigned linePos = 0;
  19.  
  20. typedef enum
  21. {
  22.   SEARCH, READING
  23. } NmeaState_t;
  24.  
  25. NmeaState_t lineState = SEARCH;
  26.  
  27. static bool
  28. decodePacket (char *linebuff, int linePos, Location *loc);
  29.  
  30. bool
  31. updateLocation (Location *loc, usart_ctl *uc)
  32. {
  33.   while (1)
  34.     {
  35.       if (!SerialCharsReceived (uc))
  36.         return false; // nothing to read, return immediately
  37.  
  38.       char c = GetCharSerial (uc);
  39.       switch (lineState)
  40.         {
  41.         case SEARCH:
  42.           if (c == '$')
  43.             lineState = READING;
  44.           linePos = 0;
  45.           break;
  46.         case READING:
  47.           if (c == '\r')
  48.             {
  49.               // handle the read code
  50.               bool success = decodePacket (linebuff, linePos, loc);
  51.               lineState = SEARCH;
  52.  
  53.               linePos = 0;
  54.               return success;
  55.             }
  56.           if (linePos < sizeof(linebuff))
  57.             linebuff[linePos++] = c;
  58.           else
  59.             {
  60.               lineState = SEARCH;
  61.               // search for the $  in any unread string
  62.               int i;
  63.  
  64.               for (i = 0; i < linePos; i++)
  65.                 if (linebuff[i] == '$')
  66.                   {
  67.                     int n = i + 1;
  68.                     memcpy (linebuff, linebuff + n, linePos - i);
  69.                     linePos = linePos - i;
  70.                     linebuff[linePos++] = c;
  71.                     lineState = READING;
  72.                   }
  73.  
  74.               if (lineState == SEARCH)
  75.                 linePos = 0;
  76.             }
  77.           break;
  78.         }
  79.     }
  80.   return false;
  81. }
  82.  
  83. static int8_t
  84. decodeDec (char c)
  85. {
  86.   if (c > '9' && c < '0')
  87.     return -1;
  88.   return c - '0';
  89. }
  90.  
  91. static uint8_t
  92. decodeHex (char c)
  93. {
  94.   int8_t v = decodeDec (c);
  95.   if (v >= 0)
  96.     return v;
  97.   c = tolower (c);
  98.   if (c >= 'a' && c <= 'f')
  99.     return c - 'a' + 10;
  100.   return 0;
  101. }
  102.  
  103. // lat/long decoder
  104. static float
  105. decodeLL (char *ptr, int msDigitWeight)
  106. {
  107.   float digitWeight = msDigitWeight;
  108.   float result = 0;
  109.   int i = 0;
  110.   while (1)
  111.     {
  112.       char c = ptr[i++];
  113.       if (c == '.')
  114.         continue;
  115.       int8_t v = decodeDec (c);
  116.       if (v > 0)
  117.         {
  118.           result += digitWeight * v;
  119.           if (fabs (digitWeight - 1) < 0.01)
  120.             digitWeight = 1 / 6.0;
  121.           else
  122.             digitWeight = digitWeight / 10;
  123.  
  124.           continue;
  125.         }
  126.       break;
  127.     }
  128.  
  129.   return result;
  130.  
  131. }
  132.  
  133. static float
  134. decodeFP (char *ptr)
  135. {
  136.   return strtof (ptr, NULL);
  137. }
  138.  
  139. static int
  140. decodeDecimal (char *ptr)
  141. {
  142.   int i;
  143.   int res = 0;
  144.   int const width = 2;
  145.   for (i = 0; i < width; i++)
  146.     {
  147.       int8_t v = decodeDec (*ptr++);
  148.       if (v < 0)
  149.         return 0;
  150.       res *= 10;
  151.       res += v;
  152.     }
  153.   return res;
  154. }
  155.  
  156. bool
  157. decodePacket (char *linebuff, int linePos, Location *loc)
  158. {
  159.   uint8_t checksum = 0;
  160.   for (int i = 0; i < linePos - 3; i++)
  161.     checksum ^= linebuff[i];
  162.   uint8_t givenSum = (decodeHex (linebuff[linePos - 2]) << 4)
  163.       + decodeHex (linebuff[linePos - 1]);
  164.   if (givenSum != checksum)
  165.     return false;
  166.  
  167.   char *fieldPos[20];
  168.   int fieldCnt = 0;
  169.   // split fields
  170.   for (int i = 5; i < linePos - 3; i++)
  171.     {
  172.       if (linebuff[i] == ',')
  173.         {
  174.           fieldPos[fieldCnt++] = linebuff + i + 1;
  175.         }
  176.  
  177.     }
  178.  
  179.   // decode RMC
  180.   if (linebuff[2] == 'R' && linebuff[3] == 'M' && linebuff[4] == 'C')
  181.  
  182.     {
  183.       // decode the fields
  184.       loc->valid = *fieldPos[1];
  185.       if (loc->valid == 'A')
  186.         {
  187.           memcpy (loc->time, fieldPos[0], 6);
  188.           loc->lat = decodeLL (fieldPos[2], 10);
  189.           loc->ns = *fieldPos[3];
  190.           loc->lon = decodeLL (fieldPos[4], 100);
  191.           loc->ew = *fieldPos[5];
  192.           loc->speed = decodeFP (fieldPos[6]);
  193.           loc->heading = decodeFP (fieldPos[7]);
  194.           memcpy (loc->date, fieldPos[1], 6);
  195.  
  196.           loc->tv.tm_sec = decodeDecimal (&loc->time[4]);
  197.           loc->tv.tm_min = decodeDecimal (&loc->time[2]);
  198.           loc->tv.tm_hour = decodeDecimal (&loc->time[0]);
  199.  
  200.           loc->tv.tm_mday = decodeDecimal (&loc->date[0]);
  201.           loc->tv.tm_mon = decodeDecimal (&loc->date[2]) - 1;
  202.           loc->tv.tm_year = decodeDecimal (&loc->date[4]) + 100; //
  203.  
  204.           loc->tv.tm_isdst = 0;
  205.           loc->utc = mktime (&loc->tv);
  206.  
  207.           loc->good = true;
  208.         }
  209.     }
  210.   return true;
  211. }
  212.