rwHandler rwSpeedActuationSet handling

mission/devices/RwHandler.cpp

@@ -13,7 +13,8 @@ RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCooki
       enableGpio(enableGpio),
       statusSet(this),
       lastResetStatusSet(this),
-      tmDataset(this) {
+      tmDataset(this),
+      rwSpeedActuationSet(*this) {
   if (comCookie == nullptr) {
     sif::error << "RwHandler: Invalid com cookie" << std::endl;
   }
@@ -97,16 +98,36 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
       return returnvalue::OK;
     }
     case (RwDefinitions::SET_SPEED): {
-      if (commandDataLen != 6) {
+      if (commandData != nullptr && commandDataLen != 6) {
         sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
                    << " invalid length" << std::endl;
         return SET_SPEED_COMMAND_INVALID_LENGTH;
       }
-      result = checkSpeedAndRampTime(commandData, commandDataLen);
+      // Commands override anything which was set in the software
+      if (commandData != nullptr) {
+        rwSpeedActuationSet.setValidityBufferGeneration(false);
+        result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen,
+                                                 SerializeIF::Endianness::NETWORK);
+        rwSpeedActuationSet.setValidityBufferGeneration(true);
+        if (result != returnvalue::OK) {
+          return result;
+        }
+      } else {
+        // Read set rw speed value from local pool
+        PoolReadGuard pg(&rwSpeedActuationSet);
+      }
+      if (ACTUATION_WIRETAPPING) {
+        int32_t speed;
+        uint16_t rampTime;
+        rwSpeedActuationSet.getRwSpeed(speed, rampTime);
+        sif::debug << "Actuating RW with speed = " << speed << " and rampTime = " << rampTime
+                   << std::endl;
+      }
+      result = checkSpeedAndRampTime();
       if (result != returnvalue::OK) {
         return result;
       }
-      prepareSetSpeedCmd(commandData, commandDataLen);
+      result = prepareSetSpeedCmd();
       return result;
     }
     case (RwDefinitions::GET_TEMPERATURE): {
@@ -298,17 +319,15 @@ void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) {
   rawPacketLen = 3;
 }
 
-ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen) {
-  int32_t speed =
-      *commandData << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 | *(commandData + 3);
-
+ReturnValue_t RwHandler::checkSpeedAndRampTime() {
+  int32_t speed = 0;
+  uint16_t rampTime = 0;
+  rwSpeedActuationSet.getRwSpeed(speed, rampTime);
   if ((speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) && (speed != 0)) {
     sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl;
     return INVALID_SPEED;
   }
 
-  uint16_t rampTime = (*(commandData + 4) << 8) | *(commandData + 5);
-
   if (rampTime < 10 || rampTime > 20000) {
     sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time" << std::endl;
     return INVALID_RAMP_TIME;
@@ -317,23 +336,24 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_
   return returnvalue::OK;
 }
 
-void RwHandler::prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen) {
+ReturnValue_t RwHandler::prepareSetSpeedCmd() {
   commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::SET_SPEED);
-
-  /** Speed (0.1 RPM) */
-  commandBuffer[1] = *(commandData + 3);
-  commandBuffer[2] = *(commandData + 2);
-  commandBuffer[3] = *(commandData + 1);
-  commandBuffer[4] = *commandData;
-  /** Ramp time (ms) */
-  commandBuffer[5] = *(commandData + 5);
-  commandBuffer[6] = *(commandData + 4);
+  uint8_t* serPtr = commandBuffer + 1;
+  size_t serSize = 1;
+  rwSpeedActuationSet.setValidityBufferGeneration(false);
+  ReturnValue_t result = rwSpeedActuationSet.serialize(&serPtr, &serSize, sizeof(commandBuffer),
+                                                       SerializeIF::Endianness::LITTLE);
+  rwSpeedActuationSet.setValidityBufferGeneration(true);
+  if (result != returnvalue::OK) {
+    return result;
+  }
 
   uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF);
   commandBuffer[7] = static_cast<uint8_t>(crc & 0xFF);
-  commandBuffer[8] = static_cast<uint8_t>(crc >> 8 & 0xFF);
+  commandBuffer[8] = static_cast<uint8_t>((crc >> 8) & 0xFF);
   rawPacket = commandBuffer;
   rawPacketLen = 9;
+  return result;
 }
 
 void RwHandler::handleResetStatusReply(const uint8_t* packet) {

mission/devices/RwHandler.h

@@ -9,6 +9,8 @@
 #include "events/subsystemIdRanges.h"
 #include "returnvalues/classIds.h"
 
+static constexpr bool ACTUATION_WIRETAPPING = false;
+
 class GpioIF;
 
 /**
@@ -93,6 +95,7 @@ class RwHandler : public DeviceHandlerBase {
   RwDefinitions::StatusSet statusSet;
   RwDefinitions::LastResetSatus lastResetStatusSet;
   RwDefinitions::TmDataset tmDataset;
+  RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet;
 
   uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
 
@@ -117,13 +120,14 @@ class RwHandler : public DeviceHandlerBase {
    *          range.
    * @return  returnvalue::OK if successful, otherwise error code.
    */
-  ReturnValue_t checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t checkSpeedAndRampTime();
 
   /**
-   * @brief   This function prepares the set speed command from the commandData received with
-   *          an action message.
+   * @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.
    */
-  void prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen);
+  ReturnValue_t prepareSetSpeedCmd();
 
   /**
    * @brief   This function writes the last reset status retrieved with the get last reset status