Gps.cpp

00001 #include "Gps.h"
00002 
00003 Gps::Gps(PinName gpsPinTx, PinName gpsPinRx)
00004 {
00005     _values = Value();
00006     _gps = new MODSERIAL(gpsPinTx, gpsPinRx);
00007     _gps->baud(115200);
00008 
00009     DEBUG("GPS initialised\r\n");
00010 }
00011 
00012 Gps::~Gps(){}
00013 
00014 Gps::Value Gps::getValues()
00015 {
00016     while(_gps->readable() > 0)
00017     {
00018         int c = _gps->getc();
00019         if(_tinyGPS.encode(c))
00020         {
00021             unsigned long fix_age;
00022             _tinyGPS.f_get_position(&_values.latitude, &_values.longitude, &fix_age);
00023           
00024             _values.altitude = _tinyGPS.f_altitude();
00025             
00026             if ((fix_age == TinyGPS::GPS_INVALID_AGE) || (fix_age > 5000)) _values.fix = false;
00027             else _values.fix = true;
00028         }
00029     }
00030 
00031     return _values;
00032 }
00033 
00034 Gps::Difference Gps::getDifference(Value latLong1, Value latLong2)
00035 {
00036     int R = 637100000; //cm
00037     float dLat = deg2rad(latLong2.latitude - latLong1.latitude);
00038     float dLon = deg2rad(latLong2.longitude - latLong1.longitude);
00039     latLong1.latitude = deg2rad(latLong1.latitude);
00040     latLong2.latitude = deg2rad(latLong2.latitude);
00041     
00042     float a = sin(dLat/2) * sin(dLat/2) + sin(dLon/2) * sin(dLon/2) * cos(latLong1.latitude) * cos(latLong2.latitude); 
00043     float c = 2 * atan2(sqrt(a), sqrt(1-a)); 
00044     float d = R * c;
00045     
00046     float y = sin(dLon) * cos(latLong2.latitude);
00047     float x = cos(latLong1.latitude) * sin(latLong2.latitude) - sin(latLong1.latitude) * cos(latLong2.latitude) * cos(dLat);
00048     float brng = rad2deg(atan2(y, x));
00049     
00050     float oppAng = 90 - brng;
00051     float xDiff = d * sin(deg2rad(brng));
00052     float yDiff = d * sin(deg2rad(oppAng));
00053     
00054     //printf("d %f, opp %f, diff %f \r\n", d, oppAng, yDiff);
00055     //printf("lat1 %f long1 %f lat2 %f long2 %f ", latLong1.latitude, latLong1.longitude, latLong2.latitude, latLong2.longitude);
00056     //printf(" Bearing %f", brng);
00057     //printf(" Distance %f", d);
00058     //printf(" X %f, Y %f\r\n", xDiff, yDiff);   
00059     
00060     Difference difference = Difference();
00061     difference.x = xDiff;
00062     difference.y = yDiff;
00063     difference.bearing = brng;
00064     difference.distance = d;
00065     
00066     return difference;  
00067 }
00068 
00069 double Gps::deg2rad(double deg)
00070 {
00071     return (deg * PI / 180.0);
00072 }
00073 
00074 double Gps::rad2deg(double rad) 
00075 {
00076     return (rad * 180.0 / PI);
00077 }