GPSTracker.cpp

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