371 lines
13 KiB
C++
371 lines
13 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), lastResetStatusSet(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 (!startupPerformed) {
|
|
return;
|
|
}
|
|
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";
|
|
}
|
|
startupPerformed = false;
|
|
}
|
|
|
|
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
|
|
switch (communicationStep) {
|
|
case CommunicationStep::GET_RESET_STATUS:
|
|
*id = RwDefinitions::GET_LAST_RESET_STATUS;
|
|
communicationStep = CommunicationStep::READ_TEMPERATURE;
|
|
break;
|
|
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::GET_RESET_STATUS;
|
|
break;
|
|
default:
|
|
sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid communication step"
|
|
<< std::endl;
|
|
break;
|
|
}
|
|
return buildCommandFromCommand(*id, NULL, 0);
|
|
}
|
|
|
|
ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
|
|
switch (startupStep) {
|
|
case StartupStep::GET_RESET_STATUS:
|
|
*id = RwDefinitions::GET_LAST_RESET_STATUS;
|
|
startupStep = StartupStep::CLEAR_RESET_STATUS;
|
|
break;
|
|
case StartupStep::CLEAR_RESET_STATUS:
|
|
*id = RwDefinitions::CLEAR_LAST_RESET_STATUS;
|
|
startupStep = StartupStep::INIT_RW;
|
|
break;
|
|
case StartupStep::INIT_RW:
|
|
*id = RwDefinitions::INIT_RW_CONTROLLER;
|
|
startupStep = StartupStep::GET_RESET_STATUS;
|
|
startupPerformed = true;
|
|
break;
|
|
default:
|
|
sif::debug << "RwHandler::buildTransitionDeviceCommand: Invalid startup step"
|
|
<< std::endl;
|
|
break;
|
|
}
|
|
return buildCommandFromCommand(*id, nullptr, 0);;
|
|
}
|
|
|
|
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_LAST_RESET_STATUS): {
|
|
prepareGetLastResetStatusCommand();
|
|
return RETURN_OK;
|
|
}
|
|
case (RwDefinitions::CLEAR_LAST_RESET_STATUS): {
|
|
prepareClearResetStatusCommand();
|
|
return RETURN_OK;
|
|
}
|
|
case (RwDefinitions::GET_RW_STATUS): {
|
|
prepareGetStatusCmd(commandData, commandDataLen);
|
|
return RETURN_OK;
|
|
}
|
|
case (RwDefinitions::INIT_RW_CONTROLLER): {
|
|
prepareInitRwCommand();
|
|
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_LAST_RESET_STATUS, 1, &lastResetStatusSet,
|
|
RwDefinitions::SIZE_GET_RESET_STATUS);
|
|
this->insertInCommandMap(RwDefinitions::CLEAR_LAST_RESET_STATUS);
|
|
this->insertInCommandAndReplyMap(RwDefinitions::GET_RW_STATUS, 1, &statusSet,
|
|
RwDefinitions::SIZE_GET_RW_STATUS);
|
|
this->insertInCommandMap(RwDefinitions::INIT_RW_CONTROLLER);
|
|
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_LAST_RESET_STATUS)): {
|
|
*foundLen = RwDefinitions::SIZE_GET_RESET_STATUS;
|
|
*foundId = RwDefinitions::GET_LAST_RESET_STATUS;
|
|
break;
|
|
}
|
|
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_LAST_RESET_STATUS): {
|
|
handleResetStatusReply(packet);
|
|
break;
|
|
}
|
|
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::prepareGetLastResetStatusCommand() {
|
|
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_LAST_RESET_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;
|
|
}
|
|
|
|
void RwHandler::prepareClearResetStatusCommand() {
|
|
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::CLEAR_LAST_RESET_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;
|
|
}
|
|
|
|
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 + 4) << 8 | *(commandData + 5);
|
|
|
|
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::prepareInitRwCommand() {
|
|
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::INIT_RW_CONTROLLER);
|
|
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::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 + 5);
|
|
commandBuffer[6] = *(commandData + 4);
|
|
|
|
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::handleResetStatusReply(const uint8_t* packet) {
|
|
uint8_t offset = 2;
|
|
lastResetStatusSet.lastResetStatus = *(packet + offset);
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
|
|
sif::info << "RwHandler::handleResetStatusReply: Last reset status: "
|
|
<< static_cast<unsigned int>(lastResetStatusSet.lastResetStatus.value) << std::endl;
|
|
#endif
|
|
}
|
|
|
|
void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
|
|
uint8_t offset = 2;
|
|
statusSet.currSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
|
| *(packet + offset + 1) << 8 | *(packet + offset);
|
|
offset += 4;
|
|
statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
|
| *(packet + offset + 1) << 8 | *(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) << 8 | *(packet + offset);
|
|
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
|
|
sif::info << "RwHandler::handleTemperatureReply: Temperature: "
|
|
<< temperatureSet.temperatureCelcius << " °C" << std::endl;
|
|
#endif
|
|
}
|