Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

Ublox6.cpp@19:0d1728091519, 2018-12-15 (annotated)

Committer:
shimniok
Date:
Sat Dec 15 21:38:11 2018 +0000
Revision:
19:0d1728091519
Parent:
18:3f8a8f6e3cc1
Child:
20:043987d06f8d
Testing ublox

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 18:3f8a8f6e3cc1 47 }
shimniok 16:eb28d0f64a9b 48
shimniok 19:0d1728091519 49 int Ublox6::parse(unsigned char cc)
shimniok 16:eb28d0f64a9b 50 {
shimniok 16:eb28d0f64a9b 51 //unsigned char cc = buf[out++];
shimniok 16:eb28d0f64a9b 52 //out &= (MAX_LENGTH-1);
shimniok 16:eb28d0f64a9b 53 static unsigned char ck1, ck2, state, code, id, idx, length, chk1, chk2;
shimniok 16:eb28d0f64a9b 54 static bool checkOk;
shimniok 16:eb28d0f64a9b 55 static unsigned char data[MAX_LENGTH];
shimniok 19:0d1728091519 56 int status = 0;
shimniok 16:eb28d0f64a9b 57
shimniok 16:eb28d0f64a9b 58 switch (state) {
shimniok 16:eb28d0f64a9b 59 case 0: // wait for sync 1 (0xB5)
shimniok 16:eb28d0f64a9b 60 ck1 = ck2 = 0;
shimniok 16:eb28d0f64a9b 61 checkOk = false;
shimniok 19:0d1728091519 62 if (cc == SYNC1) {
shimniok 16:eb28d0f64a9b 63 state++;
shimniok 19:0d1728091519 64 puts("SYNC1");
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 puts("SYNC2");
shimniok 19:0d1728091519 71 } else {
shimniok 16:eb28d0f64a9b 72 state = 0;
shimniok 19:0d1728091519 73 puts("NO SYNC2");
shimniok 19:0d1728091519 74 }
shimniok 16:eb28d0f64a9b 75 break;
shimniok 16:eb28d0f64a9b 76 case 2: // wait for class code
shimniok 16:eb28d0f64a9b 77 code = cc;
shimniok 16:eb28d0f64a9b 78 ck1 += cc;
shimniok 16:eb28d0f64a9b 79 ck2 += ck1;
shimniok 16:eb28d0f64a9b 80 state++;
shimniok 16:eb28d0f64a9b 81 break;
shimniok 16:eb28d0f64a9b 82 case 3: // wait for Id
shimniok 16:eb28d0f64a9b 83 id = cc;
shimniok 16:eb28d0f64a9b 84 ck1 += cc;
shimniok 16:eb28d0f64a9b 85 ck2 += ck1;
shimniok 16:eb28d0f64a9b 86 state++;
shimniok 16:eb28d0f64a9b 87 break;
shimniok 16:eb28d0f64a9b 88 case 4: // wait for length uint8_t 1
shimniok 16:eb28d0f64a9b 89 length = cc;
shimniok 16:eb28d0f64a9b 90 ck1 += cc;
shimniok 16:eb28d0f64a9b 91 ck2 += ck1;
shimniok 16:eb28d0f64a9b 92 state++;
shimniok 16:eb28d0f64a9b 93 break;
shimniok 16:eb28d0f64a9b 94 case 5: // wait for length uint8_t 2
shimniok 16:eb28d0f64a9b 95 length |= (unsigned int) cc << 8;
shimniok 19:0d1728091519 96 printf("len=%d\n", length);
shimniok 16:eb28d0f64a9b 97 ck1 += cc;
shimniok 16:eb28d0f64a9b 98 ck2 += ck1;
shimniok 16:eb28d0f64a9b 99 idx = 0;
shimniok 16:eb28d0f64a9b 100 state++;
shimniok 16:eb28d0f64a9b 101 if (length > MAX_LENGTH)
shimniok 16:eb28d0f64a9b 102 state= 0;
shimniok 16:eb28d0f64a9b 103 break;
shimniok 16:eb28d0f64a9b 104 case 6: // wait for <length> payload uint8_ts
shimniok 16:eb28d0f64a9b 105 data[idx++] = cc;
shimniok 16:eb28d0f64a9b 106 ck1 += cc;
shimniok 16:eb28d0f64a9b 107 ck2 += ck1;
shimniok 16:eb28d0f64a9b 108 if (idx >= length) {
shimniok 16:eb28d0f64a9b 109 state++;
shimniok 16:eb28d0f64a9b 110 }
shimniok 16:eb28d0f64a9b 111 break;
shimniok 16:eb28d0f64a9b 112 case 7: // wait for checksum 1
shimniok 16:eb28d0f64a9b 113 chk1 = cc;
shimniok 16:eb28d0f64a9b 114 state++;
shimniok 16:eb28d0f64a9b 115 break;
shimniok 16:eb28d0f64a9b 116 case 8: { // wait for checksum 2
shimniok 16:eb28d0f64a9b 117 chk2 = cc;
shimniok 16:eb28d0f64a9b 118 checkOk = ck1 == chk1 && ck2 == chk2;
shimniok 16:eb28d0f64a9b 119 if (!checkOk) {
shimniok 19:0d1728091519 120 puts("BAD CHKSUM\n");
shimniok 16:eb28d0f64a9b 121 } else {
shimniok 16:eb28d0f64a9b 122 switch (code) {
shimniok 19:0d1728091519 123 case 0x01: // NAV-
shimniok 19:0d1728091519 124 switch (id) {
shimniok 19:0d1728091519 125 case POSLLH_MSG: // NAV-POSLLH
shimniok 19:0d1728091519 126 tmp.lon = ((float)LONG(4))/10000000.0;
shimniok 19:0d1728091519 127 tmp.lat = ((float)LONG(8))/10000000.0;
shimniok 19:0d1728091519 128 // vAcc = ULONG(24); // mm
shimniok 19:0d1728091519 129 // hAcc = ULONG(20); // mm
shimniok 19:0d1728091519 130 _ready |= POSLLH_BIT;
shimniok 19:0d1728091519 131 puts("LAT/LON");
shimniok 19:0d1728091519 132 break;
shimniok 19:0d1728091519 133 case DOP_MSG: // NAV-DOP
shimniok 19:0d1728091519 134 //gDOP = ((float) UINT(4))/100.0;
shimniok 19:0d1728091519 135 //tDOP = ((float) UINT(8))/100.0;
shimniok 19:0d1728091519 136 //vDOP = ((float) UINT(10))/100.0;
shimniok 19:0d1728091519 137 tmp.hdop = ((float) UINT(12))/100.0;
shimniok 19:0d1728091519 138 puts("HDOP");
shimniok 19:0d1728091519 139 _ready |= DOP_BIT;
shimniok 16:eb28d0f64a9b 140 break;
shimniok 19:0d1728091519 141 case SOL_MSG: // NAV-SOL
shimniok 19:0d1728091519 142 //week = UINT(8);
shimniok 19:0d1728091519 143 //pDOP = ((float) UINT(44))/ 100.0;
shimniok 19:0d1728091519 144 //pAcc = ULONG(24);
shimniok 19:0d1728091519 145 tmp.svcount = data[47];
shimniok 19:0d1728091519 146 _ready |= SOL_BIT;
shimniok 19:0d1728091519 147 puts("SVCOUNT");
shimniok 19:0d1728091519 148 break;
shimniok 19:0d1728091519 149 case VELNED_MSG: // NAV-VELNED
shimniok 19:0d1728091519 150 tmp.speed = ULONG(20)/100.0;
shimniok 19:0d1728091519 151 //sAcc = ULONG(28)/100.0;
shimniok 19:0d1728091519 152 tmp.course = ((float) LONG(24))/100000.0;
shimniok 19:0d1728091519 153 //cAcc = ((float) LONG(32))/100000.0;
shimniok 19:0d1728091519 154 _ready |= VELNED_BIT;
shimniok 19:0d1728091519 155 puts("SPD/COURSE");
shimniok 16:eb28d0f64a9b 156 break;
shimniok 19:0d1728091519 157 default:
shimniok 19:0d1728091519 158 printf("OTHER=0x%04x\n", id);
shimniok 19:0d1728091519 159 break;
shimniok 19:0d1728091519 160 }
shimniok 19:0d1728091519 161 if (_ready)
shimniok 19:0d1728091519 162 printf("_ready = 0x%04x 0x%04x\n", _ready, READY_BITS);
shimniok 19:0d1728091519 163 break;
shimniok 19:0d1728091519 164 case 0x05: // ACK-
shimniok 19:0d1728091519 165 switch (id) {
shimniok 19:0d1728091519 166 case 0x00: // ACK-NAK
shimniok 19:0d1728091519 167 break;
shimniok 19:0d1728091519 168 case 0x01: // ACK-ACK
shimniok 19:0d1728091519 169 break;
shimniok 19:0d1728091519 170 }
shimniok 19:0d1728091519 171 break;
shimniok 16:eb28d0f64a9b 172 }
shimniok 16:eb28d0f64a9b 173 }
shimniok 16:eb28d0f64a9b 174 state = 0;
shimniok 16:eb28d0f64a9b 175 break;
shimniok 16:eb28d0f64a9b 176 }
shimniok 16:eb28d0f64a9b 177 default:
shimniok 16:eb28d0f64a9b 178 break;
shimniok 16:eb28d0f64a9b 179 }
shimniok 19:0d1728091519 180
shimniok 19:0d1728091519 181 if ((_ready & READY_BITS) == READY_BITS) {
shimniok 18:3f8a8f6e3cc1 182 latest = tmp;
shimniok 19:0d1728091519 183 tmp.lat = 0;
shimniok 19:0d1728091519 184 tmp.lon = 0;
shimniok 19:0d1728091519 185 tmp.speed = 0;
shimniok 19:0d1728091519 186 tmp.course = 0;
shimniok 19:0d1728091519 187 tmp.hdop = 0;
shimniok 19:0d1728091519 188 tmp.svcount = 0;
shimniok 18:3f8a8f6e3cc1 189 _ready = 0;
shimniok 19:0d1728091519 190 status = 1;
shimniok 18:3f8a8f6e3cc1 191 _available = true;
shimniok 18:3f8a8f6e3cc1 192 }
shimniok 18:3f8a8f6e3cc1 193
shimniok 19:0d1728091519 194 return status;
shimniok 16:eb28d0f64a9b 195 }
shimniok 16:eb28d0f64a9b 196
shimniok 18:3f8a8f6e3cc1 197 void Ublox6::read(double& lat, double& lon, float& course, float& speed, float& hdop, int& svcount) {
shimniok 18:3f8a8f6e3cc1 198 lat = latest.lat;
shimniok 18:3f8a8f6e3cc1 199 lon = latest.lon;
shimniok 18:3f8a8f6e3cc1 200 course = latest.course;
shimniok 18:3f8a8f6e3cc1 201 speed = latest.speed;
shimniok 18:3f8a8f6e3cc1 202 hdop = latest.hdop;
shimniok 18:3f8a8f6e3cc1 203 svcount = latest.svcount;
shimniok 18:3f8a8f6e3cc1 204
shimniok 18:3f8a8f6e3cc1 205 return;
shimniok 18:3f8a8f6e3cc1 206 }
shimniok 18:3f8a8f6e3cc1 207
shimniok 19:0d1728091519 208 /*
shimniok 16:eb28d0f64a9b 209 bool Ublox6::available(void)
shimniok 16:eb28d0f64a9b 210 {
shimniok 18:3f8a8f6e3cc1 211 return _available;
shimniok 16:eb28d0f64a9b 212 }
shimniok 16:eb28d0f64a9b 213
shimniok 16:eb28d0f64a9b 214 double Ublox6::latitude(void)
shimniok 16:eb28d0f64a9b 215 {
shimniok 16:eb28d0f64a9b 216 return _latitude;
shimniok 16:eb28d0f64a9b 217 }
shimniok 16:eb28d0f64a9b 218
shimniok 16:eb28d0f64a9b 219 double Ublox6::longitude(void)
shimniok 16:eb28d0f64a9b 220 {
shimniok 16:eb28d0f64a9b 221 return _longitude;
shimniok 16:eb28d0f64a9b 222 }
shimniok 16:eb28d0f64a9b 223
shimniok 16:eb28d0f64a9b 224 float Ublox6::speed_mps(void)
shimniok 16:eb28d0f64a9b 225 {
shimniok 16:eb28d0f64a9b 226 return _speed_mps;
shimniok 16:eb28d0f64a9b 227 }
shimniok 16:eb28d0f64a9b 228
shimniok 16:eb28d0f64a9b 229 float Ublox6::heading_deg(void)
shimniok 16:eb28d0f64a9b 230 {
shimniok 16:eb28d0f64a9b 231 return _course_deg;
shimniok 16:eb28d0f64a9b 232 }
shimniok 16:eb28d0f64a9b 233
shimniok 16:eb28d0f64a9b 234 float Ublox6::hdop(void)
shimniok 16:eb28d0f64a9b 235 {
shimniok 16:eb28d0f64a9b 236 return _hdop;
shimniok 16:eb28d0f64a9b 237 }
shimniok 16:eb28d0f64a9b 238
shimniok 16:eb28d0f64a9b 239 int Ublox6::sat_count(void)
shimniok 16:eb28d0f64a9b 240 {
shimniok 18:3f8a8f6e3cc1 241 return _svcount;
shimniok 16:eb28d0f64a9b 242 }
shimniok 18:3f8a8f6e3cc1 243 */