![](/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@16:eb28d0f64a9b, 2018-12-13 (annotated)
- Committer:
- shimniok
- Date:
- Thu Dec 13 17:35:29 2018 +0000
- Revision:
- 16:eb28d0f64a9b
- Child:
- 18:3f8a8f6e3cc1
Initial gps receive implementation
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 | 16:eb28d0f64a9b | 10 | #define SYNC1 0xB5 |
shimniok | 16:eb28d0f64a9b | 11 | #define SYNC2 0x62 |
shimniok | 16:eb28d0f64a9b | 12 | #define POSLLH_MSG 0x02 |
shimniok | 16:eb28d0f64a9b | 13 | #define SBAS_MSG 0x32 |
shimniok | 16:eb28d0f64a9b | 14 | #define VELNED_MSG 0x12 |
shimniok | 16:eb28d0f64a9b | 15 | #define STATUS_MSG 0x03 |
shimniok | 16:eb28d0f64a9b | 16 | #define SOL_MSG 0x06 |
shimniok | 16:eb28d0f64a9b | 17 | #define DOP_MSG 0x04 |
shimniok | 16:eb28d0f64a9b | 18 | #define DGPS_MSG 0x31 |
shimniok | 16:eb28d0f64a9b | 19 | #define SVINFO_MSG 0x30 |
shimniok | 16:eb28d0f64a9b | 20 | #define CFG 0x06 |
shimniok | 16:eb28d0f64a9b | 21 | #define NAV 0x01 |
shimniok | 16:eb28d0f64a9b | 22 | #define MSG 0x01 |
shimniok | 16:eb28d0f64a9b | 23 | |
shimniok | 16:eb28d0f64a9b | 24 | #define LONG(X) *(int32_t *)(&data[X]) |
shimniok | 16:eb28d0f64a9b | 25 | #define ULONG(X) *(uint32_t *)(&data[X]) |
shimniok | 16:eb28d0f64a9b | 26 | #define INT(X) *(int16_t *)(&data[X]) |
shimniok | 16:eb28d0f64a9b | 27 | #define UINT(X) *(uint16_t *)(&data[X]) |
shimniok | 16:eb28d0f64a9b | 28 | |
shimniok | 16:eb28d0f64a9b | 29 | // TODO 2: abstract LED/status to some kind of pub/sub thing |
shimniok | 16:eb28d0f64a9b | 30 | |
shimniok | 16:eb28d0f64a9b | 31 | void Ublox6::parse(unsigned char cc) |
shimniok | 16:eb28d0f64a9b | 32 | { |
shimniok | 16:eb28d0f64a9b | 33 | //unsigned char cc = buf[out++]; |
shimniok | 16:eb28d0f64a9b | 34 | //out &= (MAX_LENGTH-1); |
shimniok | 16:eb28d0f64a9b | 35 | static unsigned char ck1, ck2, state, code, id, idx, length, chk1, chk2; |
shimniok | 16:eb28d0f64a9b | 36 | static bool checkOk; |
shimniok | 16:eb28d0f64a9b | 37 | static unsigned char data[MAX_LENGTH]; |
shimniok | 16:eb28d0f64a9b | 38 | |
shimniok | 16:eb28d0f64a9b | 39 | switch (state) { |
shimniok | 16:eb28d0f64a9b | 40 | case 0: // wait for sync 1 (0xB5) |
shimniok | 16:eb28d0f64a9b | 41 | ck1 = ck2 = 0; |
shimniok | 16:eb28d0f64a9b | 42 | checkOk = false; |
shimniok | 16:eb28d0f64a9b | 43 | if (cc == SYNC1) |
shimniok | 16:eb28d0f64a9b | 44 | state++; |
shimniok | 16:eb28d0f64a9b | 45 | break; |
shimniok | 16:eb28d0f64a9b | 46 | case 1: // wait for sync 2 (0x62) |
shimniok | 16:eb28d0f64a9b | 47 | if (cc == SYNC2) |
shimniok | 16:eb28d0f64a9b | 48 | state++; |
shimniok | 16:eb28d0f64a9b | 49 | else |
shimniok | 16:eb28d0f64a9b | 50 | state = 0; |
shimniok | 16:eb28d0f64a9b | 51 | break; |
shimniok | 16:eb28d0f64a9b | 52 | case 2: // wait for class code |
shimniok | 16:eb28d0f64a9b | 53 | code = cc; |
shimniok | 16:eb28d0f64a9b | 54 | ck1 += cc; |
shimniok | 16:eb28d0f64a9b | 55 | ck2 += ck1; |
shimniok | 16:eb28d0f64a9b | 56 | state++; |
shimniok | 16:eb28d0f64a9b | 57 | break; |
shimniok | 16:eb28d0f64a9b | 58 | case 3: // wait for Id |
shimniok | 16:eb28d0f64a9b | 59 | id = cc; |
shimniok | 16:eb28d0f64a9b | 60 | ck1 += cc; |
shimniok | 16:eb28d0f64a9b | 61 | ck2 += ck1; |
shimniok | 16:eb28d0f64a9b | 62 | state++; |
shimniok | 16:eb28d0f64a9b | 63 | break; |
shimniok | 16:eb28d0f64a9b | 64 | case 4: // wait for length uint8_t 1 |
shimniok | 16:eb28d0f64a9b | 65 | length = cc; |
shimniok | 16:eb28d0f64a9b | 66 | ck1 += cc; |
shimniok | 16:eb28d0f64a9b | 67 | ck2 += ck1; |
shimniok | 16:eb28d0f64a9b | 68 | state++; |
shimniok | 16:eb28d0f64a9b | 69 | break; |
shimniok | 16:eb28d0f64a9b | 70 | case 5: // wait for length uint8_t 2 |
shimniok | 16:eb28d0f64a9b | 71 | length |= (unsigned int) cc << 8; |
shimniok | 16:eb28d0f64a9b | 72 | ck1 += cc; |
shimniok | 16:eb28d0f64a9b | 73 | ck2 += ck1; |
shimniok | 16:eb28d0f64a9b | 74 | idx = 0; |
shimniok | 16:eb28d0f64a9b | 75 | state++; |
shimniok | 16:eb28d0f64a9b | 76 | if (length > MAX_LENGTH) |
shimniok | 16:eb28d0f64a9b | 77 | state= 0; |
shimniok | 16:eb28d0f64a9b | 78 | break; |
shimniok | 16:eb28d0f64a9b | 79 | case 6: // wait for <length> payload uint8_ts |
shimniok | 16:eb28d0f64a9b | 80 | data[idx++] = cc; |
shimniok | 16:eb28d0f64a9b | 81 | ck1 += cc; |
shimniok | 16:eb28d0f64a9b | 82 | ck2 += ck1; |
shimniok | 16:eb28d0f64a9b | 83 | if (idx >= length) { |
shimniok | 16:eb28d0f64a9b | 84 | state++; |
shimniok | 16:eb28d0f64a9b | 85 | } |
shimniok | 16:eb28d0f64a9b | 86 | break; |
shimniok | 16:eb28d0f64a9b | 87 | case 7: // wait for checksum 1 |
shimniok | 16:eb28d0f64a9b | 88 | chk1 = cc; |
shimniok | 16:eb28d0f64a9b | 89 | state++; |
shimniok | 16:eb28d0f64a9b | 90 | break; |
shimniok | 16:eb28d0f64a9b | 91 | case 8: { // wait for checksum 2 |
shimniok | 16:eb28d0f64a9b | 92 | chk2 = cc; |
shimniok | 16:eb28d0f64a9b | 93 | checkOk = ck1 == chk1 && ck2 == chk2; |
shimniok | 16:eb28d0f64a9b | 94 | if (!checkOk) { |
shimniok | 16:eb28d0f64a9b | 95 | // do something...? |
shimniok | 16:eb28d0f64a9b | 96 | } else { |
shimniok | 16:eb28d0f64a9b | 97 | switch (code) { |
shimniok | 16:eb28d0f64a9b | 98 | case 0x01: // NAV- |
shimniok | 16:eb28d0f64a9b | 99 | switch (id) { |
shimniok | 16:eb28d0f64a9b | 100 | case POSLLH_MSG: // NAV-POSLLH |
shimniok | 16:eb28d0f64a9b | 101 | _longitude = ((float)LONG(4))/10000000.0; |
shimniok | 16:eb28d0f64a9b | 102 | _latitude = ((float)LONG(8))/10000000.0; |
shimniok | 16:eb28d0f64a9b | 103 | // vAcc = ULONG(24); // mm |
shimniok | 16:eb28d0f64a9b | 104 | // hAcc = ULONG(20); // mm |
shimniok | 16:eb28d0f64a9b | 105 | _available |= POSLLH_BIT; |
shimniok | 16:eb28d0f64a9b | 106 | break; |
shimniok | 16:eb28d0f64a9b | 107 | case DOP_MSG: // NAV-DOP |
shimniok | 16:eb28d0f64a9b | 108 | //gDOP = ((float) UINT(4))/100.0; |
shimniok | 16:eb28d0f64a9b | 109 | //tDOP = ((float) UINT(8))/100.0; |
shimniok | 16:eb28d0f64a9b | 110 | //vDOP = ((float) UINT(10))/100.0; |
shimniok | 16:eb28d0f64a9b | 111 | _hdop = ((float) UINT(12))/100.0; |
shimniok | 16:eb28d0f64a9b | 112 | _available |= DOP_BIT; |
shimniok | 16:eb28d0f64a9b | 113 | break; |
shimniok | 16:eb28d0f64a9b | 114 | case SOL_MSG: // NAV-SOL |
shimniok | 16:eb28d0f64a9b | 115 | //week = UINT(8); |
shimniok | 16:eb28d0f64a9b | 116 | //pDOP = ((float) UINT(44))/ 100.0; |
shimniok | 16:eb28d0f64a9b | 117 | //pAcc = ULONG(24); |
shimniok | 16:eb28d0f64a9b | 118 | _sat_count = data[47]; |
shimniok | 16:eb28d0f64a9b | 119 | _available |= SOL_BIT; |
shimniok | 16:eb28d0f64a9b | 120 | break; |
shimniok | 16:eb28d0f64a9b | 121 | case VELNED_MSG: // NAV-VELNED |
shimniok | 16:eb28d0f64a9b | 122 | _speed_mps = ULONG(20)/100.0; |
shimniok | 16:eb28d0f64a9b | 123 | //sAcc = ULONG(28)/100.0; |
shimniok | 16:eb28d0f64a9b | 124 | _course_deg = ((float) LONG(24))/100000.0; |
shimniok | 16:eb28d0f64a9b | 125 | //cAcc = ((float) LONG(32))/100000.0; |
shimniok | 16:eb28d0f64a9b | 126 | _available |= VELNED_BIT; |
shimniok | 16:eb28d0f64a9b | 127 | break; |
shimniok | 16:eb28d0f64a9b | 128 | default: |
shimniok | 16:eb28d0f64a9b | 129 | break; |
shimniok | 16:eb28d0f64a9b | 130 | } |
shimniok | 16:eb28d0f64a9b | 131 | break; |
shimniok | 16:eb28d0f64a9b | 132 | case 0x05: // ACK- |
shimniok | 16:eb28d0f64a9b | 133 | switch (id) { |
shimniok | 16:eb28d0f64a9b | 134 | case 0x00: // ACK-NAK |
shimniok | 16:eb28d0f64a9b | 135 | break; |
shimniok | 16:eb28d0f64a9b | 136 | case 0x01: // ACK-ACK |
shimniok | 16:eb28d0f64a9b | 137 | break; |
shimniok | 16:eb28d0f64a9b | 138 | } |
shimniok | 16:eb28d0f64a9b | 139 | break; |
shimniok | 16:eb28d0f64a9b | 140 | } |
shimniok | 16:eb28d0f64a9b | 141 | } |
shimniok | 16:eb28d0f64a9b | 142 | state = 0; |
shimniok | 16:eb28d0f64a9b | 143 | break; |
shimniok | 16:eb28d0f64a9b | 144 | } |
shimniok | 16:eb28d0f64a9b | 145 | default: |
shimniok | 16:eb28d0f64a9b | 146 | break; |
shimniok | 16:eb28d0f64a9b | 147 | } |
shimniok | 16:eb28d0f64a9b | 148 | } |
shimniok | 16:eb28d0f64a9b | 149 | |
shimniok | 16:eb28d0f64a9b | 150 | bool Ublox6::available(void) |
shimniok | 16:eb28d0f64a9b | 151 | { |
shimniok | 16:eb28d0f64a9b | 152 | return (_available & (DOP_BIT|POSLLH_BIT|SOL_BIT|VELNED_BIT)); |
shimniok | 16:eb28d0f64a9b | 153 | } |
shimniok | 16:eb28d0f64a9b | 154 | |
shimniok | 16:eb28d0f64a9b | 155 | void Ublox6::reset_available(void) |
shimniok | 16:eb28d0f64a9b | 156 | { |
shimniok | 16:eb28d0f64a9b | 157 | _available = 0; |
shimniok | 16:eb28d0f64a9b | 158 | } |
shimniok | 16:eb28d0f64a9b | 159 | |
shimniok | 16:eb28d0f64a9b | 160 | double Ublox6::latitude(void) |
shimniok | 16:eb28d0f64a9b | 161 | { |
shimniok | 16:eb28d0f64a9b | 162 | return _latitude; |
shimniok | 16:eb28d0f64a9b | 163 | } |
shimniok | 16:eb28d0f64a9b | 164 | |
shimniok | 16:eb28d0f64a9b | 165 | double Ublox6::longitude(void) |
shimniok | 16:eb28d0f64a9b | 166 | { |
shimniok | 16:eb28d0f64a9b | 167 | return _longitude; |
shimniok | 16:eb28d0f64a9b | 168 | } |
shimniok | 16:eb28d0f64a9b | 169 | |
shimniok | 16:eb28d0f64a9b | 170 | float Ublox6::speed_mps(void) |
shimniok | 16:eb28d0f64a9b | 171 | { |
shimniok | 16:eb28d0f64a9b | 172 | return _speed_mps; |
shimniok | 16:eb28d0f64a9b | 173 | } |
shimniok | 16:eb28d0f64a9b | 174 | |
shimniok | 16:eb28d0f64a9b | 175 | float Ublox6::heading_deg(void) |
shimniok | 16:eb28d0f64a9b | 176 | { |
shimniok | 16:eb28d0f64a9b | 177 | return _course_deg; |
shimniok | 16:eb28d0f64a9b | 178 | } |
shimniok | 16:eb28d0f64a9b | 179 | |
shimniok | 16:eb28d0f64a9b | 180 | float Ublox6::hdop(void) |
shimniok | 16:eb28d0f64a9b | 181 | { |
shimniok | 16:eb28d0f64a9b | 182 | return _hdop; |
shimniok | 16:eb28d0f64a9b | 183 | } |
shimniok | 16:eb28d0f64a9b | 184 | |
shimniok | 16:eb28d0f64a9b | 185 | int Ublox6::sat_count(void) |
shimniok | 16:eb28d0f64a9b | 186 | { |
shimniok | 16:eb28d0f64a9b | 187 | return _sat_count; |
shimniok | 16:eb28d0f64a9b | 188 | } |