RX
Dependencies: mbed BufferedSerial SX1276GenericLib X_NUCLEO_IKS01A2
UbxGpsNavSol.cpp@2:e7d7e80256cc, 2019-06-17 (annotated)
- Committer:
- LucasKB
- Date:
- Mon Jun 17 00:10:40 2019 +0000
- Revision:
- 2:e7d7e80256cc
RX;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LucasKB | 2:e7d7e80256cc | 1 | #include "UbxGpsNavSol.hh" |
LucasKB | 2:e7d7e80256cc | 2 | #include "mbed.h" |
LucasKB | 2:e7d7e80256cc | 3 | |
LucasKB | 2:e7d7e80256cc | 4 | UbxGpsNavSol::UbxGpsNavSol(PinName tx, PinName rx, int baud):UARTSerial(tx, rx, baud){ |
LucasKB | 2:e7d7e80256cc | 5 | this->carriagePosition = 0; |
LucasKB | 2:e7d7e80256cc | 6 | // this->setLength(52); |
LucasKB | 2:e7d7e80256cc | 7 | offsetClassProperties = 8; |
LucasKB | 2:e7d7e80256cc | 8 | offsetHeaders = 4; |
LucasKB | 2:e7d7e80256cc | 9 | } |
LucasKB | 2:e7d7e80256cc | 10 | |
LucasKB | 2:e7d7e80256cc | 11 | UbxGpsNavSol::~UbxGpsNavSol(){} |
LucasKB | 2:e7d7e80256cc | 12 | |
LucasKB | 2:e7d7e80256cc | 13 | bool UbxGpsNavSol::ready(){ |
LucasKB | 2:e7d7e80256cc | 14 | unsigned char buffer[60]; |
LucasKB | 2:e7d7e80256cc | 15 | |
LucasKB | 2:e7d7e80256cc | 16 | if(this->readable()){ |
LucasKB | 2:e7d7e80256cc | 17 | for(int k = 0; k < 60; k++) |
LucasKB | 2:e7d7e80256cc | 18 | buffer[k] = 0; |
LucasKB | 2:e7d7e80256cc | 19 | |
LucasKB | 2:e7d7e80256cc | 20 | this->read(buffer, 60); |
LucasKB | 2:e7d7e80256cc | 21 | } |
LucasKB | 2:e7d7e80256cc | 22 | |
LucasKB | 2:e7d7e80256cc | 23 | if(buffer[0] != UBX_NAV_SOL_HEADER_1 || buffer[1] != UBX_NAV_SOL_HEADER_2) |
LucasKB | 2:e7d7e80256cc | 24 | return false; |
LucasKB | 2:e7d7e80256cc | 25 | |
LucasKB | 2:e7d7e80256cc | 26 | if(buffer[2] != UBX_NAV_SOL_CLASS || buffer[3] != UBX_NAV_SOL_ID) |
LucasKB | 2:e7d7e80256cc | 27 | return false; |
LucasKB | 2:e7d7e80256cc | 28 | |
LucasKB | 2:e7d7e80256cc | 29 | payload_length = buffer[5]; |
LucasKB | 2:e7d7e80256cc | 30 | payload_length = payload_length << 8; |
LucasKB | 2:e7d7e80256cc | 31 | payload_length = buffer[4]; |
LucasKB | 2:e7d7e80256cc | 32 | |
LucasKB | 2:e7d7e80256cc | 33 | if(payload_length != UBX_NAV_SOL_PAYLOAD_LENGTH) |
LucasKB | 2:e7d7e80256cc | 34 | return false; |
LucasKB | 2:e7d7e80256cc | 35 | |
LucasKB | 2:e7d7e80256cc | 36 | // if(calculateChecksum(buffer) == false) |
LucasKB | 2:e7d7e80256cc | 37 | // return false; |
LucasKB | 2:e7d7e80256cc | 38 | |
LucasKB | 2:e7d7e80256cc | 39 | iTOW = buffer[9] << 8; |
LucasKB | 2:e7d7e80256cc | 40 | iTOW |= buffer[8]; |
LucasKB | 2:e7d7e80256cc | 41 | iTOW <<= 8; |
LucasKB | 2:e7d7e80256cc | 42 | iTOW |= buffer[7]; |
LucasKB | 2:e7d7e80256cc | 43 | iTOW <<= 8; |
LucasKB | 2:e7d7e80256cc | 44 | iTOW |= buffer[6]; |
LucasKB | 2:e7d7e80256cc | 45 | |
LucasKB | 2:e7d7e80256cc | 46 | fTOW = buffer[13] << 8; |
LucasKB | 2:e7d7e80256cc | 47 | fTOW |= buffer[12]; |
LucasKB | 2:e7d7e80256cc | 48 | fTOW <<= 8; |
LucasKB | 2:e7d7e80256cc | 49 | fTOW |= buffer[11]; |
LucasKB | 2:e7d7e80256cc | 50 | fTOW <<= 8; |
LucasKB | 2:e7d7e80256cc | 51 | fTOW |= buffer[10]; |
LucasKB | 2:e7d7e80256cc | 52 | |
LucasKB | 2:e7d7e80256cc | 53 | week = buffer[15] << 8; |
LucasKB | 2:e7d7e80256cc | 54 | week |= buffer[14]; |
LucasKB | 2:e7d7e80256cc | 55 | |
LucasKB | 2:e7d7e80256cc | 56 | gpsFix = buffer[16]; |
LucasKB | 2:e7d7e80256cc | 57 | |
LucasKB | 2:e7d7e80256cc | 58 | flags = buffer[17]; |
LucasKB | 2:e7d7e80256cc | 59 | |
LucasKB | 2:e7d7e80256cc | 60 | ecefX = buffer[21] << 8; |
LucasKB | 2:e7d7e80256cc | 61 | ecefX |= buffer[20]; |
LucasKB | 2:e7d7e80256cc | 62 | ecefX <<= 8; |
LucasKB | 2:e7d7e80256cc | 63 | ecefX |= buffer[19]; |
LucasKB | 2:e7d7e80256cc | 64 | ecefX <<= 8; |
LucasKB | 2:e7d7e80256cc | 65 | ecefX |= buffer[18]; |
LucasKB | 2:e7d7e80256cc | 66 | |
LucasKB | 2:e7d7e80256cc | 67 | ecefY = buffer[25] << 8; |
LucasKB | 2:e7d7e80256cc | 68 | ecefY |= buffer[24]; |
LucasKB | 2:e7d7e80256cc | 69 | ecefY <<= 8; |
LucasKB | 2:e7d7e80256cc | 70 | ecefY |= buffer[23]; |
LucasKB | 2:e7d7e80256cc | 71 | ecefY <<= 8; |
LucasKB | 2:e7d7e80256cc | 72 | ecefY |= buffer[22]; |
LucasKB | 2:e7d7e80256cc | 73 | |
LucasKB | 2:e7d7e80256cc | 74 | ecefZ = buffer[29] << 8; |
LucasKB | 2:e7d7e80256cc | 75 | ecefZ |= buffer[28]; |
LucasKB | 2:e7d7e80256cc | 76 | ecefZ <<= 8; |
LucasKB | 2:e7d7e80256cc | 77 | ecefZ |= buffer[27]; |
LucasKB | 2:e7d7e80256cc | 78 | ecefZ <<= 8; |
LucasKB | 2:e7d7e80256cc | 79 | ecefZ |= buffer[26]; |
LucasKB | 2:e7d7e80256cc | 80 | |
LucasKB | 2:e7d7e80256cc | 81 | pAcc = buffer[33] << 8; |
LucasKB | 2:e7d7e80256cc | 82 | pAcc |= buffer[32]; |
LucasKB | 2:e7d7e80256cc | 83 | pAcc <<= 8; |
LucasKB | 2:e7d7e80256cc | 84 | pAcc |= buffer[31]; |
LucasKB | 2:e7d7e80256cc | 85 | pAcc <<= 8; |
LucasKB | 2:e7d7e80256cc | 86 | pAcc |= buffer[30]; |
LucasKB | 2:e7d7e80256cc | 87 | |
LucasKB | 2:e7d7e80256cc | 88 | ecefVX = buffer[37] << 8; |
LucasKB | 2:e7d7e80256cc | 89 | ecefVX |= buffer[36]; |
LucasKB | 2:e7d7e80256cc | 90 | ecefVX <<= 8; |
LucasKB | 2:e7d7e80256cc | 91 | ecefVX |= buffer[35]; |
LucasKB | 2:e7d7e80256cc | 92 | ecefVX <<= 8; |
LucasKB | 2:e7d7e80256cc | 93 | ecefVX |= buffer[34]; |
LucasKB | 2:e7d7e80256cc | 94 | |
LucasKB | 2:e7d7e80256cc | 95 | ecefVY = buffer[41] << 8; |
LucasKB | 2:e7d7e80256cc | 96 | ecefVY |= buffer[40]; |
LucasKB | 2:e7d7e80256cc | 97 | ecefVY <<= 8; |
LucasKB | 2:e7d7e80256cc | 98 | ecefVY |= buffer[39]; |
LucasKB | 2:e7d7e80256cc | 99 | ecefVY <<= 8; |
LucasKB | 2:e7d7e80256cc | 100 | ecefVY |= buffer[38]; |
LucasKB | 2:e7d7e80256cc | 101 | |
LucasKB | 2:e7d7e80256cc | 102 | ecefVZ = buffer[45] << 8; |
LucasKB | 2:e7d7e80256cc | 103 | ecefVZ |= buffer[44]; |
LucasKB | 2:e7d7e80256cc | 104 | ecefVZ <<= 8; |
LucasKB | 2:e7d7e80256cc | 105 | ecefVZ |= buffer[43]; |
LucasKB | 2:e7d7e80256cc | 106 | ecefVZ <<= 8; |
LucasKB | 2:e7d7e80256cc | 107 | ecefVZ |= buffer[42]; |
LucasKB | 2:e7d7e80256cc | 108 | |
LucasKB | 2:e7d7e80256cc | 109 | sAcc = buffer[49] << 8; |
LucasKB | 2:e7d7e80256cc | 110 | sAcc |= buffer[48]; |
LucasKB | 2:e7d7e80256cc | 111 | sAcc <<= 8; |
LucasKB | 2:e7d7e80256cc | 112 | sAcc |= buffer[47]; |
LucasKB | 2:e7d7e80256cc | 113 | sAcc <<= 8; |
LucasKB | 2:e7d7e80256cc | 114 | sAcc |= buffer[46]; |
LucasKB | 2:e7d7e80256cc | 115 | |
LucasKB | 2:e7d7e80256cc | 116 | pDOP = buffer[51] << 8; |
LucasKB | 2:e7d7e80256cc | 117 | pDOP |= buffer[50]; |
LucasKB | 2:e7d7e80256cc | 118 | |
LucasKB | 2:e7d7e80256cc | 119 | reserved1 = buffer[52]; |
LucasKB | 2:e7d7e80256cc | 120 | |
LucasKB | 2:e7d7e80256cc | 121 | numSV = buffer[53]; |
LucasKB | 2:e7d7e80256cc | 122 | |
LucasKB | 2:e7d7e80256cc | 123 | reserved2 = buffer[57] << 8; |
LucasKB | 2:e7d7e80256cc | 124 | reserved2 |= buffer[56]; |
LucasKB | 2:e7d7e80256cc | 125 | reserved2 <<= 8; |
LucasKB | 2:e7d7e80256cc | 126 | reserved2 |= buffer[55]; |
LucasKB | 2:e7d7e80256cc | 127 | reserved2 <<= 8; |
LucasKB | 2:e7d7e80256cc | 128 | reserved2 |= buffer[54]; |
LucasKB | 2:e7d7e80256cc | 129 | |
LucasKB | 2:e7d7e80256cc | 130 | return true; |
LucasKB | 2:e7d7e80256cc | 131 | } |
LucasKB | 2:e7d7e80256cc | 132 | |
LucasKB | 2:e7d7e80256cc | 133 | bool UbxGpsNavSol::calculateChecksum(unsigned char *buffer){ |
LucasKB | 2:e7d7e80256cc | 134 | uint8_t check_a, check_b; |
LucasKB | 2:e7d7e80256cc | 135 | check_a = check_b = 0; |
LucasKB | 2:e7d7e80256cc | 136 | |
LucasKB | 2:e7d7e80256cc | 137 | for(int i = 2; i < 58; i++){ |
LucasKB | 2:e7d7e80256cc | 138 | check_a += buffer[i]; |
LucasKB | 2:e7d7e80256cc | 139 | check_b += check_a; |
LucasKB | 2:e7d7e80256cc | 140 | } |
LucasKB | 2:e7d7e80256cc | 141 | |
LucasKB | 2:e7d7e80256cc | 142 | if(check_a == buffer[59] && check_b == buffer[58]) |
LucasKB | 2:e7d7e80256cc | 143 | return true; |
LucasKB | 2:e7d7e80256cc | 144 | |
LucasKB | 2:e7d7e80256cc | 145 | else |
LucasKB | 2:e7d7e80256cc | 146 | return false; |
LucasKB | 2:e7d7e80256cc | 147 | } |
LucasKB | 2:e7d7e80256cc | 148 | |
LucasKB | 2:e7d7e80256cc | 149 | bool UbxGpsNavSol::enableNAVSOL(){ |
LucasKB | 2:e7d7e80256cc | 150 | const uint8_t buffer[] = { |
LucasKB | 2:e7d7e80256cc | 151 | 0xB5, // sync char 1 |
LucasKB | 2:e7d7e80256cc | 152 | 0x62, // sync char 2 |
LucasKB | 2:e7d7e80256cc | 153 | 0x06, // class |
LucasKB | 2:e7d7e80256cc | 154 | 0x01, // id |
LucasKB | 2:e7d7e80256cc | 155 | 0x08, // length |
LucasKB | 2:e7d7e80256cc | 156 | 0x00, // length |
LucasKB | 2:e7d7e80256cc | 157 | 0x01, // payload |
LucasKB | 2:e7d7e80256cc | 158 | 0x06, // payload |
LucasKB | 2:e7d7e80256cc | 159 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 160 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 161 | 0x01, // payload |
LucasKB | 2:e7d7e80256cc | 162 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 163 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 164 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 165 | 0x17, // CK_A |
LucasKB | 2:e7d7e80256cc | 166 | 0xD9, // CK_B |
LucasKB | 2:e7d7e80256cc | 167 | }; |
LucasKB | 2:e7d7e80256cc | 168 | if(this->writable()){ |
LucasKB | 2:e7d7e80256cc | 169 | this->write(buffer, 16); |
LucasKB | 2:e7d7e80256cc | 170 | return true; |
LucasKB | 2:e7d7e80256cc | 171 | } |
LucasKB | 2:e7d7e80256cc | 172 | |
LucasKB | 2:e7d7e80256cc | 173 | return false; |
LucasKB | 2:e7d7e80256cc | 174 | } |
LucasKB | 2:e7d7e80256cc | 175 | |
LucasKB | 2:e7d7e80256cc | 176 | bool UbxGpsNavSol::restoreDefaults(){ |
LucasKB | 2:e7d7e80256cc | 177 | const uint8_t packet[] = { |
LucasKB | 2:e7d7e80256cc | 178 | 0xB5, // sync char 1 |
LucasKB | 2:e7d7e80256cc | 179 | 0x62, // sync char 2 |
LucasKB | 2:e7d7e80256cc | 180 | 0x06, // class |
LucasKB | 2:e7d7e80256cc | 181 | 0x09, // id |
LucasKB | 2:e7d7e80256cc | 182 | 0x0D, // length |
LucasKB | 2:e7d7e80256cc | 183 | 0x00, // length |
LucasKB | 2:e7d7e80256cc | 184 | 0xFF, // payload |
LucasKB | 2:e7d7e80256cc | 185 | 0xFF, // payload |
LucasKB | 2:e7d7e80256cc | 186 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 187 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 188 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 189 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 190 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 191 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 192 | 0xFF, // payload |
LucasKB | 2:e7d7e80256cc | 193 | 0xFF, // payload |
LucasKB | 2:e7d7e80256cc | 194 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 195 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 196 | 0x17, // payload |
LucasKB | 2:e7d7e80256cc | 197 | 0x2F, // CK_A |
LucasKB | 2:e7d7e80256cc | 198 | 0xAE, // CK_B |
LucasKB | 2:e7d7e80256cc | 199 | }; |
LucasKB | 2:e7d7e80256cc | 200 | |
LucasKB | 2:e7d7e80256cc | 201 | if(this->writable()){ |
LucasKB | 2:e7d7e80256cc | 202 | this->write(packet, 21); |
LucasKB | 2:e7d7e80256cc | 203 | return true; |
LucasKB | 2:e7d7e80256cc | 204 | } |
LucasKB | 2:e7d7e80256cc | 205 | |
LucasKB | 2:e7d7e80256cc | 206 | return false; |
LucasKB | 2:e7d7e80256cc | 207 | } |
LucasKB | 2:e7d7e80256cc | 208 | |
LucasKB | 2:e7d7e80256cc | 209 | void UbxGpsNavSol::disableNmea(){ |
LucasKB | 2:e7d7e80256cc | 210 | const uint8_t messages[][2] = { |
LucasKB | 2:e7d7e80256cc | 211 | {0xF0, 0x0A}, |
LucasKB | 2:e7d7e80256cc | 212 | {0xF0, 0x09}, |
LucasKB | 2:e7d7e80256cc | 213 | {0xF0, 0x00}, |
LucasKB | 2:e7d7e80256cc | 214 | {0xF0, 0x01}, |
LucasKB | 2:e7d7e80256cc | 215 | {0xF0, 0x0D}, |
LucasKB | 2:e7d7e80256cc | 216 | {0xF0, 0x06}, |
LucasKB | 2:e7d7e80256cc | 217 | {0xF0, 0x02}, |
LucasKB | 2:e7d7e80256cc | 218 | {0xF0, 0x07}, |
LucasKB | 2:e7d7e80256cc | 219 | {0xF0, 0x03}, |
LucasKB | 2:e7d7e80256cc | 220 | {0xF0, 0x04}, |
LucasKB | 2:e7d7e80256cc | 221 | {0xF0, 0x0E}, |
LucasKB | 2:e7d7e80256cc | 222 | {0xF0, 0x0F}, |
LucasKB | 2:e7d7e80256cc | 223 | {0xF0, 0x05}, |
LucasKB | 2:e7d7e80256cc | 224 | {0xF0, 0x08}, |
LucasKB | 2:e7d7e80256cc | 225 | {0xF1, 0x00}, |
LucasKB | 2:e7d7e80256cc | 226 | {0xF1, 0x01}, |
LucasKB | 2:e7d7e80256cc | 227 | {0xF1, 0x03}, |
LucasKB | 2:e7d7e80256cc | 228 | {0xF1, 0x04}, |
LucasKB | 2:e7d7e80256cc | 229 | {0xF1, 0x05}, |
LucasKB | 2:e7d7e80256cc | 230 | {0xF1, 0x06}, |
LucasKB | 2:e7d7e80256cc | 231 | }; |
LucasKB | 2:e7d7e80256cc | 232 | |
LucasKB | 2:e7d7e80256cc | 233 | // CFG-MSG packet buffer |
LucasKB | 2:e7d7e80256cc | 234 | uint8_t packet[] = { |
LucasKB | 2:e7d7e80256cc | 235 | 0xB5, // sync char 1 |
LucasKB | 2:e7d7e80256cc | 236 | 0x62, // sync char 2 |
LucasKB | 2:e7d7e80256cc | 237 | 0x06, // class |
LucasKB | 2:e7d7e80256cc | 238 | 0x01, // id |
LucasKB | 2:e7d7e80256cc | 239 | 0x03, // length |
LucasKB | 2:e7d7e80256cc | 240 | 0x00, // length |
LucasKB | 2:e7d7e80256cc | 241 | 0x00, // payload (first byte from messages array element) |
LucasKB | 2:e7d7e80256cc | 242 | 0x00, // payload (second byte from messages array element) |
LucasKB | 2:e7d7e80256cc | 243 | 0x00, // payload (not changed in the case) |
LucasKB | 2:e7d7e80256cc | 244 | 0x00, // CK_A |
LucasKB | 2:e7d7e80256cc | 245 | 0x00, // CK_B |
LucasKB | 2:e7d7e80256cc | 246 | }; |
LucasKB | 2:e7d7e80256cc | 247 | |
LucasKB | 2:e7d7e80256cc | 248 | uint8_t packetSize = sizeof(packet); |
LucasKB | 2:e7d7e80256cc | 249 | |
LucasKB | 2:e7d7e80256cc | 250 | // Offset to the place where payload starts |
LucasKB | 2:e7d7e80256cc | 251 | uint8_t payloadOffset = 6; |
LucasKB | 2:e7d7e80256cc | 252 | |
LucasKB | 2:e7d7e80256cc | 253 | // Iterate over the messages array |
LucasKB | 2:e7d7e80256cc | 254 | for (uint8_t i = 0; i < sizeof(messages) / sizeof(*messages); i++) |
LucasKB | 2:e7d7e80256cc | 255 | { |
LucasKB | 2:e7d7e80256cc | 256 | // Copy two bytes of payload to the packet buffer |
LucasKB | 2:e7d7e80256cc | 257 | for (uint8_t j = 0; j < sizeof(*messages); j++) |
LucasKB | 2:e7d7e80256cc | 258 | { |
LucasKB | 2:e7d7e80256cc | 259 | packet[payloadOffset + j] = messages[i][j]; |
LucasKB | 2:e7d7e80256cc | 260 | } |
LucasKB | 2:e7d7e80256cc | 261 | |
LucasKB | 2:e7d7e80256cc | 262 | // Set checksum bytes to the null |
LucasKB | 2:e7d7e80256cc | 263 | packet[packetSize - 2] = 0x00; |
LucasKB | 2:e7d7e80256cc | 264 | packet[packetSize - 1] = 0x00; |
LucasKB | 2:e7d7e80256cc | 265 | |
LucasKB | 2:e7d7e80256cc | 266 | // Calculate checksum over the packet buffer excluding sync (first two) and checksum chars (last two) |
LucasKB | 2:e7d7e80256cc | 267 | for (uint8_t j = 0; j < packetSize - 4; j++) |
LucasKB | 2:e7d7e80256cc | 268 | { |
LucasKB | 2:e7d7e80256cc | 269 | packet[packetSize - 2] += packet[2 + j]; |
LucasKB | 2:e7d7e80256cc | 270 | packet[packetSize - 1] += packet[packetSize - 2]; |
LucasKB | 2:e7d7e80256cc | 271 | } |
LucasKB | 2:e7d7e80256cc | 272 | |
LucasKB | 2:e7d7e80256cc | 273 | if(this->writable()){ |
LucasKB | 2:e7d7e80256cc | 274 | this->write(packet, packetSize); |
LucasKB | 2:e7d7e80256cc | 275 | } |
LucasKB | 2:e7d7e80256cc | 276 | } |
LucasKB | 2:e7d7e80256cc | 277 | } |
LucasKB | 2:e7d7e80256cc | 278 | |
LucasKB | 2:e7d7e80256cc | 279 | bool UbxGpsNavSol::changeBaudrate(){ |
LucasKB | 2:e7d7e80256cc | 280 | const uint8_t packet[] = { |
LucasKB | 2:e7d7e80256cc | 281 | 0xB5, // sync char 1 |
LucasKB | 2:e7d7e80256cc | 282 | 0x62, // sync char 2 |
LucasKB | 2:e7d7e80256cc | 283 | 0x06, // class |
LucasKB | 2:e7d7e80256cc | 284 | 0x00, // id |
LucasKB | 2:e7d7e80256cc | 285 | 0x14, // length |
LucasKB | 2:e7d7e80256cc | 286 | 0x00, // length |
LucasKB | 2:e7d7e80256cc | 287 | 0x01, // payload |
LucasKB | 2:e7d7e80256cc | 288 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 289 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 290 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 291 | 0xD0, // payload |
LucasKB | 2:e7d7e80256cc | 292 | 0x08, // payload |
LucasKB | 2:e7d7e80256cc | 293 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 294 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 295 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 296 | 0xC2, // payload |
LucasKB | 2:e7d7e80256cc | 297 | 0x01, // payload |
LucasKB | 2:e7d7e80256cc | 298 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 299 | 0x07, // payload |
LucasKB | 2:e7d7e80256cc | 300 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 301 | 0x03, // payload |
LucasKB | 2:e7d7e80256cc | 302 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 303 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 304 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 305 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 306 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 307 | 0xC0, // CK_A |
LucasKB | 2:e7d7e80256cc | 308 | 0x7E, // CK_B |
LucasKB | 2:e7d7e80256cc | 309 | }; |
LucasKB | 2:e7d7e80256cc | 310 | |
LucasKB | 2:e7d7e80256cc | 311 | if(this->writable()){ |
LucasKB | 2:e7d7e80256cc | 312 | this->write(packet, sizeof(packet)); |
LucasKB | 2:e7d7e80256cc | 313 | return true; |
LucasKB | 2:e7d7e80256cc | 314 | } |
LucasKB | 2:e7d7e80256cc | 315 | |
LucasKB | 2:e7d7e80256cc | 316 | return false; |
LucasKB | 2:e7d7e80256cc | 317 | } |
LucasKB | 2:e7d7e80256cc | 318 | |
LucasKB | 2:e7d7e80256cc | 319 | bool UbxGpsNavSol::disableUnnecessaryChannels(){ |
LucasKB | 2:e7d7e80256cc | 320 | const uint8_t packet[] = { |
LucasKB | 2:e7d7e80256cc | 321 | 0xB5, // sync char 1 |
LucasKB | 2:e7d7e80256cc | 322 | 0x62, // sync char 2 |
LucasKB | 2:e7d7e80256cc | 323 | 0x06, // class |
LucasKB | 2:e7d7e80256cc | 324 | 0x3E, // id |
LucasKB | 2:e7d7e80256cc | 325 | 0x24, // length |
LucasKB | 2:e7d7e80256cc | 326 | 0x00, // length |
LucasKB | 2:e7d7e80256cc | 327 | |
LucasKB | 2:e7d7e80256cc | 328 | 0x00, 0x00, 0x16, 0x04, 0x00, 0x04, 0xFF, 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 329 | 0x01, 0x00, 0x00, 0x01, 0x01, 0x01, 0x03, 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 330 | 0x00, 0x00, 0x00, 0x01, 0x05, 0x00, 0x03, 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 331 | 0x00, 0x00, 0x00, 0x01, 0x06, 0x08, 0xFF, 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 332 | 0x00, 0x00, 0x00, 0x01, // payload |
LucasKB | 2:e7d7e80256cc | 333 | |
LucasKB | 2:e7d7e80256cc | 334 | 0xA4, // CK_A |
LucasKB | 2:e7d7e80256cc | 335 | 0x25, // CK_B |
LucasKB | 2:e7d7e80256cc | 336 | }; |
LucasKB | 2:e7d7e80256cc | 337 | |
LucasKB | 2:e7d7e80256cc | 338 | if(this->writable()){ |
LucasKB | 2:e7d7e80256cc | 339 | this->write(packet, sizeof(packet)); |
LucasKB | 2:e7d7e80256cc | 340 | return true; |
LucasKB | 2:e7d7e80256cc | 341 | } |
LucasKB | 2:e7d7e80256cc | 342 | |
LucasKB | 2:e7d7e80256cc | 343 | return false; |
LucasKB | 2:e7d7e80256cc | 344 | } |
LucasKB | 2:e7d7e80256cc | 345 | |
LucasKB | 2:e7d7e80256cc | 346 | bool UbxGpsNavSol::changeFrequency(){ |
LucasKB | 2:e7d7e80256cc | 347 | const uint8_t packet[] = { |
LucasKB | 2:e7d7e80256cc | 348 | 0xB5, // sync char 1 |
LucasKB | 2:e7d7e80256cc | 349 | 0x62, // sync char 2 |
LucasKB | 2:e7d7e80256cc | 350 | 0x06, // class |
LucasKB | 2:e7d7e80256cc | 351 | 0x08, // id |
LucasKB | 2:e7d7e80256cc | 352 | 0x06, // length |
LucasKB | 2:e7d7e80256cc | 353 | 0x00, // length |
LucasKB | 2:e7d7e80256cc | 354 | 0x64, // payload |
LucasKB | 2:e7d7e80256cc | 355 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 356 | 0x01, // payload |
LucasKB | 2:e7d7e80256cc | 357 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 358 | 0x01, // payload |
LucasKB | 2:e7d7e80256cc | 359 | 0x00, // payload |
LucasKB | 2:e7d7e80256cc | 360 | 0x7A, // CK_A |
LucasKB | 2:e7d7e80256cc | 361 | 0x12, // CK_B |
LucasKB | 2:e7d7e80256cc | 362 | }; |
LucasKB | 2:e7d7e80256cc | 363 | |
LucasKB | 2:e7d7e80256cc | 364 | if(this->writable()){ |
LucasKB | 2:e7d7e80256cc | 365 | this->write(packet, sizeof(packet)); |
LucasKB | 2:e7d7e80256cc | 366 | return true; |
LucasKB | 2:e7d7e80256cc | 367 | } |
LucasKB | 2:e7d7e80256cc | 368 | |
LucasKB | 2:e7d7e80256cc | 369 | return false; |
LucasKB | 2:e7d7e80256cc | 370 | } |
LucasKB | 2:e7d7e80256cc | 371 | |
LucasKB | 2:e7d7e80256cc | 372 | bool UbxGpsNavSol::disableGNSS(){ |
LucasKB | 2:e7d7e80256cc | 373 | const uint8_t packet[] = { |
LucasKB | 2:e7d7e80256cc | 374 | 0xB5, 0x62, 0x06, 0x3E, 0x3C, 0x00, |
LucasKB | 2:e7d7e80256cc | 375 | 0x00, 0x00, 0x20, 0x07, 0x00, 0x08, |
LucasKB | 2:e7d7e80256cc | 376 | 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, |
LucasKB | 2:e7d7e80256cc | 377 | 0x01, 0x01, 0x03, 0x00, 0x00, 0x00, |
LucasKB | 2:e7d7e80256cc | 378 | 0x01, 0x01, 0x02, 0x04, 0x08, 0x00, |
LucasKB | 2:e7d7e80256cc | 379 | 0x00, 0x00, 0x01, 0x01, 0x03, 0x08, |
LucasKB | 2:e7d7e80256cc | 380 | 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, |
LucasKB | 2:e7d7e80256cc | 381 | 0x04, 0x00, 0x08, 0x00, 0x00, 0x00, |
LucasKB | 2:e7d7e80256cc | 382 | 0x01, 0x01, 0x05, 0x00, 0x03, 0x00, |
LucasKB | 2:e7d7e80256cc | 383 | 0x00, 0x00, 0x01, 0x01, 0x06, 0x08, |
LucasKB | 2:e7d7e80256cc | 384 | 0x0E, 0x00, 0x00, 0x00, 0x01, 0x01, |
LucasKB | 2:e7d7e80256cc | 385 | 0x2C, 0x4D |
LucasKB | 2:e7d7e80256cc | 386 | }; |
LucasKB | 2:e7d7e80256cc | 387 | |
LucasKB | 2:e7d7e80256cc | 388 | if(this->writable()){ |
LucasKB | 2:e7d7e80256cc | 389 | this->write(packet, sizeof(packet)); |
LucasKB | 2:e7d7e80256cc | 390 | return true; |
LucasKB | 2:e7d7e80256cc | 391 | } |
LucasKB | 2:e7d7e80256cc | 392 | |
LucasKB | 2:e7d7e80256cc | 393 | return false; |
LucasKB | 2:e7d7e80256cc | 394 | } |
LucasKB | 2:e7d7e80256cc | 395 | |
LucasKB | 2:e7d7e80256cc | 396 | void UbxGpsNavSol::baud(int baud){ |
LucasKB | 2:e7d7e80256cc | 397 | this->set_baud(baud); |
LucasKB | 2:e7d7e80256cc | 398 | } |