diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index c8744301..f0cef059 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -20,6 +20,12 @@ RwPollingTask::RwPollingTask(object_id_t objectId, SpiComIF* spiIF) } ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { + for (unsigned i = 0; i < 4; i++) { + if (rwCookies[i] == nullptr) { + sif::error << "Invalid RW cookie at index" << i << std::endl; + return returnvalue::FAILED; + } + } while (true) { ipcLock->lockMutex(); state = InternalState::IDLE; @@ -31,9 +37,11 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { continue; } for (unsigned idx = 0; idx < rwCookies.size(); idx++) { - prepareSetSpeedCmd(idx); - if (writeOneRwCmd(idx, fd) != returnvalue::OK) { - continue; + if (rwCookies[idx]->setSpeed) { + prepareSetSpeedCmd(idx); + if (writeOneRwCmd(idx, fd) != returnvalue::OK) { + continue; + } } } closeSpi(fd); @@ -57,6 +65,7 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { if (writeAndReadAllRws(rws::CLEAR_LAST_RESET_STATUS) != returnvalue::OK) { continue; } + // TODO: Special requests } return returnvalue::OK; @@ -84,6 +93,12 @@ ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) { modeAndSpeedWasSet = true; } + auto* rwCookie = dynamic_cast(cookie); + if (rwCookie == nullptr) { + return returnvalue::FAILED; + } + rwCookies[rwCookie->rwIdx] = rwCookie; + return returnvalue::OK; } @@ -94,11 +109,15 @@ ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendDa } int32_t speed = 0; uint16_t rampTime = 0; - SerializeAdapter::deSerialize(&speed, &sendData, &sendLen, SerializeIF::Endianness::MACHINE); - SerializeAdapter::deSerialize(&rampTime, &sendData, &sendLen, SerializeIF::Endianness::MACHINE); + const uint8_t* currentBuf = sendData; + bool setSpeed = currentBuf[0]; + currentBuf += 1; + sendLen -= 1; + SerializeAdapter::deSerialize(&speed, ¤tBuf, &sendLen, SerializeIF::Endianness::MACHINE); + SerializeAdapter::deSerialize(&rampTime, ¤tBuf, &sendLen, SerializeIF::Endianness::MACHINE); rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE; - if (sendLen == 7 and sendData[6] < static_cast(rws::SpecialRwRequest::NUM_REQUESTS)) { - specialRequest = static_cast(sendData[6]); + if (sendLen == 8 and sendData[7] < static_cast(rws::SpecialRwRequest::NUM_REQUESTS)) { + specialRequest = static_cast(sendData[7]); } RwCookie* rwCookie = dynamic_cast(cookie); if (rwCookie == nullptr) { @@ -106,6 +125,7 @@ ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendDa } { MutexGuard mg(ipcLock); + rwCookie->setSpeed = setSpeed; rwCookie->currentRwSpeed = speed; rwCookie->currentRampTime = rampTime; rwCookie->specialRequest = specialRequest; @@ -299,6 +319,9 @@ ReturnValue_t RwPollingTask::readAllRws(int fd, DeviceCommandId_t id) { return result; } for (unsigned idx = 0; idx < rwCookies.size(); idx++) { + if (id == rws::SET_SPEED and !rwCookies[idx]->setSpeed) { + continue; + } if (spiLock == nullptr) { sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; return returnvalue::FAILED; @@ -319,31 +342,31 @@ size_t RwPollingTask::idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, switch (id) { case (rws::GET_RW_STATUS): { *ptr = replies.rwStatusReply; - return rws::SIZE_GET_RW_STATUS; + break; } case (rws::SET_SPEED): { *ptr = replies.setSpeedReply; - return rws::SIZE_SET_SPEED_REPLY; + break; } case (rws::CLEAR_LAST_RESET_STATUS): { *ptr = replies.clearLastResetStatusReply; - return rws::SIZE_CLEAR_RESET_STATUS; + break; } case (rws::GET_LAST_RESET_STATUS): { *ptr = replies.getLastResetStatusReply; - return rws::SIZE_GET_RESET_STATUS; + break; } case (rws::GET_TEMPERATURE): { *ptr = replies.readTemperatureReply; - return rws::SIZE_GET_TEMPERATURE_REPLY; + break; } case (rws::GET_TM): { *ptr = replies.hkDataReply; - return rws::SIZE_GET_TELEMETRY_REPLY; + break; } case (rws::INIT_RW_CONTROLLER): { *ptr = replies.initRwControllerReply; - return rws::SIZE_INIT_RW; + break; } default: { sif::error << "no reply buffer for rw command " << id << std::endl; @@ -351,6 +374,7 @@ size_t RwPollingTask::idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, return 0; } } + return rws::idToPacketLen(id); } // This closes the SPI diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h index b9895865..f22ad1fe 100644 --- a/linux/devices/RwPollingTask.h +++ b/linux/devices/RwPollingTask.h @@ -21,6 +21,7 @@ class RwCookie : public SpiCookie { private: std::array replyBuf{}; + bool setSpeed = true; int32_t currentRwSpeed = 0; uint16_t currentRampTime = 0; rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE; diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index c5bd1e6e..456e0940 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -27,11 +27,12 @@ RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCooki RwHandler::~RwHandler() {} void RwHandler::doStartUp() { - internalState = InternalState::GET_RESET_STATUS; + internalState = InternalState::DEFAULT; if (gpioComIF->pullHigh(enableGpio) != returnvalue::OK) { sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high"; } + updatePeriodicReply(true, rws::REPLY_ID); setMode(_MODE_TO_ON); } @@ -39,32 +40,37 @@ void RwHandler::doShutDown() { if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) { sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low"; } + internalState = InternalState::DEFAULT; + updatePeriodicReply(false, rws::REPLY_ID); setMode(_MODE_POWER_DOWN); } ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { switch (internalState) { - case InternalState::SET_SPEED: - *id = rws::SET_SPEED; - internalState = InternalState::GET_RESET_STATUS; - break; - case InternalState::GET_RESET_STATUS: - *id = rws::GET_LAST_RESET_STATUS; - internalState = InternalState::READ_TEMPERATURE; - break; - case InternalState::READ_TEMPERATURE: - *id = rws::GET_TEMPERATURE; - internalState = InternalState::GET_RW_SATUS; - break; - case InternalState::GET_RW_SATUS: - *id = rws::GET_RW_STATUS; - internalState = InternalState::CLEAR_RESET_STATUS; - break; - case InternalState::CLEAR_RESET_STATUS: - *id = rws::CLEAR_LAST_RESET_STATUS; - /** After reset status is cleared, reset status will be polled again for verification */ - internalState = InternalState::GET_RESET_STATUS; + case InternalState::DEFAULT: { + *id = rws::REQUEST_ID; break; + } + // case InternalState::SET_SPEED: + // *id = rws::SET_SPEED; + // internalState = InternalState::GET_RESET_STATUS; + // break; + // case InternalState::GET_RESET_STATUS: + // *id = rws::GET_LAST_RESET_STATUS; + // internalState = InternalState::READ_TEMPERATURE; + // break; + // case InternalState::READ_TEMPERATURE: + // *id = rws::GET_TEMPERATURE; + // internalState = InternalState::GET_RW_SATUS; + // break; + // case InternalState::GET_RW_SATUS: + // *id = rws::GET_RW_STATUS; + // internalState = InternalState::CLEAR_RESET_STATUS; + // break; + // case InternalState::CLEAR_RESET_STATUS: + // *id = rws::CLEAR_LAST_RESET_STATUS; + // /** After reset status is cleared, reset status will be polled again for verification + // */ internalState = InternalState::GET_RESET_STATUS; break; default: sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid internal step" << std::endl; break; @@ -82,27 +88,7 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand ReturnValue_t result = returnvalue::OK; switch (deviceCommand) { - case (rws::RESET_MCU): { - prepareSimpleCommand(deviceCommand); - return returnvalue::OK; - } - case (rws::GET_LAST_RESET_STATUS): { - prepareSimpleCommand(deviceCommand); - return returnvalue::OK; - } - case (rws::CLEAR_LAST_RESET_STATUS): { - prepareSimpleCommand(deviceCommand); - return returnvalue::OK; - } - case (rws::GET_RW_STATUS): { - prepareSimpleCommand(deviceCommand); - return returnvalue::OK; - } - case (rws::INIT_RW_CONTROLLER): { - prepareSimpleCommand(deviceCommand); - return returnvalue::OK; - } - case (rws::SET_SPEED): { + case (rws::REQUEST_ID): { if (commandData != nullptr && commandDataLen != 6) { sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" << " invalid length" << std::endl; @@ -133,17 +119,94 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand if (result != returnvalue::OK) { return result; } - result = prepareSetSpeedCmd(); - return result; + // set speed flag. + commandBuffer[0] = true; + rawPacketLen = 1; + uint8_t* currentCmdBuf = commandBuffer + 1; + rwSpeedActuationSet.serialize(¤tCmdBuf, &rawPacketLen, sizeof(commandBuffer), + SerializeIF::Endianness::MACHINE); + commandBuffer[rawPacketLen] = static_cast(rws::SpecialRwRequest::REQUEST_NONE); + break; } - case (rws::GET_TEMPERATURE): { - prepareSimpleCommand(deviceCommand); + case (rws::RESET_MCU): { + commandBuffer[0] = false; + commandBuffer[7] = static_cast(rws::SpecialRwRequest::RESET_MCU); + internalState = InternalState::RESET_MCU; + return returnvalue::OK; + } + + case (rws::INIT_RW_CONTROLLER): { + commandBuffer[0] = false; + commandBuffer[7] = static_cast(rws::SpecialRwRequest::INIT_RW_CONTROLLER); + internalState = InternalState::INIT_RW_CONTROLLER; return returnvalue::OK; } case (rws::GET_TM): { - prepareSimpleCommand(deviceCommand); + commandBuffer[0] = false; + commandBuffer[7] = static_cast(rws::SpecialRwRequest::GET_TM); + internalState = InternalState::GET_TM; return returnvalue::OK; } + + // case (rws::GET_LAST_RESET_STATUS): { + // prepareSimpleCommand(deviceCommand); + // return returnvalue::OK; + // } + // case (rws::CLEAR_LAST_RESET_STATUS): { + // prepareSimpleCommand(deviceCommand); + // return returnvalue::OK; + // } + // case (rws::GET_RW_STATUS): { + // prepareSimpleCommand(deviceCommand); + // return returnvalue::OK; + // } + // case (rws::INIT_RW_CONTROLLER): { + // prepareSimpleCommand(deviceCommand); + // return returnvalue::OK; + // } + // case (rws::SET_SPEED): { + // if (commandData != nullptr && commandDataLen != 6) { + // sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" + // << " invalid length" << std::endl; + // return SET_SPEED_COMMAND_INVALID_LENGTH; + // } + // + // { + // PoolReadGuard pg(&rwSpeedActuationSet); + // // Commands override anything which was set in the software + // if (commandData != nullptr) { + // rwSpeedActuationSet.setValidityBufferGeneration(false); + // result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen, + // SerializeIF::Endianness::NETWORK); + // rwSpeedActuationSet.setValidityBufferGeneration(true); + // if (result != returnvalue::OK) { + // return result; + // } + // } + // } + // if (ACTUATION_WIRETAPPING) { + // int32_t speed = 0; + // uint16_t rampTime = 0; + // rwSpeedActuationSet.getRwSpeed(speed, rampTime); + // sif::debug << "Actuating RW " << static_cast(rwIdx) << " with speed = " << + // speed + // << " and rampTime = " << rampTime << std::endl; + // } + // result = checkSpeedAndRampTime(); + // if (result != returnvalue::OK) { + // return result; + // } + // result = prepareSetSpeedCmd(); + // return result; + // } + // case (rws::GET_TEMPERATURE): { + // prepareSimpleCommand(deviceCommand); + // return returnvalue::OK; + // } + // case (rws::GET_TM): { + // prepareSimpleCommand(deviceCommand); + // return returnvalue::OK; + // } default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } @@ -151,7 +214,13 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand } void RwHandler::fillCommandAndReplyMap() { - this->insertInCommandMap(rws::RESET_MCU); + insertInCommandMap(rws::REQUEST_ID); + insertInReplyMap(rws::REPLY_ID, 5, nullptr, 0, true); + + insertInCommandMap(rws::RESET_MCU); + insertInCommandAndReplyMap(rws::INIT_RW_CONTROLLER, 5, nullptr, 0, false, true, rws::REPLY_ID); + insertInCommandAndReplyMap(rws::GET_TM, 5, nullptr, 0, false, true, rws::REPLY_ID); + /* this->insertInCommandAndReplyMap(rws::GET_LAST_RESET_STATUS, 1, &lastResetStatusSet, rws::SIZE_GET_RESET_STATUS); this->insertInCommandAndReplyMap(rws::CLEAR_LAST_RESET_STATUS, 1, nullptr, @@ -162,103 +231,106 @@ void RwHandler::fillCommandAndReplyMap() { rws::SIZE_GET_TEMPERATURE_REPLY); this->insertInCommandAndReplyMap(rws::SET_SPEED, 1, nullptr, rws::SIZE_SET_SPEED_REPLY); this->insertInCommandAndReplyMap(rws::GET_TM, 1, &tmDataset, rws::SIZE_GET_TELEMETRY_REPLY); + */ } ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { - uint8_t replyByte = *start; - switch (replyByte) { - case (rws::GET_LAST_RESET_STATUS): { - *foundLen = rws::SIZE_GET_RESET_STATUS; - *foundId = rws::GET_LAST_RESET_STATUS; - break; - } - case (rws::CLEAR_LAST_RESET_STATUS): { - *foundLen = rws::SIZE_CLEAR_RESET_STATUS; - *foundId = rws::CLEAR_LAST_RESET_STATUS; - break; - } - case (rws::GET_RW_STATUS): { - *foundLen = rws::SIZE_GET_RW_STATUS; - *foundId = rws::GET_RW_STATUS; - break; - } - case (rws::INIT_RW_CONTROLLER): { - *foundLen = rws::SIZE_INIT_RW; - *foundId = rws::INIT_RW_CONTROLLER; - break; - } - case (rws::SET_SPEED): { - *foundLen = rws::SIZE_SET_SPEED_REPLY; - *foundId = rws::SET_SPEED; - break; - } - case (rws::GET_TEMPERATURE): { - *foundLen = rws::SIZE_GET_TEMPERATURE_REPLY; - *foundId = rws::GET_TEMPERATURE; - break; - } - case (rws::GET_TM): { - *foundLen = rws::SIZE_GET_TELEMETRY_REPLY; - *foundId = rws::GET_TM; - break; - } - default: { - sif::warning << "RwHandler::scanForReply: Reply contains invalid command code" << std::endl; - *foundLen = remainingSize; - return returnvalue::FAILED; - } + // uint8_t replyByte = *start; + if (remainingSize > 0) { + *foundLen = remainingSize; + *foundId = rws::REPLY_ID; } - - sizeOfReply = *foundLen; - + // RwReplies replies(start); + // switch (replyByte) { + // case (rws::GET_LAST_RESET_STATUS): { + // *foundLen = rws::SIZE_GET_RESET_STATUS; + // *foundId = rws::GET_LAST_RESET_STATUS; + // break; + // } + // case (rws::CLEAR_LAST_RESET_STATUS): { + // *foundLen = rws::SIZE_CLEAR_RESET_STATUS; + // *foundId = rws::CLEAR_LAST_RESET_STATUS; + // break; + // } + // case (rws::GET_RW_STATUS): { + // *foundLen = rws::SIZE_GET_RW_STATUS; + // *foundId = rws::GET_RW_STATUS; + // break; + // } + // case (rws::INIT_RW_CONTROLLER): { + // *foundLen = rws::SIZE_INIT_RW; + // *foundId = rws::INIT_RW_CONTROLLER; + // break; + // } + // case (rws::SET_SPEED): { + // *foundLen = rws::SIZE_SET_SPEED_REPLY; + // *foundId = rws::SET_SPEED; + // break; + // } + // case (rws::GET_TEMPERATURE): { + // *foundLen = rws::SIZE_GET_TEMPERATURE_REPLY; + // *foundId = rws::GET_TEMPERATURE; + // break; + // } + // case (rws::GET_TM): { + // *foundLen = rws::SIZE_GET_TELEMETRY_REPLY; + // *foundId = rws::GET_TM; + // break; + // } + // default: { + // sif::warning << "RwHandler::scanForReply: Reply contains invalid command code" << + // std::endl; *foundLen = remainingSize; return returnvalue::FAILED; + // } + // } return returnvalue::OK; } ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { - /** Check result code */ - if (*(packet + 1) == rws::STATE_ERROR) { - sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: " << id - << std::endl; - return EXECUTION_FAILED; + RwReplies replies(packet); + ReturnValue_t result = returnvalue::OK; + auto checkPacket = [&](DeviceCommandId_t id, const uint8_t* packetPtr) { + auto packetLen = rws::idToPacketLen(id); + uint16_t replyCrc = (*(packet + packetLen - 1) << 8) | *(packet + packetLen - 2); + if (CRC::crc16ccitt(packet, packetLen - 2, 0xFFFF) != replyCrc) { + sif::error << "RwHandler::interpretDeviceReply: CRC error for ID " << id << std::endl; + return CRC_ERROR; + } + if (packetPtr[1] == rws::STATE_ERROR) { + sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: " << id + << std::endl; + result = EXECUTION_FAILED; + } + return returnvalue::OK; + }; + checkPacket(rws::DeviceCommandId::SET_SPEED, replies.getSetSpeedReply()); + if (returnvalue::OK == + checkPacket(rws::DeviceCommandId::GET_RW_STATUS, replies.getRwStatusReply())) { + handleGetRwStatusReply(replies.getRwStatusReply()); } - - /** 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; + if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_LAST_RESET_STATUS, + replies.getGetLastResetStatusReply())) { + handleResetStatusReply(replies.getGetLastResetStatusReply()); } - - switch (id) { - case (rws::GET_LAST_RESET_STATUS): { - handleResetStatusReply(packet); - break; - } - case (rws::GET_RW_STATUS): { - handleGetRwStatusReply(packet); - break; - } - case (rws::CLEAR_LAST_RESET_STATUS): - case (rws::INIT_RW_CONTROLLER): - case (rws::SET_SPEED): - // no reply data expected - break; - case (rws::GET_TEMPERATURE): { - handleTemperatureReply(packet); - break; - } - case (rws::GET_TM): { - handleGetTelemetryReply(packet); - break; - } - default: { - sif::debug << "RwHandler::interpretDeviceReply: Unknown device reply id" << std::endl; - return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; - } + checkPacket(rws::DeviceCommandId::CLEAR_LAST_RESET_STATUS, + replies.getClearLastResetStatusReply()); + if (returnvalue::OK == + checkPacket(rws::DeviceCommandId::GET_TEMPERATURE, replies.getReadTemperatureReply())) { + handleTemperatureReply(replies.getReadTemperatureReply()); + } + if (internalState == InternalState::GET_TM) { + if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_TM, replies.getHkDataReply())) { + handleGetTelemetryReply(replies.getHkDataReply()); + } + internalState = InternalState::DEFAULT; + } + if (internalState == InternalState::INIT_RW_CONTROLLER) { + checkPacket(rws::DeviceCommandId::INIT_RW_CONTROLLER, replies.getInitRwControllerReply()); + internalState = InternalState::DEFAULT; + } + if (internalState == InternalState::RESET_MCU) { + internalState = InternalState::DEFAULT; } - return returnvalue::OK; } @@ -312,6 +384,7 @@ ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataP return returnvalue::OK; } +/* void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) { commandBuffer[0] = static_cast(id); uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF); @@ -320,6 +393,7 @@ void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) { rawPacket = commandBuffer; rawPacketLen = 3; } +*/ ReturnValue_t RwHandler::checkSpeedAndRampTime() { int32_t speed = 0; @@ -338,6 +412,7 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime() { return returnvalue::OK; } +/* ReturnValue_t RwHandler::prepareSetSpeedCmd() { commandBuffer[0] = static_cast(rws::SET_SPEED); uint8_t* serPtr = commandBuffer + 1; @@ -357,13 +432,14 @@ ReturnValue_t RwHandler::prepareSetSpeedCmd() { rawPacketLen = 9; return result; } +*/ void RwHandler::handleResetStatusReply(const uint8_t* packet) { PoolReadGuard rg(&lastResetStatusSet); uint8_t offset = 2; uint8_t resetStatus = packet[offset]; if (resetStatus != 0) { - internalState = InternalState::CLEAR_RESET_STATUS; + // internalState = InternalState::CLEAR_RESET_STATUS; lastResetStatusSet.lastNonClearedResetStatus = resetStatus; triggerEvent(rws::RESET_OCCURED, resetStatus, 0); } diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index 7db3ef3b..beba8376 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -79,30 +79,32 @@ class RwHandler : public DeviceHandlerBase { rws::TmDataset tmDataset; rws::RwSpeedActuationSet rwSpeedActuationSet; - uint8_t commandBuffer[rws::MAX_CMD_SIZE]; + uint8_t commandBuffer[32]; uint8_t rwIdx; PoolEntry rwSpeed = PoolEntry({0}); PoolEntry rampTime = PoolEntry({10}); enum class InternalState { - GET_RESET_STATUS, - CLEAR_RESET_STATUS, - READ_TEMPERATURE, - SET_SPEED, - GET_RW_SATUS + DEFAULT, + GET_TM, + INIT_RW_CONTROLLER, + RESET_MCU, + // GET_RESET_STATUS, + // CLEAR_RESET_STATUS, + // READ_TEMPERATURE, + // SET_SPEED, + // GET_RW_SATUS }; - InternalState internalState = InternalState::GET_RESET_STATUS; - - size_t sizeOfReply = 0; + InternalState internalState = InternalState::DEFAULT; /** * @brief This function can be used to build commands which do not contain any data apart * from the command id and the CRC. * @param commandId The command id of the command to build. */ - void prepareSimpleCommand(DeviceCommandId_t id); + // void prepareSimpleCommand(DeviceCommandId_t id); /** * @brief This function checks if the receiced speed and ramp time to set are in a valid @@ -116,7 +118,7 @@ class RwHandler : public DeviceHandlerBase { * an action message or set in the software. * @return returnvalue::OK if successful, otherwise error code. */ - ReturnValue_t prepareSetSpeedCmd(); + // ReturnValue_t prepareSetSpeedCmd(); /** * @brief This function writes the last reset status retrieved with the get last reset status diff --git a/mission/devices/devicedefinitions/rwHelpers.cpp b/mission/devices/devicedefinitions/rwHelpers.cpp index 6ef1daca..d1f6a1bd 100644 --- a/mission/devices/devicedefinitions/rwHelpers.cpp +++ b/mission/devices/devicedefinitions/rwHelpers.cpp @@ -21,4 +21,34 @@ void rws::encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, uint8_t* encode encodedBuffer[encodedLen++] = rws::FRAME_DELIMITER; } +size_t rws::idToPacketLen(DeviceCommandId_t id) { + switch (id) { + case (rws::GET_RW_STATUS): { + return rws::SIZE_GET_RW_STATUS; + } + case (rws::SET_SPEED): { + return rws::SIZE_SET_SPEED_REPLY; + } + case (rws::CLEAR_LAST_RESET_STATUS): { + return rws::SIZE_CLEAR_RESET_STATUS; + } + case (rws::GET_LAST_RESET_STATUS): { + return rws::SIZE_GET_RESET_STATUS; + } + case (rws::GET_TEMPERATURE): { + return rws::SIZE_GET_TEMPERATURE_REPLY; + } + case (rws::GET_TM): { + return rws::SIZE_GET_TELEMETRY_REPLY; + } + case (rws::INIT_RW_CONTROLLER): { + return rws::SIZE_INIT_RW; + } + default: { + sif::error << "no reply buffer for rw command " << id << std::endl; + return 0; + } + } +} + #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ */ diff --git a/mission/devices/devicedefinitions/rwHelpers.h b/mission/devices/devicedefinitions/rwHelpers.h index 9ae68e02..832cf275 100644 --- a/mission/devices/devicedefinitions/rwHelpers.h +++ b/mission/devices/devicedefinitions/rwHelpers.h @@ -13,6 +13,7 @@ namespace rws { void encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, uint8_t* encodedBuffer, size_t& encodedLen); +size_t idToPacketLen(DeviceCommandId_t id); static const size_t SIZE_GET_RESET_STATUS = 5; static const size_t SIZE_CLEAR_RESET_STATUS = 4; @@ -112,21 +113,26 @@ enum LastResetStatusBitPos : uint8_t { LOW_POWER_RESET = 5 }; -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 DeviceCommandId_t GET_TM = 9; +enum DeviceCommandId : DeviceCommandId_t { + RESET_MCU = 1, + + GET_LAST_RESET_STATUS = 2, + CLEAR_LAST_RESET_STATUS = 3, + GET_RW_STATUS = 4, + INIT_RW_CONTROLLER = 5, + SET_SPEED = 6, + GET_TEMPERATURE = 8, + GET_TM = 9 +}; + +static constexpr DeviceCommandId_t REQUEST_ID = 0x77; +static constexpr DeviceCommandId_t REPLY_ID = 0x78; enum SetIds : uint32_t { - TEMPERATURE_SET_ID = GET_TEMPERATURE, - STATUS_SET_ID = GET_RW_STATUS, - LAST_RESET_ID = GET_LAST_RESET_STATUS, - TM_SET_ID = GET_TM, + TEMPERATURE_SET_ID = DeviceCommandId::GET_TEMPERATURE, + STATUS_SET_ID = DeviceCommandId::GET_RW_STATUS, + LAST_RESET_ID = DeviceCommandId::GET_LAST_RESET_STATUS, + TM_SET_ID = DeviceCommandId::GET_TM, SPEED_CMD_SET = 10, };