Thiago Lima / Mbed 2 deprecated Telemetria_RX_SD_GPS

RX

Dependencies:   mbed BufferedSerial SX1276GenericLib X_NUCLEO_IKS01A2

UbxGpsNavSol.cpp@0:fb7bf6d81e5f, 2019-06-05 (annotated)

Committer:
TMRL123
Date:
Wed Jun 05 00:25:53 2019 +0000
Revision:
0:fb7bf6d81e5f
RX

Who changed what in which revision?

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