From 83fa6e7a4b1a02ed2809ac47a62e87ddfb459733 Mon Sep 17 00:00:00 2001 From: "Jakob.Meier" Date: Wed, 30 Jun 2021 15:07:26 +0200 Subject: [PATCH] added get telemetry command --- mission/devices/RwHandler.cpp | 202 ++++++++++++------ mission/devices/RwHandler.h | 41 +--- .../devices/devicedefinitions/RwDefinitions.h | 10 +- 3 files changed, 155 insertions(+), 98 deletions(-) diff --git a/mission/devices/RwHandler.cpp b/mission/devices/RwHandler.cpp index b637cbba..beb17602 100644 --- a/mission/devices/RwHandler.cpp +++ b/mission/devices/RwHandler.cpp @@ -77,23 +77,23 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand switch (deviceCommand) { case (RwDefinitions::RESET_MCU): { - prepareResetMcuCommand(); + prepareSimpleCommand(deviceCommand); return RETURN_OK; } case (RwDefinitions::GET_LAST_RESET_STATUS): { - prepareGetLastResetStatusCommand(); + prepareSimpleCommand(deviceCommand); return RETURN_OK; } case (RwDefinitions::CLEAR_LAST_RESET_STATUS): { - prepareClearResetStatusCommand(); + prepareSimpleCommand(deviceCommand); return RETURN_OK; } case (RwDefinitions::GET_RW_STATUS): { - prepareGetStatusCmd(commandData, commandDataLen); + prepareSimpleCommand(deviceCommand); return RETURN_OK; } case (RwDefinitions::INIT_RW_CONTROLLER): { - prepareInitRwCommand(); + prepareSimpleCommand(deviceCommand); return RETURN_OK; } case (RwDefinitions::SET_SPEED): { @@ -110,11 +110,11 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand return result; } case (RwDefinitions::GET_TEMPERATURE): { - prepareGetTemperatureCmd(); + prepareSimpleCommand(deviceCommand); return RETURN_OK; } case (RwDefinitions::GET_TM): { - prepareGetTelemetryCmd(); + prepareSimpleCommand(deviceCommand); return RETURN_OK; } default: @@ -137,6 +137,8 @@ void RwHandler::fillCommandAndReplyMap() { RwDefinitions::SIZE_GET_TEMPERATURE_REPLY); this->insertInCommandAndReplyMap(RwDefinitions::SET_SPEED, 1, nullptr, RwDefinitions::SIZE_SET_SPEED_REPLY); + this->insertInCommandAndReplyMap(RwDefinitions::GET_TM, 1, &tmDataset, + RwDefinitions::SIZE_GET_TELEMETRY_REPLY); } ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize, @@ -173,6 +175,12 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize *foundId = RwDefinitions::GET_TEMPERATURE; break; } + case (static_cast(RwDefinitions::GET_TM)): { +// *foundLen = RwDefinitions::SIZE_GET_TELEMETRY_REPLY; + *foundLen = 91; + *foundId = RwDefinitions::GET_TM; + break; + } default: { sif::debug << "RwHandler::scanForReply: Reply contains invalid command code" << std::endl; return RETURN_FAILED; @@ -220,6 +228,10 @@ ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_ handleTemperatureReply(packet); break; } + case (RwDefinitions::GET_TM): { + handleGetTelemetryReply(packet); + break; + } default: { sif::debug << "RwHandler::interpretDeviceReply: Unknown device reply id" << std::endl; return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; @@ -251,11 +263,11 @@ ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataP localDataPoolMap.emplace(RwDefinitions::CURRRENT_RESET_STATUS, new PoolEntry( { 0 })); localDataPoolMap.emplace(RwDefinitions::TM_LAST_RESET_STATUS, new PoolEntry( { 0 })); - localDataPoolMap.emplace(RwDefinitions::TM_MCU_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(RwDefinitions::TM_MCU_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(RwDefinitions::TM_RW_STATE, new PoolEntry( { 0 })); localDataPoolMap.emplace(RwDefinitions::TM_CLC_MODE, new PoolEntry( { 0 })); - localDataPoolMap.emplace(RwDefinitions::TM_RW_CURR_SPEED, new PoolEntry( { 0 })); - localDataPoolMap.emplace(RwDefinitions::TM_RW_REF_SPEED, new PoolEntry( { 0 })); + localDataPoolMap.emplace(RwDefinitions::TM_RW_CURR_SPEED, new PoolEntry( { 0 })); + localDataPoolMap.emplace(RwDefinitions::TM_RW_REF_SPEED, new PoolEntry( { 0 })); localDataPoolMap.emplace(RwDefinitions::INVALID_CRC_PACKETS, new PoolEntry( { 0 })); localDataPoolMap.emplace(RwDefinitions::INVALID_LEN_PACKETS, new PoolEntry( { 0 })); localDataPoolMap.emplace(RwDefinitions::INVALID_CMD_PACKETS, new PoolEntry( { 0 })); @@ -276,33 +288,8 @@ ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataP return RETURN_OK; } -void RwHandler::prepareResetMcuCommand() { - commandBuffer[0] = static_cast(RwDefinitions::RESET_MCU); - rawPacket = commandBuffer; - rawPacketLen = 1; -} - -void RwHandler::prepareGetLastResetStatusCommand() { - commandBuffer[0] = static_cast(RwDefinitions::GET_LAST_RESET_STATUS); - 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; -} - -void RwHandler::prepareClearResetStatusCommand() { - commandBuffer[0] = static_cast(RwDefinitions::CLEAR_LAST_RESET_STATUS); - 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; -} - -void RwHandler::prepareGetStatusCmd(const uint8_t * commandData, size_t commandDataLen) { - commandBuffer[0] = static_cast(RwDefinitions::GET_RW_STATUS); - +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); @@ -330,15 +317,6 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_ return RETURN_OK; } -void RwHandler::prepareInitRwCommand() { - commandBuffer[0] = static_cast(RwDefinitions::INIT_RW_CONTROLLER); - 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; -} - void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDataLen) { commandBuffer[0] = static_cast(RwDefinitions::SET_SPEED); @@ -358,24 +336,6 @@ void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDa rawPacketLen = 9; } -void RwHandler::prepareGetTemperatureCmd() { - commandBuffer[0] = static_cast(RwDefinitions::GET_TEMPERATURE); - 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; -} - -void RwHandler::prepareGetTelemetryCmd() { - commandBuffer[0] = static_cast(RwDefinitions::GET_TM); - 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; -} - void RwHandler::handleResetStatusReply(const uint8_t* packet) { PoolReadGuard rg(&lastResetStatusSet); uint8_t offset = 2; @@ -438,3 +398,117 @@ void RwHandler::handleTemperatureReply(const uint8_t* packet) { << temperatureSet.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; + offset += 8; + 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 OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1 + sif::info << "RwHandler::handleTemperatureReply: Last reset status: " + << static_cast(tmDataset.lastResetStatus.value) << std::endl; + sif::info << "RwHandler::handleTemperatureReply: MCU temperature: " << tmDataset.mcuTemperature + << std::endl; + sif::info << "RwHandler::handleTemperatureReply: State: " + << static_cast(tmDataset.rwState.value) << std::endl; + sif::info << "RwHandler::handleTemperatureReply: CLC mode: " + << static_cast(tmDataset.rwClcMode.value) << std::endl; + sif::info << "RwHandler::handleTemperatureReply: Current speed: " << tmDataset.rwCurrSpeed + << std::endl; + sif::info << "RwHandler::handleTemperatureReply: Reference speed: " << tmDataset.rwRefSpeed + << std::endl; + sif::info << "RwHandler::handleTemperatureReply: Number of invalid CRC packets: " + << tmDataset.numOfInvalidCrcPackets << std::endl; + sif::info << "RwHandler::handleTemperatureReply: Number of invalid length packets: " + << tmDataset.numOfInvalidLenPackets << std::endl; + sif::info << "RwHandler::handleTemperatureReply: Number of invalid command packets: " + << tmDataset.numOfInvalidCmdPackets << std::endl; + sif::info << "RwHandler::handleTemperatureReply: Number of command executed replies: " + << tmDataset.numOfCmdExecutedReplies << std::endl; + sif::info << "RwHandler::handleTemperatureReply: Number of command replies: " + << tmDataset.numOfCmdReplies << std::endl; + sif::info << "RwHandler::handleTemperatureReply: UART number of bytes written: " + << tmDataset.uartNumOfBytesWritten << std::endl; + sif::info << "RwHandler::handleTemperatureReply: UART number of bytes read: " + << tmDataset.uartNumOfBytesRead << std::endl; + sif::info << "RwHandler::handleTemperatureReply: UART number of parity errors: " + << tmDataset.uartNumOfParityErrors << std::endl; + sif::info << "RwHandler::handleTemperatureReply: UART number of noise errors: " + << tmDataset.uartNumOfNoiseErrors << std::endl; + sif::info << "RwHandler::handleTemperatureReply: UART number of frame errors: " + << tmDataset.uartNumOfFrameErrors << std::endl; + sif::info << "RwHandler::handleTemperatureReply: UART number of register overrun errors: " + << tmDataset.uartNumOfRegisterOverrunErrors << std::endl; + sif::info << "RwHandler::handleTemperatureReply: UART number of total errors: " + << tmDataset.uartTotalNumOfErrors << std::endl; + sif::info << "RwHandler::handleTemperatureReply: SPI number of bytes written: " + << tmDataset.spiNumOfBytesWritten << std::endl; + sif::info << "RwHandler::handleTemperatureReply: SPI number of bytes read: " + << tmDataset.spiNumOfBytesRead << std::endl; + sif::info << "RwHandler::handleTemperatureReply: SPI number of register overrun errors: " + << tmDataset.spiNumOfRegisterOverrunErrors << std::endl; + sif::info << "RwHandler::handleTemperatureReply: SPI number of register total errors: " + << tmDataset.spiTotalNumOfErrors << std::endl; +#endif +} diff --git a/mission/devices/RwHandler.h b/mission/devices/RwHandler.h index e447932d..cd753be0 100644 --- a/mission/devices/RwHandler.h +++ b/mission/devices/RwHandler.h @@ -105,26 +105,11 @@ private: size_t sizeOfReply = 0; /** - * @brief This function fills the command buffer with the data to reset the MCU on a reaction - * wheel. + * @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 prepareResetMcuCommand(); - - /** - * @brief This function prepares the command to request the last reset status - */ - void prepareGetLastResetStatusCommand(); - - /** - * @brief Fills the command buffer with the command to clear the reset status. - */ - void prepareClearResetStatusCommand(); - - /** - * @brief This function prepares the send buffer with the data to request the status of - * the reaction wheel. - */ - void prepareGetStatusCmd(const uint8_t * commandData, size_t commandDataLen); + void prepareSimpleCommand(DeviceCommandId_t id); /** * @brief This function checks if the receiced speed and ramp time to set are in a valid @@ -133,24 +118,12 @@ private: */ ReturnValue_t checkSpeedAndRampTime(const uint8_t * commandData, size_t commandDataLen); - /** - * @brief This function fills the commandBuffer with the data to request initialize the - * reaction wheel controller. This command must be sent as soon as the state of a - * reaction wheel is equal to 1 which indicates an error. - */ - void prepareInitRwCommand(); - /** * @brief This function prepares the set speed command from the commandData received with * an action message. */ void prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDataLen); - /** - * @brief This function fills the commandBuffer with the data to request the temperature. - */ - void prepareGetTemperatureCmd(); - /** * @brief This function writes the last reset status retrieved with the get last reset status * command into the reset status dataset. @@ -170,6 +143,12 @@ private: * @brief This function fills the status set with the data from the get-status-reply. */ void handleGetRwStatusReply(const uint8_t* packet); + + /** + * @brief This function fills the tmDataset with the reply data requested with get telemetry + * command. + */ + void handleGetTelemetryReply(const uint8_t* packet); }; #endif /* MISSION_DEVICES_RWHANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/RwDefinitions.h b/mission/devices/devicedefinitions/RwDefinitions.h index a477f47c..d87c09fd 100644 --- a/mission/devices/devicedefinitions/RwDefinitions.h +++ b/mission/devices/devicedefinitions/RwDefinitions.h @@ -87,7 +87,11 @@ static const size_t SIZE_GET_TELEMETRY_REPLY = 83; /** Set speed command has maximum size */ static const size_t MAX_CMD_SIZE = 9; -static const size_t MAX_REPLY_SIZE = SIZE_GET_TELEMETRY_REPLY; +/** + * Max reply is reached when each byte is replaced by its substitude which should normally never + * happen. + */ +static const size_t MAX_REPLY_SIZE = 2 * SIZE_GET_TELEMETRY_REPLY; static const uint8_t LAST_RESET_ENTRIES = 2; static const uint8_t TEMPERATURE_SET_ENTRIES = 1; @@ -178,7 +182,7 @@ public: lp_var_t lastResetStatus = lp_var_t(sid.objectId, PoolIds::TM_LAST_RESET_STATUS, this); - lp_var_t mcuTemperature = lp_var_t(sid.objectId, + lp_var_t mcuTemperature = lp_var_t(sid.objectId, PoolIds::TM_MCU_TEMPERATURE, this); lp_var_t rwState = lp_var_t(sid.objectId, PoolIds::TM_RW_STATE, this); @@ -196,7 +200,7 @@ public: PoolIds::INVALID_CMD_PACKETS, this); lp_var_t numOfCmdExecutedReplies = lp_var_t(sid.objectId, PoolIds::EXECUTED_REPLIES, this); - lp_var_t numOfCmdeplies = lp_var_t(sid.objectId, + lp_var_t numOfCmdReplies = lp_var_t(sid.objectId, PoolIds::COMMAND_REPLIES, this); lp_var_t uartNumOfBytesWritten = lp_var_t(sid.objectId, PoolIds::UART_BYTES_WRITTEN, this);