Merge pull request 'RW shutdown' (#580) from feature_proper_rw_shutdown into develop

Reviewed-on: #580
Reviewed-by: Marius Eggert <eggertm@irs.uni-stuttgart.de>

CHANGELOG.md

@@ -21,6 +21,14 @@ will consitute of a breaking change warranting a new major release:
 - RW Assembly: Correctly transition back to off when more than 1 devices is OFF. Also do this
   when this was due to two devices being marked faulty.
 - RW dummy and STR dummy components: Set/Update modes correctly.
+- RW handlers: Bugfix for TM set retrieval and special request handling in general where the CRC
+  check always failed for special request. Also removed an unnecessary delay for special requests.
+- RW handlers: Polling is now disabled for RWs which are off.
+
+## Changed
+
+- RW shutdown now waits for the speed to be near 0 or for a OFF transition countdown to be expired
+  before going to off.
 
 # [v1.43.2] 2023-04-05
 

bsp_q7s/core/scheduling.cpp

@@ -237,7 +237,7 @@ void scheduling::initTasks() {
 
 #if OBSW_ADD_RW == 1
   PeriodicTaskIF* rwPolling =
-      factory->createPeriodicTask("RW_POLLING_TASK", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2,
+      factory->createPeriodicTask("RW_POLLING_TASK", 75, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2,
                                   0.4, missedDeadlineFunc, &RR_SCHEDULING);
   result = rwPolling->addComponent(objects::RW_POLLING_TASK);
   if (result != returnvalue::OK) {

linux/acs/RwPollingTask.cpp

@@ -11,6 +11,7 @@
 #include <unistd.h>
 
 #include "devConf.h"
+#include "mission/acs/defs.h"
 #include "mission/acs/rwHelpers.h"
 
 RwPollingTask::RwPollingTask(object_id_t objectId, const char* spiDev, GpioIF& gpioIF)
@@ -35,6 +36,7 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) {
     semaphore->acquire();
     // This loop takes 50 ms on a debug build.
     // Stopwatch watch;
+    // Give all device handlers some time to submit requests.
     TaskFactory::delayTask(5);
     int fd = 0;
     for (auto& skip : skipCommandingForRw) {
@@ -45,13 +47,24 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) {
     if (result != returnvalue::OK) {
       continue;
     }
+    acs::SimpleSensorMode currentMode;
+    rws::SpecialRwRequest specialRequest;
+
     for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
-      if (rwCookies[idx]->specialRequest == rws::SpecialRwRequest::RESET_MCU) {
+      {
+        MutexGuard mg(ipcLock);
+        currentMode = rwRequests[idx].mode;
+        specialRequest = rwRequests[idx].specialRequest;
+        skipSetSpeedReply[idx] = rwRequests[idx].setSpeed;
+      }
+      if (currentMode == acs::SimpleSensorMode::OFF) {
+        skipCommandingForRw[idx] = true;
+      } else if (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) {
+      } else if (skipSetSpeedReply[idx]) {
         prepareSetSpeedCmd(idx);
         if (writeOneRwCmd(idx, fd) != returnvalue::OK) {
           continue;
@@ -121,31 +134,14 @@ ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) {
 
 ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
                                          size_t sendLen) {
-  if (sendData == nullptr or sendLen < 8) {
+  if (sendData == nullptr or sendLen != sizeof(rws::RwRequest)) {
     return DeviceHandlerIF::INVALID_DATA;
   }
-  int32_t speed = 0;
-  uint16_t rampTime = 0;
-  const uint8_t* currentBuf = sendData;
-  bool setSpeed = currentBuf[0];
-  currentBuf += 1;
-  sendLen -= 1;
-  SerializeAdapter::deSerialize(&speed, &currentBuf, &sendLen, SerializeIF::Endianness::MACHINE);
-  SerializeAdapter::deSerialize(&rampTime, &currentBuf, &sendLen, SerializeIF::Endianness::MACHINE);
-  rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
-  if (sendLen == 8 and sendData[7] < static_cast<uint8_t>(rws::SpecialRwRequest::NUM_REQUESTS)) {
-    specialRequest = static_cast<rws::SpecialRwRequest>(sendData[7]);
-  }
-  RwCookie* rwCookie = dynamic_cast<RwCookie*>(cookie);
-  if (rwCookie == nullptr) {
-    return returnvalue::FAILED;
-  }
+  const rws::RwRequest* rwRequest = reinterpret_cast<const rws::RwRequest*>(sendData);
+  uint8_t rwIdx = rwRequest->rwIdx;
   {
     MutexGuard mg(ipcLock);
-    rwCookie->setSpeed = setSpeed;
-    rwCookie->currentRwSpeed = speed;
-    rwCookie->currentRampTime = rampTime;
-    rwCookie->specialRequest = specialRequest;
+    std::memcpy(&rwRequests[rwIdx], rwRequest, sizeof(rws::RwRequest));
     if (state == InternalState::IDLE) {
       state = InternalState::IS_BUSY;
       semaphore->release();
@@ -332,7 +328,7 @@ ReturnValue_t RwPollingTask::writeOneRwCmd(uint8_t rwIdx, int fd) {
 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]) {
+    if (((id == rws::SET_SPEED) and !skipSetSpeedReply[idx]) or skipCommandingForRw[idx]) {
       continue;
     }
     uint8_t* replyBuf;
@@ -395,7 +391,7 @@ void RwPollingTask::fillSpecialRequestArray() {
       specialRequestIds[idx] = DeviceHandlerIF::NO_COMMAND_ID;
       continue;
     }
-    switch (rwCookies[idx]->specialRequest) {
+    switch (rwRequests[idx].specialRequest) {
       case (rws::SpecialRwRequest::GET_TM): {
         specialRequestIds[idx] = rws::GET_TM;
         break;
@@ -426,14 +422,14 @@ void RwPollingTask::handleSpecialRequests() {
     writeOneRwCmd(idx, fd);
   }
   closeSpi(fd);
-  usleep(rws::SPI_REPLY_DELAY);
   for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
     if (specialRequestIds[idx] == DeviceHandlerIF::NO_COMMAND_ID) {
       continue;
     }
     uint8_t* replyBuf;
     size_t maxReadLen = idAndIdxToReadBuffer(specialRequestIds[idx], idx, &replyBuf);
-    result = readNextReply(*rwCookies[idx], replyBuf, maxReadLen);
+    // Skip first byte for actual read buffer: Valid byte
+    result = readNextReply(*rwCookies[idx], replyBuf + 1, maxReadLen);
     if (result == returnvalue::OK) {
       // The first byte is always a flag which shows whether the value was read
       // properly at least once.
@@ -455,17 +451,12 @@ void RwPollingTask::setAllReadFlagsFalse() {
   }
 }
 
-// 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.
-}
+void RwPollingTask::closeSpi(int fd) { close(fd); }
 
 ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) {
   gpioId_t gpioId = rwCookie.getChipSelectPin();
   if (spiLock == nullptr) {
-    sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
+    sif::warning << "RwPollingTask: Mutex or GPIO interface invalid" << std::endl;
     return returnvalue::FAILED;
   }
   // Add datalinklayer like specified in the datasheet.
@@ -473,7 +464,7 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) {
   rws::encodeHdlc(writeBuffer.data(), writeLen, encodedBuffer.data(), lenToSend);
   pullCsLow(gpioId, gpioIF);
   if (write(fd, encodedBuffer.data(), lenToSend) != static_cast<ssize_t>(lenToSend)) {
-    sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
+    sif::error << "RwPollingTask: Write failed!" << std::endl;
     pullCsHigh(gpioId, gpioIF);
     return rws::SPI_WRITE_FAILURE;
   }
@@ -484,7 +475,7 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) {
 ReturnValue_t RwPollingTask::pullCsLow(gpioId_t gpioId, GpioIF& gpioIF) {
   ReturnValue_t result = spiLock->lockMutex(TIMEOUT_TYPE, TIMEOUT_MS);
   if (result != returnvalue::OK) {
-    sif::debug << "RwPollingTask::pullCsLow: Failed to lock mutex" << std::endl;
+    sif::warning << "RwPollingTask::pullCsLow: Failed to lock mutex" << std::endl;
     return result;
   }
   // Pull SPI CS low. For now, no support for active high given
@@ -525,8 +516,8 @@ ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) {
   uint16_t rampTimeToSet = 10;
   {
     MutexGuard mg(ipcLock);
-    speedToSet = rwCookies[rwIdx]->currentRwSpeed;
-    rampTimeToSet = rwCookies[rwIdx]->currentRampTime;
+    speedToSet = rwRequests[rwIdx].currentRwSpeed;
+    rampTimeToSet = rwRequests[rwIdx].currentRampTime;
   }
   size_t serLen = 1;
   SerializeAdapter::serialize(&speedToSet, &serPtr, &serLen, writeBuffer.size(),

linux/acs/RwPollingTask.h

@@ -8,6 +8,7 @@
 #include <fsfw_hal/common/gpio/GpioIF.h>
 #include <fsfw_hal/linux/spi/SpiComIF.h>
 #include <fsfw_hal/linux/spi/SpiCookie.h>
+#include <mission/acs/defs.h>
 
 #include "mission/acs/rwHelpers.h"
 
@@ -26,10 +27,6 @@ class RwCookie : public SpiCookie {
   std::array<uint8_t, REPLY_BUF_LEN> replyBuf{};
   std::array<uint8_t, REPLY_BUF_LEN> exchangeBuf{};
   MutexIF* bufLock;
-  bool setSpeed = true;
-  int32_t currentRwSpeed = 0;
-  uint16_t currentRampTime = 0;
-  rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
   uint8_t rwIdx;
 };
 
@@ -50,8 +47,10 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev
   const char* spiDev;
   GpioIF& gpioIF;
   std::array<bool, 4> skipCommandingForRw;
+  std::array<bool, 4> skipSetSpeedReply;
   std::array<DeviceCommandId_t, 4> specialRequestIds;
   std::array<RwCookie*, 4> rwCookies;
+  std::array<rws::RwRequest, 4> rwRequests{};
   std::array<uint8_t, rws::MAX_CMD_SIZE> writeBuffer;
   std::array<uint8_t, rws::MAX_CMD_SIZE * 2> encodedBuffer;
 

mission/acs/RwHandler.cpp

@@ -4,6 +4,7 @@
 #include <fsfw/globalfunctions/CRC.h>
 #include <fsfw/globalfunctions/arrayprinter.h>
 #include <fsfw_hal/common/gpio/GpioIF.h>
+#include <mission/acs/defs.h>
 
 #include "OBSWConfig.h"
 
@@ -23,44 +24,52 @@ RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCooki
   if (gpioComIF == nullptr) {
     sif::error << "RwHandler: Invalid gpio communication interface" << std::endl;
   }
+  currentRequest.rwIdx = rwIdx;
 }
+
 RwHandler::~RwHandler() {}
 
 void RwHandler::doStartUp() {
   internalState = InternalState::DEFAULT;
 
   if (gpioComIF->pullHigh(enableGpio) != returnvalue::OK) {
-    sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high";
+    sif::error << "RW Handler " << rwIdx << ": Failed to pull ENABLE pin high for startup";
   }
+  currentRequest.mode = acs::SimpleSensorMode::NORMAL;
   updatePeriodicReply(true, rws::REPLY_ID);
   statusSet.setReportingEnabled(true);
   setMode(_MODE_TO_ON);
 }
 
 void RwHandler::doShutDown() {
-  if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) {
-    sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low";
+  if (internalState != InternalState::SHUTDOWN) {
+    internalState = InternalState::SHUTDOWN;
+    shutdownState = ShutdownState::SET_SPEED_ZERO;
+    offTransitionCountdown.resetTimer();
   }
-  internalState = InternalState::DEFAULT;
-  updatePeriodicReply(false, rws::REPLY_ID);
-  {
-    PoolReadGuard pg(&statusSet);
-    statusSet.currSpeed = 0.0;
-    statusSet.referenceSpeed = 0.0;
-    statusSet.state = 0;
-    statusSet.setValidity(false, true);
-    statusSet.setReportingEnabled(false);
+  if ((internalState == InternalState::SHUTDOWN) and
+      (shutdownState == ShutdownState::DONE or offTransitionCountdown.hasTimedOut())) {
+    if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) {
+      sif::error << "RW Handler " << rwIdx << ": Failed to pull ENABLE pin low for shutdown";
+    }
+    updatePeriodicReply(false, rws::REPLY_ID);
+    {
+      PoolReadGuard pg(&statusSet);
+      statusSet.currSpeed = 0.0;
+      statusSet.referenceSpeed = 0.0;
+      statusSet.state = 0;
+      statusSet.setValidity(false, true);
+      statusSet.setReportingEnabled(false);
+    }
+    {
+      PoolReadGuard pg(&tmDataset);
+      tmDataset.setValidity(false, true);
+    }
+    internalState = InternalState::DEFAULT;
+    shutdownState = ShutdownState::NONE;
+    // The power switch is handled by the assembly, so we can go off here directly.
+    setMode(MODE_OFF);
   }
-  {
-    PoolReadGuard pg(&tmDataset);
-    tmDataset.setValidity(false, true);
-  }
-  {
-    PoolReadGuard pg(&rwSpeedActuationSet);
-    rwSpeedActuationSet.setRwSpeed(0, 10);
-  }
-  // The power switch is handled by the assembly, so we can go off here directly.
-  setMode(MODE_OFF);
 }
 
 ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
@@ -77,6 +86,20 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
 }
 
 ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
+  if (internalState == InternalState::SHUTDOWN) {
+    if (shutdownState == ShutdownState::SET_SPEED_ZERO) {
+      {
+        PoolReadGuard pg(&rwSpeedActuationSet);
+        rwSpeedActuationSet.setRwSpeed(0, 10);
+      }
+      *id = rws::REQUEST_ID;
+      return buildCommandFromCommand(*id, nullptr, 0);
+    } else if (shutdownState == ShutdownState::STOP_POLLING) {
+      currentRequest.mode = acs::SimpleSensorMode::OFF;
+      *id = rws::REQUEST_ID;
+      return buildCommandFromCommand(*id, nullptr, 0);
+    }
+  }
   return NOTHING_TO_SEND;
 }
 
@@ -119,32 +142,36 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
         return result;
       }
       // set speed flag.
-      commandBuffer[0] = true;
-      rawPacketLen = 1;
-      uint8_t* currentCmdBuf = commandBuffer + 1;
-      rwSpeedActuationSet.serialize(&currentCmdBuf, &rawPacketLen, sizeof(commandBuffer),
-                                    SerializeIF::Endianness::MACHINE);
-      commandBuffer[rawPacketLen++] = static_cast<uint8_t>(rws::SpecialRwRequest::REQUEST_NONE);
-      rawPacket = commandBuffer;
+      currentRequest.setSpeed = true;
+      rwSpeedActuationSet.getRwSpeed(currentRequest.currentRwSpeed, currentRequest.currentRampTime);
+      currentRequest.specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
+      rawPacket = reinterpret_cast<uint8_t*>(&currentRequest);
+      rawPacketLen = sizeof(rws::RwRequest);
       return returnvalue::OK;
     }
     case (rws::RESET_MCU): {
-      commandBuffer[0] = false;
-      commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::RESET_MCU);
+      currentRequest.setSpeed = false;
+      currentRequest.specialRequest = rws::SpecialRwRequest::RESET_MCU;
       internalState = InternalState::RESET_MCU;
+      rawPacket = reinterpret_cast<uint8_t*>(&currentRequest);
+      rawPacketLen = sizeof(rws::RwRequest);
       return returnvalue::OK;
     }
 
     case (rws::INIT_RW_CONTROLLER): {
-      commandBuffer[0] = false;
-      commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::INIT_RW_CONTROLLER);
+      currentRequest.setSpeed = false;
+      currentRequest.specialRequest = rws::SpecialRwRequest::INIT_RW_CONTROLLER;
       internalState = InternalState::INIT_RW_CONTROLLER;
+      rawPacket = reinterpret_cast<uint8_t*>(&currentRequest);
+      rawPacketLen = sizeof(rws::RwRequest);
       return returnvalue::OK;
     }
     case (rws::GET_TM): {
-      commandBuffer[0] = false;
-      commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::GET_TM);
+      currentRequest.setSpeed = false;
+      currentRequest.specialRequest = rws::SpecialRwRequest::GET_TM;
       internalState = InternalState::GET_TM;
+      rawPacket = reinterpret_cast<uint8_t*>(&currentRequest);
+      rawPacketLen = sizeof(rws::RwRequest);
       return returnvalue::OK;
     }
     default:
@@ -172,6 +199,9 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize
     *foundLen = remainingSize;
     *foundId = rws::REPLY_ID;
   }
+  if (internalState == InternalState::SHUTDOWN and shutdownState == ShutdownState::STOP_POLLING) {
+    shutdownState = ShutdownState::DONE;
+  }
   return returnvalue::OK;
 }
 
@@ -381,6 +411,12 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
 
   statusSet.setValidity(true, true);
 
+  if (internalState == InternalState::SHUTDOWN and std::abs(tmDataset.rwCurrSpeed.value) <= 2 and
+      shutdownState == ShutdownState::SET_SPEED_ZERO) {
+    // Finish transition to off.
+    shutdownState = ShutdownState::STOP_POLLING;
+  }
+
   if (statusSet.state == rws::STATE_ERROR) {
     // Trigger FDIR reaction, first recovery, then faulty if it doesnt fix the issue.
     triggerEvent(DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT, statusSet.state.value, 0);

mission/acs/RwHandler.h

@@ -75,6 +75,8 @@ class RwHandler : public DeviceHandlerBase {
   GpioIF* gpioComIF = nullptr;
   gpioId_t enableGpio = gpio::NO_GPIO;
   bool debugMode = false;
+  Countdown offTransitionCountdown = Countdown(5000);
+  rws::RwRequest currentRequest;
 
   rws::StatusSet statusSet;
   rws::LastResetSatus lastResetStatusSet;
@@ -87,27 +89,16 @@ class RwHandler : public DeviceHandlerBase {
   PoolEntry<int32_t> rwSpeed = PoolEntry<int32_t>({0});
   PoolEntry<uint16_t> rampTime = PoolEntry<uint16_t>({10});
 
-  enum class InternalState {
-    DEFAULT,
-    GET_TM,
-    INIT_RW_CONTROLLER,
-    RESET_MCU,
-    // GET_RESET_STATUS,
-    // CLEAR_RESET_STATUS,
-    // READ_TEMPERATURE,
-    // SET_SPEED,
-    // GET_RW_SATUS
-  };
+  enum class InternalState { DEFAULT, GET_TM, INIT_RW_CONTROLLER, RESET_MCU, SHUTDOWN };
+  enum class ShutdownState {
+    NONE,
+    SET_SPEED_ZERO,
+    STOP_POLLING,
+    DONE,
+  } shutdownState = ShutdownState::NONE;
 
   InternalState internalState = InternalState::DEFAULT;
 
-  /**
-   * @brief   This function can be used to build commands which do not contain any data apart
-   *          from the command id and the CRC.
-   * @param commandId The command id of the command to build.
-   */
-  // void prepareSimpleCommand(DeviceCommandId_t id);
-
   /**
    * @brief   This function checks if the receiced speed and ramp time to set are in a valid
    *          range.
@@ -115,13 +106,6 @@ class RwHandler : public DeviceHandlerBase {
    */
   ReturnValue_t checkSpeedAndRampTime();
 
-  /**
-   * @brief   This function prepares the set speed command from the dataSet received with
-   *          an action message or set in the software.
-   * @return  returnvalue::OK if successful, otherwise error code.
-   */
-  // ReturnValue_t prepareSetSpeedCmd();
-
   /**
    * @brief   This function writes the last reset status retrieved with the get last reset status
    *          command into the reset status dataset.

mission/acs/rwHelpers.h

@@ -4,6 +4,7 @@
 #include <fsfw/datapoollocal/LocalPoolVariable.h>
 #include <fsfw/datapoollocal/StaticLocalDataSet.h>
 #include <fsfw/devicehandlers/DeviceHandlerIF.h>
+#include <mission/acs/defs.h>
 
 #include "eive/resultClassIds.h"
 #include "events/subsystemIdRanges.h"
@@ -36,6 +37,15 @@ enum class SpecialRwRequest : uint8_t {
   NUM_REQUESTS
 };
 
+struct RwRequest {
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
+  bool setSpeed = true;
+  int32_t currentRwSpeed = 0;
+  uint16_t currentRampTime = 0;
+  rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
+  uint8_t rwIdx = 0;
+};
+
 static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER;
 
 static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0);

tmtc

@@ -1 +1 @@
-Subproject commit 91a8a2e89519ac20d9ddabec2d8eaeb7707718d5
+Subproject commit 98a9601dd7610dc7d2f47a622da898900c7e7f04