Subversion Repositories libNMEA

Rev

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