bug fix

pilotak · Mar 17, 2021 · ff35b1d · ff35b1d
1 parent 817b542
commit ff35b1d
Show file tree

Hide file tree

Showing 2 changed files with 53 additions and 33 deletions.
diff --git a/README.md b/README.md
@@ -10,7 +10,7 @@ Ublox GPS I2C async library for mbed.
 
 #define GPS_READ_INTERVAL 1000
 
-EventQueue eQueue(1 * EVENTS_EVENT_SIZE);
+EventQueue eQueue(2 * EVENTS_EVENT_SIZE);
 UbxGpsI2C gps(I2C_SDA, I2C_SCL, &eQueue);
 
 struct gps_data_t {
@@ -71,43 +71,63 @@ int main() {
 
     if (eQueueThread.start(callback(&eQueue, &EventQueue::dispatch_forever)) != osOK) {
         printf("eQueueThread error\n");
+        return 0;
     }
 
-    if (gps.init()) {
-        if (gps.set_odometer(true, UbxGpsI2C::ODO_RUNNING)) {
-            if (gps.set_output_rate(GPS_READ_INTERVAL)) {
-                if (gps.auto_send(UbxGpsI2C::UBX_NAV, UBX_NAV_ODO, 1)) {
-                    if (gps.auto_send(UbxGpsI2C::UBX_NAV, UBX_NAV_PVT, 1)) {
+    if (!gps.init()) {
+        printf("Cound not init\n");
+        return 0;
+    }
 
-                        gps.oob(UbxGpsI2C::UBX_NAV, UBX_NAV_PVT, gpsPVT);
-                        gps.oob(UbxGpsI2C::UBX_NAV, UBX_NAV_ODO, gpsOdo);
+    if (!gps.set_output_rate(GPS_READ_INTERVAL)) {
+        printf("PVT rate FAILED\n");
+        return 0;
+    }
 
-                        while (1) {
-                            ThisThread::sleep_for(GPS_READ_INTERVAL);
+    if (!gps.set_odometer(true, UbxGpsI2C::ODO_RUNNING)) {
+        printf("Odo FAILED\n");
+        return 0;
+    }
 
-                            if (!gps.read()) {
-                                printf("Request failed\n");
-                            }
-                        }
+    if (!gps.auto_send(UbxGpsI2C::UBX_NAV, UBX_NAV_PVT, 1)) {
+        printf("Auto pvt FAILED\n");
+        return 0;
+    }
 
-                    } else {
-                        printf("Auto PVT FAILED\n");
-                    }
+    UbxGpsI2C::cfg_sbas_t cfg_sbas;
+    char *buffer = new char[sizeof(UbxGpsI2C::cfg_sbas_t)];
 
-                } else {
-                    printf("Auto odo FAILED\n");
-                }
+    cfg_sbas.mode = 1; // enable
+    cfg_sbas.usage = 0b111; // integrity, diffCorr, range
+    cfg_sbas.maxSBAS = 3;
+    cfg_sbas.scanmode2 = 0;
+    cfg_sbas.scanmode1 = 0b100001011001; // EGNOS: PRN131, PRN126, PRN124, PRN123, PRN120
 
-            } else {
-                printf("PVT rate FAILED\n");
-            }
+    memcpy(buffer, &cfg_sbas, sizeof(UbxGpsI2C::cfg_sbas_t));
 
-        } else {
-            printf("Odo FAILED\n");
-        }
+    if (!gps.send_ack(UbxGpsI2C::UBX_CFG, UBX_CFG_SBAS, buffer, sizeof(UbxGpsI2C::cfg_sbas_t))) {
+        printf("SBAS FAILED\n");
+        return 0;
+    }
 
-    } else {
-        printf("Cound not init\n");
+    delete[] buffer;
+
+    if (!gps.auto_send(UbxGpsI2C::UBX_NAV, UBX_NAV_ODO, 1)) {
+         printf("Auto odo FAILED\n");
+         return 0;
+    }
+
+    gps.oob(UbxGpsI2C::UBX_NAV, UBX_NAV_PVT, gpsPVT);
+    gps.oob(UbxGpsI2C::UBX_NAV, UBX_NAV_ODO, gpsOdo);
+
+    printf("OK\n");
+
+    while (1) {
+        ThisThread::sleep_for(GPS_READ_INTERVAL);
+
+        if (!gps.read()) {
+            printf("Request failed\n");
+        }
     }
 
     MBED_ASSERT(false);

diff --git a/UbxGpsI2C.cpp b/UbxGpsI2C.cpp
@@ -231,8 +231,8 @@ uint16_t UbxGpsI2C::checksum(const char *packet, uint16_t len) {
 }
 
 bool UbxGpsI2C::get_data() {
-    uint16_t len = (_bytes_available - _data_len) > MBED_CONF_UBXGPSI2C_DATA_SIZE ?
-                   MBED_CONF_UBXGPSI2C_DATA_SIZE :
+    uint16_t len = _bytes_available > MBED_CONF_UBXGPSI2C_DATA_SIZE ?
+                   MBED_CONF_UBXGPSI2C_DATA_SIZE - _data_len :
                    (_bytes_available - _data_len);
 
     if (len > 0) {
@@ -444,12 +444,12 @@ bool UbxGpsI2C::get_cfg(char id) {
                 tr_error("CFG timeout");
                 ok = false;
             }
+
+        } else {
+            ok = false;
         }
 
         remove_oob(UBX_CFG, id);
-
-    } else {
-        ok = false;
     }
 
     return ok;