git.gag.com Git - debian/gnuradio/blob - ezdop/src/host/hunter/src/gps.cc

   1 /*
   2  Copyright 2006 Johnathan Corgan.
   3
   4  This program is free software; you can redistribute it and/or modify
   5  it under the terms of the GNU General Public License version 2
   6  as published by the Free Software Foundation.
   7
   8  This software is distributed in the hope that it will be useful,
   9  but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  GNU General Public License for more details.
  12
  13  You should have received a copy of the GNU General Public License
  14  along with GNU Radio; see the file COPYING.  If not, write to
  15  the Free Software Foundation, Inc., 51 Franklin Street,
  16  Boston, MA 02110-1301, USA.
  17 */
  18
  19 // Application level includes
  20 #include "gps.h"
  21 #include "serial.h"
  22
  23 // wxWidgets includes
  24 #include <wx/log.h>
  25
  26 #define NMEA_BAUD       4800
  27 #define NMEA_BUFSIZE    82      // Maximum NMEA sentence length
  28
  29 const wxEventType wxEVT_GPS_UPDATE = wxNewEventType();
  30
  31 GPSUpdate::GPSUpdate(const wxEventType &event, GPRMC *gprmc) :
  32 wxNotifyEvent(event)
  33 {
  34     m_gprmc = gprmc;
  35 }
  36
  37 GPSBackground::GPSBackground(GPS *gps)
  38 {
  39     wxLogDebug(_T("GPSBackground::GPSBackground()"));
  40     wxASSERT(gps);
  41
  42     m_running = false;
  43     m_gps = gps;
  44     m_port = gps->GetPort();
  45         Create();
  46 }
  47
  48 // It's in thread.h but somehow gets undef'd
  49 typedef void *ExitCode;
  50 ExitCode GPSBackground::Entry()
  51 {
  52     wxLogDebug(_T("GPSBackground::GPSBackground: entry"));
  53
  54     m_running = true;
  55         while (!TestDestroy())
  56         PerLoop();
  57         m_running = false;
  58
  59     wxLogDebug(_T("GPSBackground::GPSBackground: exit"));
  60 }
  61
  62 void GPSBackground::PerLoop()
  63 {
  64     static char buffer[NMEA_BUFSIZE];
  65     static int offset = 0;
  66
  67     while(m_port->RxReady() > 0) {
  68         while (offset < NMEA_BUFSIZE) {
  69             // Read a byte into the buffer from the GPS data
  70             if (m_port->Read(&buffer[offset], 1) != 1)
  71                 return;  // No more to read or read error/timeout, bail
  72
  73             // Test for end of NMEA message
  74             if (buffer[offset] == '\r' || buffer[offset] == '\n') {
  75                 buffer[offset] = '\0'; // Append end of string null
  76                 if (strlen(buffer))
  77                     m_gps->RxData(buffer);
  78                 offset = 0;
  79             }
  80             else
  81                 offset++;
  82         }
  83
  84         wxLogDebug(_T("GPSBackground: discarding too long input"));
  85         offset = 0;
  86     }
  87
  88     wxMilliSleep(500);
  89 }
  90
  91 GPS::GPS(wxEvtHandler *dest)
  92 {
  93     wxLogDebug(_T("GPS::GPS()"));
  94     m_thread = NULL;
  95     m_dest = dest;
  96 }
  97
  98 GPS::~GPS()
  99 {
 100     wxLogDebug(_T("GPS::~GPS()"));
 101 }
 102
 103 bool GPS::Start(wxString &port)
 104 {
 105     wxLogDebug(_T("GPS::Start(): %s"), port.c_str());
 106     m_port = new SerialPort(port);
 107
 108     if (m_port->Open(NMEA_BAUD) == false) {
 109         delete m_port;
 110         return false;
 111     }
 112
 113     m_thread = new GPSBackground(this);
 114     m_thread->Run();
 115     return true;
 116 }
 117
 118 bool GPS::Stop()
 119 {
 120     wxLogDebug(_T("GPS::Stop()"));
 121
 122     if (m_thread && m_thread->IsRunning()) {
 123         m_thread->Delete();
 124         while (m_thread->IsRunning()) {
 125             wxYieldIfNeeded();
 126         }
 127     }
 128
 129     m_thread = NULL;
 130
 131     m_port->Close();
 132     if (m_port)
 133         delete m_port;
 134
 135     return true;
 136 }
 137
 138 bool NMEA::Checksum(char *sentence)
 139 {
 140     unsigned char checksum = '\0';
 141     char ch, *pos = sentence, ctxt[3];
 142
 143     while ((ch = *pos++) != '*' && ch != '\0')
 144         checksum ^= ch;
 145
 146     sprintf(ctxt, "%02X", checksum);
 147     if (strncmp(ctxt, pos, 2))
 148         return false;
 149     else
 150         return true;
 151 }
 152
 153 char *NMEA::Field(char *sentence, int num)
 154 {
 155     static char result[NMEA_BUFSIZE];
 156     char ch, *pos = sentence;
 157
 158     while (num-- > 0)
 159         while ((ch = *pos++) != ',' && ch != '\0')
 160             continue;
 161
 162     strncpy(result, pos, NMEA_BUFSIZE-1);
 163     int i = 0;
 164     pos = result;
 165     while (*pos && *pos != ',' && *pos != '*' && *pos != '\r' && ++i < NMEA_BUFSIZE)
 166         pos++;
 167
 168     *pos = 0;
 169     return result;
 170 }
 171
 172 double NMEA::Coord(char *sentence, int num)
 173 {
 174     double coord, degrees, minutes;
 175
 176     sscanf(Field(sentence, num), "%lf", &coord);
 177     minutes = 100.0*modf(coord/100.0, &degrees);
 178     coord = degrees+minutes/60.0;
 179
 180     char *ptr = Field(sentence, num+1);
 181     if (*ptr == 'S' || *ptr == 'W')
 182         coord = -coord;
 183
 184     return coord;
 185 }
 186
 187 void GPS::RxData(char *buffer)
 188 {
 189     wxASSERT(buffer);
 190
 191     if (NMEA::Checksum(buffer+1)) {
 192         if (strncmp("$GPRMC", buffer, 6) == 0) {
 193             GPRMC *fix = new GPRMC(buffer);
 194             GPSUpdate update(wxEVT_GPS_UPDATE, fix);
 195                 wxPostEvent(m_dest, update);
 196         }
 197     }
 198     else
 199         wxLogDebug(_T("GPS::RxData: NMEA checksum failed for input"));
 200 }
 201
 202 GPRMC::GPRMC(char *sentence)
 203 {
 204     wxASSERT(sentence);
 205
 206     struct tm stamp;
 207     char digits[2];
 208
 209     char *p = Field(sentence, 1);
 210     wxASSERT(p);
 211     strncpy(digits, p, 2);
 212     stamp.tm_hour = atoi(digits);
 213     strncpy(digits, p+2, 2);
 214     stamp.tm_min = atoi(digits);
 215     strncpy(digits, p+4, 2);
 216     stamp.tm_sec = atoi(digits);
 217
 218     p = Field(sentence, 9);
 219     wxASSERT(p);
 220     strncpy(digits, p, 2);
 221     stamp.tm_mday = atoi(digits);
 222     strncpy(digits, p+2, 2);
 223     stamp.tm_mon = atoi(digits)-1;
 224     strncpy(digits, p+4, 2);
 225     stamp.tm_year = atoi(digits)+100;
 226
 227     m_stamp = mktime(&stamp);
 228     m_valid = !strcmp(Field(sentence, 2), "A");
 229     m_fix.SetLatitude(Coord(sentence, 3));
 230     m_fix.SetLongitude(Coord(sentence, 5));
 231     sscanf(Field(sentence, 7), "%f", &m_speed);
 232     sscanf(Field(sentence, 8), "%f", &m_heading);
 233     sscanf(Field(sentence, 10), "%f", &m_magnetic);
 234     if (!strcmp(Field(sentence, 11), "W"))
 235         m_magnetic = -m_magnetic;
 236     m_mode = *Field(sentence, 12);
 237 }
 238
 239 void GPRMC::AsString(char *buf)
 240 {
 241     sprintf(buf, "%s %lf %lf %f %f %f, %s",
 242             ctime(&m_stamp),
 243             m_fix.Latitude(),
 244             m_fix.Longitude(),
 245             m_speed, m_heading, m_magnetic,
 246             m_valid ? "valid" : "invalid");
 247 }