eive-obsw/mission/payload/RadiationSensorHandler.cpp

#include <OBSWConfig.h>
#include <devices/gpioIds.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/tasks/TaskFactory.h>
#include <mission/payload/RadiationSensorHandler.h>
#include <mission/power/gsDefs.h>
#include <mission/tcs/max1227.h>

RadiationSensorHandler::RadiationSensorHandler(object_id_t objectId, object_id_t comIF,
                                               CookieIF *comCookie, GpioIF *gpioIF,
                                               Stack5VHandler &stackHandler)
    : DeviceHandlerBase(objectId, comIF, comCookie),
      dataset(this),
      gpioIF(gpioIF),
      stackHandler(stackHandler) {
  if (comCookie == nullptr) {
    sif::error << "RadiationSensorHandler: Invalid com cookie" << std::endl;
  }
  // Time out immediately so we get an immediate measurement at device startup.
  measurementCd.timeOut();
}

RadiationSensorHandler::~RadiationSensorHandler() {}

void RadiationSensorHandler::doStartUp() {
  if (internalState == InternalState::OFF) {
    ReturnValue_t retval = stackHandler.deviceToOn(StackCommander::RAD_SENSOR, true);
    if (retval == BUSY) {
      return;
    }
    internalState = InternalState::POWER_SWITCHING;
  }
  if (internalState == InternalState::POWER_SWITCHING) {
    if (stackHandler.isSwitchOn()) {
      internalState = InternalState::SETUP;
    }
  }
  if (internalState == InternalState::CONFIGURED) {
    if (goToNormalMode) {
      setMode(MODE_NORMAL);
    } else {
      setMode(_MODE_TO_ON);
    }
  }
}

void RadiationSensorHandler::doShutDown() {
  ReturnValue_t retval = stackHandler.deviceToOff(StackCommander::RAD_SENSOR, true);
  if (retval == BUSY) {
    return;
  }
  internalState = InternalState::OFF;
  setMode(_MODE_POWER_DOWN);
}

