Projet tuteuré S2 / Mbed 2 deprecated GPSINT

Bibliothèque pur gps, trame NMEA, conversion des coordonnées au format WSG84

Dependencies:   mbed

Fork of GPSINT by Joseph Bradshaw

GPSINT.cpp@2:de2d5174457f, 2016-06-01 (annotated)

Committer:
maxlh23
Date:
Wed Jun 01 15:09:42 2016 +0000
Revision:
2:de2d5174457f
Parent:
1:c266b90b4c74 
Ticket envoi de la position

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jebradshaw 0:f3a7d716faea 1 /* GPSINT.cpp
jebradshaw 1:c266b90b4c74 2 * jbradshaw (20141101)
jebradshaw 1:c266b90b4c74 3 * GPS functions are work of Tyler Weavers mbed gps library page
jebradshaw 1:c266b90b4c74 4 * (http://mbed.org/users/tylerjw/code/GPS/file/39d75e44b214/GPS.cpp)
jebradshaw 0:f3a7d716faea 5 *
jebradshaw 0:f3a7d716faea 6 * Permission is hereby granted, free of charge, to any person obtaining a copy
jebradshaw 0:f3a7d716faea 7 * of this software and associated documentation files (the "Software"), to deal
jebradshaw 0:f3a7d716faea 8 * in the Software without restriction, including without limitation the rights
jebradshaw 0:f3a7d716faea 9 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
jebradshaw 0:f3a7d716faea 10 * copies of the Software, and to permit persons to whom the Software is
jebradshaw 0:f3a7d716faea 11 * furnished to do so, subject to the following conditions:
jebradshaw 0:f3a7d716faea 12 *
jebradshaw 0:f3a7d716faea 13 * The above copyright notice and this permission notice shall be included in
jebradshaw 0:f3a7d716faea 14 * all copies or substantial portions of the Software.
jebradshaw 0:f3a7d716faea 15 *
jebradshaw 0:f3a7d716faea 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
jebradshaw 0:f3a7d716faea 17 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
jebradshaw 0:f3a7d716faea 18 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
jebradshaw 0:f3a7d716faea 19 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
jebradshaw 0:f3a7d716faea 20 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
jebradshaw 0:f3a7d716faea 21 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
jebradshaw 0:f3a7d716faea 22 * THE SOFTWARE.
jebradshaw 0:f3a7d716faea 23 */
jebradshaw 0:f3a7d716faea 24
jebradshaw 0:f3a7d716faea 25 #include "GPSINT.h"
jebradshaw 0:f3a7d716faea 26
jebradshaw 0:f3a7d716faea 27 GPSINT::GPSINT(PinName tx, PinName rx) : _gps(tx, rx) {
maxlh23 2:de2d5174457f 28 _gps.baud(9600);
jebradshaw 0:f3a7d716faea 29 GPSidx=0; // Index for GPS buffer
jebradshaw 0:f3a7d716faea 30 GPSstate=0; // Used to wait for '$' in GPS interrupt
jebradshaw 0:f3a7d716faea 31 _gps.attach(this,&GPSINT::GPSSerialRecvInterrupt, _gps.RxIrq); // Recv interrupt handler
jebradshaw 0:f3a7d716faea 32 }
jebradshaw 0:f3a7d716faea 33
jebradshaw 0:f3a7d716faea 34 int GPSINT::nmea_validate(char *nmeastr){
jebradshaw 0:f3a7d716faea 35 char check[3];
jebradshaw 0:f3a7d716faea 36 char checkcalcstr[3];
jebradshaw 0:f3a7d716faea 37 int i;
jebradshaw 0:f3a7d716faea 38 int calculated_check;
jebradshaw 0:f3a7d716faea 39
jebradshaw 0:f3a7d716faea 40 i=0;
jebradshaw 0:f3a7d716faea 41 calculated_check=0;
jebradshaw 0:f3a7d716faea 42
jebradshaw 0:f3a7d716faea 43 // check to ensure that the string starts with a $
jebradshaw 0:f3a7d716faea 44 if(nmeastr[i] == '$')
jebradshaw 0:f3a7d716faea 45 i++;
jebradshaw 0:f3a7d716faea 46 else
jebradshaw 0:f3a7d716faea 47 return 0;
jebradshaw 0:f3a7d716faea 48
jebradshaw 0:f3a7d716faea 49 //No NULL reached, 75 char largest possible NMEA message, no '*' reached
jebradshaw 0:f3a7d716faea 50 while((nmeastr[i] != 0) && (nmeastr[i] != '*') && (i < 75)){
jebradshaw 0:f3a7d716faea 51 calculated_check ^= nmeastr[i];// calculate the checksum
jebradshaw 0:f3a7d716faea 52 i++;
jebradshaw 0:f3a7d716faea 53 }
jebradshaw 0:f3a7d716faea 54
jebradshaw 0:f3a7d716faea 55 if(i >= 75){
jebradshaw 0:f3a7d716faea 56 return 0;// the string was too long so return an error
jebradshaw 0:f3a7d716faea 57 }
jebradshaw 0:f3a7d716faea 58
jebradshaw 0:f3a7d716faea 59 if (nmeastr[i] == '*'){
jebradshaw 0:f3a7d716faea 60 check[0] = nmeastr[i+1]; //put hex chars in check string
jebradshaw 0:f3a7d716faea 61 check[1] = nmeastr[i+2];
jebradshaw 0:f3a7d716faea 62 check[2] = 0;
jebradshaw 0:f3a7d716faea 63 }
jebradshaw 0:f3a7d716faea 64 else
jebradshaw 0:f3a7d716faea 65 return 0;// no checksum separator found therefor invalid
jebradshaw 0:f3a7d716faea 66
jebradshaw 0:f3a7d716faea 67 sprintf(checkcalcstr,"%02X",calculated_check);
jebradshaw 0:f3a7d716faea 68 return((checkcalcstr[0] == check[0])
jebradshaw 0:f3a7d716faea 69 && (checkcalcstr[1] == check[1])) ? 1 : 0 ;
jebradshaw 0:f3a7d716faea 70 }
jebradshaw 0:f3a7d716faea 71
jebradshaw 0:f3a7d716faea 72 void GPSINT::parseGPSString(char *GPSstrParse){
jebradshaw 0:f3a7d716faea 73 //check if $GPGGA string
maxlh23 2:de2d5174457f 74 if(!strncmp(GPSstrParse, "$GNGGA", 6)){
maxlh23 2:de2d5174457f 75 if (sscanf(GPSstrParse, "$GNGGA,%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) {
jebradshaw 0:f3a7d716faea 76 // printf("%s", GPSstrParse);
jebradshaw 0:f3a7d716faea 77 return;
jebradshaw 0:f3a7d716faea 78 }
jebradshaw 0:f3a7d716faea 79 else{
jebradshaw 0:f3a7d716faea 80 // printf("BAD parse %s", GPSstrParse);
jebradshaw 0:f3a7d716faea 81 }
jebradshaw 0:f3a7d716faea 82 }
jebradshaw 0:f3a7d716faea 83 // Check if $GPRMC string
jebradshaw 0:f3a7d716faea 84 else if (!strncmp(GPSstrParse, "$GPRMC", 6)){
jebradshaw 0:f3a7d716faea 85 if(sscanf(GPSstrParse, "$GPRMC,%f,%f,%c,%f,%c,%f,%f,%d", &utc_time, &nmea_latitude, &ns, &nmea_longitude, &ew, &speed_k, &course_d, &date) >= 1) {
jebradshaw 0:f3a7d716faea 86 // printf("%s", GPSstrParse);
jebradshaw 0:f3a7d716faea 87 return;
jebradshaw 0:f3a7d716faea 88 }
jebradshaw 0:f3a7d716faea 89 else{
jebradshaw 0:f3a7d716faea 90 // printf("BAD parse %s", GPSstrParse);
jebradshaw 0:f3a7d716faea 91 }
jebradshaw 0:f3a7d716faea 92 }
jebradshaw 0:f3a7d716faea 93 // GLL - Geographic Position-Lat/Lon
jebradshaw 0:f3a7d716faea 94 else if (!strncmp(GPSstrParse, "$GPGLL", 6)){
jebradshaw 0:f3a7d716faea 95 if(sscanf(GPSstrParse, "$GPGLL,%f,%c,%f,%c,%f,%c", &nmea_latitude, &ns, &nmea_longitude, &ew, &utc_time, &gll_status) >= 1) {
jebradshaw 0:f3a7d716faea 96 // printf("%s", GPSstrParse);
jebradshaw 0:f3a7d716faea 97 return;
jebradshaw 0:f3a7d716faea 98 }
jebradshaw 0:f3a7d716faea 99 else{
jebradshaw 0:f3a7d716faea 100 // printf("BAD parse %s", GPSstrParse);
jebradshaw 0:f3a7d716faea 101 }
jebradshaw 0:f3a7d716faea 102 }
jebradshaw 0:f3a7d716faea 103 // VTG-Course Over Ground and Ground Speed
jebradshaw 0:f3a7d716faea 104 else if (!strncmp(GPSstrParse, "$GPVTG", 6)){
jebradshaw 0:f3a7d716faea 105 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) {
jebradshaw 0:f3a7d716faea 106 // printf("%s", GPSstrParse);
jebradshaw 0:f3a7d716faea 107 return;
jebradshaw 0:f3a7d716faea 108 }
jebradshaw 0:f3a7d716faea 109 else{
jebradshaw 0:f3a7d716faea 110 // printf("BAD parse %s", GPSstrParse);
jebradshaw 0:f3a7d716faea 111 }
jebradshaw 0:f3a7d716faea 112 }
jebradshaw 0:f3a7d716faea 113 }//parseGPSstring()
jebradshaw 0:f3a7d716faea 114
jebradshaw 0:f3a7d716faea 115 void GPSINT::GPSSerialRecvInterrupt(void)
jebradshaw 0:f3a7d716faea 116 {
jebradshaw 0:f3a7d716faea 117 char c;
jebradshaw 0:f3a7d716faea 118 c =_gps.getc(); // On receive interrupt, get the character.
jebradshaw 0:f3a7d716faea 119 // pc.printf("%c", c);
jebradshaw 0:f3a7d716faea 120
jebradshaw 0:f3a7d716faea 121 switch(GPSstate){
jebradshaw 0:f3a7d716faea 122 case 0:
jebradshaw 0:f3a7d716faea 123 if(c =='$'){
jebradshaw 0:f3a7d716faea 124 GPSidx=0;
jebradshaw 0:f3a7d716faea 125 Temp_GPSbuf[GPSidx] = c; //load char in current idx of array
jebradshaw 0:f3a7d716faea 126 GPSidx++;
jebradshaw 0:f3a7d716faea 127 GPSstate = 1;
jebradshaw 0:f3a7d716faea 128 }
jebradshaw 0:f3a7d716faea 129 break;
jebradshaw 0:f3a7d716faea 130 case 1:
jebradshaw 0:f3a7d716faea 131 Temp_GPSbuf[GPSidx] = c; //load char in current idx of array
jebradshaw 0:f3a7d716faea 132 GPSidx++;
jebradshaw 0:f3a7d716faea 133 if(c == '\n'){ //if last char was a newline
jebradshaw 0:f3a7d716faea 134 Temp_GPSbuf[GPSidx] = '\0'; //append a NULL
jebradshaw 0:f3a7d716faea 135 strcpy(GPSbuf, Temp_GPSbuf); //copy temp buf into GPS buf
jebradshaw 0:f3a7d716faea 136 GPSidx=0; //reset index
jebradshaw 0:f3a7d716faea 137 GPSstate = 0; //reset GPS state
jebradshaw 0:f3a7d716faea 138 if(nmea_validate(GPSbuf)){
jebradshaw 0:f3a7d716faea 139 parseGPSString(GPSbuf);
jebradshaw 0:f3a7d716faea 140 }
jebradshaw 0:f3a7d716faea 141 }
jebradshaw 0:f3a7d716faea 142 break;
jebradshaw 0:f3a7d716faea 143
jebradshaw 0:f3a7d716faea 144 default:
jebradshaw 0:f3a7d716faea 145 break;
jebradshaw 0:f3a7d716faea 146
jebradshaw 0:f3a7d716faea 147 }//switch state
jebradshaw 0:f3a7d716faea 148 }
jebradshaw 0:f3a7d716faea 149
jebradshaw 0:f3a7d716faea 150 float GPSINT::nmea_to_dec(float deg_coord, char nsew) {
jebradshaw 0:f3a7d716faea 151 int degree = (int)(deg_coord/100);
jebradshaw 0:f3a7d716faea 152 float minutes = deg_coord - degree*100;
jebradshaw 0:f3a7d716faea 153 float dec_deg = minutes / 60;
jebradshaw 0:f3a7d716faea 154 float decimal = degree + dec_deg;
jebradshaw 0:f3a7d716faea 155 if (nsew == 'S' || nsew == 'W') { // return negative
jebradshaw 0:f3a7d716faea 156 decimal *= -1;
jebradshaw 0:f3a7d716faea 157 }
jebradshaw 0:f3a7d716faea 158 return decimal;
jebradshaw 0:f3a7d716faea 159 }
jebradshaw 0:f3a7d716faea 160
jebradshaw 0:f3a7d716faea 161 // NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
jebradshaw 0:f3a7d716faea 162 float GPSINT::calc_course_to(float pointLat, float pontLong) {
jebradshaw 0:f3a7d716faea 163 const double d2r = PI / 180.0;
jebradshaw 0:f3a7d716faea 164 const double r2d = 180.0 / PI;
jebradshaw 0:f3a7d716faea 165 double dlat = abs(pointLat - dec_latitude) * d2r;
jebradshaw 0:f3a7d716faea 166 double dlong = abs(pontLong - dec_longitude) * d2r;
jebradshaw 0:f3a7d716faea 167 double y = sin(dlong) * cos(pointLat * d2r);
jebradshaw 0:f3a7d716faea 168 double x = cos(dec_latitude*d2r)*sin(pointLat*d2r) - sin(dec_latitude*d2r)*cos(pointLat*d2r)*cos(dlong);
jebradshaw 0:f3a7d716faea 169 return atan2(y,x)*r2d;
jebradshaw 0:f3a7d716faea 170 }
jebradshaw 0:f3a7d716faea 171
jebradshaw 0:f3a7d716faea 172 /*
jebradshaw 0:f3a7d716faea 173 var y = Math.sin(dLon) * Math.cos(lat2);
jebradshaw 0:f3a7d716faea 174 var x = Math.cos(lat1)*Math.sin(lat2) -
jebradshaw 0:f3a7d716faea 175 Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
jebradshaw 0:f3a7d716faea 176 var brng = Math.atan2(y, x).toDeg();
jebradshaw 0:f3a7d716faea 177 */
jebradshaw 0:f3a7d716faea 178
jebradshaw 0:f3a7d716faea 179 /*
jebradshaw 0:f3a7d716faea 180 The Haversine formula according to Dr. Math.
jebradshaw 0:f3a7d716faea 181 http://mathforum.org/library/drmath/view/51879.html
jebradshaw 0:f3a7d716faea 182
jebradshaw 0:f3a7d716faea 183 dlon = lon2 - lon1
jebradshaw 0:f3a7d716faea 184 dlat = lat2 - lat1
jebradshaw 0:f3a7d716faea 185 a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
jebradshaw 0:f3a7d716faea 186 c = 2 * atan2(sqrt(a), sqrt(1-a))
jebradshaw 0:f3a7d716faea 187 d = R * c
jebradshaw 0:f3a7d716faea 188
jebradshaw 0:f3a7d716faea 189 Where
jebradshaw 0:f3a7d716faea 190 * dlon is the change in longitude
jebradshaw 0:f3a7d716faea 191 * dlat is the change in latitude
jebradshaw 0:f3a7d716faea 192 * c is the great circle distance in Radians.
jebradshaw 0:f3a7d716faea 193 * R is the radius of a spherical Earth.
jebradshaw 0:f3a7d716faea 194 * The locations of the two points in
jebradshaw 0:f3a7d716faea 195 spherical coordinates (longitude and
jebradshaw 0:f3a7d716faea 196 latitude) are lon1,lat1 and lon2, lat2.
jebradshaw 0:f3a7d716faea 197 */
jebradshaw 0:f3a7d716faea 198 double GPSINT::calc_dist_to_mi(float pointLat, float pontLong) {
jebradshaw 0:f3a7d716faea 199 const double d2r = PI / 180.0;
jebradshaw 0:f3a7d716faea 200 double dlat = pointLat - dec_latitude;
jebradshaw 0:f3a7d716faea 201 double dlong = pontLong - dec_longitude;
jebradshaw 0:f3a7d716faea 202 double a = pow(sin(dlat/2.0),2.0) + cos(dec_latitude*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
jebradshaw 0:f3a7d716faea 203 double c = 2.0 * asin(sqrt(abs(a)));
jebradshaw 0:f3a7d716faea 204 double d = 63.765 * c;
jebradshaw 0:f3a7d716faea 205
jebradshaw 0:f3a7d716faea 206 return d;
jebradshaw 0:f3a7d716faea 207 }
jebradshaw 0:f3a7d716faea 208
jebradshaw 0:f3a7d716faea 209 double GPSINT::calc_dist_to_ft(float pointLat, float pontLong) {
jebradshaw 0:f3a7d716faea 210 return calc_dist_to_mi(pointLat, pontLong)*5280.0;
jebradshaw 0:f3a7d716faea 211 }
jebradshaw 0:f3a7d716faea 212
jebradshaw 0:f3a7d716faea 213 double GPSINT::calc_dist_to_km(float pointLat, float pontLong) {
jebradshaw 0:f3a7d716faea 214 return calc_dist_to_mi(pointLat, pontLong)*1.609344;
jebradshaw 0:f3a7d716faea 215 }
jebradshaw 0:f3a7d716faea 216
jebradshaw 0:f3a7d716faea 217 double GPSINT::calc_dist_to_m(float pointLat, float pontLong) {
jebradshaw 0:f3a7d716faea 218 return calc_dist_to_mi(pointLat, pontLong)*1609.344;
jebradshaw 0:f3a7d716faea 219 }