David Spillman / GPS

GPS

Fork of GPS by Sam Clarke

GPS.cpp@7:1440e1e56a15, 2015-03-04 (annotated)

Committer:
Spilly
Date:
Wed Mar 04 17:47:53 2015 +0000
Revision:
7:1440e1e56a15
Parent:
6:94e2037a8bc8 
Modified

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SamClarke 1:0a034c2dbea6 1 #include "GPS.h"
Spilly 7:1440e1e56a15 2
SamClarke 1:0a034c2dbea6 3 GPS::GPS(PinName tx, PinName rx) : _UltimateGps(tx, rx)
SamClarke 1:0a034c2dbea6 4 {
SamClarke 1:0a034c2dbea6 5 _UltimateGps.baud(9600);
SamClarke 1:0a034c2dbea6 6 }
SamClarke 1:0a034c2dbea6 7
SamClarke 1:0a034c2dbea6 8 int GPS::parseData()
SamClarke 1:0a034c2dbea6 9 {
SamClarke 1:0a034c2dbea6 10 while(1) {
SamClarke 1:0a034c2dbea6 11 getData();
SamClarke 1:0a034c2dbea6 12 if(sscanf(NEMA, "GPGGA, %*f, %*f, %*c, %*f, %*c, %d, %d, %*f, %f", &fixtype, &satellites, &altitude) >=1);
SamClarke 4:9ac674d05370 13 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) {
SamClarke 1:0a034c2dbea6 14 if(fixtype == 0) {
SamClarke 1:0a034c2dbea6 15 return 0;
SamClarke 1:0a034c2dbea6 16 }
SamClarke 4:9ac674d05370 17 year += 2000;
SamClarke 1:0a034c2dbea6 18 if(ns =='S') {
SamClarke 1:0a034c2dbea6 19 latitude *= -1.0;
SamClarke 1:0a034c2dbea6 20 }
SamClarke 1:0a034c2dbea6 21 if(ew =='W') {
SamClarke 1:0a034c2dbea6 22 longitude *= -1.0;
SamClarke 1:0a034c2dbea6 23 }
SamClarke 1:0a034c2dbea6 24 float degrees = trunc(latitude / 100.0f);
SamClarke 1:0a034c2dbea6 25 float minutes = latitude - (degrees * 100.0f);
SamClarke 1:0a034c2dbea6 26 latitude = degrees + minutes / 60.0f;
SamClarke 1:0a034c2dbea6 27 degrees = trunc(longitude / 100.0f);
SamClarke 1:0a034c2dbea6 28 minutes = longitude - (degrees * 100.0f);
SamClarke 1:0a034c2dbea6 29 longitude = degrees + minutes / 60.0f;
SamClarke 3:5cb504ce2793 30 if(fixtype == 1) {
SamClarke 3:5cb504ce2793 31 fix = "Positive";
SamClarke 3:5cb504ce2793 32 }
SamClarke 3:5cb504ce2793 33 if(fixtype == 2) {
SamClarke 3:5cb504ce2793 34 fix = "Differential";
SamClarke 3:5cb504ce2793 35 }
Spilly 6:94e2037a8bc8 36 if(heading > 0.0f && heading < 45.0f) {
SamClarke 3:5cb504ce2793 37 cardinal = "NNE";
SamClarke 3:5cb504ce2793 38 }
Spilly 6:94e2037a8bc8 39 if(heading == 45.0f) {
SamClarke 3:5cb504ce2793 40 cardinal = "NE";
SamClarke 3:5cb504ce2793 41 }
Spilly 6:94e2037a8bc8 42 if(heading > 45.0f && heading < 90.0f) {
SamClarke 3:5cb504ce2793 43 cardinal = "ENE";
SamClarke 3:5cb504ce2793 44 }
Spilly 6:94e2037a8bc8 45 if(heading == 90.0f) {
SamClarke 3:5cb504ce2793 46 cardinal = "E";
SamClarke 3:5cb504ce2793 47 }
Spilly 6:94e2037a8bc8 48 if(heading > 90.0f && heading < 135.0f) {
SamClarke 3:5cb504ce2793 49 cardinal = "ESE";
SamClarke 3:5cb504ce2793 50 }
Spilly 6:94e2037a8bc8 51 if(heading == 135.0f) {
SamClarke 3:5cb504ce2793 52 cardinal = "SE";
SamClarke 3:5cb504ce2793 53 }
Spilly 6:94e2037a8bc8 54 if(heading > 135.0f && heading < 180.0f) {
SamClarke 3:5cb504ce2793 55 cardinal = "SSE";
SamClarke 3:5cb504ce2793 56 }
Spilly 6:94e2037a8bc8 57 if(heading == 180.0f) {
SamClarke 3:5cb504ce2793 58 cardinal = "S";
SamClarke 3:5cb504ce2793 59 }
Spilly 6:94e2037a8bc8 60 if(heading > 180.0f && heading < 225.0f) {
SamClarke 3:5cb504ce2793 61 cardinal = "SSW";
SamClarke 3:5cb504ce2793 62 }
Spilly 6:94e2037a8bc8 63 if(heading == 225.0f) {
SamClarke 3:5cb504ce2793 64 cardinal = "SW";
SamClarke 3:5cb504ce2793 65 }
Spilly 6:94e2037a8bc8 66 if(heading > 225.0f && heading < 270.0f) {
SamClarke 3:5cb504ce2793 67 cardinal = "WSW";
SamClarke 3:5cb504ce2793 68 }
Spilly 6:94e2037a8bc8 69 if(heading == 270.0f) {
SamClarke 3:5cb504ce2793 70 cardinal = "W";
SamClarke 3:5cb504ce2793 71 }
Spilly 6:94e2037a8bc8 72 if(heading > 270.0f && heading < 315.0f) {
SamClarke 3:5cb504ce2793 73 cardinal = "WNW";
SamClarke 3:5cb504ce2793 74 }
Spilly 6:94e2037a8bc8 75 if(heading == 315.0f) {
SamClarke 3:5cb504ce2793 76 cardinal = "NW";
SamClarke 3:5cb504ce2793 77 }
Spilly 6:94e2037a8bc8 78 if(heading > 315.0f && heading < 360.0f) {
SamClarke 3:5cb504ce2793 79 cardinal = "NNW";
SamClarke 3:5cb504ce2793 80 }
Spilly 6:94e2037a8bc8 81 if(heading == 360.0f || heading == 0.0f) {
SamClarke 3:5cb504ce2793 82 cardinal = "N";
SamClarke 3:5cb504ce2793 83 }
Spilly 6:94e2037a8bc8 84 kph = speed*1.852f;
SamClarke 1:0a034c2dbea6 85 return 1;
SamClarke 1:0a034c2dbea6 86 }
SamClarke 1:0a034c2dbea6 87 }
SamClarke 1:0a034c2dbea6 88 }
SamClarke 1:0a034c2dbea6 89
SamClarke 1:0a034c2dbea6 90
SamClarke 1:0a034c2dbea6 91 float GPS::trunc(float v)
SamClarke 1:0a034c2dbea6 92 {
Spilly 6:94e2037a8bc8 93 if(v < 0.0f) {
Spilly 6:94e2037a8bc8 94 v*= -1.0f;
SamClarke 1:0a034c2dbea6 95 v = floor(v);
Spilly 6:94e2037a8bc8 96 v*=-1.0f;
SamClarke 1:0a034c2dbea6 97 } else {
SamClarke 1:0a034c2dbea6 98 v = floor(v);
SamClarke 1:0a034c2dbea6 99 }
SamClarke 1:0a034c2dbea6 100 return v;
SamClarke 1:0a034c2dbea6 101 }
SamClarke 1:0a034c2dbea6 102
SamClarke 1:0a034c2dbea6 103 void GPS::getData()
SamClarke 1:0a034c2dbea6 104 {
SamClarke 1:0a034c2dbea6 105 while(_UltimateGps.getc() != '$');
SamClarke 1:0a034c2dbea6 106 for(int i=0; i<256; i++) {
SamClarke 1:0a034c2dbea6 107 NEMA[i] = _UltimateGps.getc();
SamClarke 1:0a034c2dbea6 108 if(NEMA[i] == '\r') {
SamClarke 1:0a034c2dbea6 109 NEMA[i] = 0;
SamClarke 1:0a034c2dbea6 110 return;
SamClarke 1:0a034c2dbea6 111 }
SamClarke 1:0a034c2dbea6 112 }
SamClarke 1:0a034c2dbea6 113 error("overflowed message limit");
SamClarke 1:0a034c2dbea6 114 }
SamClarke 1:0a034c2dbea6 115
SamClarke 1:0a034c2dbea6 116 void GPS::Init()
SamClarke 1:0a034c2dbea6 117 {
SamClarke 5:96d5736d9613 118 wait(1);
SamClarke 2:dcc14e81f8be 119 _UltimateGps.printf("$PMTK220,200*2C\r\n");
SamClarke 5:96d5736d9613 120 wait(0.2);
SamClarke 1:0a034c2dbea6 121 _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");
SamClarke 1:0a034c2dbea6 122 wait(1);
SamClarke 1:0a034c2dbea6 123 }