GPS.cpp

00001 #include "GPS.h"
00002 GPS::GPS(PinName tx, PinName rx) : _UltimateGps(tx, rx)
00003 {
00004     _UltimateGps.baud(9600);
00005 }
00006 
00007 int GPS::parseData()
00008 {
00009     while(1) {
00010         getData();
00011         if(sscanf(NEMA, "GPGGA, %*f, %*f, %*c, %*f, %*c, %d, %d, %*f, %f", &fixtype, &satellites, &altitude) >=1);
00012         if(sscanf(NEMA, "GPRMC, %2d%2d%f, %c, %f, %c, %f, %c, %f, %f, %2d%2d%2d", &hours, &minutes, &seconds, &validity, &latitude, &ns, &longitude, &ew, &speed, &heading, &day, &month, &year) >=1) {
00013             if(fixtype == 0) {
00014                 return 0;
00015             }
00016             year += 2000;
00017             if(ns =='S') {
00018                 latitude   *= -1.0;
00019             }
00020             if(ew =='W') {
00021                 longitude  *= -1.0;
00022             }
00023             float degrees = trunc(latitude / 100.0f);
00024             float minutes = latitude - (degrees * 100.0f);
00025             latitude = degrees + minutes / 60.0f;
00026             degrees = trunc(longitude / 100.0f);
00027             minutes = longitude - (degrees * 100.0f);
00028             longitude = degrees + minutes / 60.0f;
00029             if(fixtype == 1) {
00030                 fix = "Positive";
00031             }
00032             if(fixtype == 2) {
00033                 fix = "Differential";
00034             }
00035             if(heading > 0.00 && heading < 45.00) {
00036                 cardinal = "NNE";
00037             }
00038             if(heading == 45.00) {
00039                 cardinal = "NE";
00040             }
00041             if(heading > 45.00 && heading < 90.00) {
00042                 cardinal = "ENE";
00043             }
00044             if(heading == 90.00) {
00045                 cardinal = "E";
00046             }
00047             if(heading > 90.00 && heading < 135.00) {
00048                 cardinal = "ESE";
00049             }
00050             if(heading == 135.00) {
00051                 cardinal = "SE";
00052             }
00053             if(heading > 135.00 && heading < 180.00) {
00054                 cardinal = "SSE";
00055             }
00056             if(heading == 180.00) {
00057                 cardinal = "S";
00058             }
00059             if(heading > 180.00 && heading < 225.00) {
00060                 cardinal = "SSW";
00061             }
00062             if(heading == 225.00) {
00063                 cardinal = "SW";
00064             }
00065             if(heading > 225.00 && heading < 270.00) {
00066                 cardinal = "WSW";
00067             }
00068             if(heading == 270.00) {
00069                 cardinal = "W";
00070             }
00071             if(heading > 270.00 && heading < 315.00) {
00072                 cardinal = "WNW";
00073             }
00074             if(heading == 315.00) {
00075                 cardinal = "NW";
00076             }
00077             if(heading > 315.00 && heading < 360.00) {
00078                 cardinal = "NNW";
00079             }
00080             if(heading == 360.00 || heading == 0.00) {
00081                 cardinal = "N";
00082             }
00083             kph = speed*1.852;
00084             return 1;
00085         }
00086     }
00087 }
00088 
00089 
00090 float GPS::trunc(float v)
00091 {
00092     if(v < 0.0) {
00093         v*= -1.0;
00094         v = floor(v);
00095         v*=-1.0;
00096     } else {
00097         v = floor(v);
00098     }
00099     return v;
00100 }
00101 
00102 void GPS::getData()
00103 {
00104     while(_UltimateGps.getc() != '$');
00105     for(int i=0; i<256; i++) {
00106         NEMA[i] = _UltimateGps.getc();
00107         if(NEMA[i] == '\r') {
00108             NEMA[i] = 0;
00109             return;
00110         }
00111     }
00112     error("overflowed message limit");
00113 }
00114 
00115 void GPS::Init()
00116 {
00117     wait(1);
00118     _UltimateGps.printf("$PMTK220,200*2C\r\n");
00119     wait(0.2);
00120     _UltimateGps.printf("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n");
00121     wait(1);
00122 }