diff --git a/CHANGELOG.md b/CHANGELOG.md index 21c92c59..940e1435 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,14 @@ will consitute of a breaking change warranting a new major release: - RW Assembly: Correctly transition back to off when more than 1 devices is OFF. Also do this when this was due to two devices being marked faulty. - RW dummy and STR dummy components: Set/Update modes correctly. +- RW handlers: Bugfix for TM set retrieval and special request handling in general where the CRC + check always failed for special request. Also removed an unnecessary delay for special requests. +- RW handlers: Polling is now disabled for RWs which are off. + +## Changed + +- RW shutdown now waits for the speed to be near 0 or for a OFF transition countdown to be expired + before going to off. # [v1.43.2] 2023-04-05 diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index e248938a..3f7000c7 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -237,7 +237,7 @@ void scheduling::initTasks() { #if OBSW_ADD_RW == 1 PeriodicTaskIF* rwPolling = - factory->createPeriodicTask("RW_POLLING_TASK", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, + factory->createPeriodicTask("RW_POLLING_TASK", 75, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc, &RR_SCHEDULING); result = rwPolling->addComponent(objects::RW_POLLING_TASK); if (result != returnvalue::OK) { diff --git a/linux/acs/RwPollingTask.cpp b/linux/acs/RwPollingTask.cpp index 07958d8b..34ff13fb 100644 --- a/linux/acs/RwPollingTask.cpp +++ b/linux/acs/RwPollingTask.cpp @@ -11,6 +11,7 @@ #include #include "devConf.h" +#include "mission/acs/defs.h" #include "mission/acs/rwHelpers.h" RwPollingTask::RwPollingTask(object_id_t objectId, const char* spiDev, GpioIF& gpioIF) @@ -35,6 +36,7 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { semaphore->acquire(); // This loop takes 50 ms on a debug build. // Stopwatch watch; + // Give all device handlers some time to submit requests. TaskFactory::delayTask(5); int fd = 0; for (auto& skip : skipCommandingForRw) { @@ -45,13 +47,24 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { if (result != returnvalue::OK) { continue; } + acs::SimpleSensorMode currentMode; + rws::SpecialRwRequest specialRequest; + for (unsigned idx = 0; idx < rwCookies.size(); idx++) { - if (rwCookies[idx]->specialRequest == rws::SpecialRwRequest::RESET_MCU) { + { + MutexGuard mg(ipcLock); + currentMode = rwRequests[idx].mode; + specialRequest = rwRequests[idx].specialRequest; + skipSetSpeedReply[idx] = rwRequests[idx].setSpeed; + } + if (currentMode == acs::SimpleSensorMode::OFF) { + skipCommandingForRw[idx] = true; + } else if (specialRequest == rws::SpecialRwRequest::RESET_MCU) { prepareSimpleCommand(rws::RESET_MCU); // No point in commanding that specific RW for the cycle. skipCommandingForRw[idx] = true; writeOneRwCmd(idx, fd); - } else if (rwCookies[idx]->setSpeed) { + } else if (skipSetSpeedReply[idx]) { prepareSetSpeedCmd(idx); if (writeOneRwCmd(idx, fd) != returnvalue::OK) { continue; @@ -121,31 +134,14 @@ ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) { ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { - if (sendData == nullptr or sendLen < 8) { + if (sendData == nullptr or sendLen != sizeof(rws::RwRequest)) { return DeviceHandlerIF::INVALID_DATA; } - int32_t speed = 0; - uint16_t rampTime = 0; - 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 == 8 and sendData[7] < static_cast(rws::SpecialRwRequest::NUM_REQUESTS)) { - specialRequest = static_cast(sendData[7]); - } - RwCookie* rwCookie = dynamic_cast(cookie); - if (rwCookie == nullptr) { - return returnvalue::FAILED; - } + const rws::RwRequest* rwRequest = reinterpret_cast(sendData); + uint8_t rwIdx = rwRequest->rwIdx; { MutexGuard mg(ipcLock); - rwCookie->setSpeed = setSpeed; - rwCookie->currentRwSpeed = speed; - rwCookie->currentRampTime = rampTime; - rwCookie->specialRequest = specialRequest; + std::memcpy(&rwRequests[rwIdx], rwRequest, sizeof(rws::RwRequest)); if (state == InternalState::IDLE) { state = InternalState::IS_BUSY; semaphore->release(); @@ -332,7 +328,7 @@ ReturnValue_t RwPollingTask::writeOneRwCmd(uint8_t rwIdx, int fd) { ReturnValue_t RwPollingTask::readAllRws(DeviceCommandId_t id) { // SPI dev will be opened in readNextReply on demand. for (unsigned idx = 0; idx < rwCookies.size(); idx++) { - if (((id == rws::SET_SPEED) and !rwCookies[idx]->setSpeed) or skipCommandingForRw[idx]) { + if (((id == rws::SET_SPEED) and !skipSetSpeedReply[idx]) or skipCommandingForRw[idx]) { continue; } uint8_t* replyBuf; @@ -395,7 +391,7 @@ void RwPollingTask::fillSpecialRequestArray() { specialRequestIds[idx] = DeviceHandlerIF::NO_COMMAND_ID; continue; } - switch (rwCookies[idx]->specialRequest) { + switch (rwRequests[idx].specialRequest) { case (rws::SpecialRwRequest::GET_TM): { specialRequestIds[idx] = rws::GET_TM; break; @@ -426,14 +422,14 @@ void RwPollingTask::handleSpecialRequests() { writeOneRwCmd(idx, fd); } closeSpi(fd); - usleep(rws::SPI_REPLY_DELAY); for (unsigned idx = 0; idx < rwCookies.size(); idx++) { if (specialRequestIds[idx] == DeviceHandlerIF::NO_COMMAND_ID) { continue; } uint8_t* replyBuf; size_t maxReadLen = idAndIdxToReadBuffer(specialRequestIds[idx], idx, &replyBuf); - result = readNextReply(*rwCookies[idx], replyBuf, maxReadLen); + // Skip first byte for actual read buffer: Valid byte + 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. @@ -455,17 +451,12 @@ void RwPollingTask::setAllReadFlagsFalse() { } } -// This closes the SPI -void RwPollingTask::closeSpi(int fd) { - // This will perform the function to close the SPI - close(fd); - // The SPI is now closed. -} +void RwPollingTask::closeSpi(int fd) { close(fd); } ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) { gpioId_t gpioId = rwCookie.getChipSelectPin(); if (spiLock == nullptr) { - sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; + sif::warning << "RwPollingTask: Mutex or GPIO interface invalid" << std::endl; return returnvalue::FAILED; } // Add datalinklayer like specified in the datasheet. @@ -473,7 +464,7 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) { 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; + sif::error << "RwPollingTask: Write failed!" << std::endl; pullCsHigh(gpioId, gpioIF); return rws::SPI_WRITE_FAILURE; } @@ -484,7 +475,7 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) { ReturnValue_t RwPollingTask::pullCsLow(gpioId_t gpioId, GpioIF& gpioIF) { ReturnValue_t result = spiLock->lockMutex(TIMEOUT_TYPE, TIMEOUT_MS); if (result != returnvalue::OK) { - sif::debug << "RwPollingTask::pullCsLow: Failed to lock mutex" << std::endl; + sif::warning << "RwPollingTask::pullCsLow: Failed to lock mutex" << std::endl; return result; } // Pull SPI CS low. For now, no support for active high given @@ -525,8 +516,8 @@ ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) { uint16_t rampTimeToSet = 10; { MutexGuard mg(ipcLock); - speedToSet = rwCookies[rwIdx]->currentRwSpeed; - rampTimeToSet = rwCookies[rwIdx]->currentRampTime; + speedToSet = rwRequests[rwIdx].currentRwSpeed; + rampTimeToSet = rwRequests[rwIdx].currentRampTime; } size_t serLen = 1; SerializeAdapter::serialize(&speedToSet, &serPtr, &serLen, writeBuffer.size(), diff --git a/linux/acs/RwPollingTask.h b/linux/acs/RwPollingTask.h index 5fb25d1d..f06bc622 100644 --- a/linux/acs/RwPollingTask.h +++ b/linux/acs/RwPollingTask.h @@ -8,6 +8,7 @@ #include #include #include +#include #include "mission/acs/rwHelpers.h" @@ -26,10 +27,6 @@ class RwCookie : public SpiCookie { std::array replyBuf{}; std::array exchangeBuf{}; MutexIF* bufLock; - bool setSpeed = true; - int32_t currentRwSpeed = 0; - uint16_t currentRampTime = 0; - rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE; uint8_t rwIdx; }; @@ -50,8 +47,10 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev const char* spiDev; GpioIF& gpioIF; std::array skipCommandingForRw; + std::array skipSetSpeedReply; std::array specialRequestIds; std::array rwCookies; + std::array rwRequests{}; std::array writeBuffer; std::array encodedBuffer; diff --git a/mission/acs/RwHandler.cpp b/mission/acs/RwHandler.cpp index 87979ae8..3a5cabf8 100644 --- a/mission/acs/RwHandler.cpp +++ b/mission/acs/RwHandler.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "OBSWConfig.h" @@ -23,44 +24,52 @@ RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCooki if (gpioComIF == nullptr) { sif::error << "RwHandler: Invalid gpio communication interface" << std::endl; } + currentRequest.rwIdx = rwIdx; } + RwHandler::~RwHandler() {} void RwHandler::doStartUp() { internalState = InternalState::DEFAULT; if (gpioComIF->pullHigh(enableGpio) != returnvalue::OK) { - sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high"; + sif::error << "RW Handler " << rwIdx << ": Failed to pull ENABLE pin high for startup"; } + currentRequest.mode = acs::SimpleSensorMode::NORMAL; updatePeriodicReply(true, rws::REPLY_ID); statusSet.setReportingEnabled(true); setMode(_MODE_TO_ON); } void RwHandler::doShutDown() { - if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) { - sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low"; + if (internalState != InternalState::SHUTDOWN) { + internalState = InternalState::SHUTDOWN; + shutdownState = ShutdownState::SET_SPEED_ZERO; + offTransitionCountdown.resetTimer(); } - internalState = InternalState::DEFAULT; - updatePeriodicReply(false, rws::REPLY_ID); - { - PoolReadGuard pg(&statusSet); - statusSet.currSpeed = 0.0; - statusSet.referenceSpeed = 0.0; - statusSet.state = 0; - statusSet.setValidity(false, true); - statusSet.setReportingEnabled(false); + if ((internalState == InternalState::SHUTDOWN) and + (shutdownState == ShutdownState::DONE or offTransitionCountdown.hasTimedOut())) { + if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) { + sif::error << "RW Handler " << rwIdx << ": Failed to pull ENABLE pin low for shutdown"; + } + updatePeriodicReply(false, rws::REPLY_ID); + { + PoolReadGuard pg(&statusSet); + statusSet.currSpeed = 0.0; + statusSet.referenceSpeed = 0.0; + statusSet.state = 0; + statusSet.setValidity(false, true); + statusSet.setReportingEnabled(false); + } + { + PoolReadGuard pg(&tmDataset); + tmDataset.setValidity(false, true); + } + internalState = InternalState::DEFAULT; + shutdownState = ShutdownState::NONE; + // The power switch is handled by the assembly, so we can go off here directly. + setMode(MODE_OFF); } - { - PoolReadGuard pg(&tmDataset); - tmDataset.setValidity(false, true); - } - { - PoolReadGuard pg(&rwSpeedActuationSet); - rwSpeedActuationSet.setRwSpeed(0, 10); - } - // The power switch is handled by the assembly, so we can go off here directly. - setMode(MODE_OFF); } ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { @@ -77,6 +86,20 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { } ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { + if (internalState == InternalState::SHUTDOWN) { + if (shutdownState == ShutdownState::SET_SPEED_ZERO) { + { + PoolReadGuard pg(&rwSpeedActuationSet); + rwSpeedActuationSet.setRwSpeed(0, 10); + } + *id = rws::REQUEST_ID; + return buildCommandFromCommand(*id, nullptr, 0); + } else if (shutdownState == ShutdownState::STOP_POLLING) { + currentRequest.mode = acs::SimpleSensorMode::OFF; + *id = rws::REQUEST_ID; + return buildCommandFromCommand(*id, nullptr, 0); + } + } return NOTHING_TO_SEND; } @@ -119,32 +142,36 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand 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); - rawPacket = commandBuffer; + currentRequest.setSpeed = true; + rwSpeedActuationSet.getRwSpeed(currentRequest.currentRwSpeed, currentRequest.currentRampTime); + currentRequest.specialRequest = rws::SpecialRwRequest::REQUEST_NONE; + rawPacket = reinterpret_cast(¤tRequest); + rawPacketLen = sizeof(rws::RwRequest); return returnvalue::OK; } case (rws::RESET_MCU): { - commandBuffer[0] = false; - commandBuffer[7] = static_cast(rws::SpecialRwRequest::RESET_MCU); + currentRequest.setSpeed = false; + currentRequest.specialRequest = rws::SpecialRwRequest::RESET_MCU; internalState = InternalState::RESET_MCU; + rawPacket = reinterpret_cast(¤tRequest); + rawPacketLen = sizeof(rws::RwRequest); return returnvalue::OK; } case (rws::INIT_RW_CONTROLLER): { - commandBuffer[0] = false; - commandBuffer[7] = static_cast(rws::SpecialRwRequest::INIT_RW_CONTROLLER); + currentRequest.setSpeed = false; + currentRequest.specialRequest = rws::SpecialRwRequest::INIT_RW_CONTROLLER; internalState = InternalState::INIT_RW_CONTROLLER; + rawPacket = reinterpret_cast(¤tRequest); + rawPacketLen = sizeof(rws::RwRequest); return returnvalue::OK; } case (rws::GET_TM): { - commandBuffer[0] = false; - commandBuffer[7] = static_cast(rws::SpecialRwRequest::GET_TM); + currentRequest.setSpeed = false; + currentRequest.specialRequest = rws::SpecialRwRequest::GET_TM; internalState = InternalState::GET_TM; + rawPacket = reinterpret_cast(¤tRequest); + rawPacketLen = sizeof(rws::RwRequest); return returnvalue::OK; } default: @@ -172,6 +199,9 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize *foundLen = remainingSize; *foundId = rws::REPLY_ID; } + if (internalState == InternalState::SHUTDOWN and shutdownState == ShutdownState::STOP_POLLING) { + shutdownState = ShutdownState::DONE; + } return returnvalue::OK; } @@ -381,6 +411,12 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) { statusSet.setValidity(true, true); + if (internalState == InternalState::SHUTDOWN and std::abs(tmDataset.rwCurrSpeed.value) <= 2 and + shutdownState == ShutdownState::SET_SPEED_ZERO) { + // Finish transition to off. + shutdownState = ShutdownState::STOP_POLLING; + } + if (statusSet.state == rws::STATE_ERROR) { // Trigger FDIR reaction, first recovery, then faulty if it doesnt fix the issue. triggerEvent(DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT, statusSet.state.value, 0); diff --git a/mission/acs/RwHandler.h b/mission/acs/RwHandler.h index 0537ef8c..a8903b81 100644 --- a/mission/acs/RwHandler.h +++ b/mission/acs/RwHandler.h @@ -75,6 +75,8 @@ class RwHandler : public DeviceHandlerBase { GpioIF* gpioComIF = nullptr; gpioId_t enableGpio = gpio::NO_GPIO; bool debugMode = false; + Countdown offTransitionCountdown = Countdown(5000); + rws::RwRequest currentRequest; rws::StatusSet statusSet; rws::LastResetSatus lastResetStatusSet; @@ -87,27 +89,16 @@ class RwHandler : public DeviceHandlerBase { PoolEntry rwSpeed = PoolEntry({0}); PoolEntry rampTime = PoolEntry({10}); - enum class InternalState { - DEFAULT, - GET_TM, - INIT_RW_CONTROLLER, - RESET_MCU, - // GET_RESET_STATUS, - // CLEAR_RESET_STATUS, - // READ_TEMPERATURE, - // SET_SPEED, - // GET_RW_SATUS - }; + enum class InternalState { DEFAULT, GET_TM, INIT_RW_CONTROLLER, RESET_MCU, SHUTDOWN }; + enum class ShutdownState { + NONE, + SET_SPEED_ZERO, + STOP_POLLING, + DONE, + } shutdownState = ShutdownState::NONE; 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); - /** * @brief This function checks if the receiced speed and ramp time to set are in a valid * range. @@ -115,13 +106,6 @@ class RwHandler : public DeviceHandlerBase { */ ReturnValue_t checkSpeedAndRampTime(); - /** - * @brief This function prepares the set speed command from the dataSet received with - * an action message or set in the software. - * @return returnvalue::OK if successful, otherwise error code. - */ - // ReturnValue_t prepareSetSpeedCmd(); - /** * @brief This function writes the last reset status retrieved with the get last reset status * command into the reset status dataset. diff --git a/mission/acs/rwHelpers.h b/mission/acs/rwHelpers.h index 05e63db0..76512119 100644 --- a/mission/acs/rwHelpers.h +++ b/mission/acs/rwHelpers.h @@ -4,6 +4,7 @@ #include #include #include +#include #include "eive/resultClassIds.h" #include "events/subsystemIdRanges.h" @@ -36,6 +37,15 @@ enum class SpecialRwRequest : uint8_t { NUM_REQUESTS }; +struct RwRequest { + acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF; + bool setSpeed = true; + int32_t currentRwSpeed = 0; + uint16_t currentRampTime = 0; + rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE; + uint8_t rwIdx = 0; +}; + static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER; static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0); diff --git a/tmtc b/tmtc index 91a8a2e8..98a9601d 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 91a8a2e89519ac20d9ddabec2d8eaeb7707718d5 +Subproject commit 98a9601dd7610dc7d2f47a622da898900c7e7f04