eive-obsw/mission/devices/RwHandler.cpp

#include "RwHandler.h"

#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw_hal/common/gpio/GpioIF.h>

#include "OBSWConfig.h"

RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
                     GpioIF* gpioComIF, gpioId_t enableGpio, uint8_t rwIdx)
    : DeviceHandlerBase(objectId, comIF, comCookie),
      gpioComIF(gpioComIF),
      enableGpio(enableGpio),
      statusSet(this),
      lastResetStatusSet(this),
      tmDataset(this),
      rwSpeedActuationSet(*this),
      rwIdx(rwIdx) {
  if (comCookie == nullptr) {
    sif::error << "RwHandler: Invalid com cookie" << std::endl;
  }
  if (gpioComIF == nullptr) {
    sif::error << "RwHandler: Invalid gpio communication interface" << std::endl;
  }
}

RwHandler::~RwHandler() {}

void RwHandler::doStartUp() {
  internalState = InternalState::DEFAULT;

  if (gpioComIF->pullHigh(enableGpio) != returnvalue::OK) {
    sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high";
  }
  updatePeriodicReply(true, rws::REPLY_ID);
  setMode(_MODE_TO_ON);
}

void RwHandler::doShutDown() {
  if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) {
    sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low";
  }
  internalState = InternalState::DEFAULT;
  updatePeriodicReply(false, rws::REPLY_ID);
  setMode(_MODE_POWER_DOWN);
}

ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
  switch (internalState) {
    case InternalState::DEFAULT: {
      *id = rws::REQUEST_ID;
      break;
    }
      //    case InternalState::SET_SPEED:
      //      *id = rws::SET_SPEED;
      //      internalState = InternalState::GET_RESET_STATUS;
      //      break;
      //    case InternalState::GET_RESET_STATUS:
      //      *id = rws::GET_LAST_RESET_STATUS;
      //      internalState = InternalState::READ_TEMPERATURE;
      //      break;
      //    case InternalState::READ_TEMPERATURE:
      //      *id = rws::GET_TEMPERATURE;
      //      internalState = InternalState::GET_RW_SATUS;
      //      break;
      //    case InternalState::GET_RW_SATUS:
      //      *id = rws::GET_RW_STATUS;
      //      internalState = InternalState::CLEAR_RESET_STATUS;
      //      break;
      //    case InternalState::CLEAR_RESET_STATUS:
      //      *id = rws::CLEAR_LAST_RESET_STATUS;
      //      /** After reset status is cleared, reset status will be polled again for verification
      //      */ internalState = InternalState::GET_RESET_STATUS; break;
    default:
      sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid internal step" << std::endl;
      break;
  }
  return buildCommandFromCommand(*id, NULL, 0);
}

ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
  return NOTHING_TO_SEND;
}

ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                 const uint8_t* commandData,
                                                 size_t commandDataLen) {
  ReturnValue_t result = returnvalue::OK;

  switch (deviceCommand) {
    case (rws::REQUEST_ID): {
      if (commandData != nullptr && commandDataLen != 6) {
        sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
                   << " invalid length" << std::endl;
        return SET_SPEED_COMMAND_INVALID_LENGTH;
      }

      {
        PoolReadGuard pg(&rwSpeedActuationSet);
        // Commands override anything which was set in the software
        if (commandData != nullptr) {
          rwSpeedActuationSet.setValidityBufferGeneration(false);
          result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen,
                                                   SerializeIF::Endianness::NETWORK);
          rwSpeedActuationSet.setValidityBufferGeneration(true);
          if (result != returnvalue::OK) {
            return result;
          }
        }
      }
      if (ACTUATION_WIRETAPPING) {
        int32_t speed = 0;
        uint16_t rampTime = 0;
        rwSpeedActuationSet.getRwSpeed(speed, rampTime);
        sif::debug << "Actuating RW " << static_cast<int>(rwIdx) << " with speed = " << speed
                   << " and rampTime = " << rampTime << std::endl;
      }
      result = checkSpeedAndRampTime();
      if (result != returnvalue::OK) {
        return result;
      }
      // set speed flag.
      commandBuffer[0] = true;
      rawPacketLen = 1;
      uint8_t* currentCmdBuf = commandBuffer + 1;
      rwSpeedActuationSet.serialize(&currentCmdBuf, &rawPacketLen, sizeof(commandBuffer),
                                    SerializeIF::Endianness::MACHINE);
      commandBuffer[rawPacketLen] = static_cast<uint8_t>(rws::SpecialRwRequest::REQUEST_NONE);
      break;
    }
    case (rws::RESET_MCU): {
      commandBuffer[0] = false;
      commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::RESET_MCU);
      internalState = InternalState::RESET_MCU;
      return returnvalue::OK;
    }

    case (rws::INIT_RW_CONTROLLER): {
      commandBuffer[0] = false;
      commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::INIT_RW_CONTROLLER);
      internalState = InternalState::INIT_RW_CONTROLLER;
      return returnvalue::OK;
    }
    case (rws::GET_TM): {
      commandBuffer[0] = false;
      commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::GET_TM);
      internalState = InternalState::GET_TM;
      return returnvalue::OK;
    }

      //    case (rws::GET_LAST_RESET_STATUS): {
      //      prepareSimpleCommand(deviceCommand);
      //      return returnvalue::OK;
      //    }
      //    case (rws::CLEAR_LAST_RESET_STATUS): {
      //      prepareSimpleCommand(deviceCommand);
      //      return returnvalue::OK;
      //    }
      //    case (rws::GET_RW_STATUS): {
      //      prepareSimpleCommand(deviceCommand);
      //      return returnvalue::OK;
      //    }
      //    case (rws::INIT_RW_CONTROLLER): {
      //      prepareSimpleCommand(deviceCommand);
      //      return returnvalue::OK;
      //    }
      //    case (rws::SET_SPEED): {
      //      if (commandData != nullptr && commandDataLen != 6) {
      //        sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
      //                   << " invalid length" << std::endl;
      //        return SET_SPEED_COMMAND_INVALID_LENGTH;
      //      }
      //
      //      {
      //        PoolReadGuard pg(&rwSpeedActuationSet);
      //        // Commands override anything which was set in the software
      //        if (commandData != nullptr) {
      //          rwSpeedActuationSet.setValidityBufferGeneration(false);
      //          result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen,
      //                                                   SerializeIF::Endianness::NETWORK);
      //          rwSpeedActuationSet.setValidityBufferGeneration(true);
      //          if (result != returnvalue::OK) {
      //            return result;
      //          }
      //        }
      //      }
      //      if (ACTUATION_WIRETAPPING) {
      //        int32_t speed = 0;
      //        uint16_t rampTime = 0;
      //        rwSpeedActuationSet.getRwSpeed(speed, rampTime);
      //        sif::debug << "Actuating RW " << static_cast<int>(rwIdx) << " with speed = " <<
      //        speed
      //                   << " and rampTime = " << rampTime << std::endl;
      //      }
      //      result = checkSpeedAndRampTime();
      //      if (result != returnvalue::OK) {
      //        return result;
      //      }
      //      result = prepareSetSpeedCmd();
      //      return result;
      //    }
      //    case (rws::GET_TEMPERATURE): {
      //      prepareSimpleCommand(deviceCommand);
      //      return returnvalue::OK;
      //    }
      //    case (rws::GET_TM): {
      //      prepareSimpleCommand(deviceCommand);
      //      return returnvalue::OK;
      //    }
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return returnvalue::FAILED;
}

