![](/media/cache/profiles/2012_sf_avc_095.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | */ |