Subversion Repositories dashGPS

Rev

Rev 5 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 5 Rev 9
Line 21... Line 21...
21 
  SEARCH, READING
 21 
  SEARCH, READING
22 
} NmeaState_t;
 22 
} NmeaState_t;
23 
 
 23 
 
24 
NmeaState_t lineState = SEARCH;
 24 
NmeaState_t lineState = SEARCH;
25 
 
 25 
 
26 
static uint8_t
 26 
static bool
27 
decodePacket (char *linebuff, int linePos, Location *loc);
 27 
decodePacket (char *linebuff, int linePos, Location *loc);
28 
 
 28 
 
29 
uint8_t
 29 
bool
30 
updateLocation (Location *loc)
 30 
updateLocation (Location *loc)
31 
{
 31 
{
32 
  while (1)
 32 
  while (1)
33 
    {
 33 
    {
34 
      if (!SerialCharsReceived (&uc1))
 34 
      if (!SerialCharsReceived (&uc1))
Line 38... Line 38...
38 
      switch (lineState)
 38 
      switch (lineState)
39 
        {
 39 
        {
40 
        case SEARCH:
 40 
        case SEARCH:
41 
          if (c == '$')
 41 
          if (c == '$')
42 
            lineState = READING;
 42 
            lineState = READING;
- 
 
 43 
          linePos = 0;
43 
          break;
 44 
          break;
44 
        case READING:
 45 
        case READING:
45 
          if (c == '\r')
 46 
          if (c == '\r')
46 
            {
 47 
            {
47 
              // handle the read code
 48 
              // handle the read code
48 
              uint8_t success = decodePacket (linebuff, linePos, loc);
 49 
              bool success = decodePacket (linebuff, linePos, loc);
49 
              lineState = SEARCH;
 50 
              lineState = SEARCH;
50 
 
 51 
 
51 
              linePos = 0;
 52 
              linePos = 0;
52 
              return success;
 53 
              return success;
53 
            }
 54 
            }
54 
          if (linePos < sizeof(linebuff))
 55 
          if (linePos < sizeof(linebuff))
55 
            linebuff[linePos++] = c;
 56 
            linebuff[linePos++] = c;
56 
          else
 57 
          else
57 
            {
 58 
            {
58 
              linePos = 0;
 - 
 
59 
              lineState = SEARCH;
 59 
              lineState = SEARCH;
- 
 
 60 
              // search for the $  in any unread string
- 
 
 61 
              int i;
- 
 
 62 
 
- 
 
 63 
              for (i = 0; i < linePos; i++)
- 
 
 64 
                if (linebuff[i] == '$')
- 
 
 65 
                  {
- 
 
 66 
                    int n = i+1;
- 
 
 67 
                    memcpy (linebuff, linebuff + n, linePos - i);
- 
 
 68 
                    linePos = linePos - i;
- 
 
 69 
                    linebuff[linePos++] = c;
- 
 
 70 
                    lineState = READING;
- 
 
 71 
                  }
- 
 
 72 
 
- 
 
 73 
              if (lineState == SEARCH)
- 
 
 74 
                linePos = 0;
60 
            }
 75 
            }
61 
          break;
 76 
          break;
62 
        }
 77 
        }
63 
    }
 78 
    }
64 
  return 0;
 79 
  return 0;
Line 102... Line 117...
102 
 
 117 
 
103 
  return result;
 118 
  return result;
104 
 
 119 
 
105 
}
 120 
}
106 
 
 121 
 
- 
 
 122 
static float
107 
static float decodeFP(char * ptr)
 123 
decodeFP (char *ptr)
108 
{
 124 
{
109 
  return strtof(ptr,NULL);
 125 
  return strtof (ptr, NULL);
110 
}
 126 
}
111 
 
 127 
 
112 
uint8_t
 128 
bool
113 
decodePacket (char *linebuff, int linePos, Location *loc)
 129 
decodePacket (char *linebuff, int linePos, Location *loc)
114 
{
 130 
{
115 
  uint8_t checksum = 0;
 131 
  uint8_t checksum = 0;
116 
  for (int i = 0; i < linePos - 3; i++)
 132 
  for (int i = 0; i < linePos - 3; i++)
117 
    checksum ^= linebuff[i];
 133 
    checksum ^= linebuff[i];
118 
  uint8_t givenSum = (decodeHex (linebuff[linePos - 2]) << 4)
 134 
  uint8_t givenSum = (decodeHex (linebuff[linePos - 2]) << 4)
119 
      + decodeHex (linebuff[linePos - 1]);
 135 
      + decodeHex (linebuff[linePos - 1]);
120 
  if (givenSum != checksum)
 136 
  if (givenSum != checksum)
121 
    return 0;
 137 
    return false;
122 
 
 138 
 
123 
  char *fieldPos[20];
 139 
  char *fieldPos[20];
124 
  int fieldCnt = 0;
 140 
  int fieldCnt = 0;
125 
  // split fields
 141 
  // split fields
126 
  for (int i = 5; i < linePos - 3; i++)
 142 
  for (int i = 5; i < linePos - 3; i++)
Line 130... Line 146...
130 
          fieldPos[fieldCnt++] = linebuff + i + 1;
 146 
          fieldPos[fieldCnt++] = linebuff + i + 1;
131 
        }
 147 
        }
132 
 
 148 
 
133 
    }
 149 
    }
134 
 
 150 
 
- 
 
 151 
  // decode RMC
135 
  if (linebuff[2] == 'R' && linebuff[3] == 'M' && linebuff[4] == 'C')
 152 
  if (linebuff[2] == 'R' && linebuff[3] == 'M' && linebuff[4] == 'C')
136 
 
 153 
 
137 
    {
 154 
    {
138 
      // decode the fields
 155 
      // decode the fields
- 
 
 156 
      memcpy (loc->time, fieldPos[0], 6);
139 
      loc->valid = *fieldPos[1];
 157 
      loc->valid = *fieldPos[1];
140 
      loc->lat = decodeLL (fieldPos[2], 10);
 158 
      loc->lat = decodeLL (fieldPos[2], 10);
141 
      loc->ns = *fieldPos[3];
 159 
      loc->ns = *fieldPos[3];
142 
      loc->lon = decodeLL (fieldPos[4], 100);
 160 
      loc->lon = decodeLL (fieldPos[4], 100);
143 
      loc->ew = *fieldPos[5];
 161 
      loc->ew = *fieldPos[5];
144 
      loc->speed =  decodeFP (fieldPos[6]);
 162 
      loc->speed = decodeFP (fieldPos[6]);
145 
      loc->heading = decodeFP (fieldPos[7]);
 163 
      loc->heading = decodeFP (fieldPos[7]);
146 
 
 - 
 
- 
 
 164 
      loc->good = true;
147 
    }
 165 
    }
148 
return linePos;
 166 
  return true;
149 
}
 167 
}