diff --git a/bsp_q7s/core/ObjectFactory.cpp b/bsp_q7s/core/ObjectFactory.cpp index ea310f87..943f0cce 100644 --- a/bsp_q7s/core/ObjectFactory.cpp +++ b/bsp_q7s/core/ObjectFactory.cpp @@ -678,11 +678,11 @@ void ObjectFactory::createReactionWheelComponents(LinuxLibgpioIF* gpioComIF, std::array rwGpioIds = {gpioIds::EN_RW1, gpioIds::EN_RW2, gpioIds::EN_RW3, gpioIds::EN_RW4}; std::array rws = {}; - new RwPollingTask(objects::SPI_RW_COM_IF, q7s::SPI_RW_DEV, *gpioComIF); + new RwPollingTask(objects::RW_POLLING_TASK, q7s::SPI_RW_DEV, *gpioComIF); for (uint8_t idx = 0; idx < rwCookies.size(); idx++) { rwCookies[idx] = new RwCookie(idx, rwCookieParams[idx].first, rwCookieParams[idx].second, rws::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED); - auto* rwHandler = new RwHandler(rwIds[idx], objects::SPI_RW_COM_IF, rwCookies[idx], gpioComIF, + auto* rwHandler = new RwHandler(rwIds[idx], objects::RW_POLLING_TASK, rwCookies[idx], gpioComIF, rwGpioIds[idx], idx); rwCookies[idx]->setCallbackArgs(rwHandler); #if OBSW_TEST_RW == 1 diff --git a/bsp_q7s/core/scheduling.cpp b/bsp_q7s/core/scheduling.cpp index ff38d642..9d96184b 100644 --- a/bsp_q7s/core/scheduling.cpp +++ b/bsp_q7s/core/scheduling.cpp @@ -199,9 +199,9 @@ void scheduling::initTasks() { #if OBSW_ADD_RW == 1 PeriodicTaskIF* rwPolling = factory->createPeriodicTask( "RW_POLLING_TASK", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 0.4, missedDeadlineFunc); - result = rwPolling->addComponent(objects::SPI_RW_COM_IF); + result = rwPolling->addComponent(objects::RW_POLLING_TASK); if (result != returnvalue::OK) { - scheduling::printAddObjectError("RW_POLLING_TASK", objects::SPI_RW_COM_IF); + scheduling::printAddObjectError("RW_POLLING_TASK", objects::RW_POLLING_TASK); } #endif diff --git a/fsfw b/fsfw index 9de6c4b3..fe41d738 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 9de6c4b3aa20ee63c28051d486be8a12df147f22 +Subproject commit fe41d73895270cbe4d80ebfbc41ff9f0b8b02126 diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index 8b6320ff..feb906c8 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -32,6 +32,7 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { state = InternalState::IDLE; ipcLock->unlockMutex(); semaphore->acquire(); + TaskFactory::delayTask(5); int fd = 0; for (auto& skip : skipCommandingForRw) { skip = false; @@ -55,7 +56,7 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { } closeSpi(fd); usleep(rws::SPI_REPLY_DELAY); - if (readAllRws(fd, rws::SET_SPEED) != returnvalue::OK) { + if (readAllRws(rws::SET_SPEED) != returnvalue::OK) { continue; } prepareSimpleCommand(rws::GET_LAST_RESET_STATUS); @@ -117,7 +118,7 @@ ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) { ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { - if (sendLen < 6) { + if (sendData == nullptr or sendLen < 8) { return DeviceHandlerIF::INVALID_DATA; } int32_t speed = 0; @@ -142,7 +143,7 @@ ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendDa rwCookie->currentRwSpeed = speed; rwCookie->currentRampTime = rampTime; rwCookie->specialRequest = specialRequest; - if (state == InternalState::IDLE and rwCookie->rwIdx == 3) { + if (state == InternalState::IDLE) { state = InternalState::BUSY; semaphore->release(); } @@ -188,13 +189,13 @@ ReturnValue_t RwPollingTask::writeAndReadAllRws(DeviceCommandId_t id) { closeSpi(fd); usleep(rws::SPI_REPLY_DELAY); - return readAllRws(fd, id); + return readAllRws(id); } 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; + sif::error << "RwPollingTask::openSpi: Failed to open device file" << std::endl; return SpiComIF::OPENING_FILE_FAILED; } @@ -206,27 +207,27 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf ReturnValue_t result = returnvalue::OK; int fd = 0; gpioId_t gpioId = rwCookie.getChipSelectPin(); - 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) { return result; } + pullCsLow(gpioId, spiLock, gpioIF); /** * The reaction wheel responds with empty frames while preparing the reply data. * However, receiving more than 5 empty frames will be interpreted as an error. */ for (int idx = 0; idx < 5; idx++) { if (read(fd, &byteRead, 1) != 1) { - sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + sif::error << "RwPollingTask: Read failed" << std::endl; pullCsHigh(gpioId, gpioIF); closeSpi(fd); return rws::SPI_READ_FAILURE; } if (idx == 0) { if (byteRead != rws::FRAME_DELIMITER) { - sif::error << "Invalid data, expected start marker" << std::endl; + sif::error << "Invalid data, expected start marker, got " << (int)byteRead << std::endl; pullCsHigh(gpioId, gpioIF); closeSpi(fd); return rws::NO_START_MARKER; @@ -257,7 +258,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf if (decodedFrameLen != 0) { byteRead = 0; if (read(fd, &byteRead, 1) != 1) { - sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + sif::error << "RwPollingTask: Read failed" << std::endl; result = rws::SPI_READ_FAILURE; break; } @@ -268,7 +269,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf break; } else if (byteRead == 0x7D) { if (read(fd, &byteRead, 1) != 1) { - sif::error << "rwSpiCallback::spiCallback: Read failed" << std::endl; + sif::error << "RwPollingTask: Read failed" << std::endl; result = rws::SPI_READ_FAILURE; break; } @@ -281,7 +282,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf decodedFrameLen++; continue; } else { - sif::error << "rwSpiCallback::spiCallback: Invalid substitute" << std::endl; + sif::error << "RwPollingTask: Invalid substitute" << std::endl; result = rws::INVALID_SUBSTITUTE; break; } @@ -327,7 +328,7 @@ ReturnValue_t RwPollingTask::writeOneRwCmd(uint8_t rwIdx, int fd) { return returnvalue::OK; } -ReturnValue_t RwPollingTask::readAllRws(int fd, DeviceCommandId_t id) { +ReturnValue_t RwPollingTask::readAllRws(DeviceCommandId_t id) { // SPI dev will be opened in readNextReply on demand. for (unsigned idx = 0; idx < rwCookies.size(); idx++) { if (((id == rws::SET_SPEED) and !rwCookies[idx]->setSpeed) or skipCommandingForRw[idx]) { @@ -463,14 +464,14 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) { 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; + sif::debug << "RwPollingTask::pullCsLow: 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; + sif::error << "RwPollingTask::pullCsLow: Failed to pull chip select low" << std::endl; return result; } } @@ -484,7 +485,7 @@ void RwPollingTask::pullCsHigh(gpioId_t gpioId, GpioIF& gpioIF) { } } if (spiLock->unlockMutex() != returnvalue::OK) { - sif::error << "rwSpiCallback::closeSpi: Failed to unlock mutex" << std::endl; + sif::error << "RwPollingTask::pullCsHigh: Failed to unlock mutex" << std::endl; ; } } diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h index 1ccb7a6c..a1bd4cc5 100644 --- a/linux/devices/RwPollingTask.h +++ b/linux/devices/RwPollingTask.h @@ -58,7 +58,7 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev ReturnValue_t writeAndReadAllRws(DeviceCommandId_t id); ReturnValue_t writeOneRwCmd(uint8_t rwIdx, int fd); - ReturnValue_t readAllRws(int fd, DeviceCommandId_t id); + ReturnValue_t readAllRws(DeviceCommandId_t id); ReturnValue_t sendOneMessage(int fd, RwCookie& rwCookie); ReturnValue_t readNextReply(RwCookie& rwCookie, uint8_t* replyBuf, size_t maxReplyLen); diff --git a/linux/fsfwconfig/objects/systemObjectList.h b/linux/fsfwconfig/objects/systemObjectList.h index 7624cd5f..6cb70a06 100644 --- a/linux/fsfwconfig/objects/systemObjectList.h +++ b/linux/fsfwconfig/objects/systemObjectList.h @@ -47,7 +47,7 @@ enum sourceObjects : uint32_t { GPIO_IF = 0x49010005, /* Custom device handler */ - SPI_RW_COM_IF = 0x49020005, + RW_POLLING_TASK = 0x49020005, /* 0x54 ('T') for test handlers */ TEST_TASK = 0x54694269, diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index 1c1263b4..cd176a67 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -125,8 +125,9 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand uint8_t* currentCmdBuf = commandBuffer + 1; rwSpeedActuationSet.serialize(¤tCmdBuf, &rawPacketLen, sizeof(commandBuffer), SerializeIF::Endianness::MACHINE); - commandBuffer[rawPacketLen] = static_cast(rws::SpecialRwRequest::REQUEST_NONE); - break; + commandBuffer[rawPacketLen++] = static_cast(rws::SpecialRwRequest::REQUEST_NONE); + rawPacket = commandBuffer; + return returnvalue::OK; } case (rws::RESET_MCU): { commandBuffer[0] = false; @@ -236,7 +237,10 @@ void RwHandler::fillCommandAndReplyMap() { ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { - // uint8_t replyByte = *start; + if (getMode() == _MODE_WAIT_OFF) { + return IGNORE_FULL_PACKET; + } + // sif::debug << "base mode: " << baseMode << std::endl; if (remainingSize > 0) { *foundLen = remainingSize; *foundId = rws::REPLY_ID; @@ -293,7 +297,7 @@ ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_ auto packetLen = rws::idToPacketLen(id); uint16_t replyCrc = (*(packet + packetLen - 1) << 8) | *(packet + packetLen - 2); if (CRC::crc16ccitt(packet, packetLen - 2, 0xFFFF) != replyCrc) { - sif::error << "RwHandler::interpretDeviceReply: CRC error for ID " << id << std::endl; + // sif::error << "RwHandler::interpretDeviceReply: CRC error for ID " << id << std::endl; return CRC_ERROR; } if (packetPtr[1] == rws::STATE_ERROR) {