GPSTracker.cpp

00001 #include <stdlib.h>
00002 #include <string.h>
00003 #include "GPSTracker.h"
00004 #include "logging.h"
00005 
00006 
00007 GPSTracker::GPSTracker() :
00008     _gps(),
00009     _thread(GPSTracker::thread_func, this),
00010     _positionSet(false)
00011 {
00012     _gps.init();
00013 }
00014 
00015 
00016 bool GPSTracker::position(GPSTracker::Position *position)
00017 {
00018     bool result = false;
00019     
00020     _mutex.lock();
00021     if (_positionSet) {
00022         memcpy(position, &_position, sizeof(GPSTracker::Position));
00023         _positionSet = false;
00024         result = true;
00025     }
00026     _mutex.unlock();
00027     
00028     return result;
00029 }
00030 
00031 
00032 void GPSTracker::thread()
00033 {
00034     char buf[256], chr; // needs to be that big otherwise mdm isn't working
00035     int ret, len, n;
00036     double altitude, latitude, longitude;
00037     while (true) {
00038         ret = _gps.getMessage(buf, sizeof(buf));
00039         if (ret <= 0) {
00040             Thread::wait(100);
00041             continue;
00042         }        
00043         len = LENGTH(ret);
00044         if ((PROTOCOL(ret) != GPSParser::NMEA) || (len <= 6)) {
00045             aWarning("GPS data is not in NMEA protocol!\n");
00046             continue;
00047         }
00048 
00049         // we're only interested in fixed GPS positions, data type: GPGGA
00050         if (strncmp("$GPGGA", buf, 6) != 0 && strncmp("$GNGGA", buf, 6) != 0)
00051             continue;
00052         if (!_gps.getNmeaItem(6, buf, len, n, 10) || n == 0)
00053             continue;
00054         
00055         // get altitude, latitude and longitude
00056         if ((!_gps.getNmeaAngle(2, buf, len, latitude)) ||
00057             (!_gps.getNmeaAngle(4, buf, len, longitude)) ||
00058             (!_gps.getNmeaItem(9, buf, len, altitude)) ||
00059             (!_gps.getNmeaItem(10, buf, len, chr)) ||
00060             (chr != 'M')) {
00061             continue;
00062         }
00063 
00064         _mutex.lock();
00065         _position.altitude = altitude;
00066         _position.latitude = latitude;
00067         _position.longitude = longitude;
00068         _positionSet = true;
00069         _mutex.unlock();
00070     }
00071 }
00072 
00073 
00074 void GPSTracker::thread_func(void const *arg)
00075 {
00076     GPSTracker *that;
00077     that = (GPSTracker*)arg;
00078     that->thread();
00079 }