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_ */