From c6c92e1140f32963a09ec69ebe487082785f6b9f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 17 Feb 2023 02:10:08 +0100 Subject: [PATCH] it appears to work well now --- fsfw | 2 +- linux/devices/RwPollingTask.cpp | 131 ++++----- linux/devices/RwPollingTask.h | 1 + mission/devices/RwHandler.cpp | 258 +++++------------- mission/devices/RwHandler.h | 1 + mission/devices/devicedefinitions/rwHelpers.h | 47 +++- mission/system/objects/RwAssembly.cpp | 7 +- tmtc | 2 +- 8 files changed, 179 insertions(+), 270 deletions(-) diff --git a/fsfw b/fsfw index fe41d738..a6d707a7 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit fe41d73895270cbe4d80ebfbc41ff9f0b8b02126 +Subproject commit a6d707a7db589136ac2bd917cd8b3a3e2c16a0e4 diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index 9a4fd87a..a0008082 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -32,11 +33,14 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { state = InternalState::IDLE; ipcLock->unlockMutex(); semaphore->acquire(); + // This loop takes 50 ms on a debug build. + // Stopwatch watch; TaskFactory::delayTask(5); int fd = 0; for (auto& skip : skipCommandingForRw) { skip = false; } + setAllReadFlagsFalse(); ReturnValue_t result = openSpi(O_RDWR, fd); if (result != returnvalue::OK) { continue; @@ -55,27 +59,26 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { } } closeSpi(fd); - usleep(rws::SPI_REPLY_DELAY); if (readAllRws(rws::SET_SPEED) != returnvalue::OK) { continue; } -// prepareSimpleCommand(rws::GET_LAST_RESET_STATUS); -// if (writeAndReadAllRws(rws::GET_LAST_RESET_STATUS) != returnvalue::OK) { -// continue; -// } -// prepareSimpleCommand(rws::GET_RW_STATUS); -// if (writeAndReadAllRws(rws::GET_RW_STATUS) != returnvalue::OK) { -// continue; -// } -// prepareSimpleCommand(rws::GET_TEMPERATURE); -// if (writeAndReadAllRws(rws::GET_TEMPERATURE) != returnvalue::OK) { -// continue; -// } -// prepareSimpleCommand(rws::CLEAR_LAST_RESET_STATUS); -// if (writeAndReadAllRws(rws::CLEAR_LAST_RESET_STATUS) != returnvalue::OK) { -// continue; -// } -// handleSpecialRequests(); + prepareSimpleCommand(rws::GET_LAST_RESET_STATUS); + if (writeAndReadAllRws(rws::GET_LAST_RESET_STATUS) != returnvalue::OK) { + continue; + } + prepareSimpleCommand(rws::GET_RW_STATUS); + if (writeAndReadAllRws(rws::GET_RW_STATUS) != returnvalue::OK) { + continue; + } + prepareSimpleCommand(rws::GET_TEMPERATURE); + if (writeAndReadAllRws(rws::GET_TEMPERATURE) != returnvalue::OK) { + continue; + } + prepareSimpleCommand(rws::CLEAR_LAST_RESET_STATUS); + if (writeAndReadAllRws(rws::CLEAR_LAST_RESET_STATUS) != returnvalue::OK) { + continue; + } + handleSpecialRequests(); } return returnvalue::OK; @@ -188,7 +191,6 @@ ReturnValue_t RwPollingTask::writeAndReadAllRws(DeviceCommandId_t id) { } closeSpi(fd); - usleep(rws::SPI_REPLY_DELAY); return readAllRws(id); } @@ -208,50 +210,39 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf int fd = 0; gpioId_t gpioId = rwCookie.getChipSelectPin(); uint8_t byteRead = 0; - bool exitOuter = false; - for (unsigned retryIdx = 0; retryIdx < MAX_RETRIES_REPLY; retryIdx++) { - /** - * The reaction wheel responds with empty frames while preparing the reply data. - * However, receiving more than 5 empty frames will be interpreted as an error. - */ - for (int idx = 0; idx < 5; idx++) { - result = openSpi(O_RDWR, fd); - if (result != returnvalue::OK) { - return result; - } - pullCsLow(gpioId, gpioIF); - if (read(fd, &byteRead, 1) != 1) { - sif::error << "RwPollingTask: Read failed. " << strerror(errno) << std::endl; - pullCsHigh(gpioId, gpioIF); - closeSpi(fd); - return rws::SPI_READ_FAILURE; - } - if (idx == 0) { - if (byteRead != rws::FRAME_DELIMITER) { - sif::error << "Invalid data, expected start marker, got " << (int)byteRead << std::endl; - pullCsHigh(gpioId, gpioIF); - closeSpi(fd); - return rws::NO_START_MARKER; - } - } - - if (byteRead != rws::FRAME_DELIMITER) { - exitOuter = true; - break; - } + result = openSpi(O_RDWR, fd); + if (result != returnvalue::OK) { + return result; + } + pullCsLow(gpioId, gpioIF); + bool lastByteWasFrameMarker = false; + Countdown cd(3000); + size_t readIdx = 0; + while (true) { + lastByteWasFrameMarker = false; + if (read(fd, &byteRead, 1) != 1) { + sif::error << "RwPollingTask: Read failed. " << strerror(errno) << std::endl; pullCsHigh(gpioId, gpioIF); closeSpi(fd); - if (idx == MAX_RETRIES_REPLY - 1) { - sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl; - return rws::NO_REPLY; - } + return rws::SPI_READ_FAILURE; } - if(exitOuter) { + if (byteRead == rws::FRAME_DELIMITER) { + lastByteWasFrameMarker = true; + } + // Start of frame detected. + if (byteRead != rws::FRAME_DELIMITER and not lastByteWasFrameMarker) { break; } - TaskFactory::delayTask(5); + + if (readIdx % 100 == 0 && cd.hasTimedOut()) { + pullCsHigh(gpioId, gpioIF); + closeSpi(fd); + return rws::SPI_READ_FAILURE; + } + readIdx++; } + #if FSFW_HAL_SPI_WIRETAPPING == 1 sif::info << "RW start marker detected" << std::endl; #endif @@ -339,13 +330,14 @@ ReturnValue_t RwPollingTask::readAllRws(DeviceCommandId_t id) { if (((id == rws::SET_SPEED) and !rwCookies[idx]->setSpeed) or skipCommandingForRw[idx]) { continue; } - if (spiLock == nullptr) { - sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; - return returnvalue::FAILED; - } uint8_t* replyBuf; size_t maxReadLen = idAndIdxToReadBuffer(id, idx, &replyBuf); - readNextReply(*rwCookies[idx], replyBuf, maxReadLen); + ReturnValue_t result = readNextReply(*rwCookies[idx], replyBuf + 1, maxReadLen); + if (result == returnvalue::OK) { + // The first byte is always a flag which shows whether the value was read + // properly at least once. + replyBuf[0] = true; + } } // SPI is closed in readNextReply as well. return returnvalue::OK; @@ -440,6 +432,19 @@ void RwPollingTask::handleSpecialRequests() { } } +void RwPollingTask::setAllReadFlagsFalse() { + for (auto& rwCookie : rwCookies) { + RwReplies replies(rwCookie->replyBuf.data()); + replies.getLastResetStatusReply[0] = false; + replies.clearLastResetStatusReply[0] = false; + replies.hkDataReply[0] = false; + replies.readTemperatureReply[0] = false; + replies.rwStatusReply[0] = false; + replies.setSpeedReply[0] = false; + replies.initRwControllerReply[0] = false; + } +} + // This closes the SPI void RwPollingTask::closeSpi(int fd) { // This will perform the function to close the SPI @@ -453,10 +458,10 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) { sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; return returnvalue::FAILED; } - pullCsLow(gpioId, gpioIF); // Add datalinklayer like specified in the datasheet. size_t lenToSend = 0; rws::encodeHdlc(writeBuffer.data(), writeLen, encodedBuffer.data(), lenToSend); + pullCsLow(gpioId, gpioIF); if (write(fd, encodedBuffer.data(), lenToSend) != static_cast(lenToSend)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; pullCsHigh(gpioId, gpioIF); @@ -513,7 +518,7 @@ ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) { speedToSet = rwCookies[rwIdx]->currentRwSpeed; rampTimeToSet = rwCookies[rwIdx]->currentRampTime; } - size_t serLen = 0; + size_t serLen = 1; SerializeAdapter::serialize(&speedToSet, &serPtr, &serLen, writeBuffer.size(), SerializeIF::Endianness::LITTLE); SerializeAdapter::serialize(&rampTimeToSet, &serPtr, &serLen, writeBuffer.size(), diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h index ada6c390..ae4bbeb1 100644 --- a/linux/devices/RwPollingTask.h +++ b/linux/devices/RwPollingTask.h @@ -80,6 +80,7 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev size_t idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** readPtr); void fillSpecialRequestArray(); + void setAllReadFlagsFalse(); void pullCsHigh(gpioId_t gpioId, GpioIF& gpioIF); void closeSpi(int); diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index cd176a67..f60dd498 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include "OBSWConfig.h" @@ -51,26 +52,6 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { *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; @@ -88,6 +69,7 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand ReturnValue_t result = returnvalue::OK; switch (deviceCommand) { + case (rws::SET_SPEED): case (rws::REQUEST_ID): { if (commandData != nullptr && commandDataLen != 6) { sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" @@ -148,66 +130,6 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand 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; } @@ -219,20 +141,9 @@ void RwHandler::fillCommandAndReplyMap() { insertInReplyMap(rws::REPLY_ID, 5, nullptr, 0, true); insertInCommandMap(rws::RESET_MCU); + insertInCommandMap(rws::SET_SPEED); 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, - rws::SIZE_CLEAR_RESET_STATUS); - this->insertInCommandAndReplyMap(rws::GET_RW_STATUS, 1, &statusSet, rws::SIZE_GET_RW_STATUS); - this->insertInCommandAndReplyMap(rws::INIT_RW_CONTROLLER, 1, nullptr, rws::SIZE_INIT_RW); - this->insertInCommandAndReplyMap(rws::GET_TEMPERATURE, 1, nullptr, - 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, @@ -240,64 +151,28 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize if (getMode() == _MODE_WAIT_OFF) { return IGNORE_FULL_PACKET; } - // sif::debug << "base mode: " << baseMode << std::endl; if (remainingSize > 0) { *foundLen = remainingSize; *foundId = rws::REPLY_ID; } - // 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) { RwReplies replies(packet); ReturnValue_t result = returnvalue::OK; + ReturnValue_t status; auto checkPacket = [&](DeviceCommandId_t id, const uint8_t* packetPtr) { + // This is just the packet length of the frame from the RW. The actual + // data is one more flag byte to show whether the value was read at least once. 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; + // arrayprinter::print(packetPtr, packetLen); + uint16_t replyCrc; + size_t dummy = 0; + SerializeAdapter::deSerialize(&replyCrc, packetPtr + packetLen - 2, &dummy, + SerializeIF::Endianness::LITTLE); + if (CRC::crc16ccitt(packetPtr, packetLen - 2) != replyCrc) { + sif::error << "RwHandler::interpretDeviceReply: CRC error for ID " << id << std::endl; return CRC_ERROR; } if (packetPtr[1] == rws::STATE_ERROR) { @@ -307,35 +182,73 @@ ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_ } return returnvalue::OK; }; - checkPacket(rws::DeviceCommandId::SET_SPEED, replies.getSetSpeedReply()); - if (returnvalue::OK == - checkPacket(rws::DeviceCommandId::GET_RW_STATUS, replies.getRwStatusReply())) { - handleGetRwStatusReply(replies.getRwStatusReply()); + if (replies.wasSetSpeedReplyRead()) { + status = checkPacket(rws::DeviceCommandId::SET_SPEED, replies.getSetSpeedReply()); + if (status != returnvalue::OK) { + result = status; + } } - if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_LAST_RESET_STATUS, - replies.getGetLastResetStatusReply())) { - handleResetStatusReply(replies.getGetLastResetStatusReply()); + + if (replies.wasRwStatusRead()) { + status = checkPacket(rws::DeviceCommandId::GET_RW_STATUS, replies.getRwStatusReply()); + if (status == returnvalue::OK) { + handleGetRwStatusReply(replies.getRwStatusReply()); + } else { + result = status; + } } - checkPacket(rws::DeviceCommandId::CLEAR_LAST_RESET_STATUS, - replies.getClearLastResetStatusReply()); - if (returnvalue::OK == - checkPacket(rws::DeviceCommandId::GET_TEMPERATURE, replies.getReadTemperatureReply())) { - handleTemperatureReply(replies.getReadTemperatureReply()); + + if (replies.wasGetLastStatusReplyRead()) { + status = checkPacket(rws::DeviceCommandId::GET_LAST_RESET_STATUS, + replies.getGetLastResetStatusReply()); + if (status == returnvalue::OK) { + handleResetStatusReply(replies.getGetLastResetStatusReply()); + } else { + result = status; + } + } + + if (replies.wasClearLastRsetStatusReplyRead()) { + status = checkPacket(rws::DeviceCommandId::CLEAR_LAST_RESET_STATUS, + replies.getClearLastResetStatusReply()); + if (status != returnvalue::OK) { + result = status; + } + } + + if (replies.wasReadTemperatureReplySet()) { + status = checkPacket(rws::DeviceCommandId::GET_TEMPERATURE, replies.getReadTemperatureReply()); + if (status == returnvalue::OK) { + handleTemperatureReply(replies.getReadTemperatureReply()); + } else { + result = status; + } } if (internalState == InternalState::GET_TM) { - if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_TM, replies.getHkDataReply())) { - handleGetTelemetryReply(replies.getHkDataReply()); + if (replies.wasHkDataReplyRead()) { + status = checkPacket(rws::DeviceCommandId::GET_TM, replies.getHkDataReply()); + if (status == returnvalue::OK) { + handleGetTelemetryReply(replies.getHkDataReply()); + } else { + result = status; + } + internalState = InternalState::DEFAULT; } - internalState = InternalState::DEFAULT; } if (internalState == InternalState::INIT_RW_CONTROLLER) { - checkPacket(rws::DeviceCommandId::INIT_RW_CONTROLLER, replies.getInitRwControllerReply()); - internalState = InternalState::DEFAULT; + if (replies.wasInitRwControllerReplyRead()) { + status = + checkPacket(rws::DeviceCommandId::INIT_RW_CONTROLLER, replies.getInitRwControllerReply()); + if (status != returnvalue::OK) { + result = status; + } + internalState = InternalState::DEFAULT; + } } if (internalState == InternalState::RESET_MCU) { internalState = InternalState::DEFAULT; } - return returnvalue::OK; + return result; } uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } @@ -388,17 +301,6 @@ 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); - commandBuffer[1] = static_cast(crc & 0xFF); - commandBuffer[2] = static_cast(crc >> 8 & 0xFF); - rawPacket = commandBuffer; - rawPacketLen = 3; -} -*/ - ReturnValue_t RwHandler::checkSpeedAndRampTime() { int32_t speed = 0; uint16_t rampTime = 0; @@ -416,28 +318,6 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime() { return returnvalue::OK; } -/* -ReturnValue_t RwHandler::prepareSetSpeedCmd() { - commandBuffer[0] = static_cast(rws::SET_SPEED); - uint8_t* serPtr = commandBuffer + 1; - size_t serSize = 1; - rwSpeedActuationSet.setValidityBufferGeneration(false); - ReturnValue_t result = rwSpeedActuationSet.serialize(&serPtr, &serSize, sizeof(commandBuffer), - SerializeIF::Endianness::LITTLE); - rwSpeedActuationSet.setValidityBufferGeneration(true); - if (result != returnvalue::OK) { - return result; - } - - uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF); - commandBuffer[7] = static_cast(crc & 0xFF); - commandBuffer[8] = static_cast((crc >> 8) & 0xFF); - rawPacket = commandBuffer; - rawPacketLen = 9; - return result; -} -*/ - void RwHandler::handleResetStatusReply(const uint8_t* packet) { PoolReadGuard rg(&lastResetStatusSet); uint8_t offset = 2; diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index 756aed53..cdb72de1 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -70,6 +70,7 @@ class RwHandler : public DeviceHandlerBase { static const ReturnValue_t EXECUTION_FAILED = MAKE_RETURN_CODE(0xA3); //! [EXPORT] : [COMMENT] Reaction wheel reply has invalid crc static const ReturnValue_t CRC_ERROR = MAKE_RETURN_CODE(0xA4); + static const ReturnValue_t VALUE_NOT_READ = MAKE_RETURN_CODE(0xA5); GpioIF* gpioComIF = nullptr; gpioId_t enableGpio = gpio::NO_GPIO; diff --git a/mission/devices/devicedefinitions/rwHelpers.h b/mission/devices/devicedefinitions/rwHelpers.h index f9f737eb..05e63db0 100644 --- a/mission/devices/devicedefinitions/rwHelpers.h +++ b/mission/devices/devicedefinitions/rwHelpers.h @@ -53,6 +53,8 @@ static const ReturnValue_t MISSING_END_SIGN = MAKE_RETURN_CODE(0xB4); static const ReturnValue_t NO_REPLY = MAKE_RETURN_CODE(0xB5); //! [EXPORT] : [COMMENT] Expected a start marker as first byte static const ReturnValue_t NO_START_MARKER = MAKE_RETURN_CODE(0xB6); +//! [EXPORT] : [COMMENT] Timeout when reading reply +static const ReturnValue_t SPI_READ_TIMEOUT = MAKE_RETURN_CODE(0xB7); static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::RW_HANDLER; @@ -62,7 +64,7 @@ static constexpr Event ERROR_STATE = MAKE_EVENT(1, severity::HIGH); static constexpr Event RESET_OCCURED = event::makeEvent(SUBSYSTEM_ID, 2, severity::LOW); //! Minimal delay as specified by the datasheet. -static const uint32_t SPI_REPLY_DELAY = 70000; // us +static const uint32_t SPI_REPLY_DELAY = 20000; // us enum PoolIds : lp_id_t { TEMPERATURE_C, @@ -272,40 +274,55 @@ class RwSpeedActuationSet : public StaticLocalDataSet<2> { } // namespace rws +/** + * Raw pointer overlay to hold the different frames received from the reaction + * wheel in a raw buffer and send them to the device handler. + */ struct RwReplies { friend class RwPollingTask; public: RwReplies(const uint8_t* rawData) : rawData(const_cast(rawData)) { initPointers(); } - const uint8_t* getClearLastResetStatusReply() const { return clearLastResetStatusReply; } + const uint8_t* getClearLastResetStatusReply() const { return clearLastResetStatusReply + 1; } + bool wasClearLastRsetStatusReplyRead() const { return clearLastResetStatusReply[0]; } - const uint8_t* getGetLastResetStatusReply() const { return getLastResetStatusReply; } + const uint8_t* getGetLastResetStatusReply() const { return getLastResetStatusReply + 1; } + bool wasGetLastStatusReplyRead() const { return getLastResetStatusReply[0]; } - const uint8_t* getHkDataReply() const { return hkDataReply; } + const uint8_t* getHkDataReply() const { return hkDataReply + 1; } + bool wasHkDataReplyRead() const { return hkDataReply[0]; } - const uint8_t* getInitRwControllerReply() const { return initRwControllerReply; } + const uint8_t* getInitRwControllerReply() const { return initRwControllerReply + 1; } + bool wasInitRwControllerReplyRead() const { return initRwControllerReply[0]; } const uint8_t* getRawData() const { return rawData; } - const uint8_t* getReadTemperatureReply() const { return readTemperatureReply; } + const uint8_t* getReadTemperatureReply() const { return readTemperatureReply + 1; } + bool wasReadTemperatureReplySet() const { return readTemperatureReply[0]; } - const uint8_t* getRwStatusReply() const { return rwStatusReply; } + const uint8_t* getRwStatusReply() const { return rwStatusReply + 1; } + bool wasRwStatusRead() const { return rwStatusReply[0]; } - const uint8_t* getSetSpeedReply() const { return setSpeedReply; } + const uint8_t* getSetSpeedReply() const { return setSpeedReply + 1; } + bool wasSetSpeedReplyRead() const { return setSpeedReply[0]; } private: RwReplies(uint8_t* rwData) : rawData(rwData) { initPointers(); } + /** + * The first byte of the reply buffers contains a flag which shows whether that + * frame was read from the reaction wheel at least once. + */ void initPointers() { rwStatusReply = rawData; - setSpeedReply = rawData + rws::SIZE_GET_RW_STATUS; - getLastResetStatusReply = setSpeedReply + rws::SIZE_SET_SPEED_REPLY; - clearLastResetStatusReply = getLastResetStatusReply + rws::SIZE_GET_RESET_STATUS; - readTemperatureReply = clearLastResetStatusReply + rws::SIZE_CLEAR_RESET_STATUS; - hkDataReply = readTemperatureReply + rws::SIZE_GET_TEMPERATURE_REPLY; - initRwControllerReply = hkDataReply + rws::SIZE_GET_TELEMETRY_REPLY; - dummyPointer = initRwControllerReply + rws::SIZE_INIT_RW; + setSpeedReply = rawData + rws::SIZE_GET_RW_STATUS + 1; + getLastResetStatusReply = setSpeedReply + rws::SIZE_SET_SPEED_REPLY + 1; + clearLastResetStatusReply = getLastResetStatusReply + rws::SIZE_GET_RESET_STATUS + 1; + readTemperatureReply = clearLastResetStatusReply + rws::SIZE_CLEAR_RESET_STATUS + 1; + hkDataReply = readTemperatureReply + rws::SIZE_GET_TEMPERATURE_REPLY + 1; + initRwControllerReply = hkDataReply + rws::SIZE_GET_TELEMETRY_REPLY + 1; + dummyPointer = initRwControllerReply + rws::SIZE_INIT_RW + 1; } uint8_t* rawData; uint8_t* rwStatusReply; diff --git a/mission/system/objects/RwAssembly.cpp b/mission/system/objects/RwAssembly.cpp index 805863f1..886ec5ce 100644 --- a/mission/system/objects/RwAssembly.cpp +++ b/mission/system/objects/RwAssembly.cpp @@ -167,7 +167,12 @@ bool RwAssembly::isUseable(object_id_t object, Mode_t mode) { return false; } -ReturnValue_t RwAssembly::initialize() { return AssemblyBase::initialize(); } +ReturnValue_t RwAssembly::initialize() { + for (auto objId : helper.rwIds) { + updateChildModeByObjId(objId, MODE_OFF, SUBMODE_NONE); + } + return AssemblyBase::initialize(); +} void RwAssembly::handleModeTransitionFailed(ReturnValue_t result) { if (targetMode == MODE_OFF) { diff --git a/tmtc b/tmtc index 1e143ea6..08c315fb 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 1e143ea6faa608baf3118512416f5a495dbd606c +Subproject commit 08c315fbf92a3256663749e5b4dad11fcc1c02e4