File content as of revision 2:e7d7e80256cc:

#include "UbxGpsNavSol.hh"
#include "mbed.h"

UbxGpsNavSol::UbxGpsNavSol(PinName tx, PinName rx, int baud):UARTSerial(tx, rx, baud){
    this->carriagePosition = 0;
    // this->setLength(52);
    offsetClassProperties = 8;
    offsetHeaders = 4;
}

UbxGpsNavSol::~UbxGpsNavSol(){}

bool UbxGpsNavSol::ready(){
    unsigned char buffer[60];

    if(this->readable()){
      for(int k = 0; k < 60; k++)
        buffer[k] = 0;

      this->read(buffer, 60);
    }

    if(buffer[0] != UBX_NAV_SOL_HEADER_1 || buffer[1] != UBX_NAV_SOL_HEADER_2)
      return false;

    if(buffer[2] != UBX_NAV_SOL_CLASS || buffer[3] != UBX_NAV_SOL_ID)
      return false;

    payload_length = buffer[5];
    payload_length = payload_length << 8;
    payload_length = buffer[4];

    if(payload_length != UBX_NAV_SOL_PAYLOAD_LENGTH)
      return false;

    // if(calculateChecksum(buffer) == false)
    //   return false;

    iTOW = buffer[9] << 8;
    iTOW |= buffer[8];
    iTOW <<= 8;
    iTOW |= buffer[7];
    iTOW <<= 8;
    iTOW |= buffer[6];

    fTOW = buffer[13] << 8;
    fTOW |= buffer[12];
    fTOW <<= 8;
    fTOW |= buffer[11];
    fTOW <<= 8;
    fTOW |= buffer[10];

    week = buffer[15] << 8;
    week |= buffer[14];

    gpsFix = buffer[16];

    flags = buffer[17];

    ecefX = buffer[21] << 8;
    ecefX |= buffer[20];
    ecefX <<= 8;
    ecefX |= buffer[19];
    ecefX <<= 8;
    ecefX |= buffer[18];

    ecefY = buffer[25] << 8;
    ecefY |= buffer[24];
    ecefY <<= 8;
    ecefY |= buffer[23];
    ecefY <<= 8;
    ecefY |= buffer[22];

    ecefZ = buffer[29] << 8;
    ecefZ |= buffer[28];
    ecefZ <<= 8;
    ecefZ |= buffer[27];
    ecefZ <<= 8;
    ecefZ |= buffer[26];

    pAcc = buffer[33] << 8;
    pAcc |= buffer[32];
    pAcc <<= 8;
    pAcc |= buffer[31];
    pAcc <<= 8;
    pAcc |= buffer[30];

    ecefVX = buffer[37] << 8;
    ecefVX |= buffer[36];
    ecefVX <<= 8;
    ecefVX |= buffer[35];
    ecefVX <<= 8;
    ecefVX |= buffer[34];

    ecefVY = buffer[41] << 8;
    ecefVY |= buffer[40];
    ecefVY <<= 8;
    ecefVY |= buffer[39];
    ecefVY <<= 8;
    ecefVY |= buffer[38];

    ecefVZ = buffer[45] << 8;
    ecefVZ |= buffer[44];
    ecefVZ <<= 8;
    ecefVZ |= buffer[43];
    ecefVZ <<= 8;
    ecefVZ |= buffer[42];

    sAcc = buffer[49] << 8;
    sAcc |= buffer[48];
    sAcc <<= 8;
    sAcc |= buffer[47];
    sAcc <<= 8;
    sAcc |= buffer[46];

    pDOP = buffer[51] << 8;
    pDOP |= buffer[50];

    reserved1 = buffer[52];

    numSV = buffer[53];

    reserved2 = buffer[57] << 8;
    reserved2 |= buffer[56];
    reserved2 <<= 8;
    reserved2 |= buffer[55];
    reserved2 <<= 8;
    reserved2 |= 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
    0x62, // sync char 2
    0x06, // class
    0x01, // id
    0x08, // length
    0x00, // length
    0x01, // payload
    0x06, // payload
    0x00, // payload
    0x00, // payload
    0x01, // payload
    0x00, // payload
    0x00, // payload
    0x00, // payload
    0x17, // CK_A
    0xD9, // CK_B
  };
  if(this->writable()){
    this->write(buffer, 16);
    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
    0x14, // length
    0x00, // length
    0x01, // payload
    0x00, // payload
    0x00, // payload
    0x00, // payload
    0xD0, // payload
    0x08, // payload
    0x00, // payload
    0x00, // payload
    0x00, // payload
    0xC2, // payload
    0x01, // payload
    0x00, // payload
    0x07, // payload
    0x00, // payload
    0x03, // payload
    0x00, // payload
    0x00, // payload
    0x00, // payload
    0x00, // payload
    0x00, // payload
    0xC0, // CK_A
    0x7E, // 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);
}