#include "RwHandler.h" #include #include #include #include "OBSWConfig.h" RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie, GpioIF* gpioComIF, gpioId_t enableGpio, uint8_t rwIdx) : DeviceHandlerBase(objectId, comIF, comCookie), gpioComIF(gpioComIF), enableGpio(enableGpio), statusSet(this), lastResetStatusSet(this), tmDataset(this), rwSpeedActuationSet(*this), rwIdx(rwIdx) { if (comCookie == nullptr) { sif::error << "RwHandler: Invalid com cookie" << std::endl; } if (gpioComIF == nullptr) { sif::error << "RwHandler: Invalid gpio communication interface" << std::endl; } } 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"; } updatePeriodicReply(true, rws::REPLY_ID); setMode(_MODE_TO_ON); } void RwHandler::doShutDown() { if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) { sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low"; } internalState = InternalState::DEFAULT; updatePeriodicReply(false, rws::REPLY_ID); setMode(_MODE_POWER_DOWN); } ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { switch (internalState) { case InternalState::DEFAULT: { *id = rws::REQUEST_ID; break; } // case InternalState::SET_SPEED: // *id = rws::SET_SPEED; // internalState = InternalState::GET_RESET_STATUS; // break; // case InternalState::GET_RESET_STATUS: // *id = rws::GET_LAST_RESET_STATUS; // internalState = InternalState::READ_TEMPERATURE; // break; // case InternalState::READ_TEMPERATURE: // *id = rws::GET_TEMPERATURE; // internalState = InternalState::GET_RW_SATUS; // break; // case InternalState::GET_RW_SATUS: // *id = rws::GET_RW_STATUS; // internalState = InternalState::CLEAR_RESET_STATUS; // break; // case InternalState::CLEAR_RESET_STATUS: // *id = rws::CLEAR_LAST_RESET_STATUS; // /** After reset status is cleared, reset status will be polled again for verification // */ internalState = InternalState::GET_RESET_STATUS; break; default: sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid internal step" << std::endl; break; } return buildCommandFromCommand(*id, NULL, 0); } ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { return NOTHING_TO_SEND; } ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, size_t commandDataLen) { ReturnValue_t result = returnvalue::OK; switch (deviceCommand) { case (rws::REQUEST_ID): { if (commandData != nullptr && commandDataLen != 6) { sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" << " invalid length" << std::endl; return SET_SPEED_COMMAND_INVALID_LENGTH; } { PoolReadGuard pg(&rwSpeedActuationSet); // 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; } } } if (ACTUATION_WIRETAPPING) { int32_t speed = 0; uint16_t rampTime = 0; rwSpeedActuationSet.getRwSpeed(speed, rampTime); sif::debug << "Actuating RW " << static_cast(rwIdx) << " with speed = " << speed << " and rampTime = " << rampTime << std::endl; } result = checkSpeedAndRampTime(); if (result != returnvalue::OK) { return result; } // set speed flag. commandBuffer[0] = true; rawPacketLen = 1; uint8_t* currentCmdBuf = commandBuffer + 1; rwSpeedActuationSet.serialize(¤tCmdBuf, &rawPacketLen, sizeof(commandBuffer), SerializeIF::Endianness::MACHINE); commandBuffer[rawPacketLen] = static_cast(rws::SpecialRwRequest::REQUEST_NONE); break; } case (rws::RESET_MCU): { commandBuffer[0] = false; commandBuffer[7] = static_cast(rws::SpecialRwRequest::RESET_MCU); internalState = InternalState::RESET_MCU; return returnvalue::OK; } case (rws::INIT_RW_CONTROLLER): { commandBuffer[0] = false; commandBuffer[7] = static_cast(rws::SpecialRwRequest::INIT_RW_CONTROLLER); internalState = InternalState::INIT_RW_CONTROLLER; return returnvalue::OK; } case (rws::GET_TM): { commandBuffer[0] = false; commandBuffer[7] = static_cast(rws::SpecialRwRequest::GET_TM); internalState = InternalState::GET_TM; return returnvalue::OK; } // case (rws::GET_LAST_RESET_STATUS): { // prepareSimpleCommand(deviceCommand); // return returnvalue::OK; // } // case (rws::CLEAR_LAST_RESET_STATUS): { // prepareSimpleCommand(deviceCommand); // return returnvalue::OK; // } // case (rws::GET_RW_STATUS): { // prepareSimpleCommand(deviceCommand); // return returnvalue::OK; // } // case (rws::INIT_RW_CONTROLLER): { // prepareSimpleCommand(deviceCommand); // return returnvalue::OK; // } // case (rws::SET_SPEED): { // if (commandData != nullptr && commandDataLen != 6) { // sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" // << " invalid length" << std::endl; // return SET_SPEED_COMMAND_INVALID_LENGTH; // } // // { // PoolReadGuard pg(&rwSpeedActuationSet); // // 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; // } // } // } // if (ACTUATION_WIRETAPPING) { // int32_t speed = 0; // uint16_t rampTime = 0; // rwSpeedActuationSet.getRwSpeed(speed, rampTime); // sif::debug << "Actuating RW " << static_cast(rwIdx) << " with speed = " << // speed // << " and rampTime = " << rampTime << std::endl; // } // result = checkSpeedAndRampTime(); // if (result != returnvalue::OK) { // return result; // } // result = prepareSetSpeedCmd(); // return result; // } // case (rws::GET_TEMPERATURE): { // prepareSimpleCommand(deviceCommand); // return returnvalue::OK; // } // case (rws::GET_TM): { // prepareSimpleCommand(deviceCommand); // return returnvalue::OK; // } default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } return returnvalue::FAILED; } void RwHandler::fillCommandAndReplyMap() { insertInCommandMap(rws::REQUEST_ID); insertInReplyMap(rws::REPLY_ID, 5, nullptr, 0, true); insertInCommandMap(rws::RESET_MCU); insertInCommandAndReplyMap(rws::INIT_RW_CONTROLLER, 5, nullptr, 0, false, true, rws::REPLY_ID); insertInCommandAndReplyMap(rws::GET_TM, 5, nullptr, 0, false, true, rws::REPLY_ID); /* this->insertInCommandAndReplyMap(rws::GET_LAST_RESET_STATUS, 1, &lastResetStatusSet, rws::SIZE_GET_RESET_STATUS); this->insertInCommandAndReplyMap(rws::CLEAR_LAST_RESET_STATUS, 1, nullptr, rws::SIZE_CLEAR_RESET_STATUS); this->insertInCommandAndReplyMap(rws::GET_RW_STATUS, 1, &statusSet, rws::SIZE_GET_RW_STATUS); this->insertInCommandAndReplyMap(rws::INIT_RW_CONTROLLER, 1, nullptr, rws::SIZE_INIT_RW); this->insertInCommandAndReplyMap(rws::GET_TEMPERATURE, 1, nullptr, rws::SIZE_GET_TEMPERATURE_REPLY); this->insertInCommandAndReplyMap(rws::SET_SPEED, 1, nullptr, rws::SIZE_SET_SPEED_REPLY); this->insertInCommandAndReplyMap(rws::GET_TM, 1, &tmDataset, rws::SIZE_GET_TELEMETRY_REPLY); */ } ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, size_t* foundLen) { // uint8_t replyByte = *start; if (remainingSize > 0) { *foundLen = remainingSize; *foundId = rws::REPLY_ID; } // RwReplies replies(start); // switch (replyByte) { // case (rws::GET_LAST_RESET_STATUS): { // *foundLen = rws::SIZE_GET_RESET_STATUS; // *foundId = rws::GET_LAST_RESET_STATUS; // break; // } // case (rws::CLEAR_LAST_RESET_STATUS): { // *foundLen = rws::SIZE_CLEAR_RESET_STATUS; // *foundId = rws::CLEAR_LAST_RESET_STATUS; // break; // } // case (rws::GET_RW_STATUS): { // *foundLen = rws::SIZE_GET_RW_STATUS; // *foundId = rws::GET_RW_STATUS; // break; // } // case (rws::INIT_RW_CONTROLLER): { // *foundLen = rws::SIZE_INIT_RW; // *foundId = rws::INIT_RW_CONTROLLER; // break; // } // case (rws::SET_SPEED): { // *foundLen = rws::SIZE_SET_SPEED_REPLY; // *foundId = rws::SET_SPEED; // break; // } // case (rws::GET_TEMPERATURE): { // *foundLen = rws::SIZE_GET_TEMPERATURE_REPLY; // *foundId = rws::GET_TEMPERATURE; // break; // } // case (rws::GET_TM): { // *foundLen = rws::SIZE_GET_TELEMETRY_REPLY; // *foundId = rws::GET_TM; // break; // } // default: { // sif::warning << "RwHandler::scanForReply: Reply contains invalid command code" << // std::endl; *foundLen = remainingSize; return returnvalue::FAILED; // } // } return returnvalue::OK; } ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { RwReplies replies(packet); ReturnValue_t result = returnvalue::OK; auto checkPacket = [&](DeviceCommandId_t id, const uint8_t* packetPtr) { auto packetLen = rws::idToPacketLen(id); uint16_t replyCrc = (*(packet + packetLen - 1) << 8) | *(packet + packetLen - 2); if (CRC::crc16ccitt(packet, packetLen - 2, 0xFFFF) != replyCrc) { sif::error << "RwHandler::interpretDeviceReply: CRC error for ID " << id << std::endl; return CRC_ERROR; } if (packetPtr[1] == rws::STATE_ERROR) { sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: " << id << std::endl; result = EXECUTION_FAILED; } return returnvalue::OK; }; checkPacket(rws::DeviceCommandId::SET_SPEED, replies.getSetSpeedReply()); if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_RW_STATUS, replies.getRwStatusReply())) { handleGetRwStatusReply(replies.getRwStatusReply()); } if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_LAST_RESET_STATUS, replies.getGetLastResetStatusReply())) { handleResetStatusReply(replies.getGetLastResetStatusReply()); } checkPacket(rws::DeviceCommandId::CLEAR_LAST_RESET_STATUS, replies.getClearLastResetStatusReply()); if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_TEMPERATURE, replies.getReadTemperatureReply())) { handleTemperatureReply(replies.getReadTemperatureReply()); } if (internalState == InternalState::GET_TM) { if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_TM, replies.getHkDataReply())) { handleGetTelemetryReply(replies.getHkDataReply()); } internalState = InternalState::DEFAULT; } if (internalState == InternalState::INIT_RW_CONTROLLER) { checkPacket(rws::DeviceCommandId::INIT_RW_CONTROLLER, replies.getInitRwControllerReply()); internalState = InternalState::DEFAULT; } if (internalState == InternalState::RESET_MCU) { internalState = InternalState::DEFAULT; } return returnvalue::OK; } uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; } ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { localDataPoolMap.emplace(rws::RW_SPEED, &rwSpeed); localDataPoolMap.emplace(rws::RAMP_TIME, &rampTime); localDataPoolMap.emplace(rws::TEMPERATURE_C, new PoolEntry({0})); localDataPoolMap.emplace(rws::CURR_SPEED, new PoolEntry({0})); localDataPoolMap.emplace(rws::REFERENCE_SPEED, new PoolEntry({0})); localDataPoolMap.emplace(rws::STATE, new PoolEntry({0})); localDataPoolMap.emplace(rws::CLC_MODE, new PoolEntry({0})); localDataPoolMap.emplace(rws::LAST_RESET_STATUS, new PoolEntry({0})); localDataPoolMap.emplace(rws::CURRRENT_RESET_STATUS, new PoolEntry({0})); localDataPoolMap.emplace(rws::TM_LAST_RESET_STATUS, new PoolEntry({0})); localDataPoolMap.emplace(rws::TM_MCU_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(rws::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry({0})); localDataPoolMap.emplace(rws::PRESSURE, new PoolEntry({0})); localDataPoolMap.emplace(rws::TM_RW_STATE, new PoolEntry({0})); localDataPoolMap.emplace(rws::TM_CLC_MODE, new PoolEntry({0})); localDataPoolMap.emplace(rws::TM_RW_CURR_SPEED, new PoolEntry({0})); localDataPoolMap.emplace(rws::TM_RW_REF_SPEED, new PoolEntry({0})); localDataPoolMap.emplace(rws::INVALID_CRC_PACKETS, new PoolEntry({0})); localDataPoolMap.emplace(rws::INVALID_LEN_PACKETS, new PoolEntry({0})); localDataPoolMap.emplace(rws::INVALID_CMD_PACKETS, new PoolEntry({0})); localDataPoolMap.emplace(rws::EXECUTED_REPLIES, new PoolEntry({0})); localDataPoolMap.emplace(rws::COMMAND_REPLIES, new PoolEntry({0})); localDataPoolMap.emplace(rws::UART_BYTES_WRITTEN, new PoolEntry({0})); localDataPoolMap.emplace(rws::UART_BYTES_READ, new PoolEntry({0})); localDataPoolMap.emplace(rws::UART_PARITY_ERRORS, new PoolEntry({0})); localDataPoolMap.emplace(rws::UART_NOISE_ERRORS, new PoolEntry({0})); localDataPoolMap.emplace(rws::UART_FRAME_ERRORS, new PoolEntry({0})); localDataPoolMap.emplace(rws::UART_REG_OVERRUN_ERRORS, new PoolEntry({0})); localDataPoolMap.emplace(rws::UART_TOTAL_ERRORS, new PoolEntry({0})); localDataPoolMap.emplace(rws::SPI_BYTES_WRITTEN, new PoolEntry({0})); localDataPoolMap.emplace(rws::SPI_BYTES_READ, new PoolEntry({0})); localDataPoolMap.emplace(rws::SPI_REG_OVERRUN_ERRORS, new PoolEntry({0})); localDataPoolMap.emplace(rws::SPI_TOTAL_ERRORS, new PoolEntry({0})); poolManager.subscribeForDiagPeriodicPacket( subdp::DiagnosticsHkPeriodicParams(statusSet.getSid(), false, 5.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(tmDataset.getSid(), false, 30.0)); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(lastResetStatusSet.getSid(), false, 30.0)); return returnvalue::OK; } /* void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) { commandBuffer[0] = static_cast(id); uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF); commandBuffer[1] = static_cast(crc & 0xFF); commandBuffer[2] = static_cast(crc >> 8 & 0xFF); rawPacket = commandBuffer; rawPacketLen = 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; } if (rampTime < 10 || rampTime > 20000) { sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time" << std::endl; return INVALID_RAMP_TIME; } return returnvalue::OK; } /* ReturnValue_t RwHandler::prepareSetSpeedCmd() { commandBuffer[0] = static_cast(rws::SET_SPEED); 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); rawPacket = commandBuffer; rawPacketLen = 9; return result; } */ void RwHandler::handleResetStatusReply(const uint8_t* packet) { PoolReadGuard rg(&lastResetStatusSet); uint8_t offset = 2; uint8_t resetStatus = packet[offset]; if (resetStatus != 0) { // internalState = InternalState::CLEAR_RESET_STATUS; lastResetStatusSet.lastNonClearedResetStatus = resetStatus; triggerEvent(rws::RESET_OCCURED, resetStatus, 0); } lastResetStatusSet.currentResetStatus = resetStatus; if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "RwHandler::handleResetStatusReply: Last reset status: " << static_cast(lastResetStatusSet.lastNonClearedResetStatus.value) << std::endl; sif::info << "RwHandler::handleResetStatusReply: Current reset status: " << static_cast(lastResetStatusSet.currentResetStatus.value) << std::endl; #endif } } void RwHandler::handleGetRwStatusReply(const uint8_t* packet) { PoolReadGuard rg0(&statusSet); PoolReadGuard rg1(&tmDataset); uint8_t offset = 2; statusSet.currSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); tmDataset.rwCurrSpeed = statusSet.currSpeed; tmDataset.rwCurrSpeed.setValid(true); offset += 4; statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); tmDataset.rwRefSpeed = statusSet.referenceSpeed; tmDataset.rwRefSpeed.setValid(true); offset += 4; statusSet.state = *(packet + offset); tmDataset.rwState = statusSet.state; tmDataset.rwState.setValid(true); offset += 1; statusSet.clcMode = *(packet + offset); tmDataset.rwClcMode = statusSet.clcMode; tmDataset.rwClcMode.setValid(true); statusSet.setValidity(true, true); if (statusSet.state == rws::STATE_ERROR) { // This requires the commanding of the init reaction wheel controller command to recover // from error state which must be handled by the FDIR instance. triggerEvent(rws::ERROR_STATE, statusSet.state.value, 0); sif::error << "RwHandler::handleGetRwStatusReply: Reaction wheel in error state" << std::endl; } if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "RwHandler::handleGetRwStatusReply: Current speed is: " << statusSet.currSpeed << " * 0.1 RPM" << std::endl; sif::info << "RwHandler::handleGetRwStatusReply: Reference speed is: " << statusSet.referenceSpeed << " * 0.1 RPM" << std::endl; sif::info << "RwHandler::handleGetRwStatusReply: State is: " << (unsigned int)statusSet.state.value << std::endl; sif::info << "RwHandler::handleGetRwStatusReply: clc mode is: " << (unsigned int)statusSet.clcMode.value << std::endl; #endif } } void RwHandler::handleTemperatureReply(const uint8_t* packet) { PoolReadGuard rg(&statusSet); uint8_t offset = 2; statusSet.temperatureCelcius = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "RwHandler::handleTemperatureReply: Temperature: " << statusSet.temperatureCelcius << " °C" << std::endl; #endif } } void RwHandler::handleGetTelemetryReply(const uint8_t* packet) { PoolReadGuard rg(&tmDataset); uint8_t offset = 2; tmDataset.lastResetStatus = *(packet + offset); offset += 1; tmDataset.mcuTemperature = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.pressureSensorTemperature = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.pressure = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.rwState = *(packet + offset); offset += 1; tmDataset.rwClcMode = *(packet + offset); offset += 1; tmDataset.rwCurrSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.rwRefSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.numOfInvalidCrcPackets = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.numOfInvalidLenPackets = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.numOfInvalidCmdPackets = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.numOfCmdExecutedReplies = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.numOfCmdReplies = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.uartNumOfBytesWritten = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.uartNumOfBytesRead = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.uartNumOfParityErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.uartNumOfNoiseErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.uartNumOfFrameErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.uartNumOfRegisterOverrunErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.uartTotalNumOfErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.spiNumOfBytesWritten = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.spiNumOfBytesRead = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.spiNumOfRegisterOverrunErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); offset += 4; tmDataset.spiTotalNumOfErrors = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 8 | *(packet + offset); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 sif::info << "RwHandler::handleGetTelemetryReply: Last reset status: " << static_cast(tmDataset.lastResetStatus.value) << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: MCU temperature: " << tmDataset.mcuTemperature << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: Pressure sensor temperature: " << tmDataset.pressureSensorTemperature << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: Pressure " << tmDataset.pressure << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: State: " << static_cast(tmDataset.rwState.value) << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: CLC mode: " << static_cast(tmDataset.rwClcMode.value) << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: Current speed: " << tmDataset.rwCurrSpeed << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: Reference speed: " << tmDataset.rwRefSpeed << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: Number of invalid CRC packets: " << tmDataset.numOfInvalidCrcPackets << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: Number of invalid length packets: " << tmDataset.numOfInvalidLenPackets << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: Number of invalid command packets: " << tmDataset.numOfInvalidCmdPackets << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: Number of command executed replies: " << tmDataset.numOfCmdExecutedReplies << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: Number of command replies: " << tmDataset.numOfCmdReplies << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: UART number of bytes written: " << tmDataset.uartNumOfBytesWritten << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: UART number of bytes read: " << tmDataset.uartNumOfBytesRead << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: UART number of parity errors: " << tmDataset.uartNumOfParityErrors << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: UART number of noise errors: " << tmDataset.uartNumOfNoiseErrors << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: UART number of frame errors: " << tmDataset.uartNumOfFrameErrors << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: UART number of register overrun errors: " << tmDataset.uartNumOfRegisterOverrunErrors << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: UART number of total errors: " << tmDataset.uartTotalNumOfErrors << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: SPI number of bytes written: " << tmDataset.spiNumOfBytesWritten << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: SPI number of bytes read: " << tmDataset.spiNumOfBytesRead << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: SPI number of register overrun errors: " << tmDataset.spiNumOfRegisterOverrunErrors << std::endl; sif::info << "RwHandler::handleGetTelemetryReply: SPI number of register total errors: " << tmDataset.spiTotalNumOfErrors << std::endl; #endif } } void RwHandler::setDebugMode(bool enable) { this->debugMode = enable; }