Diff: GPS/GPS.cpp

Revision:

46:a8617076f021

Child:

48:58213015bb90

diff -r 82f2f55e1fc3 -r a8617076f021 GPS/GPS.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPS/GPS.cpp	Mon Aug 21 16:24:34 2017 +0000
@@ -0,0 +1,287 @@
+#include "GPS.h"
+
+GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) {
+    _gps.baud(9600);
+    nmea_longitude = 0.0;
+    nmea_latitude = 0.0;
+    utc_time = 0;
+    ns = ' ';
+    ew = ' ';
+    lock = 0;
+    satelites = 0;
+    hdop = 0.0;
+    msl_altitude = 0.0;
+    msl_units = ' ';
+
+    rmc_status = ' ';
+    speed_k = 0.0;
+    course_d = 0.0;
+    date = 0;
+
+    dec_longitude = 0.0;
+    dec_latitude = 0.0;
+
+    gll_status = ' ';
+
+    course_t = 0.0; // ground speed true
+    course_t_unit = ' ';
+    course_m = 0.0; // magnetic
+    course_m_unit = ' ';
+    speed_k_unit = ' ';
+    speed_km = 0.0; // speek km/hr
+    speed_km_unit = ' ';
+
+    altitude_ft = 0.0;
+#ifdef OPEN_LOG
+    is_logging = false;
+#endif
+}
+
+#ifdef OPEN_LOG
+void GPS::start_log() {
+    is_logging = true;
+}
+
+void GPS::new_file(void) {
+    _openLog.newFile();
+}
+
+void GPS::stop_log(void) {
+    is_logging = false;
+}
+#endif
+
+float GPS::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;
+}
+
+int GPS::sample() {
+    int line_parsed = 0;
+
+    if (_gps.readable()) {
+        getline();
+    
+#ifdef OPEN_LOG
+        if (is_logging && lock) {
+            format_for_log();
+            _openLog.write(bfr);
+        }
+#endif
+
+        // Check if it is a GPGGA msg (matches both locked and non-locked msg)
+        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) {
+            line_parsed = GGA;
+        }
+        // Check if it is a GPRMC msg
+        else if (sscanf(msg, "GPRMC,%f,%c,%f,%c,%f,%f,%d", &utc_time, &ns, &nmea_longitude, &ew, &speed_k, &course_d, &date) >= 1) {
+            line_parsed = RMC;
+        }
+        // GLL - Geographic Position-Lat/Lon
+        else if (sscanf(msg, "GPGLL,%f,%c,%f,%c,%f,%c", &nmea_latitude, &ns, &nmea_longitude, &ew, &utc_time, &gll_status) >= 1) {
+            line_parsed = GLL;
+        }
+        // VTG-Course Over Ground and Ground Speed
+        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) {
+            line_parsed = VTG;
+        }
+        
+        if(satelites == 0) {
+            lock = 0;
+        }
+    }
+    if (!lock) {
+        return NO_LOCK;
+    } else if (line_parsed) {
+        return line_parsed;
+    } else {
+        return NOT_PARSED;
+    }
+}
+
+
+// INTERNAL FUNCTINS ////////////////////////////////////////////////////////////
+float GPS::trunc(float v) {
+    if (v < 0.0) {
+        v*= -1.0;
+        v = floor(v);
+        v*=-1.0;
+    } else {
+        v = floor(v);
+    }
+    return v;
+}
+
+void GPS::getline() {
+    while (_gps.getc() != '$');   // wait for the start of a line
+    for (int i=0; i<1022; i++) {
+        msg[i] = _gps.getc();
+        if (msg[i] == '\r') {
+            msg[i] = 0;
+            return;
+        }
+    }
+    error("Overflow in getline");
+}
+
+void GPS::format_for_log() {
+    bfr[0] = '$';
+    for (int i = 0; i < 1022; i++) {
+        bfr[i+1] = msg[i];
+        if (msg[i] == 0 || msg[i] =='$') {
+            bfr[i+1] = '\r';
+            bfr[i+2] = '\n';
+            bfr[i+3] = 0;
+            return;
+        }
+    }
+    error("Overflow in format");
+}
+
+// GET FUNCTIONS /////////////////////////////////////////////////////////////////
+float GPS::get_msl_altitude() {
+    if (!lock)
+        return 0.0;
+    else
+        return msl_altitude;
+}
+
+int GPS::get_satelites() {
+    if (!lock)
+        return 0;
+    else
+        return satelites;
+}
+
+float GPS::get_nmea_longitude() {
+    if (!lock)
+        return 0.0;
+    else
+        return nmea_longitude;
+}
+
+float GPS::get_dec_longitude() {
+    dec_longitude = nmea_to_dec(nmea_longitude, ew);
+    if (!lock)
+        return 0.0;
+    else
+        return dec_longitude;
+}
+
+float GPS::get_nmea_latitude() {
+    if (!lock)
+        return 0.0;
+    else
+        return nmea_latitude;
+}
+
+float GPS::get_dec_latitude() {
+    dec_latitude = nmea_to_dec(nmea_latitude, ns);
+    if (!lock)
+        return 0.0;
+    else
+        return dec_latitude;
+}
+
+float GPS::get_course_t() {
+    if (!lock)
+        return 0.0;
+    else
+        return course_t;
+}
+
+float GPS::get_course_m() {
+    if (!lock)
+        return 0.0;
+    else
+        return course_m;
+}
+
+float GPS::get_speed_k() {
+    if (!lock)
+        return 0.0;
+    else
+        return speed_k;
+}
+
+float GPS::get_speed_km() {
+    if (!lock)
+        return 0.0;
+    else
+        return speed_km;
+}
+
+float GPS::get_altitude_ft() {
+    if (!lock)
+        return 0.0;
+    else
+        return 3.280839895*msl_altitude;
+}
+
+// NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
+float GPS::calc_course_to(float pointLat, float pontLong) {
+    const double d2r = PI / 180.0;
+    const double r2d = 180.0 / PI;
+    double dlat = abs(pointLat - get_dec_latitude()) * d2r;
+    double dlong = abs(pontLong - get_dec_longitude()) * d2r;
+    double y = sin(dlong) * cos(pointLat * d2r);
+    double x = cos(get_dec_latitude()*d2r)*sin(pointLat*d2r) - sin(get_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 GPS::calc_dist_to_mi(float pointLat, float pontLong) {
+    const double d2r = PI / 180.0;
+    double dlat = pointLat - get_dec_latitude();
+    double dlong = pontLong - get_dec_longitude();
+    double a = pow(sin(dlat/2.0),2.0) + cos(get_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 GPS::calc_dist_to_ft(float pointLat, float pontLong) {
+    return calc_dist_to_mi(pointLat, pontLong)*5280.0;
+}
+
+double GPS::calc_dist_to_km(float pointLat, float pontLong) {
+    return calc_dist_to_mi(pointLat, pontLong)*1.609344;
+}
+
+double GPS::calc_dist_to_m(float pointLat, float pontLong) {
+    return calc_dist_to_mi(pointLat, pontLong)*1609.344;
+}
+
+    
\ No newline at end of file