File content as of revision 0:fb7bf6d81e5f:

#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;
}