Diff: UbxGpsNavSol.cpp

Revision:

0:fb7bf6d81e5f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/UbxGpsNavSol.cpp	Wed Jun 05 00:25:53 2019 +0000
@@ -0,0 +1,427 @@
+#include "UbxGpsNavSol.hh"
+#include "mbed.h"
+
+UbxGpsNavSol::UbxGpsNavSol(PinName tx, PinName rx, int baud):UARTSerial(tx, rx, baud){
+    this->carriagePosition = 0;
+    // this->setLength(52);
+    this->offsetClassProperties = 8;
+    this->offsetHeaders = 4;
+}
+
+UbxGpsNavSol::~UbxGpsNavSol(){}
+
+bool UbxGpsNavSol::ready(){
+    //unsigned char buffer[60];
+
+    if(this->readable()){
+      for(int k = 0; k < 60; k++)
+        this->buffer[k] = 0;
+
+      this->read(this->buffer, 60);
+    }
+
+    if(this->buffer[0] != UBX_NAV_SOL_HEADER_1 || this->buffer[1] != UBX_NAV_SOL_HEADER_2)
+      return false;
+
+    if(this->buffer[2] != UBX_NAV_SOL_CLASS || this->buffer[3] != UBX_NAV_SOL_ID)
+      return false;
+
+    payload_length = this->buffer[5];
+    payload_length = payload_length << 8;
+    payload_length = this->buffer[4];
+
+    if(payload_length != UBX_NAV_SOL_PAYLOAD_LENGTH)
+      return false;
+
+     if(calculateChecksum(buffer) == false)
+       return false;
+
+    iTOW = this->buffer[9] << 8;
+    iTOW |= this->buffer[8];
+    iTOW <<= 8;
+    iTOW |= this->buffer[7];
+    iTOW <<= 8;
+    iTOW |= this->buffer[6];
+
+    fTOW = this->buffer[13] << 8;
+    fTOW |= this->buffer[12];
+    fTOW <<= 8;
+    fTOW |= this->buffer[11];
+    fTOW <<= 8;
+    fTOW |= this->buffer[10];
+
+    week = this->buffer[15] << 8;
+    week |= this->buffer[14];
+
+    gpsFix = this->buffer[16];
+
+    flags = this->buffer[17];
+
+    ecefX = this->buffer[21] << 8;
+    ecefX |= this->buffer[20];
+    ecefX <<= 8;
+    ecefX |= this->buffer[19];
+    ecefX <<= 8;
+    ecefX |= this->buffer[18];
+
+    ecefY = this->buffer[25] << 8;
+    ecefY |= this->buffer[24];
+    ecefY <<= 8;
+    ecefY |= this->buffer[23];
+    ecefY <<= 8;
+    ecefY |= this->buffer[22];
+
+    ecefZ = this->buffer[29] << 8;
+    ecefZ |= this->buffer[28];
+    ecefZ <<= 8;
+    ecefZ |= this->buffer[27];
+    ecefZ <<= 8;
+    ecefZ |= this->buffer[26];
+
+    pAcc = this->buffer[33] << 8;
+    pAcc |= this->buffer[32];
+    pAcc <<= 8;
+    pAcc |= this->buffer[31];
+    pAcc <<= 8;
+    pAcc |= this->buffer[30];
+
+    ecefVX = this->buffer[37] << 8;
+    ecefVX |= this->buffer[36];
+    ecefVX <<= 8;
+    ecefVX |= this->buffer[35];
+    ecefVX <<= 8;
+    ecefVX |= this->buffer[34];
+
+    ecefVY = this->buffer[41] << 8;
+    ecefVY |= this->buffer[40];
+    ecefVY <<= 8;
+    ecefVY |= this->buffer[39];
+    ecefVY <<= 8;
+    ecefVY |= this->buffer[38];
+
+    ecefVZ = this->buffer[45] << 8;
+    ecefVZ |= this->buffer[44];
+    ecefVZ <<= 8;
+    ecefVZ |= this->buffer[43];
+    ecefVZ <<= 8;
+    ecefVZ |= this->buffer[42];
+
+    sAcc = this->buffer[49] << 8;
+    sAcc |= this->buffer[48];
+    sAcc <<= 8;
+    sAcc |= this->buffer[47];
+    sAcc <<= 8;
+    sAcc |= this->buffer[46];
+
+    pDOP = this->buffer[51] << 8;
+    pDOP |= this->buffer[50];
+
+    reserved1 = this->buffer[52];
+
+    numSV = this->buffer[53];
+
+    reserved2 = this->buffer[57] << 8;
+    reserved2 |= this->buffer[56];
+    reserved2 <<= 8;
+    reserved2 |= this->buffer[55];
+    reserved2 <<= 8;
+    reserved2 |= this->buffer[54];
+
+    return true;
+}
+
+bool UbxGpsNavSol::calculateChecksum(unsigned char *buffer){
+  uint8_t check_a, check_b;
+  check_a = check_b = 0;
+
+  for(int i = 2; i < 58; i++){
+    check_a += buffer[i];
+    check_b += check_a;
+  }
+
+  if(check_a == buffer[59] && check_b == buffer[58])
+    return true;
+
+  else
+    return false;
+}
+
+bool UbxGpsNavSol::enableNAVSOL(){
+  const uint8_t buffer[] = {
+    0xB5, // sync char 1 1
+    0x62, // sync char 2 2
+    0x06, // class 3
+    0x01, // id 4
+    0x02, // length 5
+    0x00, // length 6
+    0x01, // payload 7
+    0x06, // payload 8
+    0x10,//CKA
+    0x39,//CKB
+
+  };
+  if(this->writable()){
+    this->write(buffer, 10);
+    return true;
+  }
+
+  return false;
+}
+
+bool UbxGpsNavSol::restoreDefaults(){
+  const uint8_t packet[] = {
+    0xB5, // sync char 1
+    0x62, // sync char 2
+    0x06, // class
+    0x09, // id
+    0x0D, // length
+    0x00, // length
+    0xFF, // payload
+    0xFF, // payload
+    0x00, // payload
+    0x00, // payload
+    0x00, // payload
+    0x00, // payload
+    0x00, // payload
+    0x00, // payload
+    0xFF, // payload
+    0xFF, // payload
+    0x00, // payload
+    0x00, // payload
+    0x17, // payload
+    0x2F, // CK_A
+    0xAE, // CK_B
+  };
+
+  if(this->writable()){
+    this->write(packet, 21);
+    return true;
+  }
+
+  return false;
+}
+
+void UbxGpsNavSol::disableNmea(){
+  const uint8_t messages[][2] = {
+      {0xF0, 0x0A},
+      {0xF0, 0x09},
+      {0xF0, 0x00},
+      {0xF0, 0x01},
+      {0xF0, 0x0D},
+      {0xF0, 0x06},
+      {0xF0, 0x02},
+      {0xF0, 0x07},
+      {0xF0, 0x03},
+      {0xF0, 0x04},
+      {0xF0, 0x0E},
+      {0xF0, 0x0F},
+      {0xF0, 0x05},
+      {0xF0, 0x08},
+      {0xF1, 0x00},
+      {0xF1, 0x01},
+      {0xF1, 0x03},
+      {0xF1, 0x04},
+      {0xF1, 0x05},
+      {0xF1, 0x06},
+  };
+
+  // CFG-MSG packet buffer
+  uint8_t packet[] = {
+      0xB5, // sync char 1
+      0x62, // sync char 2
+      0x06, // class
+      0x01, // id
+      0x03, // length
+      0x00, // length
+      0x00, // payload (first byte from messages array element)
+      0x00, // payload (second byte from messages array element)
+      0x00, // payload (not changed in the case)
+      0x00, // CK_A
+      0x00, // CK_B
+  };
+
+  uint8_t packetSize = sizeof(packet);
+
+  // Offset to the place where payload starts
+  uint8_t payloadOffset = 6;
+
+  // Iterate over the messages array
+  for (uint8_t i = 0; i < sizeof(messages) / sizeof(*messages); i++)
+  {
+      // Copy two bytes of payload to the packet buffer
+      for (uint8_t j = 0; j < sizeof(*messages); j++)
+      {
+          packet[payloadOffset + j] = messages[i][j];
+      }
+
+      // Set checksum bytes to the null
+      packet[packetSize - 2] = 0x00;
+      packet[packetSize - 1] = 0x00;
+
+      // Calculate checksum over the packet buffer excluding sync (first two) and checksum chars (last two)
+      for (uint8_t j = 0; j < packetSize - 4; j++)
+      {
+          packet[packetSize - 2] += packet[2 + j];
+          packet[packetSize - 1] += packet[packetSize - 2];
+      }
+
+      if(this->writable()){
+        this->write(packet, packetSize);
+      }
+    }
+}
+
+bool UbxGpsNavSol::changeBaudrate(){
+  const uint8_t packet[] = {
+    0xB5, // sync char 1
+    0x62, // sync char 2
+    0x06, // class
+    0x00, // id
+    0x20, // length
+    0x00, // length
+    0x02, // payload - portID
+    0x00, // payload - reserved0
+    0x00, // payload - txReady
+    0x00, // payload - txReady
+    0xD0, // payload - mode
+    0x08, // payload - mode
+    0x00, // payload - mode
+    0x00, // payload - mode
+    0x00, // payload - reserved3
+    0xC2, // payload - reserved3
+    0x01, // payload - reserved3
+    0x00, // payload - reserved3
+    0x07, // payload - inProtoMask
+    0x00, // payload - inProtoMask
+    0x03, // payload - outProtoMask --> activate NMEA???
+    0x00, // payload - outProtoMask
+    0x00, // payload - flags
+    0x00, // payload - flags
+    0x00, // payload - reserved5
+    0x00, // payload - reserved5
+    0xCC, // CK_A
+    0xBA, // CK_B
+  };
+
+  if(this->writable()){
+    this->write(packet, sizeof(packet));
+    return true;
+  }
+
+  return false;
+}
+
+bool UbxGpsNavSol::disableUnnecessaryChannels(){
+  const uint8_t packet[] = {
+    0xB5, // sync char 1
+    0x62, // sync char 2
+    0x06, // class
+    0x3E, // id
+    0x24, // length
+    0x00, // length
+
+    0x00, 0x00, 0x16, 0x04, 0x00, 0x04, 0xFF, 0x00, // payload
+    0x01, 0x00, 0x00, 0x01, 0x01, 0x01, 0x03, 0x00, // payload
+    0x00, 0x00, 0x00, 0x01, 0x05, 0x00, 0x03, 0x00, // payload
+    0x00, 0x00, 0x00, 0x01, 0x06, 0x08, 0xFF, 0x00, // payload
+    0x00, 0x00, 0x00, 0x01,                         // payload
+
+    0xA4, // CK_A
+    0x25, // CK_B
+  };
+
+  if(this->writable()){
+    this->write(packet, sizeof(packet));
+    return true;
+  }
+
+  return false;
+}
+
+bool UbxGpsNavSol::changeFrequency(){
+  const uint8_t packet[] = {
+        0xB5, // sync char 1
+        0x62, // sync char 2
+        0x06, // class
+        0x08, // id
+        0x06, // length
+        0x00, // length
+        0x64, // payload
+        0x00, // payload
+        0x01, // payload
+        0x00, // payload
+        0x01, // payload
+        0x00, // payload
+        0x7A, // CK_A
+        0x12, // CK_B
+    };
+
+    if(this->writable()){
+      this->write(packet, sizeof(packet));
+      return true;
+    }
+
+    return false;
+}
+
+bool UbxGpsNavSol::disableGNSS(){
+    const uint8_t packet[] = {
+      0xB5, 0x62, 0x06, 0x3E, 0x3C, 0x00,
+      0x00, 0x00, 0x20, 0x07, 0x00, 0x08,
+      0x10, 0x00, 0x01, 0x00, 0x01, 0x01,
+      0x01, 0x01, 0x03, 0x00, 0x00, 0x00,
+      0x01, 0x01, 0x02, 0x04, 0x08, 0x00,
+      0x00, 0x00, 0x01, 0x01, 0x03, 0x08,
+      0x10, 0x00, 0x00, 0x00, 0x01, 0x01,
+      0x04, 0x00, 0x08, 0x00, 0x00, 0x00,
+      0x01, 0x01, 0x05, 0x00, 0x03, 0x00,
+      0x00, 0x00, 0x01, 0x01, 0x06, 0x08,
+      0x0E, 0x00, 0x00, 0x00, 0x01, 0x01,
+      0x2C, 0x4D
+    };
+
+    if(this->writable()){
+      this->write(packet, sizeof(packet));
+      return true;
+    }
+
+    return false;
+}
+
+void UbxGpsNavSol::baud(int baud){
+  this->set_baud(baud);
+}
+
+bool UbxGpsNavSol::saveConfiguration() {
+    const uint8_t packet[] = {
+    0xB5, // sync char 1
+    0x62, // sync char 2
+    0x06, // class
+    0x09, // id
+    0x0D, // length
+    0x00, // length
+    0x00, // payload
+    0x00, // payload
+    0x00, // payload
+    0x00, // payload
+    0xFF, // payload
+    0xFF, // payload
+    0x00, // payload
+    0x00, // payload
+    0x00, // payload
+    0x00, // payload
+    0x00, // payload
+    0x00, // payload
+    0x17, // payload
+    0x31, // CK_A
+    0xBF, // CK_B
+  };
+
+    if(this->writable()){
+      this->write(packet, sizeof(packet));
+      return true;
+    }
+
+    return false;
+}
+