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
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?
User | Revision | Line number | New 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 | } |