Michael Shimniok / Mbed OS DataBus2018

2018 revision to classic DataBus AVC code.

Dependencies:   LSM303DLM Servo SerialGraphicLCD L3G4200D IncrementalEncoder SimpleShell

Ublox6.cpp@18:3f8a8f6e3cc1, 2018-12-14 (annotated)

Committer:
shimniok
Date:
Fri Dec 14 00:36:06 2018 +0000
Revision:
18:3f8a8f6e3cc1
Parent:
16:eb28d0f64a9b
Child:
19:0d1728091519
add cmd to read gps; fixed gps flag bug, changed gps interface

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