diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index e9383c4a..7a8b52f3 100644 --- a/mission/devices/RwHandler.cpp +++ b/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(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(crc & 0xFF); - commandBuffer[8] = static_cast(crc >> 8 & 0xFF); + commandBuffer[8] = static_cast((crc >> 8) & 0xFF); rawPacket = commandBuffer; rawPacketLen = 9; + return result; } void RwHandler::handleResetStatusReply(const uint8_t* packet) { diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index 17f2b396..af59caa1 100644 --- a/mission/devices/RwHandler.h +++ b/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