Subversion Repositories libNMEA

Rev

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