Tyler Weaver / HARP3

Generation 3 of the Harp project

Dependencies:   Servo TMP36 GZ buffered-serial1 chan_fatfs_sd nmea_parser watchdog mbed-rtos mbed

Fork of HARP2 by Tyler Weaver

GPS/GPS.cpp@2:0c9ade531a5b, 2012-02-22 (annotated)

Committer:
tylerjw
Date:
Wed Feb 22 04:37:13 2012 +0000
Revision:
2:0c9ade531a5b
Parent:
1:2ace7946a246
Child:
3:9cba44dd2f2b
VTG parsing - no access

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tylerjw 2:0c9ade531a5b 1 #include "GPS.h"
tylerjw 2:0c9ade531a5b 2
tylerjw 2:0c9ade531a5b 3 GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) {
tylerjw 2:0c9ade531a5b 4 _gps.baud(4800);
tylerjw 2:0c9ade531a5b 5 nmea_longitude = 0.0;
tylerjw 2:0c9ade531a5b 6 nmea_latitude = 0.0;
tylerjw 2:0c9ade531a5b 7 utc_time = 0;
tylerjw 2:0c9ade531a5b 8 ns = ' ';
tylerjw 2:0c9ade531a5b 9 ew = ' ';
tylerjw 2:0c9ade531a5b 10 lock = 0;
tylerjw 2:0c9ade531a5b 11 satelites = 0;
tylerjw 2:0c9ade531a5b 12 hdop = 0.0;
tylerjw 2:0c9ade531a5b 13 msl_altitude = 0.0;
tylerjw 2:0c9ade531a5b 14 msl_units = ' ';
tylerjw 2:0c9ade531a5b 15
tylerjw 2:0c9ade531a5b 16 rmc_status = ' ';
tylerjw 2:0c9ade531a5b 17 speed_k = 0.0;
tylerjw 2:0c9ade531a5b 18 course_d = 0.0;
tylerjw 2:0c9ade531a5b 19 date = 0;
tylerjw 2:0c9ade531a5b 20
tylerjw 2:0c9ade531a5b 21 dec_longitude = 0.0;
tylerjw 2:0c9ade531a5b 22 dec_latitude = 0.0;
tylerjw 2:0c9ade531a5b 23
tylerjw 2:0c9ade531a5b 24 gll_status = ' ';
tylerjw 2:0c9ade531a5b 25
tylerjw 2:0c9ade531a5b 26 course_t = 0.0; // ground speed true
tylerjw 2:0c9ade531a5b 27 course_t_unit = ' ';
tylerjw 2:0c9ade531a5b 28 course_m = 0.0; // magnetic
tylerjw 2:0c9ade531a5b 29 course_m_unit = ' ';
tylerjw 2:0c9ade531a5b 30 speed_k_unit = ' ';
tylerjw 2:0c9ade531a5b 31 speed_km = 0.0; // speek km/hr
tylerjw 2:0c9ade531a5b 32 speed_km_unit = ' ';
tylerjw 2:0c9ade531a5b 33 }
tylerjw 2:0c9ade531a5b 34
tylerjw 2:0c9ade531a5b 35 float GPS::nmea_to_dec(float deg_coord, char nsew) {
tylerjw 2:0c9ade531a5b 36 int degree = (int)(deg_coord/100);
tylerjw 2:0c9ade531a5b 37 float minutes = deg_coord - degree*100;
tylerjw 2:0c9ade531a5b 38 float dec_deg = minutes / 60;
tylerjw 2:0c9ade531a5b 39 float decimal = degree + dec_deg;
tylerjw 2:0c9ade531a5b 40 if (nsew == 'S' || nsew == 'W') { // return negative
tylerjw 2:0c9ade531a5b 41 decimal *= -1;
tylerjw 2:0c9ade531a5b 42 }
tylerjw 2:0c9ade531a5b 43 return decimal;
tylerjw 2:0c9ade531a5b 44 }
tylerjw 2:0c9ade531a5b 45
tylerjw 2:0c9ade531a5b 46 int GPS::sample() {
tylerjw 2:0c9ade531a5b 47 int line_parsed = 0;
tylerjw 2:0c9ade531a5b 48
tylerjw 2:0c9ade531a5b 49 while (_gps.readable()) {
tylerjw 2:0c9ade531a5b 50 getline();
tylerjw 2:0c9ade531a5b 51
tylerjw 2:0c9ade531a5b 52 // Check if it is a GPGGA msg (matches both locked and non-locked msg)
tylerjw 2:0c9ade531a5b 53 if (sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", &utc_time, &nmea_latitude, &ns, &nmea_longitude, &ew, &lock, &satelites, &hdop, &msl_altitude, &msl_units) >= 1) {
tylerjw 2:0c9ade531a5b 54 line_parsed = 1;
tylerjw 2:0c9ade531a5b 55 }
tylerjw 2:0c9ade531a5b 56 // Check if it is a GPRMC msg
tylerjw 2:0c9ade531a5b 57 else if (sscanf(msg, "GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,%d", &utc_time, &rmc_status, &nmea_latitude, &ns, &nmea_longitude, &ew, &speed_k, &course_d, &date) >= 1) {
tylerjw 2:0c9ade531a5b 58 line_parsed = 1;
tylerjw 2:0c9ade531a5b 59 }
tylerjw 2:0c9ade531a5b 60 // GLL - Geographic Position-Lat/Lon
tylerjw 2:0c9ade531a5b 61 else if (sscanf(msg, "GPGLL,%f,%c,%f,%c,%f,%c", &nmea_latitude, &ns, &nmea_longitude, &ew, &utc_time, &gll_status) >= 1) {
tylerjw 2:0c9ade531a5b 62 line_parsed = 1;
tylerjw 2:0c9ade531a5b 63 }
tylerjw 2:0c9ade531a5b 64 // VTG-Course Over Ground and Ground Speed
tylerjw 2:0c9ade531a5b 65 else if (sscanf(msg, "GPVTG,%f,%c,%f,%c,%f,%c,%f,%c", &course_t, &course_t_unit, &course_m, &course_m_unit, &speed_k, &speed_k_unit, &speed_km, &speed_km_unit) >= 1) {
tylerjw 2:0c9ade531a5b 66 line_parsed = 1;
tylerjw 2:0c9ade531a5b 67 }
tylerjw 2:0c9ade531a5b 68 }
tylerjw 2:0c9ade531a5b 69 if (!lock) {
tylerjw 2:0c9ade531a5b 70 return NO_LOCK;
tylerjw 2:0c9ade531a5b 71 } else if (line_parsed) {
tylerjw 2:0c9ade531a5b 72 return PARSED;
tylerjw 2:0c9ade531a5b 73 } else {
tylerjw 2:0c9ade531a5b 74 return NOT_PARSED;
tylerjw 2:0c9ade531a5b 75 }
tylerjw 2:0c9ade531a5b 76 }
tylerjw 2:0c9ade531a5b 77
tylerjw 2:0c9ade531a5b 78 float GPS::trunc(float v) {
tylerjw 2:0c9ade531a5b 79 if (v < 0.0) {
tylerjw 2:0c9ade531a5b 80 v*= -1.0;
tylerjw 2:0c9ade531a5b 81 v = floor(v);
tylerjw 2:0c9ade531a5b 82 v*=-1.0;
tylerjw 2:0c9ade531a5b 83 } else {
tylerjw 2:0c9ade531a5b 84 v = floor(v);
tylerjw 2:0c9ade531a5b 85 }
tylerjw 2:0c9ade531a5b 86 return v;
tylerjw 2:0c9ade531a5b 87 }
tylerjw 2:0c9ade531a5b 88
tylerjw 2:0c9ade531a5b 89 void GPS::getline() {
tylerjw 2:0c9ade531a5b 90 while (_gps.getc() != '$'); // wait for the start of a line
tylerjw 2:0c9ade531a5b 91 for (int i=0; i<256; i++) {
tylerjw 2:0c9ade531a5b 92 msg[i] = _gps.getc();
tylerjw 2:0c9ade531a5b 93 if (msg[i] == '\r') {
tylerjw 2:0c9ade531a5b 94 msg[i] = 0;
tylerjw 2:0c9ade531a5b 95 return;
tylerjw 2:0c9ade531a5b 96 }
tylerjw 2:0c9ade531a5b 97 }
tylerjw 2:0c9ade531a5b 98 error("Overflowed message limit");
tylerjw 2:0c9ade531a5b 99 }
tylerjw 2:0c9ade531a5b 100
tylerjw 2:0c9ade531a5b 101 float GPS::get_msl_altitude() {
tylerjw 2:0c9ade531a5b 102 if (!lock)
tylerjw 2:0c9ade531a5b 103 return 0.0;
tylerjw 2:0c9ade531a5b 104 else
tylerjw 2:0c9ade531a5b 105 return msl_altitude;
tylerjw 2:0c9ade531a5b 106 }
tylerjw 2:0c9ade531a5b 107
tylerjw 2:0c9ade531a5b 108 int GPS::get_satelites() {
tylerjw 2:0c9ade531a5b 109 if (!lock)
tylerjw 2:0c9ade531a5b 110 return 0;
tylerjw 2:0c9ade531a5b 111 else
tylerjw 2:0c9ade531a5b 112 return satelites;
tylerjw 2:0c9ade531a5b 113 }
tylerjw 2:0c9ade531a5b 114
tylerjw 2:0c9ade531a5b 115 float GPS::get_nmea_longitude() {
tylerjw 2:0c9ade531a5b 116 if (!lock)
tylerjw 2:0c9ade531a5b 117 return 0.0;
tylerjw 2:0c9ade531a5b 118 else
tylerjw 2:0c9ade531a5b 119 return nmea_longitude;
tylerjw 2:0c9ade531a5b 120 }
tylerjw 2:0c9ade531a5b 121
tylerjw 2:0c9ade531a5b 122 float GPS::get_dec_longitude() {
tylerjw 2:0c9ade531a5b 123 dec_longitude = nmea_to_dec(nmea_longitude, ew);
tylerjw 2:0c9ade531a5b 124 if (!lock)
tylerjw 2:0c9ade531a5b 125 return 0.0;
tylerjw 2:0c9ade531a5b 126 else
tylerjw 2:0c9ade531a5b 127 return dec_longitude;
tylerjw 2:0c9ade531a5b 128 }
tylerjw 2:0c9ade531a5b 129
tylerjw 2:0c9ade531a5b 130 float GPS::get_nmea_latitude() {
tylerjw 2:0c9ade531a5b 131 if (!lock)
tylerjw 2:0c9ade531a5b 132 return 0.0;
tylerjw 2:0c9ade531a5b 133 else
tylerjw 2:0c9ade531a5b 134 return nmea_latitude;
tylerjw 2:0c9ade531a5b 135 }
tylerjw 2:0c9ade531a5b 136
tylerjw 2:0c9ade531a5b 137 float GPS::get_dec_latitude() {
tylerjw 2:0c9ade531a5b 138 dec_latitude = nmea_to_dec(nmea_latitude, ns);
tylerjw 2:0c9ade531a5b 139 if (!lock)
tylerjw 2:0c9ade531a5b 140 return 0.0;
tylerjw 2:0c9ade531a5b 141 else
tylerjw 2:0c9ade531a5b 142 return nmea_latitude;
tylerjw 2:0c9ade531a5b 143 }