eive-obsw/mission/devices/RwHandler.cpp

284 lines
9.6 KiB
C++

#include "RwHandler.h"
#include "OBSWConfig.h"
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/datapool/PoolReadGuard.h>
RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie,
LinuxLibgpioIF* gpioComIF, gpioId_t enableGpio) :
DeviceHandlerBase(objectId, comIF, comCookie), gpioComIF(gpioComIF), enableGpio(enableGpio),
temperatureSet(this), statusSet(this) {
if (comCookie == NULL) {
sif::error << "RwHandler: Invalid com cookie" << std::endl;
}
if (gpioComIF == NULL) {
sif::error << "RwHandler: Invalid gpio communication interface" << std::endl;
}
}
RwHandler::~RwHandler() {
}
void RwHandler::doStartUp() {
if(gpioComIF->pullHigh(enableGpio) != RETURN_OK) {
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high";
}
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
setMode(MODE_NORMAL);
#else
setMode(_MODE_TO_ON);
#endif
}
void RwHandler::doShutDown() {
if(gpioComIF->pullLow(enableGpio) != RETURN_OK) {
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low";
}
}
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
switch (communicationStep) {
case CommunicationStep::READ_TEMPERATURE:
*id = RwDefinitions::GET_TEMPERATURE;
communicationStep = CommunicationStep::GET_RW_SATUS;
break;
case CommunicationStep::GET_RW_SATUS:
*id = RwDefinitions::GET_RW_STATUS;
communicationStep = CommunicationStep::READ_TEMPERATURE;
break;
default:
sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid communication step"
<< std::endl;
break;
}
return buildCommandFromCommand(*id, NULL, 0);
}
ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
return RETURN_OK;
}
ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t * commandData, size_t commandDataLen) {
ReturnValue_t result = RETURN_OK;
switch (deviceCommand) {
case (RwDefinitions::RESET_MCU): {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::RESET_MCU);
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
}
case (RwDefinitions::GET_RW_STATUS): {
prepareGetStatusCmd(commandData, commandDataLen);
return RETURN_OK;
}
case (RwDefinitions::SET_SPEED): {
if (commandDataLen != 6) {
sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
<< " invalid length" << std::endl;
return SET_SPEED_COMMAND_INVALID_LENGTH;
}
result = checkSpeedAndRampTime(commandData, commandDataLen);
if (result != RETURN_OK) {
return result;
}
prepareSetSpeedCmd(commandData, commandDataLen);
return result;
}
case (RwDefinitions::GET_TEMPERATURE): {
prepareGetTemperatureCmd();
return RETURN_OK;
}
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
return HasReturnvaluesIF::RETURN_FAILED;
}
void RwHandler::fillCommandAndReplyMap() {
this->insertInCommandMap(RwDefinitions::RESET_MCU);
this->insertInCommandAndReplyMap(RwDefinitions::GET_RW_STATUS, 1, &statusSet,
RwDefinitions::SIZE_GET_RW_STATUS);
this->insertInCommandAndReplyMap(RwDefinitions::GET_TEMPERATURE, 1, &temperatureSet,
RwDefinitions::SIZE_GET_TEMPERATURE_REPLY);
this->insertInCommandAndReplyMap(RwDefinitions::SET_SPEED, 1, nullptr,
RwDefinitions::SIZE_SET_SPEED_REPLY);
}
ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize,
DeviceCommandId_t *foundId, size_t *foundLen) {
switch (*(start)) {
case (static_cast<uint8_t>(RwDefinitions::GET_RW_STATUS)): {
*foundLen = RwDefinitions::SIZE_GET_RW_STATUS;
*foundId = RwDefinitions::GET_RW_STATUS;
break;
}
case (static_cast<uint8_t>(RwDefinitions::SET_SPEED)): {
*foundLen = RwDefinitions::SIZE_SET_SPEED_REPLY;
*foundId = RwDefinitions::SET_SPEED;
break;
}
case (static_cast<uint8_t>(RwDefinitions::GET_TEMPERATURE)): {
*foundLen = RwDefinitions::SIZE_GET_TEMPERATURE_REPLY;
*foundId = RwDefinitions::GET_TEMPERATURE;
break;
}
default: {
sif::debug << "RwHandler::scanForReply: Reply contains invalid command code" << std::endl;
return RETURN_FAILED;
break;
}
}
sizeOfReply = *foundLen;
return RETURN_OK;
}
ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
/** Check result code */
if (*(packet + 1) == RwDefinitions::ERROR) {
return EXECUTION_FAILED;
}
/** Received in little endian byte order */
uint16_t replyCrc = *(packet + sizeOfReply - 1) << 8 | *(packet + sizeOfReply - 2) ;
if (CRC::crc16ccitt(packet, sizeOfReply - 2, 0xFFFF) != replyCrc) {
sif::error << "RwHandler::interpretDeviceReply: cRC error" << std::endl;
return CRC_ERROR;
}
switch (id) {
case (RwDefinitions::GET_RW_STATUS): {
handleGetRwStatusReply(packet);
break;
}
case (RwDefinitions::SET_SPEED):
// no reply data expected
break;
case (RwDefinitions::GET_TEMPERATURE): {
handleTemperatureReply(packet);
break;
}
default: {
sif::debug << "RwHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
}
}
return RETURN_OK;
}
void RwHandler::setNormalDatapoolEntriesInvalid() {
}
uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
return 5000;
}
ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>( { 0 }));
return RETURN_OK;
}
void RwHandler::prepareGetStatusCmd(const uint8_t * commandData, size_t commandDataLen) {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_RW_STATUS);
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(const uint8_t* commandData, size_t commandDataLen) {
int32_t speed = *commandData << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8
| *(commandData + 3);
if (speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl;
return INVALID_SPEED;
}
uint16_t rampTime = *commandData << 8 | *(commandData + 1);
if (rampTime < 10 || rampTime > 10000) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time"
<< std::endl;
return INVALID_RAMP_TIME;
}
return RETURN_OK;
}
void RwHandler::prepareGetTemperatureCmd() {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_TEMPERATURE);
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;
}
void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDataLen) {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::SET_SPEED);
/** Speed (0.1 RPM) */
commandBuffer[1] = *(commandData + 3);
commandBuffer[2] = *(commandData + 2);
commandBuffer[3] = *(commandData + 1);
commandBuffer[4] = *commandData;
/** Ramp time (ms) */
commandBuffer[5] = *(commandData + 1);
commandBuffer[6] = *commandData;
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;
}
void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
uint8_t offset = 2;
statusSet.currSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 1 | *(packet + offset);
offset += 4;
statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 1 | *(packet + offset);
offset += 4;
statusSet.state = *(packet + offset);
offset += 1;
statusSet.clcMode = *(packet + offset);
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 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) {
uint8_t offset = 2;
temperatureSet.temperatureCelcius = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 1 | *(packet + offset);
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
sif::info << "RwHandler::handleTemperatureReply: Temperature: "
<< temperatureSet.temperatureCelcius << " °C" << std::endl;
#endif
}