Revision 18:3f8a8f6e3cc1, committed 2018-12-14

Comitter:

shimniok

Date:

Fri Dec 14 00:36:06 2018 +0000

Parent:

17:5c2af0904e87

Child:

19:0d1728091519

Commit message:

add cmd to read gps; fixed gps flag bug, changed gps interface

Changed in this revision

--- a/Ublox6.cpp	Thu Dec 13 23:27:04 2018 +0000
+++ b/Ublox6.cpp	Fri Dec 14 00:36:06 2018 +0000
@@ -7,8 +7,10 @@
 #define POSLLH_BIT  0x02
 #define SOL_BIT     0x04
 #define VELNED_BIT  0x08
+
 #define SYNC1       0xB5
 #define SYNC2       0x62
+
 #define POSLLH_MSG  0x02
 #define SBAS_MSG    0x32
 #define VELNED_MSG  0x12
@@ -17,6 +19,7 @@
 #define DOP_MSG     0x04
 #define DGPS_MSG    0x31
 #define SVINFO_MSG  0x30
+
 #define CFG         0x06
 #define NAV         0x01
 #define MSG         0x01
@@ -26,7 +29,20 @@
 #define INT(X)     *(int16_t *)(&data[X])
 #define UINT(X)    *(uint16_t *)(&data[X])
 
-// TODO 2: abstract LED/status to some kind of pub/sub thing
+Ublox6::Ublox6()
+{
+    // clear flags
+    _ready = 0;
+    _available = false;
+    // clear out structs
+    tmp.lat = 0;
+    tmp.lon = 0;
+    tmp.course = 0;
+    tmp.speed = 0;
+    tmp.hdop = 0;
+    tmp.svcount = 0;
+    latest = tmp;
+}
 
 void Ublox6::parse(unsigned char cc)
 {
@@ -98,32 +114,32 @@
                     case 0x01:      // NAV-
                         switch (id) {
                             case POSLLH_MSG:  // NAV-POSLLH
-                                _longitude = ((float)LONG(4))/10000000.0;
-                                _latitude = ((float)LONG(8))/10000000.0;
+                                tmp.lon = ((float)LONG(4))/10000000.0;
+                                tmp.lat = ((float)LONG(8))/10000000.0;
                                 // vAcc = ULONG(24); // mm
                                 // hAcc = ULONG(20); // mm
-                                _available |= POSLLH_BIT;
+                                _ready |= POSLLH_BIT;
                                 break;
                             case DOP_MSG:  // NAV-DOP
                                 //gDOP = ((float) UINT(4))/100.0;
                                 //tDOP = ((float) UINT(8))/100.0;
                                 //vDOP = ((float) UINT(10))/100.0;
-                                _hdop = ((float) UINT(12))/100.0;
-                                _available |= DOP_BIT;
+                                tmp.hdop = ((float) UINT(12))/100.0;
+                                _ready |= DOP_BIT;
                                 break;
                             case SOL_MSG:  // NAV-SOL
                                 //week = UINT(8);
                                 //pDOP = ((float) UINT(44))/ 100.0;
                                 //pAcc = ULONG(24);
-                                _sat_count = data[47];
-                                _available |= SOL_BIT;
+                                tmp.svcount = data[47];
+                                _ready |= SOL_BIT;
                                 break;
                             case VELNED_MSG:  // NAV-VELNED
-                                _speed_mps = ULONG(20)/100.0;
+                                tmp.speed = ULONG(20)/100.0;
                                 //sAcc = ULONG(28)/100.0;
-                                _course_deg = ((float) LONG(24))/100000.0;
+                                tmp.course = ((float) LONG(24))/100000.0;
                                 //cAcc = ((float) LONG(32))/100000.0;
-                                _available |= VELNED_BIT;                                
+                                _ready |= VELNED_BIT;                                
                                 break;
                             default:
                                 break;
@@ -145,18 +161,35 @@
         default:
             break;
     }
+    
+    if (_ready == POSLLH_BIT|DOP_BIT|SOL_BIT|VELNED_BIT) {
+        latest = tmp;
+        _ready = 0;
+        _available = true;
+    }
+
+    return;
 }
 
+void Ublox6::read(double& lat, double& lon, float& course, float& speed, float& hdop, int& svcount) {
+    lat = latest.lat;
+    lon = latest.lon;
+    course = latest.course;
+    speed = latest.speed;
+    hdop = latest.hdop;
+    svcount = latest.svcount;
+
+    return;
+}
+
+
+
 bool Ublox6::available(void)
 {
-    return (_available & (DOP_BIT|POSLLH_BIT|SOL_BIT|VELNED_BIT));
+    return _available;
 }
 
-void Ublox6::reset_available(void)
-{
-    _available = 0;
-}
-
+/*
 double Ublox6::latitude(void)
 {
     return _latitude;
@@ -184,5 +217,6 @@
 
 int Ublox6::sat_count(void)
 {
-    return _sat_count;
+    return _svcount;
 }
+*/

--- a/Ublox6.h	Thu Dec 13 23:27:04 2018 +0000
+++ b/Ublox6.h	Fri Dec 14 00:36:06 2018 +0000
@@ -13,6 +13,8 @@
     // TODO 3 convert this to time units
     static const int lag=40;        // number of updater steps by which gps output lags reality
 
+    Ublox6();
+
     /**
      * UBX protocol parser (Wayne Holder)
      * @param cc is the character to parse
@@ -21,6 +23,8 @@
      */    
     void parse(unsigned char cc);
 
+    void read(double& lat, double& lon, float& course, float& speed, float& hdop, int& svcount);
+
     /**
      * get latitude
      */    
@@ -63,13 +67,29 @@
     void reset_available(void);
 
 private:
-    int _available;     // is data available?
+    typedef struct {
+        double lat;
+        double lon;
+        float course;
+        float speed;
+        float hdop;
+        int svcount;
+    } gps_data_t;
+
+    gps_data_t tmp;
+    gps_data_t latest;
+
+    int _ready;             // is data ready to be copied?
+    bool _available;        // 
+
+    /*
     float _latitude;        // temp storage, latitude
     float _longitude;       // temp storage, longitude
     float _hdop;            // horiz dilution of precision
     float _course_deg;      // course in degrees
     float _speed_mps;       // speed in m/s
-    int _sat_count;         // satellite count
+    int _svcount;           // space vehicle (satellite) count
+    */
 };
 
 #endif

--- a/main.cpp	Thu Dec 13 23:27:04 2018 +0000
+++ b/main.cpp	Fri Dec 14 00:36:06 2018 +0000
@@ -21,12 +21,12 @@
 LocalFileSystem lfs("etc");
 Config config;
 SimpleShell sh;
+Ublox6 ublox;
 
 DigitalOut led1(LED1);
 DigitalOut led3(LED3);
 
 void do_gps() {
-    Ublox6 ublox;
     RawSerial s(UART1TX, UART1RX, 38400);
     while (1) {
         int c = s.getc();
@@ -43,6 +43,21 @@
     printf("Hello world!\n");
 }
 
+void read_gps()
+{
+    double lat=0;
+    double lon=0;
+    float course=0;
+    float speed=0;
+    float hdop=0.0;
+    int svcount=0;
+    
+    ublox.read(lat, lon, course, speed, hdop, svcount);
+    printf("%3.7f %3.7f\n", lat, lon);
+    printf("hdg=%03.1f deg spd=%3.1f m/s\n", course, speed);
+    printf("%d %f\n", svcount, hdop);
+}
+
 void read_gyro() 
 {
     int g[3];
@@ -106,6 +121,7 @@
     sh.attach(test, "test");
     sh.attach(read_gyro, "gyro");
     sh.attach(read_enc, "enc");
+    sh.attach(read_gps, "gps");
     sh.attach(reset, "reset");
     Thread shellThread;
     shellThread.start(callback(&sh, &SimpleShell::run));