eive-obsw/linux/devices/SusHandler.cpp

#include "SusHandler.h"

#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw_hal/linux/spi/SpiComIF.h>

#include "OBSWConfig.h"

SusHandler::SusHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie,
                       LinuxLibgpioIF *gpioComIF, gpioId_t chipSelectId)
    : DeviceHandlerBase(objectId, comIF, comCookie),
      divider(5),
      dataset(this) {
}

SusHandler::~SusHandler() {}


ReturnValue_t SusHandler::initialize() {
  ReturnValue_t result = RETURN_OK;
  result = DeviceHandlerBase::initialize();
  if (result != RETURN_OK) {
    return result;
  }
  return RETURN_OK;
}

void SusHandler::doStartUp() {
  if (comState == ComStates::IDLE) {
    comState = ComStates::WRITE_SETUP;
  }
  if(comState == ComStates::WRITE_SETUP) {
    if (commandExecuted) {
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
      setMode(MODE_NORMAL);
#else
      setMode(_MODE_TO_ON);
#endif
      commandExecuted = false;
      comState = ComStates::START_CONVERSIONS;
    }
  }
}

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

ReturnValue_t SusHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  switch (comState) {
    case (ComStates::IDLE): {
      return NOTHING_TO_SEND;
    }
    case (ComStates::WRITE_SETUP): {
      *id = SUS::WRITE_SETUP;
      comState = ComStates::START_CONVERSIONS;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (ComStates::START_CONVERSIONS): {
      *id = SUS::START_CONVERSIONS;
      comState = ComStates::READ_CONVERSIONS;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (ComStates::READ_CONVERSIONS): {
      *id = SUS::READ_CONVERSIONS;
      comState = ComStates::START_CONVERSIONS;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
  }
  return NOTHING_TO_SEND;
}

ReturnValue_t SusHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  if (comState == ComStates::WRITE_SETUP) {
    *id = SUS::WRITE_SETUP;
    return buildCommandFromCommand(*id, nullptr, 0);
  }
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t SusHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                  const uint8_t *commandData,
                                                  size_t commandDataLen) {
  switch (deviceCommand) {
    case (SUS::WRITE_SETUP): {
      cmdBuffer[0] = SUS::SETUP;
      rawPacket = cmdBuffer;
      rawPacketLen = 1;
      return RETURN_OK;
    }
    case (SUS::START_CONVERSIONS): {
      std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
      cmdBuffer[0] = SUS::CONVERSION;
      rawPacket = cmdBuffer;
      rawPacketLen = 2;
      return RETURN_OK;
    }
    case (SUS::READ_CONVERSIONS): {
      std::memset(cmdBuffer, 0, sizeof(cmdBuffer));
      rawPacket = cmdBuffer;
      rawPacketLen = SUS::SIZE_READ_CONVERSIONS;
      return RETURN_OK;
    }
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return HasReturnvaluesIF::RETURN_FAILED;
}

void SusHandler::fillCommandAndReplyMap() {
  this->insertInCommandAndReplyMap(SUS::WRITE_SETUP, 1);
  this->insertInCommandAndReplyMap(SUS::START_CONVERSIONS, 1);
  this->insertInCommandAndReplyMap(SUS::READ_CONVERSIONS, 1, &dataset, SUS::SIZE_READ_CONVERSIONS);
}

ReturnValue_t SusHandler::scanForReply(const uint8_t *start, size_t remainingSize,
                                       DeviceCommandId_t *foundId, size_t *foundLen) {
  *foundId = this->getPendingCommand();
  *foundLen = remainingSize;
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
  switch (id) {
	case SUS::WRITE_SETUP: {
	      if (mode == _MODE_START_UP) {
	        commandExecuted = true;
	      }
	      return HasReturnvaluesIF::RETURN_OK;
	}
	case SUS::START_CONVERSIONS: {
		return HasReturnvaluesIF::RETURN_OK;
	}
    case SUS::READ_CONVERSIONS: {
      PoolReadGuard readSet(&dataset);
      dataset.temperatureCelcius = (*(packet) << 8 | *(packet + 1)) * 0.125;
      dataset.ain0 = (*(packet + 2) << 8 | *(packet + 3));
      dataset.ain1 = (*(packet + 4) << 8 | *(packet + 5));
      dataset.ain2 = (*(packet + 6) << 8 | *(packet + 7));
      dataset.ain3 = (*(packet + 8) << 8 | *(packet + 9));
      dataset.ain4 = (*(packet + 10) << 8 | *(packet + 11));
      dataset.ain5 = (*(packet + 12) << 8 | *(packet + 13));
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_SUS == 1
      if (divider.checkAndIncrement()) {
        sif::info << "SUS Object ID 0x" << std::hex << this->getObjectId() <<  ":" << std::endl;
        sif::info << "Temperature: " << dataset.temperatureCelcius << " C" << std::endl;
        sif::info << "AIN0: " << std::dec << dataset.ain0 << std::endl;
        sif::info << "AIN1: " << std::dec << dataset.ain1 << std::endl;
        sif::info << "AIN2: " << std::dec << dataset.ain2 << std::endl;
        sif::info << "AIN3: " << std::dec << dataset.ain3 << std::endl;
        sif::info << "AIN4: " << std::dec << dataset.ain4 << std::endl;
        sif::info << "AIN5: " << std::dec << dataset.ain5 << std::endl;
      }
#endif
      break;
    }
    default: {
      sif::debug << "SusHandler::interpretDeviceReply: Unknown reply id" << std::endl;
      return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
    }
  }
  return HasReturnvaluesIF::RETURN_OK;
}

uint32_t SusHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 1000; }

ReturnValue_t SusHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                  LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(SUS::TEMPERATURE_C, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(SUS::AIN0, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(SUS::AIN1, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(SUS::AIN2, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(SUS::AIN3, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(SUS::AIN4, new PoolEntry<uint16_t>({0}));
  localDataPoolMap.emplace(SUS::AIN5, new PoolEntry<uint16_t>({0}));
  return HasReturnvaluesIF::RETURN_OK;
}

void SusHandler::setToGoToNormalMode(bool enable) { this->goToNormalModeImmediately = enable; }