Diff: GPSM.cpp

Revision:

1:5e73e764616c

Parent:

0:68d3dfe94417

Child:

2:32c97cf5c676

--- a/GPSM.cpp	Thu Nov 29 19:58:47 2018 +0000
+++ b/GPSM.cpp	Mon Dec 03 21:19:16 2018 +0000
@@ -0,0 +1,264 @@
+#include "GPSM.h"
+int     GPSM::lock           =   0;
+int     GPSM::date           =   0;
+int     GPSM::msg_ind        =  -1;
+float   GPSM::dec_latitude   = 0.0;
+float   GPSM::dec_longitude  = 0.0;
+float   GPSM::nmea_latitude  = 0.0;
+float   GPSM::nmea_longitude = 0.0;
+float   GPSM::speed_km       = 0.0;
+float   GPSM::course_d       = 0.0;
+float   GPSM::utc_time       = 0.0;
+char    GPSM::msg[1024]      =  {};
+char    GPSM::bfr[1030]      =  {};
+char    GPSM::rmc_status     = 'V';
+char    GPSM::ns             = ' ';
+char    GPSM::ew             = ' ';
+Serial* GPSM::_gps           =   0;
+#ifdef OPEN_LOG
+    bool GPSM::is_logging = false;
+    Logger* _openLog = 0;
+#endif
+void GPSM::GPSM_Init(PinName tx, PinName rx) {
+    
+    _gps=new Serial(tx, rx, 9600);  
+    
+    nmea_longitude = 0.0;
+    nmea_latitude = 0.0;
+    utc_time = 0;
+    ns = ' ';
+    ew = ' ';
+    lock = 0;
+    
+    rmc_status = 'V';
+    speed_km = 0.0;
+    course_d = 0.0;
+    date = 0;
+
+    dec_longitude = 0.0;
+    dec_latitude = 0.0;
+#ifdef OPEN_LOG
+    is_logging = false;
+    _openLog=new Logger()
+#endif
+    init_GPS();
+    wait(1.0);
+    _gps->attach(callback(rx_interrupt));
+}
+void rx_interrupt(){
+    if(GPSM::msg_ind < 0){
+        if(GPSM::_gps->getc() == '$') GPSM::msg_ind = 0;
+        return;
+    }
+    GPSM::msg[GPSM::msg_ind] = GPSM::_gps->getc();
+    if (GPSM::msg[GPSM::msg_ind] == '\r') {
+        GPSM::msg[GPSM::msg_ind] = 0;
+        GPSM::msg_ind = -2;
+        return;
+    }
+    GPSM::msg_ind++;
+}
+
+void GPSM::init_GPS(){
+    //GGA
+_gps->putc(0xB5);
+_gps->putc(0x62);
+_gps->putc(0x06);
+_gps->putc(0x01);
+_gps->putc(0x08);
+_gps->putc(0x00);
+_gps->putc(0xF0);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x01);
+_gps->putc(0x00);
+_gps->putc(0x24);
+//GLL
+_gps->putc(0xB5);
+_gps->putc(0x62);
+_gps->putc(0x06);
+_gps->putc(0x01);
+_gps->putc(0x08);
+_gps->putc(0x00);
+_gps->putc(0xF0);
+_gps->putc(0x01);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x01);
+_gps->putc(0x01);
+_gps->putc(0x2B);
+//GSA
+_gps->putc(0xB5);
+_gps->putc(0x62);
+_gps->putc(0x06);
+_gps->putc(0x01);
+_gps->putc(0x08);
+_gps->putc(0x00);
+_gps->putc(0xF0);
+_gps->putc(0x02);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x01);
+_gps->putc(0x02);
+_gps->putc(0x32);
+//GSV
+_gps->putc(0xB5);
+_gps->putc(0x62);
+_gps->putc(0x06);
+_gps->putc(0x01);
+_gps->putc(0x08);
+_gps->putc(0x00);
+_gps->putc(0xF0);
+_gps->putc(0x03);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x01);
+_gps->putc(0x03);
+_gps->putc(0x39);
+//VTG
+_gps->putc(0xB5);
+_gps->putc(0x62);
+_gps->putc(0x06);
+_gps->putc(0x01);
+_gps->putc(0x08);
+_gps->putc(0x00);
+_gps->putc(0xF0);
+_gps->putc(0x05);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x00);
+_gps->putc(0x01);
+_gps->putc(0x05);
+_gps->putc(0x47);
+}
+
+#ifdef OPEN_LOG
+void GPSM::start_log() {
+    is_logging = true;
+}
+
+void GPSM::new_file(void) {
+    _openLog->newFile();
+}
+
+void GPSM::stop_log(void) {
+    is_logging = false;
+}
+#endif
+
+float GPSM::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 GPSM::poll() {
+    __disable_irq();    // Disable Interrupts
+    int line_parsed = 0;
+    if (msg_ind == -2) {
+        
+    
+#ifdef OPEN_LOG
+        if (is_logging && lock) {
+            format_for_log();
+            _openLog->write(bfr);
+        }
+#endif
+
+        
+        // Check if it is a GPRMC msg
+        if (sscanf(msg, "GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,%d", &utc_time, &rmc_status, &nmea_latitude, &ns, &nmea_longitude, &ew, &speed_km, &course_d, &date) >= 2) {
+            line_parsed = RMC;
+            if(rmc_status == 'V') lock=0;
+            if(rmc_status == 'A') lock=1;
+            speed_km*=1.852;
+            dec_latitude = nmea_to_dec(nmea_latitude, ns);
+            dec_longitude = nmea_to_dec(nmea_longitude, ew);
+        }
+        
+    msg_ind = -1;   
+    __enable_irq();     // Enable Interrupts
+    } else {
+        __enable_irq();     // Enable Interrupts    
+        return NOT_READY;   
+    }
+    if (!lock) {
+        return NO_LOCK;
+    } else if (line_parsed) {
+        return line_parsed;
+    } else {
+        return NOT_PARSED;
+    }
+}
+
+
+// INTERNAL FUNCTINS ////////////////////////////////////////////////////////////
+float GPSM::trunc(float v) {
+    if (v < 0.0) {
+        v*= -1.0;
+        v = floor(v);
+        v*=-1.0;
+    } else {
+        v = floor(v);
+    }
+    return v;
+}
+
+void GPSM::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 GPSM::get_nmea_longitude() {
+        return nmea_longitude;
+}
+
+float GPSM::get_dec_longitude() {
+        return dec_longitude;
+}
+
+float GPSM::get_nmea_latitude() {
+        return nmea_latitude;
+}
+
+float GPSM::get_dec_latitude() {
+        return dec_latitude;
+}
+
+float GPSM::get_course_t() {
+        return course_d;
+}
+
+float GPSM::get_speed_km() {
+        return speed_km;
+}
\ No newline at end of file