From 8b0eceb072a4edbcc45b1abb5ca6532286987461 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Feb 2023 17:02:22 +0100 Subject: [PATCH 01/18] continue rw refactoring --- bsp_q7s/callbacks/rwSpiCallback.cpp | 24 +- bsp_q7s/core/ObjectFactory.cpp | 8 +- dummies/RwDummy.cpp | 64 +-- linux/devices/CMakeLists.txt | 2 +- linux/devices/RwPollingTask.cpp | 401 ++++++++++++++++++ linux/devices/RwPollingTask.h | 78 ++++ mission/controller/AcsController.h | 10 +- mission/controller/ThermalController.cpp | 10 +- mission/controller/acs/SensorValues.h | 11 +- mission/devices/RwHandler.cpp | 184 ++++---- mission/devices/RwHandler.h | 30 +- .../devices/devicedefinitions/rwHelpers.cpp | 6 + .../{RwDefinitions.h => rwHelpers.h} | 123 ++++-- 13 files changed, 739 insertions(+), 212 deletions(-) create mode 100644 linux/devices/RwPollingTask.cpp create mode 100644 linux/devices/RwPollingTask.h create mode 100644 mission/devices/devicedefinitions/rwHelpers.cpp rename mission/devices/devicedefinitions/{RwDefinitions.h => rwHelpers.h} (67%) diff --git a/bsp_q7s/callbacks/rwSpiCallback.cpp b/bsp_q7s/callbacks/rwSpiCallback.cpp index 74ede602..7f71a004 100644 --- a/bsp_q7s/callbacks/rwSpiCallback.cpp +++ b/bsp_q7s/callbacks/rwSpiCallback.cpp @@ -76,7 +76,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - return RwHandler::SPI_WRITE_FAILURE; + return rws::SPI_WRITE_FAILURE; } /** Encoding and sending command */ @@ -101,7 +101,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - return RwHandler::SPI_WRITE_FAILURE; + return rws::SPI_WRITE_FAILURE; } idx++; } @@ -113,7 +113,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (write(fileDescriptor, writeBuffer, writeSize) != static_cast(writeSize)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - return RwHandler::SPI_WRITE_FAILURE; + return rws::SPI_WRITE_FAILURE; } uint8_t* rxBuf = nullptr; @@ -128,7 +128,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen // There must be a delay of at least 20 ms after sending the command. // Delay for 70 ms here and release the SPI bus for that duration. closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - usleep(RwDefinitions::SPI_REPLY_DELAY); + usleep(rws::SPI_REPLY_DELAY); result = openSpi(dev, O_RDWR, &gpioIF, gpioId, mutex, timeoutType, timeoutMs, fileDescriptor); if (result != returnvalue::OK) { return result; @@ -143,13 +143,13 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (read(fileDescriptor, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - return RwHandler::SPI_READ_FAILURE; + return rws::SPI_READ_FAILURE; } if (idx == 0) { if (byteRead != FLAG_BYTE) { sif::error << "Invalid data, expected start marker" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - return RwHandler::NO_START_MARKER; + return rws::NO_START_MARKER; } } @@ -160,7 +160,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (idx == 9) { sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - return RwHandler::NO_REPLY; + return rws::NO_REPLY; } } @@ -175,7 +175,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen byteRead = 0; if (read(fileDescriptor, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - result = RwHandler::SPI_READ_FAILURE; + result = rws::SPI_READ_FAILURE; break; } } @@ -186,7 +186,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen } else if (byteRead == 0x7D) { if (read(fileDescriptor, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - result = RwHandler::SPI_READ_FAILURE; + result = rws::SPI_READ_FAILURE; break; } if (byteRead == 0x5E) { @@ -200,7 +200,7 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen } else { sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl; closeSpi(fileDescriptor, gpioId, &gpioIF, mutex); - result = RwHandler::INVALID_SUBSTITUTE; + result = rws::INVALID_SUBSTITUTE; break; } } else { @@ -217,14 +217,14 @@ ReturnValue_t spiCallback(SpiComIF* comIf, SpiCookie* cookie, const uint8_t* sen if (decodedFrameLen == replyBufferSize) { if (read(fileDescriptor, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl; - result = RwHandler::SPI_READ_FAILURE; + result = rws::SPI_READ_FAILURE; break; } if (byteRead != FLAG_BYTE) { sif::error << "rwSpiCallback::spiCallback: Missing end sign " << static_cast(FLAG_BYTE) << std::endl; decodedFrameLen--; - result = RwHandler::MISSING_END_SIGN; + result = rws::MISSING_END_SIGN; break; } } diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index 2631a628..1f920f6d 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include @@ -94,7 +95,6 @@ #include "mission/devices/devicedefinitions/GomspaceDefinitions.h" #include "mission/devices/devicedefinitions/Max31865Definitions.h" #include "mission/devices/devicedefinitions/RadSensorDefinitions.h" -#include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SyrlinksDefinitions.h" #include "mission/devices/devicedefinitions/payloadPcduDefinitions.h" #include "mission/system/objects/AcsBoardAssembly.h" @@ -679,9 +679,9 @@ void ObjectFactory::createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, gpioIds::EN_RW4}; std::array rws = {}; for (uint8_t idx = 0; idx < rwCookies.size(); idx++) { - rwCookies[idx] = new SpiCookie(rwCookieParams[idx].first, rwCookieParams[idx].second, - RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED, - &rwSpiCallback::spiCallback, nullptr); + rwCookies[idx] = + new SpiCookie(rwCookieParams[idx].first, rwCookieParams[idx].second, rws::MAX_REPLY_SIZE, + spi::RW_MODE, spi::RW_SPEED, &rwSpiCallback::spiCallback, nullptr); auto* rwHandler = new RwHandler(rwIds[idx], objects::SPI_RW_COM_IF, rwCookies[idx], gpioComIF, rwGpioIds[idx], idx); rwCookies[idx]->setCallbackArgs(rwHandler); diff --git a/dummies/RwDummy.cpp b/dummies/RwDummy.cpp index a21e7ab7..54e7ac83 100644 --- a/dummies/RwDummy.cpp +++ b/dummies/RwDummy.cpp @@ -1,6 +1,6 @@ #include "RwDummy.h" -#include +#include RwDummy::RwDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie) : DeviceHandlerBase(objectId, comif, comCookie) {} @@ -37,39 +37,39 @@ uint32_t RwDummy::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return ReturnValue_t RwDummy::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TEMPERATURE_C, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::REFERENCE_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::STATE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::CLC_MODE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::CURR_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::REFERENCE_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::STATE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::CLC_MODE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::LAST_RESET_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::CURRRENT_RESET_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::LAST_RESET_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::CURRRENT_RESET_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_LAST_RESET_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_MCU_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::PRESSURE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_RW_STATE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_CLC_MODE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_RW_CURR_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_RW_REF_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::INVALID_CRC_PACKETS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::INVALID_LEN_PACKETS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::INVALID_CMD_PACKETS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::EXECUTED_REPLIES, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::COMMAND_REPLIES, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_BYTES_WRITTEN, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_BYTES_READ, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_PARITY_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_NOISE_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_FRAME_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_REG_OVERRUN_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_TOTAL_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_WRITTEN, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_READ, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_REG_OVERRUN_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_TOTAL_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_LAST_RESET_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_MCU_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::PRESSURE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_RW_STATE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_CLC_MODE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_RW_CURR_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_RW_REF_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::INVALID_CRC_PACKETS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::INVALID_LEN_PACKETS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::INVALID_CMD_PACKETS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::EXECUTED_REPLIES, new PoolEntry({0})); + localDataPoolMap.emplace(rws::COMMAND_REPLIES, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_BYTES_WRITTEN, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_BYTES_READ, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_PARITY_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_NOISE_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_FRAME_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_REG_OVERRUN_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_TOTAL_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_BYTES_WRITTEN, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_BYTES_READ, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_REG_OVERRUN_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_TOTAL_ERRORS, new PoolEntry({0})); return returnvalue::OK; } diff --git a/linux/devices/CMakeLists.txt b/linux/devices/CMakeLists.txt index 4864e01f..7251b802 100644 --- a/linux/devices/CMakeLists.txt +++ b/linux/devices/CMakeLists.txt @@ -4,7 +4,7 @@ endif() target_sources( ${OBSW_NAME} PRIVATE Max31865RtdLowlevelHandler.cpp ScexUartReader.cpp - ScexDleParser.cpp ScexHelper.cpp) + ScexDleParser.cpp ScexHelper.cpp RwPollingTask.cpp) add_subdirectory(ploc) diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp new file mode 100644 index 00000000..ca663c07 --- /dev/null +++ b/linux/devices/RwPollingTask.cpp @@ -0,0 +1,401 @@ +#include "RwPollingTask.h" + +#include +#include +#include +#include +#include +#include + +#include "devConf.h" +#include "mission/devices/devicedefinitions/rwHelpers.h" + +RwPollingTask::RwPollingTask(object_id_t objectId, SpiComIF* spiIF) + : SystemObject(objectId), spiIF(spiIF) { + semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); + semaphore->acquire(); + ipcLock = MutexFactory::instance()->createMutex(); +} + +ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { + ipcLock->lockMutex(); + state = InternalState::IDLE; + ipcLock->unlockMutex(); + while (true) { + semaphore->acquire(); + for (unsigned idx = 0; idx < rwCookies.size(); idx++) { + prepareSetSpeedCmd(idx); + writeOneRw(idx); + } + readAllRws(fd, spiLock, dev) + // writeAndReadAllRws(sendData, sendDataLen) + int bytesRead = 0; + } + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::initialize() { return returnvalue::OK; } + +ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) { + spiIF->getSpiDev(); + // We are in protected section, so we can use the static variable here without issues. + // We don't need to set the speed because a SPI core is used, but the mode has to be set once + // correctly for all RWs + if (not modeAndSpeedWasSet) { + auto& dev = spiIF->getSpiDev(); + int fd = open(dev.c_str(), O_RDWR); + if (fd < 0) { + sif::error << "could not open RW SPI bus" << std::endl; + return returnvalue::FAILED; + } + spiIF->setSpiSpeedAndMode(fd, spi::RW_MODE, spi::RW_SPEED); + close(fd); + modeAndSpeedWasSet = true; + } + + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData, + size_t sendLen) { + if (sendLen < 6) { + return DeviceHandlerIF::INVALID_DATA; + } + int32_t speed = 0; + uint16_t rampTime = 0; + SerializeAdapter::deSerialize(&speed, &sendData, &sendLen, SerializeIF::Endianness::MACHINE); + SerializeAdapter::deSerialize(&rampTime, &sendData, &sendLen, SerializeIF::Endianness::MACHINE); + rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE; + if (sendLen == 7 and sendData[6] < rws::SpecialRwRequest::NUM_REQUESTS) { + specialRequest = static_cast(sendData[6]); + } + RwCookie* rwCookie = dynamic_cast(cookie); + { + MutexGuard mg(ipcLock); + rwCookie->currentRwSpeed = speed; + rwCookie->currentRampTime = rampTime; + rwCookie->specialRequest = specialRequest; + if (state == InternalState::IDLE and rwCookie->rwIdx == 3) { + semaphore->release(); + } + } + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } + +ReturnValue_t RwPollingTask::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { + RwCookie* rwCookie = dynamic_cast(cookie); + { + MutexGuard mg(ipcLock); + *buffer = rwCookie->replyBuf.data(); + *size = rwCookie->replyBuf.size(); + } + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::writeAndReadAllRws(const uint8_t* sendData, size_t sendDataLen) { + // Stopwatch watch; + ReturnValue_t result = returnvalue::OK; + + int fd = 0; + const std::string& dev = spiIF->getSpiDev(); + MutexIF* spiLock = spiIF->getCsMutex(); + result = openSpi(dev, O_RDWR, fd); + if (result != returnvalue::OK) { + return result; + } + for (unsigned idx = 0; idx < rwCookies.size(); idx++) { + ReturnValue_t result = sendOneMessage(fd, *rwCookies[idx], spiLock, sendData, sendDataLen); + if (result != returnvalue::OK) { + closeSpi(fd); + return returnvalue::FAILED; + } + } + + closeSpi(fd); + usleep(rws::SPI_REPLY_DELAY); + return readAllRws(fd, spiLock, dev.c_str()); +} + +ReturnValue_t RwPollingTask::openSpi(const std::string& devname, int flags, int& fd) { + fd = open(devname.c_str(), flags); + if (fd < 0) { + sif::error << "rwSpiCallback::spiCallback: Failed to open device file" << std::endl; + return SpiComIF::OPENING_FILE_FAILED; + } + + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCookie, MutexIF* spiLock, + uint8_t* replyBuf) { + ReturnValue_t result = returnvalue::OK; + int fd = 0; + gpioId_t gpioId = rwCookie.getChipSelectPin(); + GpioIF& gpioIF = spiIF->getGpioInterface(); + pullCsLow(gpioId, spiLock, gpioIF); + for (unsigned idx = 0; idx < MAX_RETRIES_REPLY; idx++) { + result = openSpi(spiDev, O_RDWR, fd); + if (result != returnvalue::OK) { + return result; + } + /** + * 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. + */ + uint8_t byteRead = 0; + for (int idx = 0; idx < 5; idx++) { + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + pullCsHigh(gpioId, spiLock, gpioIF); + closeSpi(fd); + return rws::SPI_READ_FAILURE; + } + if (idx == 0) { + if (byteRead != FLAG_BYTE) { + sif::error << "Invalid data, expected start marker" << std::endl; + pullCsHigh(gpioId, spiLock, gpioIF); + closeSpi(fd); + return rws::NO_START_MARKER; + } + } + + if (byteRead != FLAG_BYTE) { + break; + } + + pullCsHigh(gpioId, spiLock, gpioIF); + closeSpi(fd); + if (idx == MAX_RETRIES_REPLY - 1) { + sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl; + return rws::NO_REPLY; + } + TaskFactory::delayTask(5); + } + +#if FSFW_HAL_SPI_WIRETAPPING == 1 + sif::info << "RW start marker detected" << std::endl; +#endif + + size_t decodedFrameLen = 0; + + while (decodedFrameLen < processingBuf.size()) { + /** First byte already read in */ + if (decodedFrameLen != 0) { + byteRead = 0; + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + result = rws::SPI_READ_FAILURE; + break; + } + } + + if (byteRead == FLAG_BYTE) { + /** Reached end of frame */ + break; + } else if (byteRead == 0x7D) { + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + result = rws::SPI_READ_FAILURE; + break; + } + if (byteRead == 0x5E) { + *(processingBuf.data() + decodedFrameLen) = 0x7E; + decodedFrameLen++; + continue; + } else if (byteRead == 0x5D) { + *(processingBuf.data() + decodedFrameLen) = 0x7D; + decodedFrameLen++; + continue; + } else { + sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl; + closeSpi(fd); + result = rws::INVALID_SUBSTITUTE; + break; + } + } else { + *(processingBuf.data() + decodedFrameLen) = byteRead; + decodedFrameLen++; + continue; + } + + /** + * There might be the unlikely case that each byte in a get-telemetry reply has been + * replaced by its substitute. Than the next byte must correspond to the end sign 0x7E. + * Otherwise there might be something wrong. + */ + if (decodedFrameLen == processingBuf.size()) { + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl; + result = rws::SPI_READ_FAILURE; + break; + } + if (byteRead != FLAG_BYTE) { + sif::error << "rwSpiCallback::spiCallback: Missing end sign " + << static_cast(FLAG_BYTE) << std::endl; + decodedFrameLen--; + result = rws::MISSING_END_SIGN; + break; + } + } + result = returnvalue::OK; + } + } + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::writeOneRw(uint8_t rwIdx) { + int fd = 0; + const std::string& dev = spiIF->getSpiDev(); + MutexIF* spiLock = spiIF->getCsMutex(); + ReturnValue_t result = openSpi(dev, O_RDWR, fd); + if (result != returnvalue::OK) { + return result; + } + ReturnValue_t result = + sendOneMessage(fd, *rwCookies[rwIdx], spiLock, writeBuffer.data(), writeLen); + if (result != returnvalue::OK) { + closeSpi(fd); + return returnvalue::FAILED; + } + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::readAllRws(int fd, MutexIF* spiLock, const char* dev) { + for (unsigned idx = 0; idx < rwCookies.size(); idx++) { + if (spiLock == nullptr) { + sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; + return returnvalue::FAILED; + } + uint8_t* replyBuf; + readNextReply(dev, *rwCookies[idx], spiLock, replyBuf); + } + + closeSpi(fd); + return returnvalue::OK; +} + +// 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. +} + +ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* spiLock, + const uint8_t* data, size_t dataLen) { + gpioId_t gpioId = rwCookie.getChipSelectPin(); + GpioIF& gpioIF = spiIF->getGpioInterface(); + if (spiLock == nullptr) { + sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; + return returnvalue::FAILED; + } + pullCsLow(gpioId, spiLock, gpioIF); + /** Sending frame start sign */ + writeBuffer[0] = FLAG_BYTE; + size_t writeSize = 1; + if (write(fd, writeBuffer.data(), writeSize) != static_cast(writeSize)) { + sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; + pullCsHigh(gpioId, spiLock, gpioIF); + return rws::SPI_WRITE_FAILURE; + } + /** Encoding and sending command */ + size_t idx = 0; + while (idx < dataLen) { + switch (*(data + idx)) { + case 0x7E: + writeBuffer[0] = 0x7D; + writeBuffer[1] = 0x5E; + writeSize = 2; + break; + case 0x7D: + writeBuffer[0] = 0x7D; + writeBuffer[1] = 0x5D; + writeSize = 2; + break; + default: + writeBuffer[0] = *(data + idx); + writeSize = 1; + break; + } + } + if (write(fd, writeBuffer.data(), writeSize) != static_cast(writeSize)) { + sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; + pullCsHigh(gpioId, spiLock, gpioIF); + return rws::SPI_WRITE_FAILURE; + } + idx++; + /** Sending frame end sign */ + writeBuffer[0] = FLAG_BYTE; + writeSize = 1; + + if (write(fd, writeBuffer.data(), writeSize) != static_cast(writeSize)) { + sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; + pullCsHigh(gpioId, spiLock, gpioIF); + return rws::SPI_WRITE_FAILURE; + } + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::pullCsLow(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF) { + ReturnValue_t result = spiLock->lockMutex(TIMEOUT_TYPE, TIMEOUT_MS); + if (result != returnvalue::OK) { + sif::debug << "rwSpiCallback::spiCallback: Failed to lock mutex" << std::endl; + return result; + } + // Pull SPI CS low. For now, no support for active high given + if (gpioId != gpio::NO_GPIO) { + ReturnValue_t result = gpioIF.pullLow(gpioId); + if (result != returnvalue::OK) { + sif::error << "rwSpiCallback::spiCallback: Failed to pull chip select low" << std::endl; + return result; + } + } + return returnvalue::OK; +} + +void RwPollingTask::pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF) { + if (gpioId != gpio::NO_GPIO) { + if (gpioIF.pullHigh(gpioId) != returnvalue::OK) { + sif::error << "closeSpi: Failed to pull chip select high" << std::endl; + } + } + if (spiLock->unlockMutex() != returnvalue::OK) { + sif::error << "rwSpiCallback::closeSpi: Failed to unlock mutex" << std::endl; + ; + } +} + +void RwPollingTask::prepareSimpleCommand(DeviceCommandId_t id) { + writeBuffer[0] = static_cast(id); + uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 1, 0xFFFF); + writeBuffer[1] = static_cast(crc & 0xFF); + writeBuffer[2] = static_cast(crc >> 8 & 0xFF); +} + +ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) { + writeBuffer[0] = static_cast(rws::SET_SPEED); + uint8_t* serPtr = writeBuffer.data() + 1; + int32_t speedToSet = 0; + uint16_t rampTimeToSet = 10; + { + MutexGuard mg(ipcLock); + speedToSet = rwCookies[rwIdx]->currentRwSpeed; + rampTimeToSet = rwCookies[rwIdx]->currentRampTime; + } + size_t serLen = 0; + SerializeAdapter::serialize(&speedToSet, &serPtr, &serLen, writeBuffer.size(), + SerializeIF::Endianness::LITTLE); + SerializeAdapter::serialize(&rampTimeToSet, &serPtr, &serLen, writeBuffer.size(), + SerializeIF::Endianness::LITTLE); + + uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 7, 0xFFFF); + writeBuffer[7] = static_cast(crc & 0xFF); + writeBuffer[8] = static_cast((crc >> 8) & 0xFF); + return returnvalue::OK; +} diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h new file mode 100644 index 00000000..d649a66a --- /dev/null +++ b/linux/devices/RwPollingTask.h @@ -0,0 +1,78 @@ +#ifndef LINUX_DEVICES_RWPOLLINGTASK_H_ +#define LINUX_DEVICES_RWPOLLINGTASK_H_ + +#include +#include +#include +#include +#include +#include + +#include "mission/devices/devicedefinitions/rwHelpers.h" + +class RwCookie : public SpiCookie { + friend class RwPollingTask; + + public: + RwCookie(uint8_t rwIdx, address_t spiAddress, gpioId_t chipSelect, const size_t maxSize, + spi::SpiModes spiMode, uint32_t spiSpeed) + : SpiCookie(spiAddress, chipSelect, maxSize, spiMode, spiSpeed), rwIdx(rwIdx) {} + + private: + std::array replyBuf{}; + int32_t currentRwSpeed = 0; + uint16_t currentRampTime = 0; + rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::NONE; + uint8_t rwIdx; +}; + +class RwPollingTask : public SystemObject, public ExecutableObjectIF, public DeviceCommunicationIF { + public: + RwPollingTask(object_id_t objectId, SpiComIF* spiIF); + + ReturnValue_t performOperation(uint8_t operationCode) override; + ReturnValue_t initialize() override; + + private: + enum class InternalState { IDLE, BUSY } state = InternalState::IDLE; + SemaphoreIF* semaphore; + bool debugMode = false; + bool modeAndSpeedWasSet = false; + MutexIF* ipcLock; + SpiComIF* spiIF; + std::array rwCookies; + std::array writeBuffer; + size_t writeLen = 0; + std::array processingBuf; + //! This is the end and start marker of the frame datalinklayer + static constexpr uint8_t FLAG_BYTE = 0x7E; + static constexpr MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING; + static constexpr uint32_t TIMEOUT_MS = 20; + static constexpr uint8_t MAX_RETRIES_REPLY = 5; + + ReturnValue_t writeAndReadAllRws(const uint8_t* sendData, size_t sendDataLen); + ReturnValue_t writeOneRw(uint8_t rwIdx); + ReturnValue_t readAllRws(int fd, MutexIF* spiLock, const char* dev); + ReturnValue_t sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* spiLock, const uint8_t* data, + size_t dataLen); + ReturnValue_t readNextReply(const char* spiDev, RwCookie& rwCookie, MutexIF* spiLock, + uint8_t* replyBuf); + ReturnValue_t initializeInterface(CookieIF* cookie) override; + + ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; + + ReturnValue_t getSendSuccess(CookieIF* cookie) override; + + ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override; + + ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; + ReturnValue_t openSpi(const std::string& devname, int flags, int& fd); + ReturnValue_t pullCsLow(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF); + void prepareSimpleCommand(DeviceCommandId_t id); + ReturnValue_t prepareSetSpeedCmd(uint8_t rwIdx); + + void pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF); + void closeSpi(int); +}; + +#endif /* LINUX_DEVICES_RWPOLLINGTASK_H_ */ diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index c0e719f2..34fa9c33 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -5,6 +5,7 @@ #include #include #include +#include #include "acs/ActuatorCmd.h" #include "acs/Guidance.h" @@ -17,7 +18,6 @@ #include "eive/objects.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" -#include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" #include "mission/trace.h" @@ -84,10 +84,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes /* ACS Actuation Datasets */ IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER); - RwDefinitions::RwSpeedActuationSet rw1SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW1); - RwDefinitions::RwSpeedActuationSet rw2SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW2); - RwDefinitions::RwSpeedActuationSet rw3SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW3); - RwDefinitions::RwSpeedActuationSet rw4SpeedSet = RwDefinitions::RwSpeedActuationSet(objects::RW4); + rws::RwSpeedActuationSet rw1SpeedSet = rws::RwSpeedActuationSet(objects::RW1); + rws::RwSpeedActuationSet rw2SpeedSet = rws::RwSpeedActuationSet(objects::RW2); + rws::RwSpeedActuationSet rw3SpeedSet = rws::RwSpeedActuationSet(objects::RW3); + rws::RwSpeedActuationSet rw4SpeedSet = rws::RwSpeedActuationSet(objects::RW4); /* ACS Datasets */ // MGMs acsctrl::MgmDataRaw mgmDataRaw; diff --git a/mission/controller/ThermalController.cpp b/mission/controller/ThermalController.cpp index 53b7f13d..f4e46c69 100644 --- a/mission/controller/ThermalController.cpp +++ b/mission/controller/ThermalController.cpp @@ -9,10 +9,10 @@ #include #include #include -#include #include #include #include +#include #include ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater) @@ -689,7 +689,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempRw1 = lp_var_t(objects::RW1, RwDefinitions::TEMPERATURE_C); + lp_var_t tempRw1 = lp_var_t(objects::RW1, rws::TEMPERATURE_C); PoolReadGuard pg(&tempRw1, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read reaction wheel 1 temperature" << std::endl; @@ -702,7 +702,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempRw2 = lp_var_t(objects::RW2, RwDefinitions::TEMPERATURE_C); + lp_var_t tempRw2 = lp_var_t(objects::RW2, rws::TEMPERATURE_C); PoolReadGuard pg(&tempRw2, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read reaction wheel 2 temperature" << std::endl; @@ -715,7 +715,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempRw3 = lp_var_t(objects::RW3, RwDefinitions::TEMPERATURE_C); + lp_var_t tempRw3 = lp_var_t(objects::RW3, rws::TEMPERATURE_C); PoolReadGuard pg(&tempRw3, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read reaction wheel 3 temperature" << std::endl; @@ -728,7 +728,7 @@ void ThermalController::copyDevices() { } { - lp_var_t tempRw4 = lp_var_t(objects::RW4, RwDefinitions::TEMPERATURE_C); + lp_var_t tempRw4 = lp_var_t(objects::RW4, rws::TEMPERATURE_C); PoolReadGuard pg(&tempRw4, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT); if (pg.getReadResult() != returnvalue::OK) { sif::warning << "ThermalController: Failed to read reaction wheel 4 temperature" << std::endl; diff --git a/mission/controller/acs/SensorValues.h b/mission/controller/acs/SensorValues.h index ec1795fc..92d4c5ff 100644 --- a/mission/controller/acs/SensorValues.h +++ b/mission/controller/acs/SensorValues.h @@ -1,13 +1,14 @@ #ifndef SENSORVALUES_H_ #define SENSORVALUES_H_ +#include + #include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h" #include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h" #include "fsfw_hal/devicehandlers/MgmRM3100Handler.h" #include "linux/devices/devicedefinitions/StarTrackerDefinitions.h" #include "mission/devices/devicedefinitions/GPSDefinitions.h" #include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h" -#include "mission/devices/devicedefinitions/RwDefinitions.h" #include "mission/devices/devicedefinitions/SusDefinitions.h" #include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h" @@ -62,10 +63,10 @@ class SensorValues { // bool mgt0valid; - RwDefinitions::StatusSet rw1Set = RwDefinitions::StatusSet(objects::RW1); - RwDefinitions::StatusSet rw2Set = RwDefinitions::StatusSet(objects::RW2); - RwDefinitions::StatusSet rw3Set = RwDefinitions::StatusSet(objects::RW3); - RwDefinitions::StatusSet rw4Set = RwDefinitions::StatusSet(objects::RW4); + rws::StatusSet rw1Set = rws::StatusSet(objects::RW1); + rws::StatusSet rw2Set = rws::StatusSet(objects::RW2); + rws::StatusSet rw3Set = rws::StatusSet(objects::RW3); + rws::StatusSet rw4Set = rws::StatusSet(objects::RW4); }; } /* namespace ACS */ diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 9d4a7255..c5bd1e6e 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -45,23 +45,23 @@ void RwHandler::doShutDown() { ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { switch (internalState) { case InternalState::SET_SPEED: - *id = RwDefinitions::SET_SPEED; + *id = rws::SET_SPEED; internalState = InternalState::GET_RESET_STATUS; break; case InternalState::GET_RESET_STATUS: - *id = RwDefinitions::GET_LAST_RESET_STATUS; + *id = rws::GET_LAST_RESET_STATUS; internalState = InternalState::READ_TEMPERATURE; break; case InternalState::READ_TEMPERATURE: - *id = RwDefinitions::GET_TEMPERATURE; + *id = rws::GET_TEMPERATURE; internalState = InternalState::GET_RW_SATUS; break; case InternalState::GET_RW_SATUS: - *id = RwDefinitions::GET_RW_STATUS; + *id = rws::GET_RW_STATUS; internalState = InternalState::CLEAR_RESET_STATUS; break; case InternalState::CLEAR_RESET_STATUS: - *id = RwDefinitions::CLEAR_LAST_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; @@ -82,27 +82,27 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand ReturnValue_t result = returnvalue::OK; switch (deviceCommand) { - case (RwDefinitions::RESET_MCU): { + case (rws::RESET_MCU): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } - case (RwDefinitions::GET_LAST_RESET_STATUS): { + case (rws::GET_LAST_RESET_STATUS): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } - case (RwDefinitions::CLEAR_LAST_RESET_STATUS): { + case (rws::CLEAR_LAST_RESET_STATUS): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } - case (RwDefinitions::GET_RW_STATUS): { + case (rws::GET_RW_STATUS): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } - case (RwDefinitions::INIT_RW_CONTROLLER): { + case (rws::INIT_RW_CONTROLLER): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } - case (RwDefinitions::SET_SPEED): { + case (rws::SET_SPEED): { if (commandData != nullptr && commandDataLen != 6) { sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" << " invalid length" << std::endl; @@ -136,11 +136,11 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand result = prepareSetSpeedCmd(); return result; } - case (RwDefinitions::GET_TEMPERATURE): { + case (rws::GET_TEMPERATURE): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } - case (RwDefinitions::GET_TM): { + case (rws::GET_TM): { prepareSimpleCommand(deviceCommand); return returnvalue::OK; } @@ -151,60 +151,56 @@ 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->insertInCommandAndReplyMap(RwDefinitions::CLEAR_LAST_RESET_STATUS, 1, nullptr, - RwDefinitions::SIZE_CLEAR_RESET_STATUS); - this->insertInCommandAndReplyMap(RwDefinitions::GET_RW_STATUS, 1, &statusSet, - RwDefinitions::SIZE_GET_RW_STATUS); - this->insertInCommandAndReplyMap(RwDefinitions::INIT_RW_CONTROLLER, 1, nullptr, - RwDefinitions::SIZE_INIT_RW); - this->insertInCommandAndReplyMap(RwDefinitions::GET_TEMPERATURE, 1, nullptr, - RwDefinitions::SIZE_GET_TEMPERATURE_REPLY); - this->insertInCommandAndReplyMap(RwDefinitions::SET_SPEED, 1, nullptr, - RwDefinitions::SIZE_SET_SPEED_REPLY); - this->insertInCommandAndReplyMap(RwDefinitions::GET_TM, 1, &tmDataset, - RwDefinitions::SIZE_GET_TELEMETRY_REPLY); + this->insertInCommandMap(rws::RESET_MCU); + 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, DeviceCommandId_t* foundId, size_t* foundLen) { uint8_t replyByte = *start; switch (replyByte) { - case (RwDefinitions::GET_LAST_RESET_STATUS): { - *foundLen = RwDefinitions::SIZE_GET_RESET_STATUS; - *foundId = RwDefinitions::GET_LAST_RESET_STATUS; + case (rws::GET_LAST_RESET_STATUS): { + *foundLen = rws::SIZE_GET_RESET_STATUS; + *foundId = rws::GET_LAST_RESET_STATUS; break; } - case (RwDefinitions::CLEAR_LAST_RESET_STATUS): { - *foundLen = RwDefinitions::SIZE_CLEAR_RESET_STATUS; - *foundId = RwDefinitions::CLEAR_LAST_RESET_STATUS; + case (rws::CLEAR_LAST_RESET_STATUS): { + *foundLen = rws::SIZE_CLEAR_RESET_STATUS; + *foundId = rws::CLEAR_LAST_RESET_STATUS; break; } - case (RwDefinitions::GET_RW_STATUS): { - *foundLen = RwDefinitions::SIZE_GET_RW_STATUS; - *foundId = RwDefinitions::GET_RW_STATUS; + case (rws::GET_RW_STATUS): { + *foundLen = rws::SIZE_GET_RW_STATUS; + *foundId = rws::GET_RW_STATUS; break; } - case (RwDefinitions::INIT_RW_CONTROLLER): { - *foundLen = RwDefinitions::SIZE_INIT_RW; - *foundId = RwDefinitions::INIT_RW_CONTROLLER; + case (rws::INIT_RW_CONTROLLER): { + *foundLen = rws::SIZE_INIT_RW; + *foundId = rws::INIT_RW_CONTROLLER; break; } - case (RwDefinitions::SET_SPEED): { - *foundLen = RwDefinitions::SIZE_SET_SPEED_REPLY; - *foundId = RwDefinitions::SET_SPEED; + case (rws::SET_SPEED): { + *foundLen = rws::SIZE_SET_SPEED_REPLY; + *foundId = rws::SET_SPEED; break; } - case (RwDefinitions::GET_TEMPERATURE): { - *foundLen = RwDefinitions::SIZE_GET_TEMPERATURE_REPLY; - *foundId = RwDefinitions::GET_TEMPERATURE; + case (rws::GET_TEMPERATURE): { + *foundLen = rws::SIZE_GET_TEMPERATURE_REPLY; + *foundId = rws::GET_TEMPERATURE; break; } - case (RwDefinitions::GET_TM): { - *foundLen = RwDefinitions::SIZE_GET_TELEMETRY_REPLY; - *foundId = RwDefinitions::GET_TM; + case (rws::GET_TM): { + *foundLen = rws::SIZE_GET_TELEMETRY_REPLY; + *foundId = rws::GET_TM; break; } default: { @@ -221,7 +217,7 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { /** Check result code */ - if (*(packet + 1) == RwDefinitions::STATE_ERROR) { + if (*(packet + 1) == rws::STATE_ERROR) { sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: " << id << std::endl; return EXECUTION_FAILED; @@ -236,24 +232,24 @@ ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_ } switch (id) { - case (RwDefinitions::GET_LAST_RESET_STATUS): { + case (rws::GET_LAST_RESET_STATUS): { handleResetStatusReply(packet); break; } - case (RwDefinitions::GET_RW_STATUS): { + case (rws::GET_RW_STATUS): { handleGetRwStatusReply(packet); break; } - case (RwDefinitions::CLEAR_LAST_RESET_STATUS): - case (RwDefinitions::INIT_RW_CONTROLLER): - case (RwDefinitions::SET_SPEED): + case (rws::CLEAR_LAST_RESET_STATUS): + case (rws::INIT_RW_CONTROLLER): + case (rws::SET_SPEED): // no reply data expected break; - case (RwDefinitions::GET_TEMPERATURE): { + case (rws::GET_TEMPERATURE): { handleTemperatureReply(packet); break; } - case (RwDefinitions::GET_TM): { + case (rws::GET_TM): { handleGetTelemetryReply(packet); break; } @@ -270,43 +266,43 @@ uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { retur ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { - localDataPoolMap.emplace(RwDefinitions::RW_SPEED, &rwSpeed); - localDataPoolMap.emplace(RwDefinitions::RAMP_TIME, &rampTime); + localDataPoolMap.emplace(rws::RW_SPEED, &rwSpeed); + localDataPoolMap.emplace(rws::RAMP_TIME, &rampTime); - localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TEMPERATURE_C, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::REFERENCE_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::STATE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::CLC_MODE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::CURR_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::REFERENCE_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::STATE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::CLC_MODE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::LAST_RESET_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::CURRRENT_RESET_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::LAST_RESET_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::CURRRENT_RESET_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_LAST_RESET_STATUS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_MCU_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::PRESSURE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_RW_STATE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_CLC_MODE, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_RW_CURR_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::TM_RW_REF_SPEED, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::INVALID_CRC_PACKETS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::INVALID_LEN_PACKETS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::INVALID_CMD_PACKETS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::EXECUTED_REPLIES, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::COMMAND_REPLIES, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_BYTES_WRITTEN, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_BYTES_READ, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_PARITY_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_NOISE_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_FRAME_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_REG_OVERRUN_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::UART_TOTAL_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_WRITTEN, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_READ, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_REG_OVERRUN_ERRORS, new PoolEntry({0})); - localDataPoolMap.emplace(RwDefinitions::SPI_TOTAL_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_LAST_RESET_STATUS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_MCU_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::PRESSURE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_RW_STATE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_CLC_MODE, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_RW_CURR_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::TM_RW_REF_SPEED, new PoolEntry({0})); + localDataPoolMap.emplace(rws::INVALID_CRC_PACKETS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::INVALID_LEN_PACKETS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::INVALID_CMD_PACKETS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::EXECUTED_REPLIES, new PoolEntry({0})); + localDataPoolMap.emplace(rws::COMMAND_REPLIES, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_BYTES_WRITTEN, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_BYTES_READ, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_PARITY_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_NOISE_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_FRAME_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_REG_OVERRUN_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::UART_TOTAL_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_BYTES_WRITTEN, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_BYTES_READ, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_REG_OVERRUN_ERRORS, new PoolEntry({0})); + localDataPoolMap.emplace(rws::SPI_TOTAL_ERRORS, new PoolEntry({0})); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(statusSet.getSid(), false, 5.0)); poolManager.subscribeForRegularPeriodicPacket( @@ -343,7 +339,7 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime() { } ReturnValue_t RwHandler::prepareSetSpeedCmd() { - commandBuffer[0] = static_cast(RwDefinitions::SET_SPEED); + commandBuffer[0] = static_cast(rws::SET_SPEED); uint8_t* serPtr = commandBuffer + 1; size_t serSize = 1; rwSpeedActuationSet.setValidityBufferGeneration(false); @@ -369,7 +365,7 @@ void RwHandler::handleResetStatusReply(const uint8_t* packet) { if (resetStatus != 0) { internalState = InternalState::CLEAR_RESET_STATUS; lastResetStatusSet.lastNonClearedResetStatus = resetStatus; - triggerEvent(RwDefinitions::RESET_OCCURED, resetStatus, 0); + triggerEvent(rws::RESET_OCCURED, resetStatus, 0); } lastResetStatusSet.currentResetStatus = resetStatus; if (debugMode) { @@ -408,10 +404,10 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) { statusSet.setValidity(true, true); - if (statusSet.state == RwDefinitions::STATE_ERROR) { + if (statusSet.state == rws::STATE_ERROR) { // This requires the commanding of the init reaction wheel controller command to recover // from error state which must be handled by the FDIR instance. - triggerEvent(RwDefinitions::ERROR_STATE, statusSet.state.value, 0); + triggerEvent(rws::ERROR_STATE, statusSet.state.value, 0); sif::error << "RwHandler::handleGetRwStatusReply: Reaction wheel in error state" << std::endl; } diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index ee18960d..7db3ef3b 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -3,7 +3,7 @@ #include #include -#include +#include #include #include "events/subsystemIdRanges.h" @@ -42,24 +42,6 @@ class RwHandler : public DeviceHandlerBase { virtual ~RwHandler(); - static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER; - - static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0); - //! [EXPORT] : [COMMENT] Used by the spi send function to tell a failing read call - static const ReturnValue_t SPI_READ_FAILURE = MAKE_RETURN_CODE(0xB1); - //! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about a missing - //! start sign 0x7E - static const ReturnValue_t MISSING_START_SIGN = MAKE_RETURN_CODE(0xB2); - //! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about an invalid - //! substitution combination - static const ReturnValue_t INVALID_SUBSTITUTE = MAKE_RETURN_CODE(0xB3); - //! [EXPORT] : [COMMENT] HDLC decoding mechanism never receives the end sign 0x7E - static const ReturnValue_t MISSING_END_SIGN = MAKE_RETURN_CODE(0xB4); - //! [EXPORT] : [COMMENT] Reaction wheel only responds with empty frames. - 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); - protected: void doStartUp() override; void doShutDown() override; @@ -92,12 +74,12 @@ class RwHandler : public DeviceHandlerBase { gpioId_t enableGpio = gpio::NO_GPIO; bool debugMode = false; - RwDefinitions::StatusSet statusSet; - RwDefinitions::LastResetSatus lastResetStatusSet; - RwDefinitions::TmDataset tmDataset; - RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet; + rws::StatusSet statusSet; + rws::LastResetSatus lastResetStatusSet; + rws::TmDataset tmDataset; + rws::RwSpeedActuationSet rwSpeedActuationSet; - uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE]; + uint8_t commandBuffer[rws::MAX_CMD_SIZE]; uint8_t rwIdx; PoolEntry rwSpeed = PoolEntry({0}); diff --git a/mission/devices/devicedefinitions/rwHelpers.cpp b/mission/devices/devicedefinitions/rwHelpers.cpp new file mode 100644 index 00000000..4c246eb8 --- /dev/null +++ b/mission/devices/devicedefinitions/rwHelpers.cpp @@ -0,0 +1,6 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ + +#include "rwHelpers.h" + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ */ diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/rwHelpers.h similarity index 67% rename from mission/devices/devicedefinitions/RwDefinitions.h rename to mission/devices/devicedefinitions/rwHelpers.h index b3cb0fe4..743813fa 100644 --- a/mission/devices/devicedefinitions/RwDefinitions.h +++ b/mission/devices/devicedefinitions/rwHelpers.h @@ -1,5 +1,5 @@ -#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ -#define MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_H_ #include #include @@ -8,7 +8,83 @@ #include "events/subsystemIdRanges.h" #include "objects/systemObjectList.h" -namespace RwDefinitions { +namespace rws { + +static const size_t SIZE_GET_RESET_STATUS = 5; +static const size_t SIZE_CLEAR_RESET_STATUS = 4; +static const size_t SIZE_INIT_RW = 4; +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; +/** Max size when requesting telemetry */ +static const size_t SIZE_GET_TELEMETRY_REPLY = 91; + +enum SpecialRwRequest : uint8_t { + REQUEST_NONE = 0, + RESET_MCU = 1, + INIT_RW_CONTROLLER = 2, + GET_TM = 3, + NUM_REQUESTS +}; + +struct RwReplies { + friend class RwPollingTask; + + public: + RwReplies(const uint8_t* rawData) : rawData(rawData) { + rwStatusReply = rawData; + setSpeedReply = rawData + SIZE_GET_RW_STATUS; + getLastResetStatusReply = setSpeedReply + SIZE_SET_SPEED_REPLY; + clearLastResetStatusReply = getLastResetStatusReply + SIZE_GET_RESET_STATUS; + readTemperatureReply = clearLastResetStatusReply + SIZE_CLEAR_RESET_STATUS; + hkDataReply = readTemperatureReply + SIZE_GET_TEMPERATURE_REPLY; + initRwControllerReply = hkDataReply + SIZE_GET_TELEMETRY_REPLY; + } + + const uint8_t* getClearLastResetStatusReply() const { return clearLastResetStatusReply; } + + const uint8_t* getGetLastResetStatusReply() const { return getLastResetStatusReply; } + + const uint8_t* getHkDataReply() const { return hkDataReply; } + + const uint8_t* getInitRwControllerReply() const { return initRwControllerReply; } + + const uint8_t* getRawData() const { return rawData; } + + const uint8_t* getReadTemperatureReply() const { return readTemperatureReply; } + + const uint8_t* getRwStatusReply() const { return rwStatusReply; } + + const uint8_t* getSetSpeedReply() const { return setSpeedReply; } + + private: + const uint8_t* rawData; + const uint8_t* rwStatusReply; + const uint8_t* setSpeedReply; + const uint8_t* getLastResetStatusReply; + const uint8_t* clearLastResetStatusReply; + const uint8_t* readTemperatureReply; + const uint8_t* hkDataReply; + const uint8_t* initRwControllerReply; +}; + +static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER; + +static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0); +//! [EXPORT] : [COMMENT] Used by the spi send function to tell a failing read call +static const ReturnValue_t SPI_READ_FAILURE = MAKE_RETURN_CODE(0xB1); +//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about a missing +//! start sign 0x7E +static const ReturnValue_t MISSING_START_SIGN = MAKE_RETURN_CODE(0xB2); +//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about an invalid +//! substitution combination +static const ReturnValue_t INVALID_SUBSTITUTE = MAKE_RETURN_CODE(0xB3); +//! [EXPORT] : [COMMENT] HDLC decoding mechanism never receives the end sign 0x7E +static const ReturnValue_t MISSING_END_SIGN = MAKE_RETURN_CODE(0xB4); +//! [EXPORT] : [COMMENT] Reaction wheel only responds with empty frames. +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); static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::RW_HANDLER; @@ -17,7 +93,8 @@ static constexpr Event ERROR_STATE = MAKE_EVENT(1, severity::HIGH); static constexpr Event RESET_OCCURED = event::makeEvent(SUBSYSTEM_ID, 2, severity::LOW); -static const uint32_t SPI_REPLY_DELAY = 70000; // us +//! Minimal delay as specified by the datasheet. +static const uint32_t SPI_REPLY_DELAY = 20000; // us enum PoolIds : lp_id_t { TEMPERATURE_C, @@ -86,15 +163,6 @@ enum SetIds : uint32_t { SPEED_CMD_SET = 10, }; -static const size_t SIZE_GET_RESET_STATUS = 5; -static const size_t SIZE_CLEAR_RESET_STATUS = 4; -static const size_t SIZE_INIT_RW = 4; -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; -/** Max size when requesting telemetry */ -static const size_t SIZE_GET_TELEMETRY_REPLY = 91; - /** Set speed command has maximum size */ static const size_t MAX_CMD_SIZE = 9; /** @@ -112,11 +180,10 @@ static const uint8_t TM_SET_ENTRIES = 24; */ class StatusSet : public StaticLocalDataSet { public: - StatusSet(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, RwDefinitions::SetIds::STATUS_SET_ID) {} + StatusSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, rws::SetIds::STATUS_SET_ID) {} StatusSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::STATUS_SET_ID)) {} + : StaticLocalDataSet(sid_t(objectId, rws::SetIds::STATUS_SET_ID)) {} lp_var_t temperatureCelcius = lp_var_t(sid.objectId, PoolIds::TEMPERATURE_C, this); @@ -133,10 +200,10 @@ class StatusSet : public StaticLocalDataSet { class LastResetSatus : public StaticLocalDataSet { public: LastResetSatus(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, RwDefinitions::SetIds::LAST_RESET_ID) {} + : StaticLocalDataSet(owner, rws::SetIds::LAST_RESET_ID) {} LastResetSatus(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::LAST_RESET_ID)) {} + : StaticLocalDataSet(sid_t(objectId, rws::SetIds::LAST_RESET_ID)) {} // If a reset occurs, the status code will be cached into this variable lp_var_t lastNonClearedResetStatus = @@ -153,11 +220,9 @@ class LastResetSatus : public StaticLocalDataSet { */ class TmDataset : public StaticLocalDataSet { public: - TmDataset(HasLocalDataPoolIF* owner) - : StaticLocalDataSet(owner, RwDefinitions::SetIds::TM_SET_ID) {} + TmDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, rws::SetIds::TM_SET_ID) {} - TmDataset(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::TM_SET_ID)) {} + TmDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, rws::SetIds::TM_SET_ID)) {} lp_var_t lastResetStatus = lp_var_t(sid.objectId, PoolIds::TM_LAST_RESET_STATUS, this); @@ -209,9 +274,9 @@ class RwSpeedActuationSet : public StaticLocalDataSet<2> { public: RwSpeedActuationSet(HasLocalDataPoolIF& owner) - : StaticLocalDataSet(&owner, RwDefinitions::SetIds::SPEED_CMD_SET) {} + : StaticLocalDataSet(&owner, rws::SetIds::SPEED_CMD_SET) {} RwSpeedActuationSet(object_id_t objectId) - : StaticLocalDataSet(sid_t(objectId, RwDefinitions::SetIds::SPEED_CMD_SET)) {} + : StaticLocalDataSet(sid_t(objectId, rws::SetIds::SPEED_CMD_SET)) {} void setRwSpeed(int32_t rwSpeed_, uint16_t rampTime_) { if (rwSpeed.value != rwSpeed_) { @@ -228,12 +293,10 @@ class RwSpeedActuationSet : public StaticLocalDataSet<2> { } private: - lp_var_t rwSpeed = - lp_var_t(sid.objectId, RwDefinitions::PoolIds::RW_SPEED, this); - lp_var_t rampTime = - lp_var_t(sid.objectId, RwDefinitions::PoolIds::RAMP_TIME, this); + lp_var_t rwSpeed = lp_var_t(sid.objectId, rws::PoolIds::RW_SPEED, this); + lp_var_t rampTime = lp_var_t(sid.objectId, rws::PoolIds::RAMP_TIME, this); }; -} // namespace RwDefinitions +} // namespace rws -#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */ +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_H_ */ -- 2.43.0 From 8c001b64433d18c47b2b53b7de200224eb8a4d7e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Feb 2023 19:16:42 +0100 Subject: [PATCH 02/18] heading towards completion for low level rw polling task --- linux/devices/RwPollingTask.cpp | 205 ++++++++++++------ linux/devices/RwPollingTask.h | 28 ++- mission/devices/devicedefinitions/rwHelpers.h | 89 ++++---- 3 files changed, 205 insertions(+), 117 deletions(-) diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index ca663c07..657f8853 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -15,35 +15,66 @@ RwPollingTask::RwPollingTask(object_id_t objectId, SpiComIF* spiIF) semaphore = SemaphoreFactory::instance()->createBinarySemaphore(); semaphore->acquire(); ipcLock = MutexFactory::instance()->createMutex(); + spiLock = spiIF->getCsMutex(); + spiDev = spiIF->getSpiDev().c_str(); } ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { - ipcLock->lockMutex(); - state = InternalState::IDLE; - ipcLock->unlockMutex(); while (true) { + ipcLock->lockMutex(); + state = InternalState::IDLE; + ipcLock->unlockMutex(); semaphore->acquire(); + int fd = 0; + ReturnValue_t result = openSpi(O_RDWR, fd); + if (result != returnvalue::OK) { + continue; + } for (unsigned idx = 0; idx < rwCookies.size(); idx++) { prepareSetSpeedCmd(idx); - writeOneRw(idx); + if (writeOneRwCmd(idx, fd) != returnvalue::OK) { + continue; + } } - readAllRws(fd, spiLock, dev) - // writeAndReadAllRws(sendData, sendDataLen) - int bytesRead = 0; + closeSpi(fd); + usleep(rws::SPI_REPLY_DELAY); + if (readAllRws(fd, 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; + } + } + + return returnvalue::OK; +} + +ReturnValue_t RwPollingTask::initialize() { + if (spiDev == nullptr) { + sif::error << "SPI device is invalid" << std::endl; + return returnvalue::FAILED; } return returnvalue::OK; } -ReturnValue_t RwPollingTask::initialize() { return returnvalue::OK; } - ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) { - spiIF->getSpiDev(); - // We are in protected section, so we can use the static variable here without issues. // We don't need to set the speed because a SPI core is used, but the mode has to be set once // correctly for all RWs if (not modeAndSpeedWasSet) { - auto& dev = spiIF->getSpiDev(); - int fd = open(dev.c_str(), O_RDWR); + int fd = open(spiDev, O_RDWR); if (fd < 0) { sif::error << "could not open RW SPI bus" << std::endl; return returnvalue::FAILED; @@ -66,16 +97,20 @@ ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendDa SerializeAdapter::deSerialize(&speed, &sendData, &sendLen, SerializeIF::Endianness::MACHINE); SerializeAdapter::deSerialize(&rampTime, &sendData, &sendLen, SerializeIF::Endianness::MACHINE); rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE; - if (sendLen == 7 and sendData[6] < rws::SpecialRwRequest::NUM_REQUESTS) { + if (sendLen == 7 and sendData[6] < static_cast(rws::SpecialRwRequest::NUM_REQUESTS)) { specialRequest = static_cast(sendData[6]); } RwCookie* rwCookie = dynamic_cast(cookie); + if (rwCookie == nullptr) { + return returnvalue::FAILED; + } { MutexGuard mg(ipcLock); rwCookie->currentRwSpeed = speed; rwCookie->currentRampTime = rampTime; rwCookie->specialRequest = specialRequest; if (state == InternalState::IDLE and rwCookie->rwIdx == 3) { + state = InternalState::BUSY; semaphore->release(); } } @@ -98,19 +133,17 @@ ReturnValue_t RwPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buf return returnvalue::OK; } -ReturnValue_t RwPollingTask::writeAndReadAllRws(const uint8_t* sendData, size_t sendDataLen) { +ReturnValue_t RwPollingTask::writeAndReadAllRws(DeviceCommandId_t id) { // Stopwatch watch; ReturnValue_t result = returnvalue::OK; int fd = 0; - const std::string& dev = spiIF->getSpiDev(); - MutexIF* spiLock = spiIF->getCsMutex(); - result = openSpi(dev, O_RDWR, fd); + result = openSpi(O_RDWR, fd); if (result != returnvalue::OK) { return result; } for (unsigned idx = 0; idx < rwCookies.size(); idx++) { - ReturnValue_t result = sendOneMessage(fd, *rwCookies[idx], spiLock, sendData, sendDataLen); + ReturnValue_t result = sendOneMessage(fd, *rwCookies[idx]); if (result != returnvalue::OK) { closeSpi(fd); return returnvalue::FAILED; @@ -119,11 +152,11 @@ ReturnValue_t RwPollingTask::writeAndReadAllRws(const uint8_t* sendData, size_t closeSpi(fd); usleep(rws::SPI_REPLY_DELAY); - return readAllRws(fd, spiLock, dev.c_str()); + return readAllRws(fd, id); } -ReturnValue_t RwPollingTask::openSpi(const std::string& devname, int flags, int& fd) { - fd = open(devname.c_str(), flags); +ReturnValue_t RwPollingTask::openSpi(int flags, int& fd) { + fd = open(spiDev, flags); if (fd < 0) { sif::error << "rwSpiCallback::spiCallback: Failed to open device file" << std::endl; return SpiComIF::OPENING_FILE_FAILED; @@ -132,15 +165,15 @@ ReturnValue_t RwPollingTask::openSpi(const std::string& devname, int flags, int& return returnvalue::OK; } -ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCookie, MutexIF* spiLock, - uint8_t* replyBuf) { +ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf, + size_t maxReplyLen) { ReturnValue_t result = returnvalue::OK; int fd = 0; gpioId_t gpioId = rwCookie.getChipSelectPin(); GpioIF& gpioIF = spiIF->getGpioInterface(); pullCsLow(gpioId, spiLock, gpioIF); for (unsigned idx = 0; idx < MAX_RETRIES_REPLY; idx++) { - result = openSpi(spiDev, O_RDWR, fd); + result = openSpi(O_RDWR, fd); if (result != returnvalue::OK) { return result; } @@ -184,7 +217,7 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki size_t decodedFrameLen = 0; - while (decodedFrameLen < processingBuf.size()) { + while (decodedFrameLen < maxReplyLen) { /** First byte already read in */ if (decodedFrameLen != 0) { byteRead = 0; @@ -205,11 +238,11 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki break; } if (byteRead == 0x5E) { - *(processingBuf.data() + decodedFrameLen) = 0x7E; + *(replyBuf + decodedFrameLen) = 0x7E; decodedFrameLen++; continue; } else if (byteRead == 0x5D) { - *(processingBuf.data() + decodedFrameLen) = 0x7D; + *(replyBuf + decodedFrameLen) = 0x7D; decodedFrameLen++; continue; } else { @@ -219,7 +252,7 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki break; } } else { - *(processingBuf.data() + decodedFrameLen) = byteRead; + *(replyBuf + decodedFrameLen) = byteRead; decodedFrameLen++; continue; } @@ -229,7 +262,7 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki * replaced by its substitute. Than the next byte must correspond to the end sign 0x7E. * Otherwise there might be something wrong. */ - if (decodedFrameLen == processingBuf.size()) { + if (decodedFrameLen == maxReplyLen) { if (read(fd, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl; result = rws::SPI_READ_FAILURE; @@ -249,16 +282,8 @@ ReturnValue_t RwPollingTask::readNextReply(const char* spiDev, RwCookie& rwCooki return returnvalue::OK; } -ReturnValue_t RwPollingTask::writeOneRw(uint8_t rwIdx) { - int fd = 0; - const std::string& dev = spiIF->getSpiDev(); - MutexIF* spiLock = spiIF->getCsMutex(); - ReturnValue_t result = openSpi(dev, O_RDWR, fd); - if (result != returnvalue::OK) { - return result; - } - ReturnValue_t result = - sendOneMessage(fd, *rwCookies[rwIdx], spiLock, writeBuffer.data(), writeLen); +ReturnValue_t RwPollingTask::writeOneRwCmd(uint8_t rwIdx, int fd) { + ReturnValue_t result = sendOneMessage(fd, *rwCookies[rwIdx]); if (result != returnvalue::OK) { closeSpi(fd); return returnvalue::FAILED; @@ -266,20 +291,83 @@ ReturnValue_t RwPollingTask::writeOneRw(uint8_t rwIdx) { return returnvalue::OK; } -ReturnValue_t RwPollingTask::readAllRws(int fd, MutexIF* spiLock, const char* dev) { +ReturnValue_t RwPollingTask::readAllRws(int fd, DeviceCommandId_t id) { + ReturnValue_t result = openSpi(O_RDWR, fd); + if (result != returnvalue::OK) { + return result; + } for (unsigned idx = 0; idx < rwCookies.size(); idx++) { if (spiLock == nullptr) { sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; return returnvalue::FAILED; } + // TODO: Fix buffer to write to uint8_t* replyBuf; - readNextReply(dev, *rwCookies[idx], spiLock, replyBuf); + size_t maxReadLen = idAndIdxToReadBuffer(id, idx, &replyBuf); + readNextReply(*rwCookies[idx], replyBuf, maxReadLen); } closeSpi(fd); return returnvalue::OK; } +size_t RwPollingTask::idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** ptr) { + uint8_t* rawStart = rwCookies[rwIdx]->replyBuf.data(); + RwReplies replies(rawStart); + switch (id) { + case (rws::GET_RW_STATUS): { + *ptr = replies.rwStatusReply; + return rws::SIZE_GET_RW_STATUS; + } + case (rws::SET_SPEED): { + *ptr = replies.setSpeedReply; + return rws::SIZE_SET_SPEED_REPLY; + } + case (rws::CLEAR_LAST_RESET_STATUS): { + *ptr = replies.clearLastResetStatusReply; + return rws::SIZE_CLEAR_RESET_STATUS; + } + case (rws::GET_LAST_RESET_STATUS): { + *ptr = replies.getLastResetStatusReply; + return rws::SIZE_GET_RESET_STATUS; + } + case (rws::GET_TEMPERATURE): { + *ptr = replies.readTemperatureReply; + return rws::SIZE_GET_TEMPERATURE_REPLY; + } + case (rws::GET_TM): { + *ptr = replies.hkDataReply; + return rws::SIZE_GET_TELEMETRY_REPLY; + } + case (rws::INIT_RW_CONTROLLER): { + *ptr = replies.initRwControllerReply; + return rws::SIZE_INIT_RW; + } + default: { + sif::error << "no reply buffer for rw command " << id << std::endl; + *ptr = replies.dummyPointer; + return 0; + } + } +} + +void RwPollingTask::encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, size_t& encodedLen) { + encodedBuffer[0] = FLAG_BYTE; + encodedLen = 1; + for (size_t sourceIdx = 0; sourceIdx < sourceLen; sourceIdx++) { + if (sourceBuf[sourceIdx] == 0x7E) { + encodedBuffer[encodedLen++] = 0x7D; + encodedBuffer[encodedLen++] = 0x5E; + } else if (sourceBuf[sourceIdx] == 0x7D) { + encodedBuffer[encodedLen++] = 0x7D; + encodedBuffer[encodedLen++] = 0x5D; + } else { + encodedBuffer[encodedLen++] = sourceBuf[sourceIdx]; + } + } + encodedBuffer[encodedLen++] = FLAG_BYTE; +} + // This closes the SPI void RwPollingTask::closeSpi(int fd) { // This will perform the function to close the SPI @@ -287,8 +375,7 @@ void RwPollingTask::closeSpi(int fd) { // The SPI is now closed. } -ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* spiLock, - const uint8_t* data, size_t dataLen) { +ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) { gpioId_t gpioId = rwCookie.getChipSelectPin(); GpioIF& gpioIF = spiIF->getGpioInterface(); if (spiLock == nullptr) { @@ -296,15 +383,8 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* return returnvalue::FAILED; } pullCsLow(gpioId, spiLock, gpioIF); - /** Sending frame start sign */ - writeBuffer[0] = FLAG_BYTE; - size_t writeSize = 1; - if (write(fd, writeBuffer.data(), writeSize) != static_cast(writeSize)) { - sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); - return rws::SPI_WRITE_FAILURE; - } - /** Encoding and sending command */ + /* + //Encoding and sending command size_t idx = 0; while (idx < dataLen) { switch (*(data + idx)) { @@ -324,21 +404,16 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* break; } } - if (write(fd, writeBuffer.data(), writeSize) != static_cast(writeSize)) { - sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); - return rws::SPI_WRITE_FAILURE; - } - idx++; - /** Sending frame end sign */ - writeBuffer[0] = FLAG_BYTE; - writeSize = 1; - - if (write(fd, writeBuffer.data(), writeSize) != static_cast(writeSize)) { + */ + // Add datalinklayer like specified in the datasheet. + size_t lenToSend = 0; + encodeHdlc(writeBuffer.data(), writeLen, lenToSend); + if (write(fd, encodedBuffer.data(), lenToSend) != static_cast(lenToSend)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; pullCsHigh(gpioId, spiLock, gpioIF); return rws::SPI_WRITE_FAILURE; } + pullCsHigh(gpioId, spiLock, gpioIF); return returnvalue::OK; } @@ -376,6 +451,7 @@ void RwPollingTask::prepareSimpleCommand(DeviceCommandId_t id) { uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 1, 0xFFFF); writeBuffer[1] = static_cast(crc & 0xFF); writeBuffer[2] = static_cast(crc >> 8 & 0xFF); + writeLen = 3; } ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) { @@ -397,5 +473,6 @@ ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) { uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 7, 0xFFFF); writeBuffer[7] = static_cast(crc & 0xFF); writeBuffer[8] = static_cast((crc >> 8) & 0xFF); + writeLen = 9; return returnvalue::OK; } diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h index d649a66a..2b7d899f 100644 --- a/linux/devices/RwPollingTask.h +++ b/linux/devices/RwPollingTask.h @@ -14,15 +14,16 @@ class RwCookie : public SpiCookie { friend class RwPollingTask; public: + static constexpr size_t REPLY_BUF_LEN = 524; RwCookie(uint8_t rwIdx, address_t spiAddress, gpioId_t chipSelect, const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed) : SpiCookie(spiAddress, chipSelect, maxSize, spiMode, spiSpeed), rwIdx(rwIdx) {} private: - std::array replyBuf{}; + std::array replyBuf{}; int32_t currentRwSpeed = 0; uint16_t currentRampTime = 0; - rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::NONE; + rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE; uint8_t rwIdx; }; @@ -39,24 +40,26 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev bool debugMode = false; bool modeAndSpeedWasSet = false; MutexIF* ipcLock; + MutexIF* spiLock; + const char* spiDev; SpiComIF* spiIF; std::array rwCookies; std::array writeBuffer; + std::array encodedBuffer; + size_t writeLen = 0; - std::array processingBuf; //! This is the end and start marker of the frame datalinklayer static constexpr uint8_t FLAG_BYTE = 0x7E; static constexpr MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING; static constexpr uint32_t TIMEOUT_MS = 20; static constexpr uint8_t MAX_RETRIES_REPLY = 5; - ReturnValue_t writeAndReadAllRws(const uint8_t* sendData, size_t sendDataLen); - ReturnValue_t writeOneRw(uint8_t rwIdx); - ReturnValue_t readAllRws(int fd, MutexIF* spiLock, const char* dev); - ReturnValue_t sendOneMessage(int fd, RwCookie& rwCookie, MutexIF* spiLock, const uint8_t* data, - size_t dataLen); - ReturnValue_t readNextReply(const char* spiDev, RwCookie& rwCookie, MutexIF* spiLock, - uint8_t* replyBuf); + ReturnValue_t writeAndReadAllRws(DeviceCommandId_t id); + ReturnValue_t writeOneRwCmd(uint8_t rwIdx, int fd); + ReturnValue_t readAllRws(int fd, DeviceCommandId_t id); + + ReturnValue_t sendOneMessage(int fd, RwCookie& rwCookie); + ReturnValue_t readNextReply(RwCookie& rwCookie, uint8_t* replyBuf, size_t maxReplyLen); ReturnValue_t initializeInterface(CookieIF* cookie) override; ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; @@ -66,11 +69,14 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override; ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override; - ReturnValue_t openSpi(const std::string& devname, int flags, int& fd); + ReturnValue_t openSpi(int flags, int& fd); ReturnValue_t pullCsLow(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF); void prepareSimpleCommand(DeviceCommandId_t id); ReturnValue_t prepareSetSpeedCmd(uint8_t rwIdx); + size_t idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** readPtr); + void encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, size_t& encodedLen); + void pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF); void closeSpi(int); }; diff --git a/mission/devices/devicedefinitions/rwHelpers.h b/mission/devices/devicedefinitions/rwHelpers.h index 743813fa..55b17859 100644 --- a/mission/devices/devicedefinitions/rwHelpers.h +++ b/mission/devices/devicedefinitions/rwHelpers.h @@ -19,7 +19,7 @@ static const size_t SIZE_GET_TEMPERATURE_REPLY = 8; /** Max size when requesting telemetry */ static const size_t SIZE_GET_TELEMETRY_REPLY = 91; -enum SpecialRwRequest : uint8_t { +enum class SpecialRwRequest : uint8_t { REQUEST_NONE = 0, RESET_MCU = 1, INIT_RW_CONTROLLER = 2, @@ -27,47 +27,6 @@ enum SpecialRwRequest : uint8_t { NUM_REQUESTS }; -struct RwReplies { - friend class RwPollingTask; - - public: - RwReplies(const uint8_t* rawData) : rawData(rawData) { - rwStatusReply = rawData; - setSpeedReply = rawData + SIZE_GET_RW_STATUS; - getLastResetStatusReply = setSpeedReply + SIZE_SET_SPEED_REPLY; - clearLastResetStatusReply = getLastResetStatusReply + SIZE_GET_RESET_STATUS; - readTemperatureReply = clearLastResetStatusReply + SIZE_CLEAR_RESET_STATUS; - hkDataReply = readTemperatureReply + SIZE_GET_TEMPERATURE_REPLY; - initRwControllerReply = hkDataReply + SIZE_GET_TELEMETRY_REPLY; - } - - const uint8_t* getClearLastResetStatusReply() const { return clearLastResetStatusReply; } - - const uint8_t* getGetLastResetStatusReply() const { return getLastResetStatusReply; } - - const uint8_t* getHkDataReply() const { return hkDataReply; } - - const uint8_t* getInitRwControllerReply() const { return initRwControllerReply; } - - const uint8_t* getRawData() const { return rawData; } - - const uint8_t* getReadTemperatureReply() const { return readTemperatureReply; } - - const uint8_t* getRwStatusReply() const { return rwStatusReply; } - - const uint8_t* getSetSpeedReply() const { return setSpeedReply; } - - private: - const uint8_t* rawData; - const uint8_t* rwStatusReply; - const uint8_t* setSpeedReply; - const uint8_t* getLastResetStatusReply; - const uint8_t* clearLastResetStatusReply; - const uint8_t* readTemperatureReply; - const uint8_t* hkDataReply; - const uint8_t* initRwControllerReply; -}; - static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER; static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0); @@ -299,4 +258,50 @@ class RwSpeedActuationSet : public StaticLocalDataSet<2> { } // namespace rws +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* getGetLastResetStatusReply() const { return getLastResetStatusReply; } + + const uint8_t* getHkDataReply() const { return hkDataReply; } + + const uint8_t* getInitRwControllerReply() const { return initRwControllerReply; } + + const uint8_t* getRawData() const { return rawData; } + + const uint8_t* getReadTemperatureReply() const { return readTemperatureReply; } + + const uint8_t* getRwStatusReply() const { return rwStatusReply; } + + const uint8_t* getSetSpeedReply() const { return setSpeedReply; } + + private: + RwReplies(uint8_t* rwData) : rawData(rwData) { initPointers(); } + + 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; + } + uint8_t* rawData; + uint8_t* rwStatusReply; + uint8_t* setSpeedReply; + uint8_t* getLastResetStatusReply; + uint8_t* clearLastResetStatusReply; + uint8_t* readTemperatureReply; + uint8_t* hkDataReply; + uint8_t* initRwControllerReply; + uint8_t* dummyPointer; +}; + #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_H_ */ -- 2.43.0 From 655e01c2d13979111a11f226151a45d63ee42175 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Feb 2023 19:27:19 +0100 Subject: [PATCH 03/18] some more bugfixes --- linux/devices/RwPollingTask.cpp | 136 ++++++++++++++++---------------- linux/devices/RwPollingTask.h | 2 +- 2 files changed, 70 insertions(+), 68 deletions(-) diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index 657f8853..b300136a 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -172,6 +172,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf gpioId_t gpioId = rwCookie.getChipSelectPin(); GpioIF& gpioIF = spiIF->getGpioInterface(); pullCsLow(gpioId, spiLock, gpioIF); + uint8_t byteRead = 0; for (unsigned idx = 0; idx < MAX_RETRIES_REPLY; idx++) { result = openSpi(O_RDWR, fd); if (result != returnvalue::OK) { @@ -181,18 +182,17 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf * 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. */ - uint8_t byteRead = 0; for (int idx = 0; idx < 5; idx++) { if (read(fd, &byteRead, 1) != 1) { sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); closeSpi(fd); return rws::SPI_READ_FAILURE; } if (idx == 0) { if (byteRead != FLAG_BYTE) { sif::error << "Invalid data, expected start marker" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); closeSpi(fd); return rws::NO_START_MARKER; } @@ -202,7 +202,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf break; } - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); closeSpi(fd); if (idx == MAX_RETRIES_REPLY - 1) { sif::error << "rwSpiCallback::spiCallback: Empty frame timeout" << std::endl; @@ -210,76 +210,78 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf } TaskFactory::delayTask(5); } - + } #if FSFW_HAL_SPI_WIRETAPPING == 1 - sif::info << "RW start marker detected" << std::endl; + sif::info << "RW start marker detected" << std::endl; #endif - size_t decodedFrameLen = 0; + size_t decodedFrameLen = 0; - while (decodedFrameLen < maxReplyLen) { - /** First byte already read in */ - if (decodedFrameLen != 0) { - byteRead = 0; - if (read(fd, &byteRead, 1) != 1) { - sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - result = rws::SPI_READ_FAILURE; - break; - } - } - - if (byteRead == FLAG_BYTE) { - /** Reached end of frame */ + while (decodedFrameLen < maxReplyLen) { + // First byte already read in + if (decodedFrameLen != 0) { + byteRead = 0; + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + result = rws::SPI_READ_FAILURE; break; - } else if (byteRead == 0x7D) { - if (read(fd, &byteRead, 1) != 1) { - sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; - result = rws::SPI_READ_FAILURE; - break; - } - if (byteRead == 0x5E) { - *(replyBuf + decodedFrameLen) = 0x7E; - decodedFrameLen++; - continue; - } else if (byteRead == 0x5D) { - *(replyBuf + decodedFrameLen) = 0x7D; - decodedFrameLen++; - continue; - } else { - sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl; - closeSpi(fd); - result = rws::INVALID_SUBSTITUTE; - break; - } - } else { - *(replyBuf + decodedFrameLen) = byteRead; + } + } + + if (byteRead == FLAG_BYTE) { + // Reached end of frame + break; + } else if (byteRead == 0x7D) { + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + result = rws::SPI_READ_FAILURE; + break; + } + if (byteRead == 0x5E) { + *(replyBuf + decodedFrameLen) = 0x7E; decodedFrameLen++; continue; + } else if (byteRead == 0x5D) { + *(replyBuf + decodedFrameLen) = 0x7D; + decodedFrameLen++; + continue; + } else { + sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl; + result = rws::INVALID_SUBSTITUTE; + break; } - - /** - * There might be the unlikely case that each byte in a get-telemetry reply has been - * replaced by its substitute. Than the next byte must correspond to the end sign 0x7E. - * Otherwise there might be something wrong. - */ - if (decodedFrameLen == maxReplyLen) { - if (read(fd, &byteRead, 1) != 1) { - sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl; - result = rws::SPI_READ_FAILURE; - break; - } - if (byteRead != FLAG_BYTE) { - sif::error << "rwSpiCallback::spiCallback: Missing end sign " - << static_cast(FLAG_BYTE) << std::endl; - decodedFrameLen--; - result = rws::MISSING_END_SIGN; - break; - } - } - result = returnvalue::OK; + } else { + *(replyBuf + decodedFrameLen) = byteRead; + decodedFrameLen++; + continue; } + + // Check end marker. + /** + * There might be the unlikely case that each byte in a get-telemetry reply has been + * replaced by its substitute. Then the next byte must correspond to the end sign 0x7E. + * Otherwise there might be something wrong. + */ + if (decodedFrameLen == maxReplyLen) { + if (read(fd, &byteRead, 1) != 1) { + sif::error << "rwSpiCallback::spiCallback: Failed to read last byte" << std::endl; + result = rws::SPI_READ_FAILURE; + break; + } + if (byteRead != FLAG_BYTE) { + sif::error << "rwSpiCallback::spiCallback: Missing end sign " << static_cast(FLAG_BYTE) + << std::endl; + decodedFrameLen--; + result = rws::MISSING_END_SIGN; + break; + } + } + result = returnvalue::OK; } - return returnvalue::OK; + + pullCsHigh(gpioId, gpioIF); + closeSpi(fd); + return result; } ReturnValue_t RwPollingTask::writeOneRwCmd(uint8_t rwIdx, int fd) { @@ -410,10 +412,10 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) { encodeHdlc(writeBuffer.data(), writeLen, lenToSend); if (write(fd, encodedBuffer.data(), lenToSend) != static_cast(lenToSend)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); return rws::SPI_WRITE_FAILURE; } - pullCsHigh(gpioId, spiLock, gpioIF); + pullCsHigh(gpioId, gpioIF); return returnvalue::OK; } @@ -434,7 +436,7 @@ ReturnValue_t RwPollingTask::pullCsLow(gpioId_t gpioId, MutexIF* spiLock, GpioIF return returnvalue::OK; } -void RwPollingTask::pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF) { +void RwPollingTask::pullCsHigh(gpioId_t gpioId, GpioIF& gpioIF) { if (gpioId != gpio::NO_GPIO) { if (gpioIF.pullHigh(gpioId) != returnvalue::OK) { sif::error << "closeSpi: Failed to pull chip select high" << std::endl; diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h index 2b7d899f..701771f1 100644 --- a/linux/devices/RwPollingTask.h +++ b/linux/devices/RwPollingTask.h @@ -77,7 +77,7 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev size_t idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** readPtr); void encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, size_t& encodedLen); - void pullCsHigh(gpioId_t gpioId, MutexIF* spiLock, GpioIF& gpioIF); + void pullCsHigh(gpioId_t gpioId, GpioIF& gpioIF); void closeSpi(int); }; -- 2.43.0 From 9fdb41506b070ee0c0254eef417f307b984880a1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Feb 2023 19:58:32 +0100 Subject: [PATCH 04/18] added basic unittests for hdlc encoder --- linux/devices/RwPollingTask.cpp | 53 +++---------------- linux/devices/RwPollingTask.h | 3 -- misc/eclipse/.cproject | 35 +++++++----- .../devices/devicedefinitions/CMakeLists.txt | 2 +- .../devices/devicedefinitions/rwHelpers.cpp | 18 +++++++ mission/devices/devicedefinitions/rwHelpers.h | 8 +++ unittest/CMakeLists.txt | 1 + unittest/hdlcEncodingRw.cpp | 38 +++++++++++++ 8 files changed, 95 insertions(+), 63 deletions(-) create mode 100644 unittest/hdlcEncodingRw.cpp diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index b300136a..c8744301 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -190,7 +190,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf return rws::SPI_READ_FAILURE; } if (idx == 0) { - if (byteRead != FLAG_BYTE) { + if (byteRead != rws::FRAME_DELIMITER) { sif::error << "Invalid data, expected start marker" << std::endl; pullCsHigh(gpioId, gpioIF); closeSpi(fd); @@ -198,7 +198,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf } } - if (byteRead != FLAG_BYTE) { + if (byteRead != rws::FRAME_DELIMITER) { break; } @@ -228,7 +228,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf } } - if (byteRead == FLAG_BYTE) { + if (byteRead == rws::FRAME_DELIMITER) { // Reached end of frame break; } else if (byteRead == 0x7D) { @@ -268,9 +268,9 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf result = rws::SPI_READ_FAILURE; break; } - if (byteRead != FLAG_BYTE) { - sif::error << "rwSpiCallback::spiCallback: Missing end sign " << static_cast(FLAG_BYTE) - << std::endl; + if (byteRead != rws::FRAME_DELIMITER) { + sif::error << "rwSpiCallback::spiCallback: Missing end sign " + << static_cast(rws::FRAME_DELIMITER) << std::endl; decodedFrameLen--; result = rws::MISSING_END_SIGN; break; @@ -353,23 +353,6 @@ size_t RwPollingTask::idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, } } -void RwPollingTask::encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, size_t& encodedLen) { - encodedBuffer[0] = FLAG_BYTE; - encodedLen = 1; - for (size_t sourceIdx = 0; sourceIdx < sourceLen; sourceIdx++) { - if (sourceBuf[sourceIdx] == 0x7E) { - encodedBuffer[encodedLen++] = 0x7D; - encodedBuffer[encodedLen++] = 0x5E; - } else if (sourceBuf[sourceIdx] == 0x7D) { - encodedBuffer[encodedLen++] = 0x7D; - encodedBuffer[encodedLen++] = 0x5D; - } else { - encodedBuffer[encodedLen++] = sourceBuf[sourceIdx]; - } - } - encodedBuffer[encodedLen++] = FLAG_BYTE; -} - // This closes the SPI void RwPollingTask::closeSpi(int fd) { // This will perform the function to close the SPI @@ -385,31 +368,9 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) { return returnvalue::FAILED; } pullCsLow(gpioId, spiLock, gpioIF); - /* - //Encoding and sending command - size_t idx = 0; - while (idx < dataLen) { - switch (*(data + idx)) { - case 0x7E: - writeBuffer[0] = 0x7D; - writeBuffer[1] = 0x5E; - writeSize = 2; - break; - case 0x7D: - writeBuffer[0] = 0x7D; - writeBuffer[1] = 0x5D; - writeSize = 2; - break; - default: - writeBuffer[0] = *(data + idx); - writeSize = 1; - break; - } - } - */ // Add datalinklayer like specified in the datasheet. size_t lenToSend = 0; - encodeHdlc(writeBuffer.data(), writeLen, lenToSend); + rws::encodeHdlc(writeBuffer.data(), writeLen, encodedBuffer.data(), lenToSend); if (write(fd, encodedBuffer.data(), lenToSend) != static_cast(lenToSend)) { sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl; pullCsHigh(gpioId, gpioIF); diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h index 701771f1..b9895865 100644 --- a/linux/devices/RwPollingTask.h +++ b/linux/devices/RwPollingTask.h @@ -48,8 +48,6 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev std::array encodedBuffer; size_t writeLen = 0; - //! This is the end and start marker of the frame datalinklayer - static constexpr uint8_t FLAG_BYTE = 0x7E; static constexpr MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING; static constexpr uint32_t TIMEOUT_MS = 20; static constexpr uint8_t MAX_RETRIES_REPLY = 5; @@ -75,7 +73,6 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev ReturnValue_t prepareSetSpeedCmd(uint8_t rwIdx); size_t idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx, uint8_t** readPtr); - void encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, size_t& encodedLen); void pullCsHigh(gpioId_t gpioId, GpioIF& gpioIF); void closeSpi(int); diff --git a/misc/eclipse/.cproject b/misc/eclipse/.cproject index 0dfd812a..0f3348a8 100644 --- a/misc/eclipse/.cproject +++ b/misc/eclipse/.cproject @@ -57,7 +57,8 @@ - + + @@ -119,7 +120,8 @@ - + + @@ -187,7 +189,8 @@ - + + @@ -255,7 +258,8 @@ - + + @@ -418,7 +422,8 @@ - + + @@ -580,7 +585,8 @@ - + + @@ -680,7 +686,7 @@