Diff: BG96_GNSS.cpp

Revision:

1:c3a5d3a0b437

Parent:

0:6a2a480672be

Child:

2:8b0663001935

--- a/BG96_GNSS.cpp	Wed Feb 19 11:02:17 2020 +0100
+++ b/BG96_GNSS.cpp	Wed Feb 19 16:34:28 2020 +0100
@@ -1,34 +1,36 @@
 #include "BG96_GNSS.h"
+#include "mbed.h"
+#include "mbed_debug.h"
 
-BG96_GNSS::BG96_GNSS(bool debug) 
+BG96_GNSS::BG96_GNSS()
 {
-    const int baudrate = 115200;
-    const char delimiter[] = "\r\n";
+    this->_serial = new UARTSerial(D1, D0, 115200); 
 
-    _serial = new UARTSerial(MBED_CONF_QUECTEL_BG96_TX, MBED_CONF_QUECTEL_BG96_RX, baudrate); 
-    serial_at_parser_init(delimiter, debug);
+    this->_parser = new ATCmdParser(_serial);
+    this->_parser->debug_on(false);
+    this->_parser->set_delimiter("\r\n");
+    this->_parser->set_timeout(1000);
 }
 
-void BG96_GNSS::serial_at_parser_init(const char *delimiter, bool debug)
-{
-    _parser = new ATCmdParser(_serial);    
-    _parser->debug_on(debug);
-    _parser->set_delimiter(delimiter);    
-    _parser->set_timeout(BG96_AT_TIMEOUT);
-}
-
-void BG96_GNSS::check_if_ready(void)
+int BG96_GNSS::check_if_ready(void)
 {
     int counter = 0;
+    _parser->flush();
+
     while ((counter++) < 10) {
         if (_parser->recv("RDY")) {
-            printf("BG96 ready\r\n");
+            printf("GPS ready\r\n");
+            return SUCCESS;
         }
         else if (_parser->send("AT") && _parser->recv("OK")) {
-            printf("BG96 already available\r\n");
+            printf("GPS already available\r\n");
+            return SUCCESS;
         }
-        thread_sleep_for(10);
+        thread_sleep_for(1);
     }
+
+    printf("GPS not ready\r\n");
+    return FAILURE;
 }
 
 int BG96_GNSS::set_gps_power(bool state)
@@ -36,52 +38,48 @@
     char _buf[32];
 
     sprintf((char *)_buf, "%s", state ? "AT+QGPS=1" : "AT+QGPSEND");
+
     if (_parser->send(_buf) && _parser->recv("OK")) {
         printf("GPS Power: %s\r\n", state ? "On" : "Off");
         return SUCCESS;
     } 
     else {
-        printf("Set GPS Power %s failed\r\n", state ? "On" : "Off");
+        printf("Set GPS Power %s failed\r\n", state ? "On" : "Off");  
         return FAILURE;
-    } 
+    }
 }
 
-int get_gps_data(gps_data *data)
+int BG96_GNSS::get_gps_data(gps_data *data)
 { 
-    char _buf[100];
-
     bool ok = false;
-    Timer t;
-
-    // Structure init: GPS info
-    // todo external fucntion or delete
-    data->utc = data->lat = data->lon = data->hdop= data->altitude = data->cog = data->spkm = data->spkn = data->nsat=0.0;
-    data->fix=0;
-    memset(&data->date, 0x00, 7);
-
-    // timer start
-    t.start();
+    char _buf[100];
+    int error;
 
-    while(!ok && (t.read_ms() < 15000) ) {
-        _parser->flush();
-
-        /* todo: error handler
-        first check if no error, flush, ask about */
-        _parser->send((char*)"AT+QGPSLOC=2"); // MS-based mode
-        ok = _parser->recv("+QGPSLOC: ");
-        if (ok) {
-            _parser->recv("%s\r\n", _buf);
-            sscanf(_buf,"%f,%f,%f,%f,%f,%d,%f,%f,%f,%6s,%d",
-                    &data->utc, &data->lat, &data->lon, &data->hdop,
-                    &data->altitude, &data->fix, &data->cog,
-                    &data->spkm, &data->spkn, data->date, &data->nsat);            
-            ok = _parser->recv("OK");
-        }         
+    _parser->flush();
+    // first check if there is no error, possibly fixing is not done yet (error 516)
+    _parser->send((char*)"AT+QGPSLOC=2");
+    ok = _parser->recv("+CME ERROR: ");
+    if (ok) {
+        _parser->recv("%s\r\n", _buf);
+        sscanf(_buf,"%d", &error);   
+        printf("+CME ERROR: %d\r\n", error);
+        return FAILURE;
     }
 
-    if(ok == true) {
+    _parser->flush();
+    // if there is no error, repeat to save data
+    _parser->send((char*)"AT+QGPSLOC=2");
+    ok = _parser->recv("+QGPSLOC: ");
+    if (ok) {
+        _parser->recv("%s\r\n", _buf);
+        sscanf(_buf,"%f,%f,%f,%f,%f,%d,%f,%f,%f,%6s,%d",
+                &data->utc, &data->lat, &data->lon, &data->hdop,
+                &data->altitude, &data->fix, &data->cog,
+                &data->spkm, &data->spkn, data->date, &data->nsat);
+        ok = _parser->recv("OK");
         return SUCCESS;
     }
-
-    return FAILURE;
-}
\ No newline at end of file
+    else {
+        return FAILURE;
+    }
+}