diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 7254ee0e..83a136b8 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -7,7 +7,7 @@ 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) { + temperatureSet(this), statusSet(this), lastResetStatusSet(this) { if (comCookie == NULL) { sif::error << "RwHandler: Invalid com cookie" << std::endl; } @@ -20,6 +20,9 @@ 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"; } @@ -34,6 +37,7 @@ 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) { @@ -55,7 +59,25 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) { } ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) { - *id = RwDefinitions::INIT_RW_CONTROLLER; + 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);; } @@ -70,6 +92,14 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand 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; @@ -103,6 +133,9 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand 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); @@ -116,6 +149,11 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize DeviceCommandId_t *foundId, size_t *foundLen) { switch (*(start)) { + case (static_cast(RwDefinitions::GET_LAST_RESET_STATUS)): { + *foundLen = RwDefinitions::SIZE_GET_RESET_STATUS; + *foundId = RwDefinitions::GET_LAST_RESET_STATUS; + break; + } case (static_cast(RwDefinitions::GET_RW_STATUS)): { *foundLen = RwDefinitions::SIZE_GET_RW_STATUS; *foundId = RwDefinitions::GET_RW_STATUS; @@ -159,6 +197,10 @@ ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_ } switch (id) { + case (RwDefinitions::GET_LAST_RESET_STATUS): { + handleResetStatusReply(packet); + break; + } case (RwDefinitions::GET_RW_STATUS): { handleGetRwStatusReply(packet); break; @@ -195,6 +237,24 @@ ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataP return RETURN_OK; } +void RwHandler::prepareGetLastResetStatusCommand() { + commandBuffer[0] = static_cast(RwDefinitions::GET_LAST_RESET_STATUS); + uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF); + commandBuffer[1] = static_cast(crc & 0xFF); + commandBuffer[2] = static_cast(crc >> 8 & 0xFF); + rawPacket = commandBuffer; + rawPacketLen = 3; +} + +void RwHandler::prepareClearResetStatusCommand() { + commandBuffer[0] = static_cast(RwDefinitions::CLEAR_LAST_RESET_STATUS); + uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF); + commandBuffer[1] = static_cast(crc & 0xFF); + commandBuffer[2] = static_cast(crc >> 8 & 0xFF); + rawPacket = commandBuffer; + rawPacketLen = 3; +} + void RwHandler::prepareGetStatusCmd(const uint8_t * commandData, size_t commandDataLen) { commandBuffer[0] = static_cast(RwDefinitions::GET_RW_STATUS); @@ -225,15 +285,6 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_ return RETURN_OK; } -void RwHandler::prepareGetTemperatureCmd() { - commandBuffer[0] = static_cast(RwDefinitions::GET_TEMPERATURE); - uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF); - commandBuffer[1] = static_cast(crc & 0xFF); - commandBuffer[2] = static_cast(crc >> 8 & 0xFF); - rawPacket = commandBuffer; - rawPacketLen = 3; -} - void RwHandler::prepareInitRwCommand() { commandBuffer[0] = static_cast(RwDefinitions::INIT_RW_CONTROLLER); uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF); @@ -243,6 +294,14 @@ void RwHandler::prepareInitRwCommand() { rawPacketLen = 3; } +void RwHandler::prepareGetTemperatureCmd() { + commandBuffer[0] = static_cast(RwDefinitions::GET_TEMPERATURE); + uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF); + commandBuffer[1] = static_cast(crc & 0xFF); + commandBuffer[2] = static_cast(crc >> 8 & 0xFF); + rawPacket = commandBuffer; + rawPacketLen = 3; +} void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDataLen) { commandBuffer[0] = static_cast(RwDefinitions::SET_SPEED); @@ -263,6 +322,15 @@ void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDa 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(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 diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index fcde9c81..29a2ae52 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -84,6 +84,7 @@ private: RwDefinitions::TemperatureSet temperatureSet; RwDefinitions::StatusSet statusSet; + RwDefinitions::LastResetSatus lastResetStatusSet; uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE]; @@ -95,8 +96,28 @@ private: CommunicationStep communicationStep = CommunicationStep::READ_TEMPERATURE; + enum class StartupStep { + GET_RESET_STATUS, + CLEAR_RESET_STATUS, + INIT_RW + }; + + StartupStep startupStep = StartupStep::GET_RESET_STATUS; + size_t sizeOfReply = 0; + bool startupPerformed = false; + + /** + * @brief This function prepares the command to request the last reset status + */ + void prepareGetLastResetStatusCommand(); + + /** + * @brief Fills the command buffer with the command to clear the reset status. + */ + void prepareClearResetStatusCommand(); + /** * @brief This function prepares the send buffer with the data to request the status of * the reaction wheel. @@ -110,6 +131,13 @@ private: */ ReturnValue_t checkSpeedAndRampTime(const uint8_t * commandData, size_t commandDataLen); + /** + * @brief This function fills the commandBuffer with the data to request initialize the + * reaction wheel controller. This command must be sent as soon as the state of a + * reaction wheel is equal to 1 which indicates an error. + */ + void prepareInitRwCommand(); + /** * @brief This function prepares the set speed command from the commandData received with * an action message. @@ -122,11 +150,12 @@ private: void prepareGetTemperatureCmd(); /** - * @brief This function fills the commandBuffer with the data to request initialize the - * reaction wheel controller. This command must be sent as soon as the state of a - * reaction wheel is equal to 1 which indicates an error. + * @brief This function writes the last reset status retrieved with the get last reset status + * command into the reset status dataset. + * + * @param packet Pointer to the buffer holding the reply data. */ - void prepareInitRwCommand(); + void handleResetStatusReply(const uint8_t* packet); /** * @brief This function handles the reply of the get temperature command. diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h index a38b0acf..73de0e57 100644 --- a/mission/devices/devicedefinitions/RwDefinitions.h +++ b/mission/devices/devicedefinitions/RwDefinitions.h @@ -15,7 +15,8 @@ enum PoolIds: lp_id_t { CURR_SPEED, REFERENCE_SPEED, STATE, - CLC_MODE + CLC_MODE, + LAST_RESET_STATUS }; enum States: uint8_t { @@ -30,13 +31,16 @@ static const DeviceCommandId_t RESET_MCU = 1; static const DeviceCommandId_t GET_LAST_RESET_STATUS = 2; static const DeviceCommandId_t CLEAR_LAST_RESET_STATUS = 3; static const DeviceCommandId_t GET_RW_STATUS = 4; +/** This command is needed to recover from error state */ static const DeviceCommandId_t INIT_RW_CONTROLLER = 5; static const DeviceCommandId_t SET_SPEED = 6; static const DeviceCommandId_t GET_TEMPERATURE = 8; static const uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE; static const uint32_t STATUS_SET_ID = GET_RW_STATUS; +static const uint32_t LAST_RESET_ID = GET_LAST_RESET_STATUS; +static const size_t SIZE_GET_RESET_STATUS = 5; static const size_t SIZE_GET_RW_STATUS = 14; static const size_t SIZE_SET_SPEED_REPLY = 4; static const size_t SIZE_GET_TEMPERATURE_REPLY = 8; @@ -47,6 +51,7 @@ static const size_t SIZE_GET_TELEMETRY_REPLY = 83; static const size_t MAX_CMD_SIZE = 9; static const size_t MAX_REPLY_SIZE = SIZE_GET_TELEMETRY_REPLY; +static const uint8_t LAST_RESET_ENTRIES = 1; static const uint8_t TEMPERATURE_SET_ENTRIES = 1; static const uint8_t STATUS_SET_ENTRIES = 4; @@ -94,6 +99,25 @@ public: PoolIds::CLC_MODE, this); }; +/** + * @brief This dataset stores the last reset status. + */ +class LastResetSatus: + public StaticLocalDataSet { +public: + + LastResetSatus(HasLocalDataPoolIF* owner): + StaticLocalDataSet(owner, LAST_RESET_ID) { + } + + LastResetSatus(object_id_t objectId): + StaticLocalDataSet(sid_t(objectId, LAST_RESET_ID)) { + } + + lp_var_t lastResetStatus = lp_var_t(sid.objectId, + PoolIds::LAST_RESET_STATUS, this); +}; + } #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */