diff --git a/linux/devices/RwPollingTask.cpp b/linux/devices/RwPollingTask.cpp index f0cef059..f6df08f5 100644 --- a/linux/devices/RwPollingTask.cpp +++ b/linux/devices/RwPollingTask.cpp @@ -32,12 +32,20 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { ipcLock->unlockMutex(); semaphore->acquire(); int fd = 0; + for (auto& skip : skipCommandingForRw) { + skip = false; + } ReturnValue_t result = openSpi(O_RDWR, fd); if (result != returnvalue::OK) { continue; } for (unsigned idx = 0; idx < rwCookies.size(); idx++) { - if (rwCookies[idx]->setSpeed) { + if (rwCookies[idx]->specialRequest == rws::SpecialRwRequest::RESET_MCU) { + prepareSimpleCommand(rws::RESET_MCU); + // No point in commanding that specific RW for the cycle. + skipCommandingForRw[idx] = true; + writeOneRwCmd(idx, fd); + } else if (rwCookies[idx]->setSpeed) { prepareSetSpeedCmd(idx); if (writeOneRwCmd(idx, fd) != returnvalue::OK) { continue; @@ -163,6 +171,9 @@ ReturnValue_t RwPollingTask::writeAndReadAllRws(DeviceCommandId_t id) { return result; } for (unsigned idx = 0; idx < rwCookies.size(); idx++) { + if (skipCommandingForRw[idx]) { + continue; + } ReturnValue_t result = sendOneMessage(fd, *rwCookies[idx]); if (result != returnvalue::OK) { closeSpi(fd); @@ -307,32 +318,26 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf 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; } return returnvalue::OK; } ReturnValue_t RwPollingTask::readAllRws(int fd, DeviceCommandId_t id) { - ReturnValue_t result = openSpi(O_RDWR, fd); - if (result != returnvalue::OK) { - return result; - } + // 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) { + if (((id == rws::SET_SPEED) and !rwCookies[idx]->setSpeed) or skipCommandingForRw[idx]) { continue; } 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; size_t maxReadLen = idAndIdxToReadBuffer(id, idx, &replyBuf); readNextReply(*rwCookies[idx], replyBuf, maxReadLen); } - - closeSpi(fd); + // SPI is closed in readNextReply as well. return returnvalue::OK; } diff --git a/linux/devices/RwPollingTask.h b/linux/devices/RwPollingTask.h index f22ad1fe..42bae737 100644 --- a/linux/devices/RwPollingTask.h +++ b/linux/devices/RwPollingTask.h @@ -44,6 +44,7 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev MutexIF* spiLock; const char* spiDev; SpiComIF* spiIF; + std::array skipCommandingForRw; std::array rwCookies; std::array writeBuffer; std::array encodedBuffer;