Revision 0:f14093b3b3cf, committed 2019-11-28
- Comitter:
- jbeason3
- Date:
- Thu Nov 28 01:38:50 2019 +0000
- Commit message:
- Happy Turkey Day
Changed in this revision
diff -r 000000000000 -r f14093b3b3cf GPSINT.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GPSINT.cpp Thu Nov 28 01:38:50 2019 +0000
@@ -0,0 +1,201 @@
+/* GPSINT.cpp
+ * Original program from
+ * jbradshaw (20141101)
+ * The program is modified for ublox NEO 6M.
+ */
+
+#include "GPSINT.h"
+
+GPSINT::GPSINT(PinName tx, PinName rx) : _gps(tx, rx) {
+ _gps.baud(9600);
+ GPSidx=0; // Index for GPS buffer
+ GPSstate=0; // Used to wait for '$' in GPS interrupt
+ _gps.attach(this,&GPSINT::GPSSerialRecvInterrupt, _gps.RxIrq); // Recv interrupt handler
+}
+
+int GPSINT::nmea_validate(char *nmeastr){
+ char check[3];
+ char checkcalcstr[3];
+ int i;
+ int calculated_check;
+
+ i=0;
+ calculated_check=0;
+
+ // check to ensure that the string starts with a $
+ if(nmeastr[i] == '$')
+ i++;
+ else
+ return 0;
+
+ //No NULL reached, 75 char largest possible NMEA message, no '*' reached
+ while((nmeastr[i] != 0) && (nmeastr[i] != '*') && (i < 75)){
+ calculated_check ^= nmeastr[i];// calculate the checksum
+ i++;
+ }
+
+ if(i >= 75){
+ return 0;// the string was too long so return an error
+ }
+
+ if (nmeastr[i] == '*'){
+ check[0] = nmeastr[i+1]; //put hex chars in check string
+ check[1] = nmeastr[i+2];
+ check[2] = 0;
+ }
+ else
+ return 0;// no checksum separator found therefor invalid
+
+ sprintf(checkcalcstr,"%02X",calculated_check);
+ return((checkcalcstr[0] == check[0])
+ && (checkcalcstr[1] == check[1])) ? 1 : 0 ;
+}
+
+void GPSINT::parseGPSString(char *GPSstrParse){
+ //check if $GPGGA string
+ if(!strncmp(GPSstrParse, "$GPGGA", 6)){
+ 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) {
+ // printf("%s", GPSstrParse);
+ return;
+ }
+ else{
+ // printf("BAD parse %s", GPSstrParse);
+ }
+ }
+ // Check if $GPRMC string
+ else if (!strncmp(GPSstrParse, "$GPRMC", 6)){
+ 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) {
+// printf("%s", GPSstrParse);
+ return;
+ }
+ else{
+ // printf("BAD parse %s", GPSstrParse);
+ }
+ }
+ // GLL - Geographic Position-Lat/Lon
+ else if (!strncmp(GPSstrParse, "$GPGLL", 6)){
+ if(sscanf(GPSstrParse, "$GPGLL,%f,%c,%f,%c,%f,%c", &nmea_latitude, &ns, &nmea_longitude, &ew, &utc_time, &gll_status) >= 1) {
+// printf("%s", GPSstrParse);
+ return;
+ }
+ else{
+ // printf("BAD parse %s", GPSstrParse);
+ }
+ }
+ // VTG-Course Over Ground and Ground Speed
+ else if (!strncmp(GPSstrParse, "$GPVTG", 6)){
+ 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) {
+// printf("%s", GPSstrParse);
+ return;
+ }
+ else{
+// printf("BAD parse %s", GPSstrParse);
+ }
+ }
+}//parseGPSstring()
+
+void GPSINT::GPSSerialRecvInterrupt(void)
+{
+ char c;
+ c =_gps.getc(); // On receive interrupt, get the character.
+ // pc.printf("%c", c);
+
+ switch(GPSstate){
+ case 0:
+ if(c =='$'){
+ GPSidx=0;
+ Temp_GPSbuf[GPSidx] = c; //load char in current idx of array
+ GPSidx++;
+ GPSstate = 1;
+ }
+ break;
+ case 1:
+ Temp_GPSbuf[GPSidx] = c; //load char in current idx of array
+ GPSidx++;
+ if(c == '\n'){ //if last char was a newline
+ Temp_GPSbuf[GPSidx] = '\0'; //append a NULL
+ strcpy(GPSbuf, Temp_GPSbuf); //copy temp buf into GPS buf
+ GPSidx=0; //reset index
+ GPSstate = 0; //reset GPS state
+ if(nmea_validate(GPSbuf)){
+ parseGPSString(GPSbuf);
+ }
+ }
+ break;
+
+ default:
+ break;
+
+ }//switch state
+}
+
+float GPSINT::nmea_to_dec(float deg_coord, char nsew) {
+ int degree = (int)(deg_coord/100);
+ float minutes = deg_coord - degree*100;
+ float dec_deg = minutes / 60;
+ float decimal = degree + dec_deg;
+ if (nsew == 'S' || nsew == 'W') { // return negative
+ decimal *= -1;
+ }
+ return decimal;
+}
+
+// NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
+float GPSINT::calc_course_to(float pointLat, float pontLong) {
+ const double d2r = PI / 180.0;
+ const double r2d = 180.0 / PI;
+ double dlat = abs(pointLat - dec_latitude) * d2r;
+ double dlong = abs(pontLong - dec_longitude) * d2r;
+ double y = sin(dlong) * cos(pointLat * d2r);
+ double x = cos(dec_latitude*d2r)*sin(pointLat*d2r) - sin(dec_latitude*d2r)*cos(pointLat*d2r)*cos(dlong);
+ return atan2(y,x)*r2d;
+}
+
+/*
+var y = Math.sin(dLon) * Math.cos(lat2);
+var x = Math.cos(lat1)*Math.sin(lat2) -
+ Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
+var brng = Math.atan2(y, x).toDeg();
+*/
+
+/*
+ The Haversine formula according to Dr. Math.
+ http://mathforum.org/library/drmath/view/51879.html
+
+ dlon = lon2 - lon1
+ dlat = lat2 - lat1
+ a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
+ c = 2 * atan2(sqrt(a), sqrt(1-a))
+ d = R * c
+
+ Where
+ * dlon is the change in longitude
+ * dlat is the change in latitude
+ * c is the great circle distance in Radians.
+ * R is the radius of a spherical Earth.
+ * The locations of the two points in
+ spherical coordinates (longitude and
+ latitude) are lon1,lat1 and lon2, lat2.
+*/
+double GPSINT::calc_dist_to_mi(float pointLat, float pontLong) {
+ const double d2r = PI / 180.0;
+ double dlat = pointLat - dec_latitude;
+ double dlong = pontLong - dec_longitude;
+ double a = pow(sin(dlat/2.0),2.0) + cos(dec_latitude*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
+ double c = 2.0 * asin(sqrt(abs(a)));
+ double d = 63.765 * c;
+
+ return d;
+}
+
+double GPSINT::calc_dist_to_ft(float pointLat, float pontLong) {
+ return calc_dist_to_mi(pointLat, pontLong)*5280.0;
+}
+
+double GPSINT::calc_dist_to_km(float pointLat, float pontLong) {
+ return calc_dist_to_mi(pointLat, pontLong)*1.609344;
+}
+
+double GPSINT::calc_dist_to_m(float pointLat, float pontLong) {
+ return calc_dist_to_mi(pointLat, pontLong)*1609.344;
+}
\ No newline at end of file
diff -r 000000000000 -r f14093b3b3cf GPSINT.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/GPSINT.h Thu Nov 28 01:38:50 2019 +0000
@@ -0,0 +1,105 @@
+/* GPSINT.cpp
+ * jbradshaw (20141101)
+ * GPS functions are work of Tyler Weavers mbed gps library page
+ * (http://mbed.org/users/tylerjw/code/GPS/file/39d75e44b214/GPS.cpp)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+/* This GPSINT library class retrieves, parses, and updates variables using
+ a serial receive interrupt. The library was written using the Trimble
+ Copernicus II moduls bus should work on any NMEA 4800 baud output device.
+ The relatively slow 4800 baud allows for plenty of processing time during
+ byte receptions in the interrupt. Using the mbed LPC 100MHz unit, it takes
+ about 340us to process, and parse the 2 default GPGGA and GPZTG strings.*/
+
+#include "mbed.h"
+
+#ifndef GPSINT_H
+#define GPSINT_H
+
+#define PI (3.14159265359)
+#define GPSBUFSIZE 256 // GPS buffer size
+
+/**
+ * GPSINT Class.
+ */
+
+class GPSINT{
+public:
+ /**
+ * Constructor.
+ * @param gps - Serial port pins attached to the gps
+ */
+ GPSINT(PinName tx, PinName rx);
+ int nmea_validate(char *nmeastr); //runs the checksum calculation on the GPS NMEA string
+ void parseGPSString(char *GPSstrParse); //uses scanf to parse NMEA string into variables
+ void GPSSerialRecvInterrupt(void); //fills temprpary buffer for processing
+ float nmea_to_dec(float deg_coord, char nsew); //convert nmea format to decimal format
+ float calc_course_to(float pointLat, float pontLong);
+ double calc_dist_to_mi(float pointLat, float pontLong);
+ double calc_dist_to_ft(float pointLat, float pontLong);
+ double calc_dist_to_km(float pointLat, float pontLong);
+ double calc_dist_to_m(float pointLat, float pontLong);
+
+ // GPSINT variables
+ char GPSbuf[GPSBUFSIZE]; // Receive buffer.
+ char Temp_GPSbuf[GPSBUFSIZE];// Receive buffer.
+ char GPSidx; // Read by background, written by Int Handler.
+ int GPSstate; //set when successful GPS string is received
+
+ // calculated values
+ float dec_longitude;
+ float dec_latitude;
+ float altitude_ft;
+
+ // GGA - Global Positioning System Fixed Data
+ float nmea_longitude;
+ float nmea_latitude;
+ float utc_time;
+ char ns, ew;
+ int lock;
+ char status;
+ int satelites;
+ float hdop;
+ float msl_altitude;
+ char msl_units;
+
+ // RMC - Recommended Minimmum Specific GNS Data
+ char rmc_status;
+ float speed_k;
+ float course_d;
+ int date;
+
+ // GLL
+ char gll_status;
+
+ // VTG - Course over ground, ground speed
+ float course_t; // ground speed true
+ char course_t_unit;
+ float course_m; // magnetic
+ char course_m_unit;
+ char speed_k_unit;
+ float speed_km; // speek km/hr
+ char speed_km_unit;
+private:
+ Serial _gps;
+};
+
+#endif /* GPSINT_H */
\ No newline at end of file