From cd8bbaf1f94aed2973c65d693dbdd608865a782a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Apr 2023 17:02:37 +0200 Subject: [PATCH] RW fixes and test --- bsp_q7s/core/scheduling.cpp | 2 +- linux/acs/RwPollingTask.cpp | 59 ++++++++++++--------------------- linux/acs/RwPollingTask.h | 6 ++-- mission/acs/RwHandler.cpp | 66 +++++++++++++++++++++++-------------- mission/acs/RwHandler.h | 8 ++++- mission/acs/rwHelpers.h | 10 ++++++ tmtc | 2 +- 7 files changed, 84 insertions(+), 69 deletions(-) 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..d118ba69 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) { @@ -46,12 +48,14 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { continue; } for (unsigned idx = 0; idx < rwCookies.size(); idx++) { - if (rwCookies[idx]->specialRequest == rws::SpecialRwRequest::RESET_MCU) { + if (rwRequests[idx].mode == acs::SimpleSensorMode::OFF) { + skipCommandingForRw[idx] = true; + } else if (rwRequests[idx].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 (rwRequests[idx].setSpeed) { prepareSetSpeedCmd(idx); if (writeOneRwCmd(idx, fd) != returnvalue::OK) { continue; @@ -121,31 +125,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 +319,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 !rwRequests[idx].setSpeed) or skipCommandingForRw[idx]) { continue; } uint8_t* replyBuf; @@ -395,7 +382,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 +413,15 @@ void RwPollingTask::handleSpecialRequests() { writeOneRwCmd(idx, fd); } closeSpi(fd); - usleep(rws::SPI_REPLY_DELAY); + // 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 +443,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 +456,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 +467,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 +508,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..e43ae46e 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; }; @@ -52,6 +49,7 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev std::array skipCommandingForRw; 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 30dc81bc..3a5cabf8 100644 --- a/mission/acs/RwHandler.cpp +++ b/mission/acs/RwHandler.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "OBSWConfig.h" @@ -23,7 +24,9 @@ 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() { @@ -32,6 +35,7 @@ void RwHandler::doStartUp() { if (gpioComIF->pullHigh(enableGpio) != returnvalue::OK) { 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); @@ -40,11 +44,11 @@ void RwHandler::doStartUp() { void RwHandler::doShutDown() { if (internalState != InternalState::SHUTDOWN) { internalState = InternalState::SHUTDOWN; - commandExecuted = false; + shutdownState = ShutdownState::SET_SPEED_ZERO; offTransitionCountdown.resetTimer(); } - if (((internalState == InternalState::SHUTDOWN) and commandExecuted) or - offTransitionCountdown.hasTimedOut()) { + 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"; } @@ -61,8 +65,8 @@ void RwHandler::doShutDown() { PoolReadGuard pg(&tmDataset); tmDataset.setValidity(false, true); } - commandExecuted = false; internalState = InternalState::DEFAULT; + shutdownState = ShutdownState::NONE; // The power switch is handled by the assembly, so we can go off here directly. setMode(MODE_OFF); } @@ -83,12 +87,18 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { if (internalState == InternalState::SHUTDOWN) { - { - PoolReadGuard pg(&rwSpeedActuationSet); - rwSpeedActuationSet.setRwSpeed(0, 10); + 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); } - *id = rws::REQUEST_ID; - return buildCommandFromCommand(*id, nullptr, 0); } return NOTHING_TO_SEND; } @@ -132,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: @@ -185,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; } @@ -394,9 +411,10 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) { statusSet.setValidity(true, true); - if (internalState == InternalState::SHUTDOWN and std::abs(tmDataset.rwCurrSpeed.value) <= 2) { + if (internalState == InternalState::SHUTDOWN and std::abs(tmDataset.rwCurrSpeed.value) <= 2 and + shutdownState == ShutdownState::SET_SPEED_ZERO) { // Finish transition to off. - commandExecuted = true; + shutdownState = ShutdownState::STOP_POLLING; } if (statusSet.state == rws::STATE_ERROR) { diff --git a/mission/acs/RwHandler.h b/mission/acs/RwHandler.h index 07165934..a8903b81 100644 --- a/mission/acs/RwHandler.h +++ b/mission/acs/RwHandler.h @@ -75,8 +75,8 @@ class RwHandler : public DeviceHandlerBase { GpioIF* gpioComIF = nullptr; gpioId_t enableGpio = gpio::NO_GPIO; bool debugMode = false; - bool commandExecuted = false; Countdown offTransitionCountdown = Countdown(5000); + rws::RwRequest currentRequest; rws::StatusSet statusSet; rws::LastResetSatus lastResetStatusSet; @@ -90,6 +90,12 @@ class RwHandler : public DeviceHandlerBase { PoolEntry rampTime = PoolEntry({10}); 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; 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