eive-obsw/mission/devices/SyrlinksHkHandler.cpp

#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/CRC.h>
#include <mission/devices/SyrlinksHkHandler.h>

#include "OBSWConfig.h"

SyrlinksHkHandler::SyrlinksHkHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie)
    : DeviceHandlerBase(objectId, comIF, comCookie), rxDataset(this), txDataset(this) {
  if (comCookie == NULL) {
    sif::error << "SyrlinksHkHandler: Invalid com cookie" << std::endl;
  }
}

SyrlinksHkHandler::~SyrlinksHkHandler() {}

void SyrlinksHkHandler::doStartUp() {
  if (mode == _MODE_START_UP) {
    setMode(MODE_ON);
  }
}

void SyrlinksHkHandler::doShutDown() { setMode(_MODE_POWER_DOWN); }

ReturnValue_t SyrlinksHkHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  switch (nextCommand) {
    case (SYRLINKS::READ_RX_STATUS_REGISTERS):
      *id = SYRLINKS::READ_RX_STATUS_REGISTERS;
      nextCommand = SYRLINKS::READ_TX_STATUS;
      break;
    case (SYRLINKS::READ_TX_STATUS):
      *id = SYRLINKS::READ_TX_STATUS;
      nextCommand = SYRLINKS::READ_TX_WAVEFORM;
      break;
    case (SYRLINKS::READ_TX_WAVEFORM):
      *id = SYRLINKS::READ_TX_WAVEFORM;
      nextCommand = SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE;
      break;
    case (SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE):
      *id = SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE;
      nextCommand = SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE;
      break;
    case (SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE):
      *id = SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE;
      nextCommand = SYRLINKS::READ_RX_STATUS_REGISTERS;
      break;
    default:
      sif::debug << "SyrlinksHkHandler::buildNormalDeviceCommand: rememberCommandId has invalid"
                 << "command id" << std::endl;
      break;
  }
  return buildCommandFromCommand(*id, NULL, 0);
}

ReturnValue_t SyrlinksHkHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t SyrlinksHkHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                         const uint8_t* commandData,
                                                         size_t commandDataLen) {
  switch (deviceCommand) {
    case (SYRLINKS::RESET_UNIT): {
      resetCommand.copy(reinterpret_cast<char*>(commandBuffer), resetCommand.size(), 0);
      rawPacketLen = resetCommand.size();
      rawPacket = commandBuffer;
      return RETURN_OK;
    }
    case (SYRLINKS::SET_TX_MODE_STANDBY): {
      setTxModeStandby.copy(reinterpret_cast<char*>(commandBuffer), setTxModeStandby.size(), 0);
      rawPacketLen = setTxModeStandby.size();
      rawPacket = commandBuffer;
      return RETURN_OK;
    }
    case (SYRLINKS::SET_TX_MODE_MODULATION): {
      setTxModeModulation.copy(reinterpret_cast<char*>(commandBuffer), setTxModeModulation.size(),
                               0);
      rawPacketLen = setTxModeModulation.size();
      rawPacket = commandBuffer;
      return RETURN_OK;
    }
    case (SYRLINKS::SET_TX_MODE_CW): {
      setTxModeCw.copy(reinterpret_cast<char*>(commandBuffer), setTxModeCw.size(), 0);
      rawPacketLen = setTxModeCw.size();
      rawPacket = commandBuffer;
      return RETURN_OK;
    }
    case (SYRLINKS::READ_RX_STATUS_REGISTERS): {
      readRxStatusRegCommand.copy(reinterpret_cast<char*>(commandBuffer),
                                  readRxStatusRegCommand.size(), 0);
      rawPacketLen = readRxStatusRegCommand.size();
      rawPacket = commandBuffer;
      return RETURN_OK;
    }
    case (SYRLINKS::READ_TX_STATUS): {
      readTxStatus.copy(reinterpret_cast<char*>(commandBuffer), readTxStatus.size(), 0);
      rawPacketLen = readTxStatus.size();
      rememberCommandId = SYRLINKS::READ_TX_STATUS;
      rawPacket = commandBuffer;
      return RETURN_OK;
    }
    case (SYRLINKS::READ_TX_WAVEFORM): {
      readTxWaveform.copy(reinterpret_cast<char*>(commandBuffer), readTxStatus.size(), 0);
      rawPacketLen = readTxWaveform.size();
      rememberCommandId = SYRLINKS::READ_TX_WAVEFORM;
      rawPacket = commandBuffer;
      return RETURN_OK;
    }
    case (SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE): {
      readTxAgcValueHighByte.copy(reinterpret_cast<char*>(commandBuffer), readTxStatus.size(), 0);
      rawPacketLen = readTxAgcValueHighByte.size();
      rememberCommandId = SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE;
      rawPacket = commandBuffer;
      return RETURN_OK;
    }
    case (SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE): {
      readTxAgcValueLowByte.copy(reinterpret_cast<char*>(commandBuffer), readTxStatus.size(), 0);
      rawPacketLen = readTxAgcValueLowByte.size();
      rememberCommandId = SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE;
      rawPacket = commandBuffer;
      return RETURN_OK;
    }
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return HasReturnvaluesIF::RETURN_FAILED;
}

void SyrlinksHkHandler::fillCommandAndReplyMap() {
  this->insertInCommandAndReplyMap(SYRLINKS::RESET_UNIT, 1, nullptr, SYRLINKS::ACK_SIZE, false,
                                   true, SYRLINKS::ACK_REPLY);
  this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_STANDBY, 1, nullptr, SYRLINKS::ACK_SIZE,
                                   false, true, SYRLINKS::ACK_REPLY);
  this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_MODULATION, 1, nullptr, SYRLINKS::ACK_SIZE,
                                   false, true, SYRLINKS::ACK_REPLY);
  this->insertInCommandAndReplyMap(SYRLINKS::SET_TX_MODE_CW, 1, nullptr, SYRLINKS::ACK_SIZE, false,
                                   true, SYRLINKS::ACK_REPLY);
  this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_STATUS, 1, &txDataset,
                                   SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_WAVEFORM, 1, &txDataset,
                                   SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE, 1, &txDataset,
                                   SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE, 1, &txDataset,
                                   SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
  this->insertInCommandAndReplyMap(SYRLINKS::READ_RX_STATUS_REGISTERS, 1, &rxDataset,
                                   SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE);
}

