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