GPSINT.cpp

00001 /* GPSINT.cpp
00002  * Original program from
00003  * jbradshaw (20141101)
00004  * The program is modified for ublox NEO 6M.
00005  */
00006 
00007 #include "GPSINT.h"
00008 
00009 GPSINT::GPSINT(PinName tx, PinName rx) : _gps(tx, rx) {
00010     _gps.baud(9600);
00011     GPSidx=0;          // Index for GPS buffer
00012     GPSstate=0;        // Used to wait for '$' in GPS interrupt
00013     _gps.attach(this,&GPSINT::GPSSerialRecvInterrupt, _gps.RxIrq);    // Recv interrupt handler
00014 }
00015 
00016 int GPSINT::nmea_validate(char *nmeastr){
00017     char check[3];
00018     char checkcalcstr[3];
00019     int i;
00020     int calculated_check;
00021 
00022     i=0;
00023     calculated_check=0;
00024 
00025     // check to ensure that the string starts with a $
00026     if(nmeastr[i] == '$')
00027         i++;
00028     else
00029         return 0;
00030 
00031     //No NULL reached, 75 char largest possible NMEA message, no '*' reached
00032     while((nmeastr[i] != 0) && (nmeastr[i] != '*') && (i < 75)){
00033         calculated_check ^= nmeastr[i];// calculate the checksum
00034         i++;
00035     }
00036 
00037     if(i >= 75){
00038         return 0;// the string was too long so return an error
00039     }
00040 
00041     if (nmeastr[i] == '*'){
00042         check[0] = nmeastr[i+1];    //put hex chars in check string
00043         check[1] = nmeastr[i+2];
00044         check[2] = 0;
00045     }
00046     else
00047         return 0;// no checksum separator found therefor invalid
00048 
00049     sprintf(checkcalcstr,"%02X",calculated_check);
00050     return((checkcalcstr[0] == check[0])
00051         && (checkcalcstr[1] == check[1])) ? 1 : 0 ;
00052 } 
00053 
00054 void GPSINT::parseGPSString(char *GPSstrParse){
00055     //check if $GPGGA string
00056     if(!strncmp(GPSstrParse, "$GPGGA", 6)){
00057         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) {
00058   //          printf("%s", GPSstrParse);
00059             return;
00060         }
00061         else{
00062  //           printf("BAD parse %s", GPSstrParse);    
00063         }
00064     }
00065     // Check if $GPRMC string
00066     else if (!strncmp(GPSstrParse, "$GPRMC", 6)){
00067         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) {
00068 //            printf("%s", GPSstrParse);
00069             return;
00070         }
00071         else{
00072  //           printf("BAD parse %s", GPSstrParse);    
00073         }
00074     }
00075     // GLL - Geographic Position-Lat/Lon
00076     else if (!strncmp(GPSstrParse, "$GPGLL", 6)){
00077         if(sscanf(GPSstrParse, "$GPGLL,%f,%c,%f,%c,%f,%c", &nmea_latitude, &ns, &nmea_longitude, &ew, &utc_time, &gll_status) >= 1) {
00078 //            printf("%s", GPSstrParse);
00079             return;
00080         }
00081         else{
00082  //           printf("BAD parse %s", GPSstrParse);    
00083         }        
00084     }
00085     // VTG-Course Over Ground and Ground Speed
00086     else if (!strncmp(GPSstrParse, "$GPVTG", 6)){
00087         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) {
00088 //            printf("%s", GPSstrParse);
00089             return;
00090         }
00091         else{
00092 //            printf("BAD parse %s", GPSstrParse);    
00093         }        
00094     }            
00095 }//parseGPSstring()
00096 
00097 void GPSINT::GPSSerialRecvInterrupt(void)
00098 {
00099     char c;
00100     c =_gps.getc();            // On receive interrupt, get the character.          
00101    // pc.printf("%c", c);
00102     
00103     switch(GPSstate){
00104         case 0:
00105             if(c =='$'){
00106                 GPSidx=0;
00107                 Temp_GPSbuf[GPSidx] = c;  //load char in current idx of array
00108                 GPSidx++;
00109                 GPSstate = 1;
00110             }            
00111             break;
00112         case 1:                    
00113             Temp_GPSbuf[GPSidx] = c;  //load char in current idx of array
00114             GPSidx++;            
00115             if(c == '\n'){  //if last char was a newline
00116                 Temp_GPSbuf[GPSidx] = '\0'; //append a NULL
00117                 strcpy(GPSbuf, Temp_GPSbuf);    //copy temp buf into GPS buf
00118                 GPSidx=0;                       //reset index
00119                 GPSstate = 0;                   //reset GPS state
00120                 if(nmea_validate(GPSbuf)){
00121                     parseGPSString(GPSbuf);
00122                 }
00123             }            
00124             break;
00125         
00126         default:
00127             break;
00128             
00129     }//switch state                                 
00130 }
00131      
00132 float GPSINT::nmea_to_dec(float deg_coord, char nsew) {
00133     int degree = (int)(deg_coord/100);
00134     float minutes = deg_coord - degree*100;
00135     float dec_deg = minutes / 60;
00136     float decimal = degree + dec_deg;
00137     if (nsew == 'S' || nsew == 'W') { // return negative
00138         decimal *= -1;
00139     }
00140     return decimal;
00141 }
00142 
00143 // NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
00144 float GPSINT::calc_course_to(float pointLat, float pontLong) {
00145     const double d2r = PI / 180.0;
00146     const double r2d = 180.0 / PI;
00147     double dlat = abs(pointLat - dec_latitude) * d2r;
00148     double dlong = abs(pontLong - dec_longitude) * d2r;
00149     double y = sin(dlong) * cos(pointLat * d2r);
00150     double x = cos(dec_latitude*d2r)*sin(pointLat*d2r) - sin(dec_latitude*d2r)*cos(pointLat*d2r)*cos(dlong);
00151     return atan2(y,x)*r2d;
00152 }    
00153  
00154 /*
00155 var y = Math.sin(dLon) * Math.cos(lat2);
00156 var x = Math.cos(lat1)*Math.sin(lat2) -
00157         Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
00158 var brng = Math.atan2(y, x).toDeg();
00159 */
00160  
00161 /*
00162             The Haversine formula according to Dr. Math.
00163             http://mathforum.org/library/drmath/view/51879.html
00164                 
00165             dlon = lon2 - lon1
00166             dlat = lat2 - lat1
00167             a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
00168             c = 2 * atan2(sqrt(a), sqrt(1-a)) 
00169             d = R * c
00170                 
00171             Where
00172                 * dlon is the change in longitude
00173                 * dlat is the change in latitude
00174                 * c is the great circle distance in Radians.
00175                 * R is the radius of a spherical Earth.
00176                 * The locations of the two points in 
00177                     spherical coordinates (longitude and 
00178                     latitude) are lon1,lat1 and lon2, lat2.
00179 */
00180 double GPSINT::calc_dist_to_mi(float pointLat, float pontLong) {
00181     const double d2r = PI / 180.0;
00182     double dlat = pointLat - dec_latitude;
00183     double dlong = pontLong - dec_longitude;
00184     double a = pow(sin(dlat/2.0),2.0) + cos(dec_latitude*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
00185     double c = 2.0 * asin(sqrt(abs(a)));
00186     double d = 63.765 * c;
00187     
00188     return d;
00189 }
00190  
00191 double GPSINT::calc_dist_to_ft(float pointLat, float pontLong) {
00192     return calc_dist_to_mi(pointLat, pontLong)*5280.0;
00193 }
00194  
00195 double GPSINT::calc_dist_to_km(float pointLat, float pontLong) {
00196     return calc_dist_to_mi(pointLat, pontLong)*1.609344;
00197 }
00198  
00199 double GPSINT::calc_dist_to_m(float pointLat, float pontLong) {
00200     return calc_dist_to_mi(pointLat, pontLong)*1609.344;
00201 }