![](/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@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?
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 | 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 | */ |