Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

Ublox6.cpp@24:a7f92dfc5310, 2018-12-21 (annotated)

Committer:
shimniok
Date:
Fri Dec 21 20:04:09 2018 +0000
Revision:
24:a7f92dfc5310
Parent:
23:5e61cf4a8c34 
thread changes, added stats command, added sd filesystem, other minor changes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 16:eb28d0f64a9b 1 #include "mbed.h"
shimniok 16:eb28d0f64a9b 2 #include "Ublox6.h"
shimniok 16:eb28d0f64a9b 3
shimniok 16:eb28d0f64a9b 4 #define MAX_LENGTH 512
shimniok 16:eb28d0f64a9b 5
shimniok 16:eb28d0f64a9b 6 #define DOP_BIT 0x01
shimniok 16:eb28d0f64a9b 7 #define POSLLH_BIT 0x02
shimniok 16:eb28d0f64a9b 8 #define SOL_BIT 0x04
shimniok 16:eb28d0f64a9b 9 #define VELNED_BIT 0x08
shimniok 18:3f8a8f6e3cc1 10
shimniok 19:0d1728091519 11 const int READY_BITS=POSLLH_BIT|DOP_BIT|SOL_BIT|VELNED_BIT;
shimniok 19:0d1728091519 12
shimniok 16:eb28d0f64a9b 13 #define SYNC1 0xB5
shimniok 16:eb28d0f64a9b 14 #define SYNC2 0x62
shimniok 18:3f8a8f6e3cc1 15
shimniok 16:eb28d0f64a9b 16 #define POSLLH_MSG 0x02
shimniok 16:eb28d0f64a9b 17 #define SBAS_MSG 0x32
shimniok 16:eb28d0f64a9b 18 #define VELNED_MSG 0x12
shimniok 16:eb28d0f64a9b 19 #define STATUS_MSG 0x03
shimniok 16:eb28d0f64a9b 20 #define SOL_MSG 0x06
shimniok 16:eb28d0f64a9b 21 #define DOP_MSG 0x04
shimniok 16:eb28d0f64a9b 22 #define DGPS_MSG 0x31
shimniok 16:eb28d0f64a9b 23 #define SVINFO_MSG 0x30
shimniok 18:3f8a8f6e3cc1 24
shimniok 16:eb28d0f64a9b 25 #define CFG 0x06
shimniok 16:eb28d0f64a9b 26 #define NAV 0x01
shimniok 16:eb28d0f64a9b 27 #define MSG 0x01
shimniok 16:eb28d0f64a9b 28
shimniok 16:eb28d0f64a9b 29 #define LONG(X) *(int32_t *)(&data[X])
shimniok 16:eb28d0f64a9b 30 #define ULONG(X) *(uint32_t *)(&data[X])
shimniok 16:eb28d0f64a9b 31 #define INT(X) *(int16_t *)(&data[X])
shimniok 16:eb28d0f64a9b 32 #define UINT(X) *(uint16_t *)(&data[X])
shimniok 16:eb28d0f64a9b 33
shimniok 18:3f8a8f6e3cc1 34 Ublox6::Ublox6()
shimniok 18:3f8a8f6e3cc1 35 {
shimniok 18:3f8a8f6e3cc1 36 // clear flags
shimniok 18:3f8a8f6e3cc1 37 _ready = 0;
shimniok 18:3f8a8f6e3cc1 38 _available = false;
shimniok 18:3f8a8f6e3cc1 39 // clear out structs
shimniok 18:3f8a8f6e3cc1 40 tmp.lat = 0;
shimniok 18:3f8a8f6e3cc1 41 tmp.lon = 0;
shimniok 18:3f8a8f6e3cc1 42 tmp.course = 0;
shimniok 18:3f8a8f6e3cc1 43 tmp.speed = 0;
shimniok 18:3f8a8f6e3cc1 44 tmp.hdop = 0;
shimniok 18:3f8a8f6e3cc1 45 tmp.svcount = 0;
shimniok 18:3f8a8f6e3cc1 46 latest = tmp;
shimniok 23:5e61cf4a8c34 47 _callback = NULL;
shimniok 18:3f8a8f6e3cc1 48 }
shimniok 16:eb28d0f64a9b 49
shimniok 20:043987d06f8d 50 int Ublox6::parse(int cc)
shimniok 16:eb28d0f64a9b 51 {
shimniok 16:eb28d0f64a9b 52 //unsigned char cc = buf[out++];
shimniok 16:eb28d0f64a9b 53 //out &= (MAX_LENGTH-1);
shimniok 16:eb28d0f64a9b 54 static unsigned char ck1, ck2, state, code, id, idx, length, chk1, chk2;
shimniok 16:eb28d0f64a9b 55 static bool checkOk;
shimniok 16:eb28d0f64a9b 56 static unsigned char data[MAX_LENGTH];
shimniok 19:0d1728091519 57 int status = 0;
shimniok 16:eb28d0f64a9b 58
shimniok 16:eb28d0f64a9b 59 switch (state) {
shimniok 16:eb28d0f64a9b 60 case 0: // wait for sync 1 (0xB5)
shimniok 16:eb28d0f64a9b 61 ck1 = ck2 = 0;
shimniok 16:eb28d0f64a9b 62 checkOk = false;
shimniok 19:0d1728091519 63 if (cc == SYNC1) {
shimniok 16:eb28d0f64a9b 64 state++;
shimniok 19:0d1728091519 65 }
shimniok 16:eb28d0f64a9b 66 break;
shimniok 16:eb28d0f64a9b 67 case 1: // wait for sync 2 (0x62)
shimniok 19:0d1728091519 68 if (cc == SYNC2) {
shimniok 16:eb28d0f64a9b 69 state++;
shimniok 19:0d1728091519 70 } else {
shimniok 16:eb28d0f64a9b 71 state = 0;
shimniok 19:0d1728091519 72 }
shimniok 16:eb28d0f64a9b 73 break;
shimniok 16:eb28d0f64a9b 74 case 2: // wait for class code
shimniok 16:eb28d0f64a9b 75 code = cc;
shimniok 16:eb28d0f64a9b 76 ck1 += cc;
shimniok 16:eb28d0f64a9b 77 ck2 += ck1;
shimniok 16:eb28d0f64a9b 78 state++;
shimniok 16:eb28d0f64a9b 79 break;
shimniok 16:eb28d0f64a9b 80 case 3: // wait for Id
shimniok 16:eb28d0f64a9b 81 id = cc;
shimniok 16:eb28d0f64a9b 82 ck1 += cc;
shimniok 16:eb28d0f64a9b 83 ck2 += ck1;
shimniok 16:eb28d0f64a9b 84 state++;
shimniok 16:eb28d0f64a9b 85 break;
shimniok 16:eb28d0f64a9b 86 case 4: // wait for length uint8_t 1
shimniok 16:eb28d0f64a9b 87 length = cc;
shimniok 16:eb28d0f64a9b 88 ck1 += cc;
shimniok 16:eb28d0f64a9b 89 ck2 += ck1;
shimniok 16:eb28d0f64a9b 90 state++;
shimniok 16:eb28d0f64a9b 91 break;
shimniok 16:eb28d0f64a9b 92 case 5: // wait for length uint8_t 2
shimniok 20:043987d06f8d 93 length |= (unsigned int) cc << 8; ck1 += cc;
shimniok 16:eb28d0f64a9b 94 ck2 += ck1;
shimniok 16:eb28d0f64a9b 95 idx = 0;
shimniok 16:eb28d0f64a9b 96 state++;
shimniok 16:eb28d0f64a9b 97 if (length > MAX_LENGTH)
shimniok 16:eb28d0f64a9b 98 state= 0;
shimniok 16:eb28d0f64a9b 99 break;
shimniok 16:eb28d0f64a9b 100 case 6: // wait for <length> payload uint8_ts
shimniok 16:eb28d0f64a9b 101 data[idx++] = cc;
shimniok 16:eb28d0f64a9b 102 ck1 += cc;
shimniok 16:eb28d0f64a9b 103 ck2 += ck1;
shimniok 16:eb28d0f64a9b 104 if (idx >= length) {
shimniok 16:eb28d0f64a9b 105 state++;
shimniok 16:eb28d0f64a9b 106 }
shimniok 16:eb28d0f64a9b 107 break;
shimniok 16:eb28d0f64a9b 108 case 7: // wait for checksum 1
shimniok 16:eb28d0f64a9b 109 chk1 = cc;
shimniok 16:eb28d0f64a9b 110 state++;
shimniok 16:eb28d0f64a9b 111 break;
shimniok 16:eb28d0f64a9b 112 case 8: { // wait for checksum 2
shimniok 16:eb28d0f64a9b 113 chk2 = cc;
shimniok 16:eb28d0f64a9b 114 checkOk = ck1 == chk1 && ck2 == chk2;
shimniok 20:043987d06f8d 115 if (checkOk) {
shimniok 16:eb28d0f64a9b 116 switch (code) {
shimniok 19:0d1728091519 117 case 0x01: // NAV-
shimniok 19:0d1728091519 118 switch (id) {
shimniok 19:0d1728091519 119 case POSLLH_MSG: // NAV-POSLLH
shimniok 19:0d1728091519 120 tmp.lon = ((float)LONG(4))/10000000.0;
shimniok 19:0d1728091519 121 tmp.lat = ((float)LONG(8))/10000000.0;
shimniok 19:0d1728091519 122 // vAcc = ULONG(24); // mm
shimniok 19:0d1728091519 123 // hAcc = ULONG(20); // mm
shimniok 19:0d1728091519 124 _ready |= POSLLH_BIT;
shimniok 19:0d1728091519 125 break;
shimniok 19:0d1728091519 126 case DOP_MSG: // NAV-DOP
shimniok 19:0d1728091519 127 //gDOP = ((float) UINT(4))/100.0;
shimniok 19:0d1728091519 128 //tDOP = ((float) UINT(8))/100.0;
shimniok 19:0d1728091519 129 //vDOP = ((float) UINT(10))/100.0;
shimniok 19:0d1728091519 130 tmp.hdop = ((float) UINT(12))/100.0;
shimniok 19:0d1728091519 131 _ready |= DOP_BIT;
shimniok 16:eb28d0f64a9b 132 break;
shimniok 19:0d1728091519 133 case SOL_MSG: // NAV-SOL
shimniok 19:0d1728091519 134 //week = UINT(8);
shimniok 19:0d1728091519 135 //pDOP = ((float) UINT(44))/ 100.0;
shimniok 19:0d1728091519 136 //pAcc = ULONG(24);
shimniok 19:0d1728091519 137 tmp.svcount = data[47];
shimniok 19:0d1728091519 138 _ready |= SOL_BIT;
shimniok 19:0d1728091519 139 break;
shimniok 19:0d1728091519 140 case VELNED_MSG: // NAV-VELNED
shimniok 19:0d1728091519 141 tmp.speed = ULONG(20)/100.0;
shimniok 19:0d1728091519 142 //sAcc = ULONG(28)/100.0;
shimniok 19:0d1728091519 143 tmp.course = ((float) LONG(24))/100000.0;
shimniok 19:0d1728091519 144 //cAcc = ((float) LONG(32))/100000.0;
shimniok 19:0d1728091519 145 _ready |= VELNED_BIT;
shimniok 16:eb28d0f64a9b 146 break;
shimniok 19:0d1728091519 147 default:
shimniok 19:0d1728091519 148 break;
shimniok 19:0d1728091519 149 }
shimniok 19:0d1728091519 150 break;
shimniok 19:0d1728091519 151 case 0x05: // ACK-
shimniok 19:0d1728091519 152 switch (id) {
shimniok 19:0d1728091519 153 case 0x00: // ACK-NAK
shimniok 19:0d1728091519 154 break;
shimniok 19:0d1728091519 155 case 0x01: // ACK-ACK
shimniok 19:0d1728091519 156 break;
shimniok 19:0d1728091519 157 }
shimniok 19:0d1728091519 158 break;
shimniok 16:eb28d0f64a9b 159 }
shimniok 16:eb28d0f64a9b 160 }
shimniok 16:eb28d0f64a9b 161 state = 0;
shimniok 16:eb28d0f64a9b 162 break;
shimniok 16:eb28d0f64a9b 163 }
shimniok 16:eb28d0f64a9b 164 default:
shimniok 16:eb28d0f64a9b 165 break;
shimniok 16:eb28d0f64a9b 166 }
shimniok 19:0d1728091519 167
shimniok 19:0d1728091519 168 if ((_ready & READY_BITS) == READY_BITS) {
shimniok 18:3f8a8f6e3cc1 169 latest = tmp;
shimniok 20:043987d06f8d 170 _ready = 0;
shimniok 20:043987d06f8d 171 status = 1;
shimniok 20:043987d06f8d 172
shimniok 20:043987d06f8d 173 // clear tmp
shimniok 19:0d1728091519 174 tmp.lat = 0;
shimniok 19:0d1728091519 175 tmp.lon = 0;
shimniok 19:0d1728091519 176 tmp.speed = 0;
shimniok 19:0d1728091519 177 tmp.course = 0;
shimniok 19:0d1728091519 178 tmp.hdop = 0;
shimniok 19:0d1728091519 179 tmp.svcount = 0;
shimniok 23:5e61cf4a8c34 180 if (_callback)
shimniok 23:5e61cf4a8c34 181 _callback();
shimniok 18:3f8a8f6e3cc1 182 }
shimniok 18:3f8a8f6e3cc1 183
shimniok 19:0d1728091519 184 return status;
shimniok 16:eb28d0f64a9b 185 }
shimniok 16:eb28d0f64a9b 186
shimniok 24:a7f92dfc5310 187
shimniok 18:3f8a8f6e3cc1 188 void Ublox6::read(double& lat, double& lon, float& course, float& speed, float& hdop, int& svcount) {
shimniok 18:3f8a8f6e3cc1 189 lat = latest.lat;
shimniok 18:3f8a8f6e3cc1 190 lon = latest.lon;
shimniok 18:3f8a8f6e3cc1 191 course = latest.course;
shimniok 18:3f8a8f6e3cc1 192 speed = latest.speed;
shimniok 18:3f8a8f6e3cc1 193 hdop = latest.hdop;
shimniok 18:3f8a8f6e3cc1 194 svcount = latest.svcount;
shimniok 18:3f8a8f6e3cc1 195
shimniok 18:3f8a8f6e3cc1 196 return;
shimniok 18:3f8a8f6e3cc1 197 }
shimniok 18:3f8a8f6e3cc1 198
shimniok 23:5e61cf4a8c34 199
shimniok 23:5e61cf4a8c34 200 void Ublox6::subscribe(Callback<void()> cb) {
shimniok 23:5e61cf4a8c34 201 _callback = cb;
shimniok 23:5e61cf4a8c34 202
shimniok 23:5e61cf4a8c34 203 return;
shimniok 23:5e61cf4a8c34 204 }
shimniok 23:5e61cf4a8c34 205
shimniok 19:0d1728091519 206 /*
shimniok 16:eb28d0f64a9b 207 bool Ublox6::available(void)
shimniok 16:eb28d0f64a9b 208 {
shimniok 18:3f8a8f6e3cc1 209 return _available;
shimniok 16:eb28d0f64a9b 210 }
shimniok 16:eb28d0f64a9b 211
shimniok 16:eb28d0f64a9b 212 double Ublox6::latitude(void)
shimniok 16:eb28d0f64a9b 213 {
shimniok 16:eb28d0f64a9b 214 return _latitude;
shimniok 16:eb28d0f64a9b 215 }
shimniok 16:eb28d0f64a9b 216
shimniok 16:eb28d0f64a9b 217 double Ublox6::longitude(void)
shimniok 16:eb28d0f64a9b 218 {
shimniok 16:eb28d0f64a9b 219 return _longitude;
shimniok 16:eb28d0f64a9b 220 }
shimniok 16:eb28d0f64a9b 221
shimniok 16:eb28d0f64a9b 222 float Ublox6::speed_mps(void)
shimniok 16:eb28d0f64a9b 223 {
shimniok 16:eb28d0f64a9b 224 return _speed_mps;
shimniok 16:eb28d0f64a9b 225 }
shimniok 16:eb28d0f64a9b 226
shimniok 16:eb28d0f64a9b 227 float Ublox6::heading_deg(void)
shimniok 16:eb28d0f64a9b 228 {
shimniok 16:eb28d0f64a9b 229 return _course_deg;
shimniok 16:eb28d0f64a9b 230 }
shimniok 16:eb28d0f64a9b 231
shimniok 16:eb28d0f64a9b 232 float Ublox6::hdop(void)
shimniok 16:eb28d0f64a9b 233 {
shimniok 16:eb28d0f64a9b 234 return _hdop;
shimniok 16:eb28d0f64a9b 235 }
shimniok 16:eb28d0f64a9b 236
shimniok 16:eb28d0f64a9b 237 int Ublox6::sat_count(void)
shimniok 16:eb28d0f64a9b 238 {
shimniok 18:3f8a8f6e3cc1 239 return _svcount;
shimniok 16:eb28d0f64a9b 240 }
shimniok 18:3f8a8f6e3cc1 241 */