Subversion Repositories libNMEA

Rev

Rev 1 | Rev 3 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1 Rev 2
Line 15... Line 15...
15 
#include "libSerial/serial.h"
 15 
#include "libSerial/serial.h"
16 
 
 16 
 
17 
char linebuff[80];
 17 
char linebuff[80];
18 
unsigned linePos = 0;
 18 
unsigned linePos = 0;
19 
 
 19 
 
- 
 
 20 
nmeaCallback rmcCallback = NULL;
- 
 
 21 
 
20 
typedef enum
 22 
typedef enum
21 
{
 23 
{
22 
  SEARCH, READING
 24 
  SEARCH, READING
23 
} NmeaState_t;
 25 
} NmeaState_t;
24 
 
 26 
 
Line 37... Line 39...
37 
 
 39 
 
38 
      char c = GetCharSerial (uc);
 40 
      char c = GetCharSerial (uc);
39 
      switch (lineState)
 41 
      switch (lineState)
40 
        {
 42 
        {
41 
        case SEARCH:
 43 
        case SEARCH:
42 
          if (c == '$')
 44 
          if (c != '$')
43 
            lineState = READING;
 - 
 
44 
          linePos = 0;
 - 
 
45 
          break;
 45 
          break;
- 
 
 46 
          lineState = READING;
- 
 
 47 
          linePos = 0;
46 
        case READING:
 48 
        case READING:
47 
          if (c == '\r')
 49 
          if (c == '\r')
48 
            {
 50 
            {
49 
              // handle the read code
 51 
              // handle the read code
50 
              bool success = decodePacket (linebuff, linePos, loc);
 52 
              bool success = decodePacket (linebuff, linePos, loc);
Line 62... Line 64...
62 
              int i;
 64 
              int i;
63 
 
 65 
 
64 
              for (i = 0; i < linePos; i++)
 66 
              for (i = 0; i < linePos; i++)
65 
                if (linebuff[i] == '$')
 67 
                if (linebuff[i] == '$')
66 
                  {
 68 
                  {
67 
                    int n = i + 1;
 - 
 
68 
                    memcpy (linebuff, linebuff + n, linePos - i);
 69 
                    memcpy (linebuff, linebuff + i, linePos - i);
69 
                    linePos = linePos - i;
 70 
                    linePos = 1;
70 
                    linebuff[linePos++] = c;
 71 
                    linebuff[linePos++] = c;
71 
                    lineState = READING;
 72 
                    lineState = READING;
72 
                  }
 73 
                  }
73 
 
 74 
 
74 
              if (lineState == SEARCH)
 75 
              if (lineState == SEARCH)
Line 154... Line 155...
154 
}
 155 
}
155 
 
 156 
 
156 
bool
 157 
bool
157 
decodePacket (char *linebuff, int linePos, Location *loc)
 158 
decodePacket (char *linebuff, int linePos, Location *loc)
158 
{
 159 
{
- 
 
 160 
 
159 
  uint8_t checksum = 0;
 161 
  uint8_t checksum = 0;
160 
  for (int i = 0; i < linePos - 3; i++)
 162 
  for (int i = 1; i < linePos - 3; i++)
161 
    checksum ^= linebuff[i];
 163 
    checksum ^= linebuff[i];
162 
  uint8_t givenSum = (decodeHex (linebuff[linePos - 2]) << 4)
 164 
  uint8_t givenSum = (decodeHex (linebuff[linePos - 2]) << 4)
163 
      + decodeHex (linebuff[linePos - 1]);
 165 
      + decodeHex (linebuff[linePos - 1]);
164 
  if (givenSum != checksum)
 166 
  if (givenSum != checksum)
165 
    return false;
 167 
    return false;
166 
 
 168 
 
167 
  char *fieldPos[20];
 169 
  char *fieldPos[20];
168 
  int fieldCnt = 0;
 170 
  int fieldCnt = 0;
169 
  // split fields
 171 
  // split fields
170 
  for (int i = 5; i < linePos - 3; i++)
 172 
  for (int i = 6; i < linePos - 3; i++)
171 
    {
 173 
    {
172 
      if (linebuff[i] == ',')
 174 
      if (linebuff[i] == ',')
173 
        {
 175 
        {
174 
          fieldPos[fieldCnt++] = linebuff + i + 1;
 176 
          fieldPos[fieldCnt++] = linebuff + i + 1;
175 
        }
 177 
        }
176 
 
 178 
 
177 
    }
 179 
    }
178 
 
 180 
 
179 
  // decode RMC
 181 
  // decode RMC
180 
  if (linebuff[2] == 'R' && linebuff[3] == 'M' && linebuff[4] == 'C')
 182 
  if (linebuff[3] == 'R' && linebuff[4] == 'M' && linebuff[5] == 'C')
181 
 
 183 
 
182 
    {
 184 
    {
- 
 
 185 
 
- 
 
 186 
 
183 
      // decode the fields
 187 
      // decode the fields
184 
      loc->valid = *fieldPos[1];
 188 
      loc->valid = *fieldPos[1];
185 
      if (loc->valid == 'A')
 189 
      if (loc->valid == 'A')
186 
        {
 190 
        {
187 
          memcpy (loc->time, fieldPos[0], 6);
 191 
          memcpy (loc->time, fieldPos[0], 6);
Line 204... Line 208...
204 
          loc->tv.tm_isdst = 0;
 208 
          loc->tv.tm_isdst = 0;
205 
          loc->utc = mktime (&loc->tv);
 209 
          loc->utc = mktime (&loc->tv);
206 
 
 210 
 
207 
          loc->good = true;
 211 
          loc->good = true;
208 
        }
 212 
        }
- 
 
 213 
      if (rmcCallback)
- 
 
 214 
        {
- 
 
 215 
          linebuff[linePos++]='\r';
- 
 
 216 
          linebuff[linePos++]='\n';
- 
 
 217 
        rmcCallback ((uint8_t*) linebuff, linePos);
- 
 
 218 
        }
209 
    }
 219 
    }
- 
 
 220 
 
210 
  return true;
 221 
  return true;
211 
}
 222 
}
- 
 
 223 
 
- 
 
 224 
void
- 
 
 225 
setRmcCallback (nmeaCallback callback)
- 
 
 226 
{
- 
 
 227 
  rmcCallback = callback;
- 
 
 228 
}