continue rw refactoring

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)
 

linux/devices/RwPollingTask.cpp

@@ -0,0 +1,401 @@
+#include "RwPollingTask.h"
+
+#include <fcntl.h>
+#include <fsfw/globalfunctions/CRC.h>
+#include <fsfw/tasks/SemaphoreFactory.h>
+#include <fsfw/tasks/TaskFactory.h>
+#include <fsfw_hal/common/spi/spiCommon.h>
+#include <unistd.h>
+
+#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<rws::SpecialRwRequest>(sendData[6]);
+  }
+  RwCookie* rwCookie = dynamic_cast<RwCookie*>(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<RwCookie*>(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<int>(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<ssize_t>(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<ssize_t>(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<ssize_t>(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<uint8_t>(id);
+  uint16_t crc = CRC::crc16ccitt(writeBuffer.data(), 1, 0xFFFF);
+  writeBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
+  writeBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
+}
+
+ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) {
+  writeBuffer[0] = static_cast<uint8_t>(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<uint8_t>(crc & 0xFF);
+  writeBuffer[8] = static_cast<uint8_t>((crc >> 8) & 0xFF);
+  return returnvalue::OK;
+}

linux/devices/RwPollingTask.h

@@ -0,0 +1,78 @@
+#ifndef LINUX_DEVICES_RWPOLLINGTASK_H_
+#define LINUX_DEVICES_RWPOLLINGTASK_H_
+
+#include <fsfw/devicehandlers/DeviceCommunicationIF.h>
+#include <fsfw/objectmanager/SystemObject.h>
+#include <fsfw/tasks/ExecutableObjectIF.h>
+#include <fsfw/tasks/SemaphoreIF.h>
+#include <fsfw_hal/linux/spi/SpiComIF.h>
+#include <fsfw_hal/linux/spi/SpiCookie.h>
+
+#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<uint8_t, 524> 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<RwCookie*, 4> rwCookies;
+  std::array<uint8_t, rws::MAX_CMD_SIZE> writeBuffer;
+  size_t writeLen = 0;
+  std::array<uint8_t, 256> 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_ */