void RwHandler::fillCommandAndReplyMap() {
  insertInCommandMap(rws::REQUEST_ID);
  insertInReplyMap(rws::REPLY_ID, 5, nullptr, 0, true);

  insertInCommandMap(rws::RESET_MCU);
  insertInCommandAndReplyMap(rws::INIT_RW_CONTROLLER, 5, nullptr, 0, false, true, rws::REPLY_ID);
  insertInCommandAndReplyMap(rws::GET_TM, 5, nullptr, 0, false, true, rws::REPLY_ID);
  /*
  this->insertInCommandAndReplyMap(rws::GET_LAST_RESET_STATUS, 1, &lastResetStatusSet,
                                   rws::SIZE_GET_RESET_STATUS);
  this->insertInCommandAndReplyMap(rws::CLEAR_LAST_RESET_STATUS, 1, nullptr,
                                   rws::SIZE_CLEAR_RESET_STATUS);
  this->insertInCommandAndReplyMap(rws::GET_RW_STATUS, 1, &statusSet, rws::SIZE_GET_RW_STATUS);
  this->insertInCommandAndReplyMap(rws::INIT_RW_CONTROLLER, 1, nullptr, rws::SIZE_INIT_RW);
  this->insertInCommandAndReplyMap(rws::GET_TEMPERATURE, 1, nullptr,
                                   rws::SIZE_GET_TEMPERATURE_REPLY);
  this->insertInCommandAndReplyMap(rws::SET_SPEED, 1, nullptr, rws::SIZE_SET_SPEED_REPLY);
  this->insertInCommandAndReplyMap(rws::GET_TM, 1, &tmDataset, rws::SIZE_GET_TELEMETRY_REPLY);
  */
}

ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                      DeviceCommandId_t* foundId, size_t* foundLen) {
  // uint8_t replyByte = *start;
  if (remainingSize > 0) {
    *foundLen = remainingSize;
    *foundId = rws::REPLY_ID;
  }
  // RwReplies replies(start);
  //  switch (replyByte) {
  //    case (rws::GET_LAST_RESET_STATUS): {
  //      *foundLen = rws::SIZE_GET_RESET_STATUS;
  //      *foundId = rws::GET_LAST_RESET_STATUS;
  //      break;
  //    }
  //    case (rws::CLEAR_LAST_RESET_STATUS): {
  //      *foundLen = rws::SIZE_CLEAR_RESET_STATUS;
  //      *foundId = rws::CLEAR_LAST_RESET_STATUS;
  //      break;
  //    }
  //    case (rws::GET_RW_STATUS): {
  //      *foundLen = rws::SIZE_GET_RW_STATUS;
  //      *foundId = rws::GET_RW_STATUS;
  //      break;
  //    }
  //    case (rws::INIT_RW_CONTROLLER): {
  //      *foundLen = rws::SIZE_INIT_RW;
  //      *foundId = rws::INIT_RW_CONTROLLER;
  //      break;
  //    }
  //    case (rws::SET_SPEED): {
  //      *foundLen = rws::SIZE_SET_SPEED_REPLY;
  //      *foundId = rws::SET_SPEED;
  //      break;
  //    }
  //    case (rws::GET_TEMPERATURE): {
  //      *foundLen = rws::SIZE_GET_TEMPERATURE_REPLY;
  //      *foundId = rws::GET_TEMPERATURE;
  //      break;
  //    }
  //    case (rws::GET_TM): {
  //      *foundLen = rws::SIZE_GET_TELEMETRY_REPLY;
  //      *foundId = rws::GET_TM;
  //      break;
  //    }
  //    default: {
  //      sif::warning << "RwHandler::scanForReply: Reply contains invalid command code" <<
  //      std::endl; *foundLen = remainingSize; return returnvalue::FAILED;
  //    }
  //  }
  return returnvalue::OK;
}

ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
  RwReplies replies(packet);
  ReturnValue_t result = returnvalue::OK;
  auto checkPacket = [&](DeviceCommandId_t id, const uint8_t* packetPtr) {
    auto packetLen = rws::idToPacketLen(id);
    uint16_t replyCrc = (*(packet + packetLen - 1) << 8) | *(packet + packetLen - 2);
    if (CRC::crc16ccitt(packet, packetLen - 2, 0xFFFF) != replyCrc) {
      sif::error << "RwHandler::interpretDeviceReply: CRC error for ID " << id << std::endl;
      return CRC_ERROR;
    }
    if (packetPtr[1] == rws::STATE_ERROR) {
      sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: " << id
                 << std::endl;
      result = EXECUTION_FAILED;
    }
    return returnvalue::OK;
  };
  checkPacket(rws::DeviceCommandId::SET_SPEED, replies.getSetSpeedReply());
  if (returnvalue::OK ==
      checkPacket(rws::DeviceCommandId::GET_RW_STATUS, replies.getRwStatusReply())) {
    handleGetRwStatusReply(replies.getRwStatusReply());
  }
  if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_LAST_RESET_STATUS,
                                     replies.getGetLastResetStatusReply())) {
    handleResetStatusReply(replies.getGetLastResetStatusReply());
  }
  checkPacket(rws::DeviceCommandId::CLEAR_LAST_RESET_STATUS,
              replies.getClearLastResetStatusReply());
  if (returnvalue::OK ==
      checkPacket(rws::DeviceCommandId::GET_TEMPERATURE, replies.getReadTemperatureReply())) {
    handleTemperatureReply(replies.getReadTemperatureReply());
  }
  if (internalState == InternalState::GET_TM) {
    if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_TM, replies.getHkDataReply())) {
      handleGetTelemetryReply(replies.getHkDataReply());
    }
    internalState = InternalState::DEFAULT;
  }
  if (internalState == InternalState::INIT_RW_CONTROLLER) {
    checkPacket(rws::DeviceCommandId::INIT_RW_CONTROLLER, replies.getInitRwControllerReply());
    internalState = InternalState::DEFAULT;
  }
  if (internalState == InternalState::RESET_MCU) {
    internalState = InternalState::DEFAULT;
  }
  return returnvalue::OK;
}

uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }

ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
                                                 LocalDataPoolManager& poolManager) {
  localDataPoolMap.emplace(rws::RW_SPEED, &rwSpeed);
  localDataPoolMap.emplace(rws::RAMP_TIME, &rampTime);

  localDataPoolMap.emplace(rws::TEMPERATURE_C, new PoolEntry<int32_t>({0}));

  localDataPoolMap.emplace(rws::CURR_SPEED, new PoolEntry<int32_t>({0}));
  localDataPoolMap.emplace(rws::REFERENCE_SPEED, new PoolEntry<int32_t>({0}));
  localDataPoolMap.emplace(rws::STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(rws::CLC_MODE, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(rws::LAST_RESET_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(rws::CURRRENT_RESET_STATUS, new PoolEntry<uint8_t>({0}));

  localDataPoolMap.emplace(rws::TM_LAST_RESET_STATUS, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(rws::TM_MCU_TEMPERATURE, new PoolEntry<int32_t>({0}));
  localDataPoolMap.emplace(rws::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(rws::PRESSURE, new PoolEntry<float>({0}));
  localDataPoolMap.emplace(rws::TM_RW_STATE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(rws::TM_CLC_MODE, new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(rws::TM_RW_CURR_SPEED, new PoolEntry<int32_t>({0}));
  localDataPoolMap.emplace(rws::TM_RW_REF_SPEED, new PoolEntry<int32_t>({0}));
  localDataPoolMap.emplace(rws::INVALID_CRC_PACKETS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::INVALID_LEN_PACKETS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::INVALID_CMD_PACKETS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::EXECUTED_REPLIES, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::COMMAND_REPLIES, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::UART_BYTES_WRITTEN, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::UART_BYTES_READ, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::UART_PARITY_ERRORS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::UART_NOISE_ERRORS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::UART_FRAME_ERRORS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::UART_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::UART_TOTAL_ERRORS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::SPI_BYTES_WRITTEN, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::SPI_BYTES_READ, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::SPI_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>({0}));
  localDataPoolMap.emplace(rws::SPI_TOTAL_ERRORS, new PoolEntry<uint32_t>({0}));
  poolManager.subscribeForDiagPeriodicPacket(
      subdp::DiagnosticsHkPeriodicParams(statusSet.getSid(), false, 5.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(tmDataset.getSid(), false, 30.0));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(lastResetStatusSet.getSid(), false, 30.0));
  return returnvalue::OK;
}

/*
void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) {
  commandBuffer[0] = static_cast<uint8_t>(id);
  uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
  commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
  commandBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
  rawPacket = commandBuffer;
  rawPacketLen = 3;
}
*/

ReturnValue_t RwHandler::checkSpeedAndRampTime() {
  int32_t speed = 0;
  uint16_t rampTime = 0;
  rwSpeedActuationSet.getRwSpeed(speed, rampTime);
  if ((speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) && (speed != 0)) {
    sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl;
    return INVALID_SPEED;
  }

  if (rampTime < 10 || rampTime > 20000) {
    sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time" << std::endl;
    return INVALID_RAMP_TIME;
  }

  return returnvalue::OK;
}

/*
ReturnValue_t RwHandler::prepareSetSpeedCmd() {
  commandBuffer[0] = static_cast<uint8_t>(rws::SET_SPEED);
  uint8_t* serPtr = commandBuffer + 1;
  size_t serSize = 1;
  rwSpeedActuationSet.setValidityBufferGeneration(false);
  ReturnValue_t result = rwSpeedActuationSet.serialize(&serPtr, &serSize, sizeof(commandBuffer),
                                                       SerializeIF::Endianness::LITTLE);
  rwSpeedActuationSet.setValidityBufferGeneration(true);
  if (result != returnvalue::OK) {
    return result;
  }

  uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF);
  commandBuffer[7] = static_cast<uint8_t>(crc & 0xFF);
  commandBuffer[8] = static_cast<uint8_t>((crc >> 8) & 0xFF);
  rawPacket = commandBuffer;
  rawPacketLen = 9;
  return result;
}
*/

void RwHandler::handleResetStatusReply(const uint8_t* packet) {
  PoolReadGuard rg(&lastResetStatusSet);
  uint8_t offset = 2;
  uint8_t resetStatus = packet[offset];
  if (resetStatus != 0) {
    // internalState = InternalState::CLEAR_RESET_STATUS;
    lastResetStatusSet.lastNonClearedResetStatus = resetStatus;
    triggerEvent(rws::RESET_OCCURED, resetStatus, 0);
  }
  lastResetStatusSet.currentResetStatus = resetStatus;
  if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::info << "RwHandler::handleResetStatusReply: Last reset status: "
              << static_cast<unsigned int>(lastResetStatusSet.lastNonClearedResetStatus.value)
              << std::endl;
    sif::info << "RwHandler::handleResetStatusReply: Current reset status: "
              << static_cast<unsigned int>(lastResetStatusSet.currentResetStatus.value)
              << std::endl;
#endif
  }
}

void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
  PoolReadGuard rg0(&statusSet);
  PoolReadGuard rg1(&tmDataset);
  uint8_t offset = 2;
  statusSet.currSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                        *(packet + offset + 1) << 8 | *(packet + offset);
  tmDataset.rwCurrSpeed = statusSet.currSpeed;
  tmDataset.rwCurrSpeed.setValid(true);
  offset += 4;
  statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                             *(packet + offset + 1) << 8 | *(packet + offset);
  tmDataset.rwRefSpeed = statusSet.referenceSpeed;
  tmDataset.rwRefSpeed.setValid(true);
  offset += 4;
  statusSet.state = *(packet + offset);
  tmDataset.rwState = statusSet.state;
  tmDataset.rwState.setValid(true);
  offset += 1;
  statusSet.clcMode = *(packet + offset);
  tmDataset.rwClcMode = statusSet.clcMode;
  tmDataset.rwClcMode.setValid(true);

  statusSet.setValidity(true, true);

  if (statusSet.state == rws::STATE_ERROR) {
    // This requires the commanding of the init reaction wheel controller command to recover
    // from error state which must be handled by the FDIR instance.
    triggerEvent(rws::ERROR_STATE, statusSet.state.value, 0);
    sif::error << "RwHandler::handleGetRwStatusReply: Reaction wheel in error state" << std::endl;
  }

  if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::info << "RwHandler::handleGetRwStatusReply: Current speed is: " << statusSet.currSpeed
              << " * 0.1 RPM" << std::endl;
    sif::info << "RwHandler::handleGetRwStatusReply: Reference speed is: "
              << statusSet.referenceSpeed << " * 0.1 RPM" << std::endl;
    sif::info << "RwHandler::handleGetRwStatusReply: State is: "
              << (unsigned int)statusSet.state.value << std::endl;
    sif::info << "RwHandler::handleGetRwStatusReply: clc mode is: "
              << (unsigned int)statusSet.clcMode.value << std::endl;
#endif
  }
}

void RwHandler::handleTemperatureReply(const uint8_t* packet) {
  PoolReadGuard rg(&statusSet);
  uint8_t offset = 2;
  statusSet.temperatureCelcius = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                 *(packet + offset + 1) << 8 | *(packet + offset);
  if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::info << "RwHandler::handleTemperatureReply: Temperature: " << statusSet.temperatureCelcius
              << " °C" << std::endl;
#endif
  }
}

void RwHandler::handleGetTelemetryReply(const uint8_t* packet) {
  PoolReadGuard rg(&tmDataset);
  uint8_t offset = 2;
  tmDataset.lastResetStatus = *(packet + offset);
  offset += 1;
  tmDataset.mcuTemperature = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                             *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.pressureSensorTemperature = *(packet + offset + 3) << 24 |
                                        *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 |
                                        *(packet + offset);
  offset += 4;
  tmDataset.pressure = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                       *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.rwState = *(packet + offset);
  offset += 1;
  tmDataset.rwClcMode = *(packet + offset);
  offset += 1;
  tmDataset.rwCurrSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                          *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.rwRefSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                         *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.numOfInvalidCrcPackets = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                     *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.numOfInvalidLenPackets = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                     *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.numOfInvalidCmdPackets = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                     *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.numOfCmdExecutedReplies = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                      *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.numOfCmdReplies = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                              *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.uartNumOfBytesWritten = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                    *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.uartNumOfBytesRead = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                 *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.uartNumOfParityErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                    *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.uartNumOfNoiseErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                   *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.uartNumOfFrameErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                   *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.uartNumOfRegisterOverrunErrors = *(packet + offset + 3) << 24 |
                                             *(packet + offset + 2) << 16 |
                                             *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.uartTotalNumOfErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                   *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.spiNumOfBytesWritten = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                   *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.spiNumOfBytesRead = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.spiNumOfRegisterOverrunErrors = *(packet + offset + 3) << 24 |
                                            *(packet + offset + 2) << 16 |
                                            *(packet + offset + 1) << 8 | *(packet + offset);
  offset += 4;
  tmDataset.spiTotalNumOfErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
                                  *(packet + offset + 1) << 8 | *(packet + offset);
  if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
    sif::info << "RwHandler::handleGetTelemetryReply: Last reset status: "
              << static_cast<unsigned int>(tmDataset.lastResetStatus.value) << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: MCU temperature: " << tmDataset.mcuTemperature
              << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: Pressure sensor temperature: "
              << tmDataset.pressureSensorTemperature << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: Pressure " << tmDataset.pressure << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: State: "
              << static_cast<unsigned int>(tmDataset.rwState.value) << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: CLC mode: "
              << static_cast<unsigned int>(tmDataset.rwClcMode.value) << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: Current speed: " << tmDataset.rwCurrSpeed
              << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: Reference speed: " << tmDataset.rwRefSpeed
              << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: Number of invalid CRC packets: "
              << tmDataset.numOfInvalidCrcPackets << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: Number of invalid length packets: "
              << tmDataset.numOfInvalidLenPackets << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: Number of invalid command packets: "
              << tmDataset.numOfInvalidCmdPackets << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: Number of command executed replies: "
              << tmDataset.numOfCmdExecutedReplies << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: Number of command replies: "
              << tmDataset.numOfCmdReplies << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: UART number of bytes written: "
              << tmDataset.uartNumOfBytesWritten << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: UART number of bytes read: "
              << tmDataset.uartNumOfBytesRead << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: UART number of parity errors: "
              << tmDataset.uartNumOfParityErrors << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: UART number of noise errors: "
              << tmDataset.uartNumOfNoiseErrors << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: UART number of frame errors: "
              << tmDataset.uartNumOfFrameErrors << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: UART number of register overrun errors: "
              << tmDataset.uartNumOfRegisterOverrunErrors << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: UART number of total errors: "
              << tmDataset.uartTotalNumOfErrors << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: SPI number of bytes written: "
              << tmDataset.spiNumOfBytesWritten << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: SPI number of bytes read: "
              << tmDataset.spiNumOfBytesRead << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: SPI number of register overrun errors: "
              << tmDataset.spiNumOfRegisterOverrunErrors << std::endl;
    sif::info << "RwHandler::handleGetTelemetryReply: SPI number of register total errors: "
              << tmDataset.spiTotalNumOfErrors << std::endl;
#endif
  }
}

void RwHandler::setDebugMode(bool enable) { this->debugMode = enable; }