git.gag.com Git - fw/openalt/blob - gps/gps.c

   1 //
   2 //
   3 //
   4 #include <stdio.h>
   5 #include <string.h>
   6 #include <unistd.h>
   7 #include <fcntl.h>
   8
   9 #include "FreeRTOS.h"
  10 #include "task.h"
  11 #include "semphr.h"
  12
  13 #include "gps.h"
  14
  15 //
  16 //
  17 //
  18 #define GPS_MAX_NMEA_SENTENCE 128
  19
  20 //
  21 //
  22 //
  23 static gpsData_t gpsData;
  24 static xSemaphoreHandle semaphore;
  25
  26 //
  27 //
  28 //
  29 static void gpsNormalizeNMEA (char *s);
  30 static unsigned char gpsChecksumNMEA (char *sz);
  31 static void gpsHandlerGGA (char *nmeaSentence);
  32 static void gpsHandlerRMC (char *nmeaSentence);
  33 static void gpsHandlerRestart (char *nmeaSentence);
  34 static void gpsDispatchMessages (char *nmeaSentence);
  35 static int gpsProcessByte (unsigned char c, char *nmeaSentence);
  36
  37 //
  38 //
  39 //
  40 static void gpsNormalizeNMEA (char *s)
  41 {
  42   int l = -1;
  43
  44   while (*s && *s != '*')
  45   {
  46     if (*s == ',' && (*(s + 1) == ',' || *(s + 1) == '*'))
  47     {
  48       if (l == -1)
  49         l = strlen (s) + 1;
  50
  51       memmove (s + 3, s + 1, l);
  52       *++s = '-';
  53       *++s = '1';
  54       l += 2;
  55     }
  56
  57     s++;
  58
  59     if (l != -1)
  60       l--;
  61   }
  62 }
  63
  64 //
  65 //
  66 //
  67 static unsigned char gpsChecksumNMEA (char *sz)
  68 {
  69   short i;
  70   unsigned char cs;
  71
  72   for (cs = 0, i = 1; sz [i] && sz [i] != '*'; i++)
  73     cs ^= ((unsigned char) sz [i]);
  74
  75   return cs;
  76 }
  77
  78 //
  79 //
  80 //
  81 static void gpsHandlerGGA (char *nmeaSentence)
  82 {
  83   char   valid = 0;
  84   double height = 0.0;
  85   double latitude = 0.0, longitude = 0.0;
  86   char   latitudeSign = 0, longitudeSign = 0;
  87
  88   if (sscanf (nmeaSentence, "%*f,%lf,%c,%lf,%c,%c,%*d,%*f,%lf", &latitude, &latitudeSign, &longitude, &longitudeSign, &valid, &height) == 6)
  89   {
  90     if (xSemaphoreTake (semaphore, portMAX_DELAY) == pdTRUE)
  91     {
  92       gpsData.valid = (valid - '0');
  93       gpsData.height = height;
  94       gpsData.latitude  = latitude * (latitudeSign == 'N' ? 1.0 : -1.0);
  95       gpsData.longitude = longitude * (longitudeSign == 'E' ? 1.0 : -1.0);
  96
  97       xSemaphoreGive (semaphore);
  98     }
  99   }
 100 }
 101
 102 static void gpsHandlerRMC (char *nmeaSentence)
 103 {
 104   float speed, course;
 105   int gpsdate, gpstime;
 106
 107   if (sscanf (nmeaSentence, "%d.%*d,%*c,%*f,%*c,%*f,%*c,%f,%f,%d", &gpstime, &speed, &course, &gpsdate) == 4)
 108   {
 109     if (xSemaphoreTake (semaphore, portMAX_DELAY) == pdTRUE)
 110     {
 111       gpsData.utcDay      =  gpsdate / 10000;
 112       gpsData.utcMonth    = (gpsdate / 100) % 100;
 113       gpsData.utcYear     = (gpsdate % 100) + 2000;
 114       gpsData.utcHours    =  gpstime / 10000;
 115       gpsData.utcMinutes  = (gpstime / 100) % 100;
 116       gpsData.utcSeconds  =  gpstime % 100;
 117       gpsData.groundSpeed =  speed;
 118       gpsData.trueCourse  =  course;
 119
 120       xSemaphoreGive (semaphore);
 121     }
 122   }
 123 }
 124
 125 static void gpsHandlerRestart (char *nmeaSentencemea __attribute__ ((unused)))
 126 {
 127   if (xSemaphoreTake (semaphore, portMAX_DELAY) == pdTRUE)
 128   {
 129     gpsData.restarts++;
 130     xSemaphoreGive (semaphore);
 131   }
 132 }
 133
 134 //
 135 //
 136 //
 137 typedef struct nmeaDispatch_s
 138 {
 139   const char *sentence;
 140   short length;
 141   void (*handler) (char *sentence);
 142   short normalize;
 143 }
 144 nmeaDispatch_t;
 145
 146 static const nmeaDispatch_t nmeaDispatch [] =
 147 {
 148   { "$GPGGA",    6, gpsHandlerGGA,      1 },
 149   { "$GPRMC",    6, gpsHandlerRMC,      1 },
 150   { "$HW Type",  8, gpsHandlerRestart,  0 },
 151   { NULL,        0, NULL,               0 }
 152 };
 153
 154 static void gpsDispatchMessages (char *nmeaSentence)
 155 {
 156   int i;
 157
 158   for (i = 0; nmeaDispatch [i].handler; i++)
 159   {
 160     if (!strncmp (nmeaDispatch [i].sentence, nmeaSentence, nmeaDispatch [i].length))
 161     {
 162       if (nmeaDispatch [i].normalize)
 163       {
 164         gpsNormalizeNMEA (&nmeaSentence [7]);
 165         (*nmeaDispatch [i].handler) (&nmeaSentence [7]);
 166       }
 167       else
 168         (*nmeaDispatch [i].handler) (NULL);
 169
 170       break;
 171     }
 172   }
 173 }
 174
 175 //
 176 //
 177 //
 178 static int gpsProcessByte (unsigned char c, char *nmeaSentence)
 179 {
 180   short complete = 0;
 181   static short state = 0;
 182   static short pos = 0;
 183
 184   switch (state)
 185   {
 186     case 0 :
 187       {
 188         if (c == '$')
 189         {
 190           pos = 0;
 191           nmeaSentence [pos++] = '$';
 192           nmeaSentence [pos] = '\0';
 193           state = 1;
 194         }
 195         else
 196           state = 0;
 197       }
 198       break;
 199
 200     case 1 :
 201       {
 202         if (pos < GPS_MAX_NMEA_SENTENCE)
 203         {
 204           if (c == 0x0a)
 205           {
 206             char *s;
 207
 208             state = 0;
 209
 210             if ((s = strchr (nmeaSentence, '*')))
 211             {
 212               int cksum;
 213
 214               if (sscanf (s + 1, "%x", &cksum) == 1)
 215               {
 216                 if (gpsChecksumNMEA (nmeaSentence) == cksum)
 217                   complete = 1;
 218               }
 219               else
 220                 fprintf (stderr, "NMEA checksum error: got 0x%02x, want %s", cksum, s);
 221             }
 222             else
 223               fprintf (stderr, "NMEA checksum not found: \"%s\"", nmeaSentence);
 224           }
 225           else if (c != 0x0d)
 226           {
 227             nmeaSentence [pos++] = c;
 228             nmeaSentence [pos] = '\0';
 229           }
 230         }
 231         else
 232           state = 0;
 233       }
 234       break;
 235   }
 236
 237   return (complete);
 238 }
 239
 240 //
 241 //  Return 1 if got a copy, 0 if not.
 242 //
 243 int gpsCopyData (gpsData_t *dst)
 244 {
 245 #ifndef CFG_CONSOLE_UART1
 246   if (xSemaphoreTake (semaphore, 100 / portTICK_RATE_MS) == pdTRUE)
 247   {
 248     memcpy (dst, &gpsData, sizeof (gpsData_t));
 249     xSemaphoreGive (semaphore);
 250     return 1;
 251   }
 252 #endif
 253
 254   memset (dst, 0, sizeof (gpsData_t));
 255   return 0;
 256 }
 257
 258 //
 259 //
 260 //
 261 portTASK_FUNCTION (vGPSTask, pvParameters __attribute__ ((unused)))
 262 {
 263   int fd;
 264   static char nmeaSentence [GPS_MAX_NMEA_SENTENCE];
 265
 266   memset (&gpsData, 0, sizeof (gpsData));
 267
 268   vSemaphoreCreateBinary (semaphore);
 269
 270   fd = open ("/dev/uart1", O_RDONLY);
 271
 272   if ((fd == -1) || (semaphore == NULL))
 273     for (;;)
 274       vTaskDelay (100);
 275
 276   for (;;)
 277   {
 278     portCHAR c;
 279
 280     if (read (fd, &c, sizeof (c)) == sizeof (c))
 281       if (gpsProcessByte (c, nmeaSentence))
 282         gpsDispatchMessages (nmeaSentence);
 283   }
 284 }