Subversion Repositories libNMEA

Rev

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