lauren cloutier / GPSINT

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

GPSINT.cpp@0:35125ac3d3f9, 2020-11-17 (annotated)

Committer:
laurencloutier
Date:
Tue Nov 17 22:06:13 2020 +0000
Revision:
0:35125ac3d3f9
assignment 8;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
laurencloutier 0:35125ac3d3f9 1 /* GPSINT.cpp
laurencloutier 0:35125ac3d3f9 2 * Original program from
laurencloutier 0:35125ac3d3f9 3 * jbradshaw (20141101)
laurencloutier 0:35125ac3d3f9 4 * The program is modified for ublox NEO 6M.
laurencloutier 0:35125ac3d3f9 5 */
laurencloutier 0:35125ac3d3f9 6
laurencloutier 0:35125ac3d3f9 7 #include "GPSINT.h"
laurencloutier 0:35125ac3d3f9 8
laurencloutier 0:35125ac3d3f9 9 GPSINT::GPSINT(PinName tx, PinName rx) : _gps(tx, rx) {
laurencloutier 0:35125ac3d3f9 10 _gps.baud(9600);
laurencloutier 0:35125ac3d3f9 11 GPSidx=0; // Index for GPS buffer
laurencloutier 0:35125ac3d3f9 12 GPSstate=0; // Used to wait for '$' in GPS interrupt
laurencloutier 0:35125ac3d3f9 13 _gps.attach(this,&GPSINT::GPSSerialRecvInterrupt, _gps.RxIrq); // Recv interrupt handler
laurencloutier 0:35125ac3d3f9 14 }
laurencloutier 0:35125ac3d3f9 15
laurencloutier 0:35125ac3d3f9 16 int GPSINT::nmea_validate(char *nmeastr){
laurencloutier 0:35125ac3d3f9 17 char check[3];
laurencloutier 0:35125ac3d3f9 18 char checkcalcstr[3];
laurencloutier 0:35125ac3d3f9 19 int i;
laurencloutier 0:35125ac3d3f9 20 int calculated_check;
laurencloutier 0:35125ac3d3f9 21
laurencloutier 0:35125ac3d3f9 22 i=0;
laurencloutier 0:35125ac3d3f9 23 calculated_check=0;
laurencloutier 0:35125ac3d3f9 24
laurencloutier 0:35125ac3d3f9 25 // check to ensure that the string starts with a $
laurencloutier 0:35125ac3d3f9 26 if(nmeastr[i] == '$')
laurencloutier 0:35125ac3d3f9 27 i++;
laurencloutier 0:35125ac3d3f9 28 else
laurencloutier 0:35125ac3d3f9 29 return 0;
laurencloutier 0:35125ac3d3f9 30
laurencloutier 0:35125ac3d3f9 31 //No NULL reached, 75 char largest possible NMEA message, no '*' reached
laurencloutier 0:35125ac3d3f9 32 while((nmeastr[i] != 0) && (nmeastr[i] != '*') && (i < 75)){
laurencloutier 0:35125ac3d3f9 33 calculated_check ^= nmeastr[i];// calculate the checksum
laurencloutier 0:35125ac3d3f9 34 i++;
laurencloutier 0:35125ac3d3f9 35 }
laurencloutier 0:35125ac3d3f9 36
laurencloutier 0:35125ac3d3f9 37 if(i >= 75){
laurencloutier 0:35125ac3d3f9 38 return 0;// the string was too long so return an error
laurencloutier 0:35125ac3d3f9 39 }
laurencloutier 0:35125ac3d3f9 40
laurencloutier 0:35125ac3d3f9 41 if (nmeastr[i] == '*'){
laurencloutier 0:35125ac3d3f9 42 check[0] = nmeastr[i+1]; //put hex chars in check string
laurencloutier 0:35125ac3d3f9 43 check[1] = nmeastr[i+2];
laurencloutier 0:35125ac3d3f9 44 check[2] = 0;
laurencloutier 0:35125ac3d3f9 45 }
laurencloutier 0:35125ac3d3f9 46 else
laurencloutier 0:35125ac3d3f9 47 return 0;// no checksum separator found therefor invalid
laurencloutier 0:35125ac3d3f9 48
laurencloutier 0:35125ac3d3f9 49 sprintf(checkcalcstr,"%02X",calculated_check);
laurencloutier 0:35125ac3d3f9 50 return((checkcalcstr[0] == check[0])
laurencloutier 0:35125ac3d3f9 51 && (checkcalcstr[1] == check[1])) ? 1 : 0 ;
laurencloutier 0:35125ac3d3f9 52 }
laurencloutier 0:35125ac3d3f9 53
laurencloutier 0:35125ac3d3f9 54 void GPSINT::parseGPSString(char *GPSstrParse){
laurencloutier 0:35125ac3d3f9 55 //check if $GPGGA string
laurencloutier 0:35125ac3d3f9 56 if(!strncmp(GPSstrParse, "$GPGGA", 6)){
laurencloutier 0:35125ac3d3f9 57 if (sscanf(GPSstrParse, "$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) {
laurencloutier 0:35125ac3d3f9 58 // printf("%s", GPSstrParse);
laurencloutier 0:35125ac3d3f9 59 return;
laurencloutier 0:35125ac3d3f9 60 }
laurencloutier 0:35125ac3d3f9 61 else{
laurencloutier 0:35125ac3d3f9 62 // printf("BAD parse %s", GPSstrParse);
laurencloutier 0:35125ac3d3f9 63 }
laurencloutier 0:35125ac3d3f9 64 }
laurencloutier 0:35125ac3d3f9 65 // Check if $GPRMC string
laurencloutier 0:35125ac3d3f9 66 else if (!strncmp(GPSstrParse, "$GPRMC", 6)){
laurencloutier 0:35125ac3d3f9 67 if(sscanf(GPSstrParse, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,%d", &utc_time, &status,&nmea_latitude, &ns, &nmea_longitude, &ew, &speed_k, &course_d, &date) >= 1) {
laurencloutier 0:35125ac3d3f9 68 // printf("%s", GPSstrParse);
laurencloutier 0:35125ac3d3f9 69 return;
laurencloutier 0:35125ac3d3f9 70 }
laurencloutier 0:35125ac3d3f9 71 else{
laurencloutier 0:35125ac3d3f9 72 // printf("BAD parse %s", GPSstrParse);
laurencloutier 0:35125ac3d3f9 73 }
laurencloutier 0:35125ac3d3f9 74 }
laurencloutier 0:35125ac3d3f9 75 // GLL - Geographic Position-Lat/Lon
laurencloutier 0:35125ac3d3f9 76 else if (!strncmp(GPSstrParse, "$GPGLL", 6)){
laurencloutier 0:35125ac3d3f9 77 if(sscanf(GPSstrParse, "$GPGLL,%f,%c,%f,%c,%f,%c", &nmea_latitude, &ns, &nmea_longitude, &ew, &utc_time, &gll_status) >= 1) {
laurencloutier 0:35125ac3d3f9 78 // printf("%s", GPSstrParse);
laurencloutier 0:35125ac3d3f9 79 return;
laurencloutier 0:35125ac3d3f9 80 }
laurencloutier 0:35125ac3d3f9 81 else{
laurencloutier 0:35125ac3d3f9 82 // printf("BAD parse %s", GPSstrParse);
laurencloutier 0:35125ac3d3f9 83 }
laurencloutier 0:35125ac3d3f9 84 }
laurencloutier 0:35125ac3d3f9 85 // VTG-Course Over Ground and Ground Speed
laurencloutier 0:35125ac3d3f9 86 else if (!strncmp(GPSstrParse, "$GPVTG", 6)){
laurencloutier 0:35125ac3d3f9 87 if(sscanf(GPSstrParse, "$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) {
laurencloutier 0:35125ac3d3f9 88 // printf("%s", GPSstrParse);
laurencloutier 0:35125ac3d3f9 89 return;
laurencloutier 0:35125ac3d3f9 90 }
laurencloutier 0:35125ac3d3f9 91 else{
laurencloutier 0:35125ac3d3f9 92 // printf("BAD parse %s", GPSstrParse);
laurencloutier 0:35125ac3d3f9 93 }
laurencloutier 0:35125ac3d3f9 94 }
laurencloutier 0:35125ac3d3f9 95 }//parseGPSstring()
laurencloutier 0:35125ac3d3f9 96
laurencloutier 0:35125ac3d3f9 97 void GPSINT::GPSSerialRecvInterrupt(void)
laurencloutier 0:35125ac3d3f9 98 {
laurencloutier 0:35125ac3d3f9 99 char c;
laurencloutier 0:35125ac3d3f9 100 c =_gps.getc(); // On receive interrupt, get the character.
laurencloutier 0:35125ac3d3f9 101 // pc.printf("%c", c);
laurencloutier 0:35125ac3d3f9 102
laurencloutier 0:35125ac3d3f9 103 switch(GPSstate){
laurencloutier 0:35125ac3d3f9 104 case 0:
laurencloutier 0:35125ac3d3f9 105 if(c =='$'){
laurencloutier 0:35125ac3d3f9 106 GPSidx=0;
laurencloutier 0:35125ac3d3f9 107 Temp_GPSbuf[GPSidx] = c; //load char in current idx of array
laurencloutier 0:35125ac3d3f9 108 GPSidx++;
laurencloutier 0:35125ac3d3f9 109 GPSstate = 1;
laurencloutier 0:35125ac3d3f9 110 }
laurencloutier 0:35125ac3d3f9 111 break;
laurencloutier 0:35125ac3d3f9 112 case 1:
laurencloutier 0:35125ac3d3f9 113 Temp_GPSbuf[GPSidx] = c; //load char in current idx of array
laurencloutier 0:35125ac3d3f9 114 GPSidx++;
laurencloutier 0:35125ac3d3f9 115 if(c == '\n'){ //if last char was a newline
laurencloutier 0:35125ac3d3f9 116 Temp_GPSbuf[GPSidx] = '\0'; //append a NULL
laurencloutier 0:35125ac3d3f9 117 strcpy(GPSbuf, Temp_GPSbuf); //copy temp buf into GPS buf
laurencloutier 0:35125ac3d3f9 118 GPSidx=0; //reset index
laurencloutier 0:35125ac3d3f9 119 GPSstate = 0; //reset GPS state
laurencloutier 0:35125ac3d3f9 120 if(nmea_validate(GPSbuf)){
laurencloutier 0:35125ac3d3f9 121 parseGPSString(GPSbuf);
laurencloutier 0:35125ac3d3f9 122 }
laurencloutier 0:35125ac3d3f9 123 }
laurencloutier 0:35125ac3d3f9 124 break;
laurencloutier 0:35125ac3d3f9 125
laurencloutier 0:35125ac3d3f9 126 default:
laurencloutier 0:35125ac3d3f9 127 break;
laurencloutier 0:35125ac3d3f9 128
laurencloutier 0:35125ac3d3f9 129 }//switch state
laurencloutier 0:35125ac3d3f9 130 }
laurencloutier 0:35125ac3d3f9 131
laurencloutier 0:35125ac3d3f9 132 float GPSINT::nmea_to_dec(float deg_coord, char nsew) {
laurencloutier 0:35125ac3d3f9 133 int degree = (int)(deg_coord/100);
laurencloutier 0:35125ac3d3f9 134 float minutes = deg_coord - degree*100;
laurencloutier 0:35125ac3d3f9 135 float dec_deg = minutes / 60;
laurencloutier 0:35125ac3d3f9 136 float decimal = degree + dec_deg;
laurencloutier 0:35125ac3d3f9 137 if (nsew == 'S' || nsew == 'W') { // return negative
laurencloutier 0:35125ac3d3f9 138 decimal *= -1;
laurencloutier 0:35125ac3d3f9 139 }
laurencloutier 0:35125ac3d3f9 140 return decimal;
laurencloutier 0:35125ac3d3f9 141 }
laurencloutier 0:35125ac3d3f9 142
laurencloutier 0:35125ac3d3f9 143 // NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
laurencloutier 0:35125ac3d3f9 144 float GPSINT::calc_course_to(float pointLat, float pontLong) {
laurencloutier 0:35125ac3d3f9 145 const double d2r = PI / 180.0;
laurencloutier 0:35125ac3d3f9 146 const double r2d = 180.0 / PI;
laurencloutier 0:35125ac3d3f9 147 double dlat = abs(pointLat - dec_latitude) * d2r;
laurencloutier 0:35125ac3d3f9 148 double dlong = abs(pontLong - dec_longitude) * d2r;
laurencloutier 0:35125ac3d3f9 149 double y = sin(dlong) * cos(pointLat * d2r);
laurencloutier 0:35125ac3d3f9 150 double x = cos(dec_latitude*d2r)*sin(pointLat*d2r) - sin(dec_latitude*d2r)*cos(pointLat*d2r)*cos(dlong);
laurencloutier 0:35125ac3d3f9 151 return atan2(y,x)*r2d;
laurencloutier 0:35125ac3d3f9 152 }
laurencloutier 0:35125ac3d3f9 153
laurencloutier 0:35125ac3d3f9 154 /*
laurencloutier 0:35125ac3d3f9 155 var y = Math.sin(dLon) * Math.cos(lat2);
laurencloutier 0:35125ac3d3f9 156 var x = Math.cos(lat1)*Math.sin(lat2) -
laurencloutier 0:35125ac3d3f9 157 Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
laurencloutier 0:35125ac3d3f9 158 var brng = Math.atan2(y, x).toDeg();
laurencloutier 0:35125ac3d3f9 159 */
laurencloutier 0:35125ac3d3f9 160
laurencloutier 0:35125ac3d3f9 161 /*
laurencloutier 0:35125ac3d3f9 162 The Haversine formula according to Dr. Math.
laurencloutier 0:35125ac3d3f9 163 http://mathforum.org/library/drmath/view/51879.html
laurencloutier 0:35125ac3d3f9 164
laurencloutier 0:35125ac3d3f9 165 dlon = lon2 - lon1
laurencloutier 0:35125ac3d3f9 166 dlat = lat2 - lat1
laurencloutier 0:35125ac3d3f9 167 a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
laurencloutier 0:35125ac3d3f9 168 c = 2 * atan2(sqrt(a), sqrt(1-a))
laurencloutier 0:35125ac3d3f9 169 d = R * c
laurencloutier 0:35125ac3d3f9 170
laurencloutier 0:35125ac3d3f9 171 Where
laurencloutier 0:35125ac3d3f9 172 * dlon is the change in longitude
laurencloutier 0:35125ac3d3f9 173 * dlat is the change in latitude
laurencloutier 0:35125ac3d3f9 174 * c is the great circle distance in Radians.
laurencloutier 0:35125ac3d3f9 175 * R is the radius of a spherical Earth.
laurencloutier 0:35125ac3d3f9 176 * The locations of the two points in
laurencloutier 0:35125ac3d3f9 177 spherical coordinates (longitude and
laurencloutier 0:35125ac3d3f9 178 latitude) are lon1,lat1 and lon2, lat2.
laurencloutier 0:35125ac3d3f9 179 */
laurencloutier 0:35125ac3d3f9 180 double GPSINT::calc_dist_to_mi(float pointLat, float pontLong) {
laurencloutier 0:35125ac3d3f9 181 const double d2r = PI / 180.0;
laurencloutier 0:35125ac3d3f9 182 double dlat = pointLat - dec_latitude;
laurencloutier 0:35125ac3d3f9 183 double dlong = pontLong - dec_longitude;
laurencloutier 0:35125ac3d3f9 184 double a = pow(sin(dlat/2.0),2.0) + cos(dec_latitude*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
laurencloutier 0:35125ac3d3f9 185 double c = 2.0 * asin(sqrt(abs(a)));
laurencloutier 0:35125ac3d3f9 186 double d = 63.765 * c;
laurencloutier 0:35125ac3d3f9 187
laurencloutier 0:35125ac3d3f9 188 return d;
laurencloutier 0:35125ac3d3f9 189 }
laurencloutier 0:35125ac3d3f9 190
laurencloutier 0:35125ac3d3f9 191 double GPSINT::calc_dist_to_ft(float pointLat, float pontLong) {
laurencloutier 0:35125ac3d3f9 192 return calc_dist_to_mi(pointLat, pontLong)*5280.0;
laurencloutier 0:35125ac3d3f9 193 }
laurencloutier 0:35125ac3d3f9 194
laurencloutier 0:35125ac3d3f9 195 double GPSINT::calc_dist_to_km(float pointLat, float pontLong) {
laurencloutier 0:35125ac3d3f9 196 return calc_dist_to_mi(pointLat, pontLong)*1.609344;
laurencloutier 0:35125ac3d3f9 197 }
laurencloutier 0:35125ac3d3f9 198
laurencloutier 0:35125ac3d3f9 199 double GPSINT::calc_dist_to_m(float pointLat, float pontLong) {
laurencloutier 0:35125ac3d3f9 200 return calc_dist_to_mi(pointLat, pontLong)*1609.344;
laurencloutier 0:35125ac3d3f9 201 }