File content as of revision 83:f0d1d948c306:

#include "PNTSerial.h"
#include "LTCApp.h"


PNTSerial::PNTSerial(const PinName Tx, const PinName Rx) : PosSource(Tx, Rx) {
  originSet = false;
}

void PNTSerial::setOrigin(double Latitude, double Longitude, float altitude) {
  origin.SetDecimalDegrees(Latitude, Longitude, altitude);
  dataPoint.SetReferancePosition(&origin);
  originSet = true;
}

void PNTSerial::run(void) {
  _port.attach(callback(this, &PNTSerial::onSerialRx));
}

void PNTSerial::onSerialRx(void) {
  while (_port.readable()) {
    if (BypassMode) {
      vipsBypassRx(_port.getc());
    } else {
      uint8_t charIn = _port.getc();
      if (messagePrt == 0) { // wait for start character
        if (charIn == '$') {
          messageInBuffer[messagePrt++] = charIn;
        }
      } else {
        messageInBuffer[messagePrt++] = charIn;
        // basic sanity check that second byte is expected value.
        if ((messagePrt==2) && (messageInBuffer[1] != 'R')) 
            messagePrt = 0;
        // got the full message 
        else if (messagePrt == PNTSerialConstants::MessageLength) {
          parsePostion();
          messagePrt = 0;
        }
      }
    }
  }
}

bool PNTSerial::CheckValidMessage() {
  // check length
  if (messagePrt != PNTSerialConstants::MessageLength)
    return false;

  // check header
  if (messageInBuffer[0] != '$')
    return false;
  if (messageInBuffer[1] != 'R')
    return false;
  if (messageInBuffer[2] != 'L')
    return false;
  if (messageInBuffer[3] != 'P')
    return false;
  if (messageInBuffer[4] != 'N')
    return false;
  if (messageInBuffer[5] != 'T')
    return false;
  if (messageInBuffer[6] != '$')
    return false;
  if (messageInBuffer[7] != ',')
    return false;

  // check CRC
  uint16_t Polynomial = 4129;
  uint16_t CRC = 0;
  for (int i = 0; i < PNTSerialConstants::MessageLength - 2; i++) {
    int tmp = messageInBuffer[i];
    CRC ^= tmp << 8;
    for (int bit = 8; bit > 0; bit--) {
      if (CRC & 0x8000) {
        CRC <<= 1;
        CRC ^= Polynomial;
      } else {
        CRC <<= 1;
      }
    }
    CRC &= 0xffff;
  }
  uint16_t messageCRC = ((uint16_t)messageInBuffer[PNTSerialConstants::MessageLength - 2]) << 8 |
                        messageInBuffer[PNTSerialConstants::MessageLength - 1];
  return messageCRC == CRC;
}

uint32_t PNTSerial::readUInt32(int &start, int bytes) {
  uint32_t value = 0;
  do {
    value <<= 8;
    value |= messageInBuffer[start++];
  } while (--bytes);
  return value;
}

int32_t PNTSerial::readInt32(int &start, int bytes) {
  int32_t value = 0;
  if (messageInBuffer[start] & 0x80) // negative value
    value = -1;                      // set to all 1's

  do {
    value <<= 8;
    value |= messageInBuffer[start++];
  } while (--bytes);
  return value;
}

int64_t PNTSerial::readInt64(int &start, int bytes) {
  int64_t value = 0;
  if (messageInBuffer[start] & 0x80) // negative value
    value = -1;                      // set to all 1's
  do {
    value <<= 8;
    value |= messageInBuffer[start++];
  } while (--bytes);

  return value;
}

void PNTSerial::parsePostion() {
  int messageIndex = 0;
  lastPositions[nextPosition].time = TimeSinceLastFrame.read_us();
  if (!CheckValidMessage())
    return;
  // 8 byte header ("$RLPNT$,")
  messageIndex = 8;
  // index 8 = 1 byte sat count
  lastPositions[nextPosition].pos.beacons = messageInBuffer[messageIndex++];

  // 3 bytes time since midnight (10ms per tick)
  lastPositions[nextPosition].pos.time = readUInt32(messageIndex, 3) * 10;

  // 5 byte lattitude in 0.000000001 degrees steps (9 decimal places)
  int64_t latInt = readInt64(messageIndex, 5);
  // 5 byte longitude in 0.000000001 degrees steps (9 decimal places)
  int64_t lonInt = readInt64(messageIndex, 5);

  // 3 byte velocity
  messageIndex += 3;
  // 2 byte heading
  messageIndex += 2;
  // 3 byte altitude
  int32_t altInt = readInt32(messageIndex, 3);
  if (originSet) {
    dataPoint.SetDecimalDegrees(latInt * 0.000000001, lonInt * 0.000000001,
                                altInt * 0.01);
    double tmpDouble;
    dataPoint.GetENU(&(lastPositions[nextPosition].pos.X),
                     &(lastPositions[nextPosition].pos.Y), &tmpDouble);
    lastPositions[nextPosition].pos.Height = (float)tmpDouble;
    lastPositions[nextPosition].pos.LLAPosition = false;
  } else {
    lastPositions[nextPosition].pos.X = latInt * 0.000000001;
    lastPositions[nextPosition].pos.Y = lonInt * 0.000000001;
    lastPositions[nextPosition].pos.Height = altInt * 0.01f;
    lastPositions[nextPosition].pos.LLAPosition = true;
  }

  // 3 byte vert velocity
  messageIndex += 3;

  // 1 byte solution type
  lastPositions[nextPosition].pos.solutionType =
      messageInBuffer[messageIndex++];

  // 2 byte pitch
  lastPositions[nextPosition].pos.pitch = readInt32(messageIndex, 2) * 0.01;
  // 2 byte roll
  lastPositions[nextPosition].pos.roll = readInt32(messageIndex, 2) * 0.01;
  // 2 byte yaw
  lastPositions[nextPosition].pos.yaw = readInt32(messageIndex, 2) * 0.01;

  // 2 byte pitch, roll, yaw rates
  messageIndex += 2*3;

  // 2 byte X,Y,Z accelerations
  messageIndex += 2*3;

  // 2 byte KF status
  lastPositions[nextPosition].pos.KFStatus = (uint16_t)readUInt32(messageIndex, 2);

  // 3 byte wheel speed
  messageIndex += 3;
  // 1 byte spoof/jam mask
  messageIndex++;
  // 1 byte spoof/jam indicator
  messageIndex++;
  // 2 byte date
  messageIndex += 2;

  if (UserSettings.AutoHyperSmooth) {
    int testValue = (lastPositions[nextPosition].pos.KFStatus & 0xE634);

    if (((testValue & 0x400) == 0x400) && (!hyperSmoothEnabled)) {
      EnableSmoothing(true);
      // pc.write("Auto HS On\r\n", 12);
    } else if (((testValue & 0x400) != 0x400) && (hyperSmoothEnabled) &&
               (!forcedHyperSmooth)) {
      EnableSmoothing(false);
      // pc.write("Auto HS Off\r\n", 13);
    } // Auto Hypersmooth
  }

  smoothOutputPacket(&(lastPositions[nextPosition].pos));

  if (enableAllUpdates) {
    //        printf("Add pos\r\n");
    outputPtr = &outputPosition;
    memcpy(outputPtr, &(lastPositions[nextPosition].pos), sizeof(position));
  }

  nextPosition++;
  if (nextPosition == posHistoryLen) {
    nextPosition = 0;
  }
  pointCount++;
}