Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

Ublox6.cpp@20:043987d06f8d, 2018-12-16 (annotated)

Committer:
shimniok
Date:
Sun Dec 16 04:18:42 2018 +0000
Revision:
20:043987d06f8d
Parent:
19:0d1728091519
Child:
23:5e61cf4a8c34
Added gps interrupt handler, fixed command line gps query

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