Diff: GPSM.cpp

Revision:

2:32c97cf5c676

Parent:

1:5e73e764616c

Child:

3:467dee159d16

--- a/GPSM.cpp	Mon Dec 03 21:19:16 2018 +0000
+++ b/GPSM.cpp	Tue Dec 04 16:19:41 2018 +0000
@@ -1,35 +1,37 @@
 #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;
+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[128]       =    {};
+char    GPSM::bfr[1030]      =    {};
+char    GPSM::rmc_status     =   'V';
+char    GPSM::ns             =   ' ';
+char    GPSM::ew             =   ' ';
+Serial* GPSM::_gps           =     0;
+bool    GPSM::ready          = false;
 #ifdef OPEN_LOG
-    bool GPSM::is_logging = false;
-    Logger* _openLog = 0;
+bool GPSM::is_logging = false;
+Logger* _openLog = 0;
 #endif
-void GPSM::GPSM_Init(PinName tx, PinName rx) {
-    
-    _gps=new Serial(tx, rx, 9600);  
-    
+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;
@@ -43,125 +45,154 @@
 #endif
     init_GPS();
     wait(1.0);
-    _gps->attach(callback(rx_interrupt));
+    _gps->attach(callback(GPSM::rx_interrupt));
 }
-void rx_interrupt(){
-    if(GPSM::msg_ind < 0){
-        if(GPSM::_gps->getc() == '$') GPSM::msg_ind = 0;
+void GPSM::rx_interrupt()
+{
+    if(msg_ind < 0) {
+        if(_gps->getc() == '$') 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;
+    msg[msg_ind] = _gps->getc();
+    if (msg[msg_ind] == '\r' || msg_ind==90) {
+        msg[msg_ind] = 0;
+        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) {
+            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);
+            ready=true;
+        }
+        msg_ind=-1;
         return;
     }
-    GPSM::msg_ind++;
+    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);
+void GPSM::init_GPS()
+{
+//GGA OFF
+    _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 OFF
+    _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 OFF
+    _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 OFF
+    _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 OFF
+    _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);
+//RATE 5Hz
+    _gps->putc(0xB5);
+    _gps->putc(0x62);
+    _gps->putc(0x06);
+    _gps->putc(0x08);
+    _gps->putc(0x06);
+    _gps->putc(0x00);
+    _gps->putc(0xC8);
+    _gps->putc(0x00);
+    _gps->putc(0x01);
+    _gps->putc(0x00);
+    _gps->putc(0x01);
+    _gps->putc(0x00);
+    _gps->putc(0xDE);
+    _gps->putc(0x6A);
 }
 
 #ifdef OPEN_LOG
-void GPSM::start_log() {
+void GPSM::start_log()
+{
     is_logging = true;
 }
 
-void GPSM::new_file(void) {
+void GPSM::new_file(void)
+{
     _openLog->newFile();
 }
 
-void GPSM::stop_log(void) {
+void GPSM::stop_log(void)
+{
     is_logging = false;
 }
 #endif
 
-float GPSM::nmea_to_dec(float deg_coord, char nsew) {
+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;
@@ -172,48 +203,24 @@
     return decimal;
 }
 
-int GPSM::poll() {
-    __disable_irq();    // Disable Interrupts
-    int line_parsed = 0;
-    if (msg_ind == -2) {
-        
-    
+int GPSM::poll()
+{
+    if (!ready) return NOT_READY;
 #ifdef OPEN_LOG
-        if (is_logging && lock) {
-            format_for_log();
-            _openLog->write(bfr);
-        }
+    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;
-    }
+    ready=false;
+    if (lock) return GOOD;
+    else return NO_LOCK;
 }
 
 
 // INTERNAL FUNCTINS ////////////////////////////////////////////////////////////
-float GPSM::trunc(float v) {
+float GPSM::trunc(float v)
+{
     if (v < 0.0) {
         v*= -1.0;
         v = floor(v);
@@ -224,7 +231,8 @@
     return v;
 }
 
-void GPSM::format_for_log() {
+void GPSM::format_for_log()
+{
     bfr[0] = '$';
     for (int i = 0; i < 1022; i++) {
         bfr[i+1] = msg[i];
@@ -239,26 +247,32 @@
 }
 
 // GET FUNCTIONS /////////////////////////////////////////////////////////////////
-float GPSM::get_nmea_longitude() {
-        return nmea_longitude;
+float GPSM::get_nmea_longitude()
+{
+    return nmea_longitude;
 }
 
-float GPSM::get_dec_longitude() {
-        return dec_longitude;
+float GPSM::get_dec_longitude()
+{
+    return dec_longitude;
 }
 
-float GPSM::get_nmea_latitude() {
-        return nmea_latitude;
+float GPSM::get_nmea_latitude()
+{
+    return nmea_latitude;
 }
 
-float GPSM::get_dec_latitude() {
-        return dec_latitude;
+float GPSM::get_dec_latitude()
+{
+    return dec_latitude;
 }
 
-float GPSM::get_course_t() {
-        return course_d;
+float GPSM::get_course_t()
+{
+    return course_d;
 }
 
-float GPSM::get_speed_km() {
-        return speed_km;
+float GPSM::get_speed_km()
+{
+    return speed_km;
 }
\ No newline at end of file