Subversion Repositories libNMEA

Rev

Rev 2 | Rev 4 | 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.                                         // handle the read code
  53.                                         bool success = decodePacket(linebuff, linePos, loc);
  54.                                         lineState = SEARCH;
  55.  
  56.                                         linePos = 0;
  57.                                         return success;
  58.                                 }
  59.                                 if (linePos < sizeof(linebuff))
  60.                                         linebuff[linePos++] = c;
  61.                                 else
  62.                                 {
  63.                                         lineState = SEARCH;
  64.                                         // search for the $  in any unread string
  65.                                         int i;
  66.  
  67.                                         for (i = 0; i < linePos; i++)
  68.                                                 if (linebuff[i] == '$')
  69.                                                 {
  70.                                                         memcpy(linebuff, linebuff + i, linePos - i);
  71.                                                         linePos = 1;
  72.                                                         linebuff[linePos++] = c;
  73.                                                         lineState = READING;
  74.                                                 }
  75.  
  76.                                         if (lineState == SEARCH)
  77.                                                 linePos = 0;
  78.                                 }
  79.                                 break;
  80.                         }
  81.         }
  82.         return false;
  83. }
  84.  
  85. static int8_t
  86. decodeDec(char c)
  87. {
  88.         if (c > '9' && c < '0')
  89.                 return -1;
  90.         return c - '0';
  91. }
  92.  
  93. static uint8_t
  94. decodeHex(char c)
  95. {
  96.         int8_t v = decodeDec(c);
  97.         if (v >= 0)
  98.                 return v;
  99.         c = tolower(c);
  100.         if (c >= 'a' && c <= 'f')
  101.                 return c - 'a' + 10;
  102.         return 0;
  103. }
  104.  
  105. // lat/long decoder
  106. static float
  107. decodeLL(char *ptr, int msDigitWeight)
  108. {
  109.         float digitWeight = msDigitWeight;
  110.         float result = 0;
  111.         int i = 0;
  112.         while (1)
  113.         {
  114.                 char c = ptr[i++];
  115.                 if (c == '.')
  116.                         continue;
  117.                 int8_t v = decodeDec(c);
  118.                 if (v > 0)
  119.                 {
  120.                         result += digitWeight * v;
  121.                         if (fabs(digitWeight - 1) < 0.01)
  122.                                 digitWeight = 1 / 6.0;
  123.                         else
  124.                                 digitWeight = digitWeight / 10;
  125.  
  126.                         continue;
  127.                 }
  128.                 break;
  129.         }
  130.  
  131.         return result;
  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 decodePacket(char *linebuff, int linePos, Location *loc)
  158. {
  159.  
  160.         uint8_t checksum = 0;
  161.         for (int i = 1; i < linePos - 3; i++)
  162.                 checksum ^= linebuff[i];
  163.         uint8_t givenSum = (decodeHex(linebuff[linePos - 2]) << 4) + 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 = 6; i < linePos - 3; i++)
  171.         {
  172.                 if (linebuff[i] == ',')
  173.                 {
  174.                         fieldPos[fieldCnt++] = linebuff + i + 1;
  175.                 }
  176.         }
  177.  
  178.         // decode RMC
  179.         if (linebuff[3] == 'R' && linebuff[4] == 'M' && linebuff[5] == 'C')
  180.  
  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.                 if (rmcCallback)
  210.                 {
  211.                         linebuff[linePos++] = '\r';
  212.                         linebuff[linePos++] = '\n';
  213.                         linebuff[linePos++] = 0;
  214.                         rmcCallback((uint8_t *)linebuff, linePos);
  215.                 }
  216.         }
  217.  
  218.         return true;
  219. }
  220.  
  221. void setRmcCallback(nmeaCallback callback)
  222. {
  223.         rmcCallback = callback;
  224. }
  225.