ReturnValue_t SyrlinksHkHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                              DeviceCommandId_t* foundId, size_t* foundLen) {
  ReturnValue_t result = RETURN_OK;

  if (*start != '<') {
    sif::error << "SyrlinksHkHandler::scanForReply: Missing start frame character" << std::endl;
    return MISSING_START_FRAME_CHARACTER;
  }

  switch (*(start + 1)) {
    case ('A'):
      *foundLen = SYRLINKS::ACK_SIZE;
      *foundId = SYRLINKS::ACK_REPLY;
      break;
    case ('E'):
      *foundLen = SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE;
      *foundId = SYRLINKS::READ_RX_STATUS_REGISTERS;
      break;
    case ('R'):
      *foundId = rememberCommandId;
      *foundLen = SYRLINKS::READ_ONE_REGISTER_REPLY_SIE;
      break;
    default:
      sif::error << "SyrlinksHkHandler::scanForReply: Unknown reply identifier" << std::endl;
      result = IGNORE_REPLY_DATA;
      break;
  }

  return result;
}

ReturnValue_t SyrlinksHkHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
  ReturnValue_t result;

  switch (id) {
    case (SYRLINKS::ACK_REPLY):
      result = verifyReply(packet, SYRLINKS::ACK_SIZE);
      if (result != RETURN_OK) {
        sif::error << "SyrlinksHkHandler::interpretDeviceReply: Acknowledgement reply has "
                      "invalid crc"
                   << std::endl;
        return CRC_FAILURE;
      }
      result =
          parseReplyStatus(reinterpret_cast<const char*>(packet + SYRLINKS::MESSAGE_HEADER_SIZE));
      if (result != RETURN_OK) {
        return result;
      }
      break;
    case (SYRLINKS::READ_RX_STATUS_REGISTERS):
      result = verifyReply(packet, SYRLINKS::RX_STATUS_REGISTERS_REPLY_SIZE);
      if (result != RETURN_OK) {
        sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read rx status registers reply "
                   << "has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      parseRxStatusRegistersReply(packet);
      break;
    case (SYRLINKS::READ_TX_STATUS):
      result = verifyReply(packet, SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
      if (result != RETURN_OK) {
        sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx status reply "
                   << "has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      parseTxStatusReply(packet);
      break;
    case (SYRLINKS::READ_TX_WAVEFORM):
      result = verifyReply(packet, SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
      if (result != RETURN_OK) {
        sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx waveform reply "
                   << "has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      parseTxWaveformReply(packet);
      break;
    case (SYRLINKS::READ_TX_AGC_VALUE_HIGH_BYTE):
      result = verifyReply(packet, SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
      if (result != RETURN_OK) {
        sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx AGC high byte reply "
                   << "has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      parseAgcHighByte(packet);
      break;
    case (SYRLINKS::READ_TX_AGC_VALUE_LOW_BYTE):
      result = verifyReply(packet, SYRLINKS::READ_ONE_REGISTER_REPLY_SIE);
      if (result != RETURN_OK) {
        sif::error << "SyrlinksHkHandler::interpretDeviceReply: Read tx AGC low byte reply "
                   << "has invalid crc" << std::endl;
        return CRC_FAILURE;
      }
      parseAgcLowByte(packet);
      break;
    default: {
      sif::debug << "SyrlinksHkHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }

  return RETURN_OK;
}

LocalPoolDataSetBase* SyrlinksHkHandler::getDataSetHandle(sid_t sid) {
  if (sid == rxDataset.getSid()) {
    return &rxDataset;
  } else if (sid == txDataset.getSid()) {
    return &txDataset;
  } else {
    sif::error << "SyrlinksHkHandler::getDataSetHandle: Invalid sid" << std::endl;
    return nullptr;
  }
}

std::string SyrlinksHkHandler::convertUint16ToHexString(uint16_t intValue) {
  std::stringstream stream;
  stream << std::setfill('0') << std::setw(4) << std::hex << std::uppercase << intValue;
  return stream.str();
}

uint8_t SyrlinksHkHandler::convertHexStringToUint8(const char* twoChars) {
  uint32_t value;
  std::string hexString(twoChars, 2);
  std::stringstream stream;
  stream << std::hex << hexString;
  stream >> value;
  return static_cast<uint8_t>(value);
}

uint16_t SyrlinksHkHandler::convertHexStringToUint16(const char* fourChars) {
  uint16_t value = 0;
  value = convertHexStringToUint8(fourChars) << 8 | convertHexStringToUint8(fourChars + 2);
  return value;
}

uint32_t SyrlinksHkHandler::convertHexStringToUint32(const char* characters,
                                                     uint8_t numberOfChars) {
  uint32_t value = 0;

  switch (numberOfChars) {
    case 6:
      value = convertHexStringToUint8(characters) << 16 |
              convertHexStringToUint8(characters + 2) << 8 |
              convertHexStringToUint8(characters + 4);
      return value;
    case 8:
      value = convertHexStringToUint8(characters) << 24 |
              convertHexStringToUint8(characters + 2) << 16 |
              convertHexStringToUint8(characters + 4) << 8 |
              convertHexStringToUint8(characters + 4);
      return value;
    default:
      sif::debug << "SyrlinksHkHandler::convertHexStringToUint32: Invalid number of characters. "
                 << "Must be either 6 or 8" << std::endl;
      return 0;
  }
}

ReturnValue_t SyrlinksHkHandler::parseReplyStatus(const char* status) {
  switch (*status) {
    case '0':
      return RETURN_OK;
    case '1':
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Uart faming or parity error" << std::endl;
      return UART_FRAMIN_OR_PARITY_ERROR_ACK;
    case '2':
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad character detected" << std::endl;
      return BAD_CHARACTER_ACK;
    case '3':
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad parameter value (unexpected value "
                 << "detected" << std::endl;
      return BAD_PARAMETER_VALUE_ACK;
    case '4':
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad end of frame" << std::endl;
      return BAD_END_OF_FRAME_ACK;
    case '5':
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Unknown command id or attempt to access"
                 << " a protected register" << std::endl;
      return UNKNOWN_COMMAND_ID_ACK;
    case '6':
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Bad CRC" << std::endl;
      return BAD_CRC_ACK;
    default:
      sif::debug << "SyrlinksHkHandler::parseReplyStatus: Status reply contains an invalid "
                 << "status id" << std::endl;
      return RETURN_FAILED;
  }
}

ReturnValue_t SyrlinksHkHandler::verifyReply(const uint8_t* packet, uint8_t size) {
  int result = 0;
  /* Calculate crc from received packet */
  uint16_t crc =
      CRC::crc16ccitt(packet, size - SYRLINKS::SIZE_CRC_AND_TERMINATION, CRC_INITIAL_VALUE);
  std::string recalculatedCrc = convertUint16ToHexString(crc);

  const char* startOfCrc =
      reinterpret_cast<const char*>(packet + size - SYRLINKS::SIZE_CRC_AND_TERMINATION);
  const char* endOfCrc = reinterpret_cast<const char*>(packet + size - 1);

  std::string replyCrc(startOfCrc, endOfCrc);

  result = recalculatedCrc.compare(replyCrc);
  if (result != 0) {
    return RETURN_FAILED;
  }
  return RETURN_OK;
}

void SyrlinksHkHandler::parseRxStatusRegistersReply(const uint8_t* packet) {
  PoolReadGuard readHelper(&rxDataset);
  uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
  rxDataset.rxStatus = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
  offset += 2;
  rxDataset.rxSensitivity =
      convertHexStringToUint32(reinterpret_cast<const char*>(packet + offset), 6);
  offset += 6;
  rxDataset.rxFrequencyShift =
      convertHexStringToUint32(reinterpret_cast<const char*>(packet + offset), 6);
  offset += 6;
  rxDataset.rxIqPower = convertHexStringToUint16(reinterpret_cast<const char*>(packet + offset));
  offset += 4;
  rxDataset.rxAgcValue = convertHexStringToUint16(reinterpret_cast<const char*>(packet + offset));
  offset += 4;
  offset += 2;  // reserved register
  rxDataset.rxDemodEb = convertHexStringToUint32(reinterpret_cast<const char*>(packet + offset), 6);
  offset += 6;
  rxDataset.rxDemodN0 = convertHexStringToUint32(reinterpret_cast<const char*>(packet + offset), 6);
  offset += 6;
  rxDataset.rxDataRate = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));

#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_SYRLINKS == 1
  sif::info << "Syrlinks RX Status: 0x" << std::hex << (unsigned int)rxDataset.rxStatus.value
            << std::endl;
  sif::info << "Syrlinks RX Sensitivity: " << std::dec << rxDataset.rxSensitivity << std::endl;
  sif::info << "Syrlinks RX Frequency Shift: " << rxDataset.rxFrequencyShift << std::endl;
  sif::info << "Syrlinks RX IQ Power: " << rxDataset.rxIqPower << std::endl;
  sif::info << "Syrlinks RX AGC Value: " << rxDataset.rxAgcValue << std::endl;
  sif::info << "Syrlinks RX Demod Eb: " << rxDataset.rxDemodEb << std::endl;
  sif::info << "Syrlinks RX Demod N0: " << rxDataset.rxDemodN0 << std::endl;
  sif::info << "Syrlinks RX Datarate: " << (unsigned int)rxDataset.rxDataRate.value << std::endl;
#endif
}

void SyrlinksHkHandler::parseTxStatusReply(const uint8_t* packet) {
  PoolReadGuard readHelper(&txDataset);
  uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
  txDataset.txStatus = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_SYRLINKS == 1
  sif::info << "Syrlinks TX Status: 0x" << std::hex << (unsigned int)txDataset.txStatus.value
            << std::endl;
#endif
}

void SyrlinksHkHandler::parseTxWaveformReply(const uint8_t* packet) {
  PoolReadGuard readHelper(&txDataset);
  uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
  txDataset.txWaveform = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_SYRLINKS == 1
  sif::info << "Syrlinks TX Waveform: 0x" << std::hex << (unsigned int)txDataset.txWaveform.value
            << std::endl;
#endif
}

void SyrlinksHkHandler::parseAgcLowByte(const uint8_t* packet) {
  PoolReadGuard readHelper(&txDataset);
  uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
  txDataset.txAgcValue = agcValueHighByte << 8 |
                         convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_SYRLINKS == 1
  sif::info << "Syrlinks TX AGC Value: " << txDataset.txAgcValue << std::endl;
#endif
}

void SyrlinksHkHandler::parseAgcHighByte(const uint8_t* packet) {
  PoolReadGuard readHelper(&txDataset);
  uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE;
  agcValueHighByte = convertHexStringToUint8(reinterpret_cast<const char*>(packet + offset));
}

void SyrlinksHkHandler::setNormalDatapoolEntriesInvalid() {}

uint32_t SyrlinksHkHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 500; }

ReturnValue_t SyrlinksHkHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                         LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(SYRLINKS::RX_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(SYRLINKS::RX_SENSITIVITY, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(SYRLINKS::RX_FREQUENCY_SHIFT, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(SYRLINKS::RX_IQ_POWER, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(SYRLINKS::RX_AGC_VALUE, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(SYRLINKS::RX_DEMOD_EB, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(SYRLINKS::RX_DEMOD_N0, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(SYRLINKS::RX_DATA_RATE, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(SYRLINKS::TX_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(SYRLINKS::TX_WAVEFORM, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(SYRLINKS::TX_AGC_VALUE, new PoolEntry<uint16_t>({0}));

  return HasReturnvaluesIF::RETURN_OK;
}

void SyrlinksHkHandler::setModeNormal() { mode = MODE_NORMAL; }