ReturnValue_t RadiationSensorHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  if (measurementCd.isBusy()) {
    return NOTHING_TO_SEND;
  }
  switch (communicationStep) {
    case CommunicationStep::START_CONVERSION: {
      *id = radSens::START_CONVERSION;
      communicationStep = CommunicationStep::READ_CONVERSIONS;
      break;
    }
    case CommunicationStep::READ_CONVERSIONS: {
      *id = radSens::READ_CONVERSIONS;
      communicationStep = CommunicationStep::START_CONVERSION;
      break;
    }
    default: {
      sif::debug << "RadiationSensorHandler::buildNormalDeviceCommand: Unknown communication "
                 << "step" << std::endl;
      return returnvalue::OK;
    }
  }
  return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t RadiationSensorHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  if (internalState == InternalState::SETUP) {
    *id = radSens::WRITE_SETUP;
  } else {
    return NOTHING_TO_SEND;
  }
  return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t RadiationSensorHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                              const uint8_t *commandData,
                                                              size_t commandDataLen) {
  switch (deviceCommand) {
    case (radSens::WRITE_SETUP): {
      cmdBuffer[0] = radSens::SETUP_DEFINITION;
      rawPacket = cmdBuffer;
      rawPacketLen = 1;
      internalState = InternalState::CONFIGURED;
      return returnvalue::OK;
    }
    case (radSens::START_CONVERSION): {
      ReturnValue_t result = gpioIF->pullHigh(gpioIds::ENABLE_RADFET);
      // Test a small delay between pulling the RADFET high and reading the sensor. As long as this
      // delay remains small enough, this should not cause scheduling issues. Do not make this
      // delay large, this device might be scheduled inside the ACS PST!
      TaskFactory::delayTask(5);
      if (result != returnvalue::OK) {
#if OBSW_VERBOSE_LEVEL >= 1
        sif::warning
            << "RadiationSensorHandler::buildCommandFromCommand: Pulling RADFET Enable pin "
               "high failed"
            << std::endl;
#endif
      }
      /* First the fifo will be reset here */
      cmdBuffer[0] = radSens::RESET_DEFINITION;
      cmdBuffer[1] = radSens::CONVERSION_DEFINITION;
      rawPacket = cmdBuffer;
      rawPacketLen = 2;
      return returnvalue::OK;
    }
    case (radSens::READ_CONVERSIONS): {
      cmdBuffer[0] = radSens::DUMMY_BYTE;
      std::memset(cmdBuffer, radSens::DUMMY_BYTE, radSens::READ_SIZE);
      rawPacket = cmdBuffer;
      rawPacketLen = radSens::READ_SIZE;
      return returnvalue::OK;
    }
    case radSens::ENABLE_DEBUG_OUTPUT: {
      printPeriodicData = true;
      rawPacketLen = 0;
      return returnvalue::OK;
    }
    case radSens::DISABLE_DEBUG_OUTPUT: {
      rawPacketLen = 0;
      printPeriodicData = false;
      return returnvalue::OK;
    }
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return returnvalue::FAILED;
}

void RadiationSensorHandler::fillCommandAndReplyMap() {
  this->insertInCommandMap(radSens::WRITE_SETUP);
  this->insertInCommandMap(radSens::START_CONVERSION);
  this->insertInCommandMap(radSens::ENABLE_DEBUG_OUTPUT);
  this->insertInCommandMap(radSens::DISABLE_DEBUG_OUTPUT);
  this->insertInCommandAndReplyMap(radSens::READ_CONVERSIONS, 1, &dataset, radSens::READ_SIZE);
}

ReturnValue_t RadiationSensorHandler::scanForReply(const uint8_t *start, size_t remainingSize,
                                                   DeviceCommandId_t *foundId, size_t *foundLen) {
  *foundId = this->getPendingCommand();

  switch (*foundId) {
    case radSens::START_CONVERSION:
    case radSens::WRITE_SETUP:
      *foundLen = remainingSize;
      return IGNORE_REPLY_DATA;
    case radSens::READ_CONVERSIONS: {
      ReturnValue_t result = gpioIF->pullLow(gpioIds::ENABLE_RADFET);
      if (result != returnvalue::OK) {
#if OBSW_VERBOSE_LEVEL >= 1
        sif::warning << "RadiationSensorHandler::scanForReply; Pulling RADFET Enale pin "
                        "low failed"
                     << std::endl;
#endif
      }
      break;
    }
    case radSens::ENABLE_DEBUG_OUTPUT:
    case radSens::DISABLE_DEBUG_OUTPUT:
      sif::info << "RadiationSensorHandler::scanForReply: " << remainingSize << std::endl;
      break;
    default:
      break;
  }

  *foundLen = remainingSize;

  return returnvalue::OK;
}

ReturnValue_t RadiationSensorHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                           const uint8_t *packet) {
  switch (id) {
    case radSens::READ_CONVERSIONS: {
      uint8_t offset = 0;
      measurementCd.resetTimer();
      {
        PoolReadGuard readSet(&dataset);
        uint16_t tempRaw = ((packet[offset] & 0x0f) << 8) | packet[offset + 1];
        dataset.temperatureCelcius = max1227::getTemperature(tempRaw);
        offset += 2;
        dataset.ain0 = (*(packet + offset) << 8) | *(packet + offset + 1);
        offset += 2;
        dataset.ain1 = (*(packet + offset) << 8) | *(packet + offset + 1);
        offset += 6;
        dataset.ain4 = (*(packet + offset) << 8) | *(packet + offset + 1);
        offset += 2;
        dataset.ain5 = (*(packet + offset) << 8) | *(packet + offset + 1);
        offset += 2;
        dataset.ain6 = (*(packet + offset) << 8) | *(packet + offset + 1);
        offset += 2;
        dataset.ain7 = (*(packet + offset) << 8) | *(packet + offset + 1);
        dataset.setValidity(true, true);
      }
      if (printPeriodicData) {
        sif::info << "Radiation sensor temperature: " << dataset.temperatureCelcius << " °C"
                  << std::dec << std::endl;
        sif::info << "Radiation sensor ADC value channel 0: " << dataset.ain0 << std::endl;
        sif::info << "Radiation sensor ADC value channel 1: " << dataset.ain1 << std::endl;
        sif::info << "Radiation sensor ADC value channel 4: " << dataset.ain4 << std::endl;
        sif::info << "Radiation sensor ADC value channel 5: " << dataset.ain5 << std::endl;
        sif::info << "Radiation sensor ADC value channel 6: " << dataset.ain6 << std::endl;
        sif::info << "Radiation sensor ADC value channel 7: " << dataset.ain7 << std::endl;
      }
      ReturnValue_t result =
          getHkManagerHandle()->generateHousekeepingPacket(dataset.getSid(), &dataset, true);
      if (result != returnvalue::OK) {
        // TODO: Maybe add event?
        sif::error << "Generating HK set for radiation sensor failed" << std::endl;
      }
      break;
    }
    default: {
      sif::debug << "RadiationSensorHandler::interpretDeviceReply: Unknown reply id" << std::endl;
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }
  return returnvalue::OK;
}

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

ReturnValue_t RadiationSensorHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                              LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(radSens::TEMPERATURE_C, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(radSens::AIN0, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(radSens::AIN1, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(radSens::AIN4, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(radSens::AIN5, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(radSens::AIN6, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(radSens::AIN7, new PoolEntry<uint16_t>({0}));
  // It should normally not be necessary to enable this set, as a sample TM will be generated
  // after a measurement. If this is still enabled, sample with double the measurement frequency
  // to ensure we get all measurements.
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(dataset.getSid(), false, DEFAULT_MEASUREMENT_CD_MS / 2));
  return returnvalue::OK;
}

void RadiationSensorHandler::setToGoToNormalModeImmediately() { this->goToNormalMode = true; }

void RadiationSensorHandler::enablePeriodicDataPrint(bool enable) {
  this->printPeriodicData = enable;
}