added get telemetry command

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<uint8_t>(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<uint8_t>( { 0 }));
 
     localDataPoolMap.emplace(RwDefinitions::TM_LAST_RESET_STATUS, new PoolEntry<uint8_t>( { 0 }));
-    localDataPoolMap.emplace(RwDefinitions::TM_MCU_TEMPERATURE, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(RwDefinitions::TM_MCU_TEMPERATURE, new PoolEntry<int32_t>( { 0 }));
     localDataPoolMap.emplace(RwDefinitions::TM_RW_STATE, new PoolEntry<uint8_t>( { 0 }));
     localDataPoolMap.emplace(RwDefinitions::TM_CLC_MODE, new PoolEntry<uint8_t>( { 0 }));
-    localDataPoolMap.emplace(RwDefinitions::TM_RW_CURR_SPEED, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(RwDefinitions::TM_RW_CURR_SPEED, new PoolEntry<int32_t>( { 0 }));
-    localDataPoolMap.emplace(RwDefinitions::TM_RW_REF_SPEED, new PoolEntry<uint32_t>( { 0 }));
+    localDataPoolMap.emplace(RwDefinitions::TM_RW_REF_SPEED, new PoolEntry<int32_t>( { 0 }));
     localDataPoolMap.emplace(RwDefinitions::INVALID_CRC_PACKETS, new PoolEntry<uint32_t>( { 0 }));
     localDataPoolMap.emplace(RwDefinitions::INVALID_LEN_PACKETS, new PoolEntry<uint32_t>( { 0 }));
     localDataPoolMap.emplace(RwDefinitions::INVALID_CMD_PACKETS, new PoolEntry<uint32_t>( { 0 }));
@@ -276,33 +288,8 @@ ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataP
     return RETURN_OK;
 }
 
-void RwHandler::prepareResetMcuCommand() {
+void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) {
-    commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::RESET_MCU);
+    commandBuffer[0] = static_cast<uint8_t>(id);
-    rawPacket = commandBuffer;
-    rawPacketLen = 1;
-}
-
-void RwHandler::prepareGetLastResetStatusCommand() {
-    commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_LAST_RESET_STATUS);
-    uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
-    commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
-    commandBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
-    rawPacket = commandBuffer;
-    rawPacketLen = 3;
-}
-
-void RwHandler::prepareClearResetStatusCommand() {
-    commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::CLEAR_LAST_RESET_STATUS);
-    uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
-    commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
-    commandBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
-    rawPacket = commandBuffer;
-    rawPacketLen = 3;
-}
-
-void RwHandler::prepareGetStatusCmd(const uint8_t * commandData, size_t commandDataLen) {
-    commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_RW_STATUS);
-
     uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
     commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
     commandBuffer[2] = static_cast<uint8_t>(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<uint8_t>(RwDefinitions::INIT_RW_CONTROLLER);
-    uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
-    commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
-    commandBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
-    rawPacket = commandBuffer;
-    rawPacketLen = 3;
-}
-
 void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDataLen) {
     commandBuffer[0] = static_cast<uint8_t>(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<uint8_t>(RwDefinitions::GET_TEMPERATURE);
-    uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
-    commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
-    commandBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
-    rawPacket = commandBuffer;
-    rawPacketLen = 3;
-}
-
-void RwHandler::prepareGetTelemetryCmd() {
-    commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_TM);
-    uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
-    commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
-    commandBuffer[2] = static_cast<uint8_t>(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<unsigned int>(tmDataset.lastResetStatus.value) << std::endl;
+    sif::info << "RwHandler::handleTemperatureReply: MCU temperature: " << tmDataset.mcuTemperature
+            << std::endl;
+    sif::info << "RwHandler::handleTemperatureReply: State: "
+            << static_cast<unsigned int>(tmDataset.rwState.value) << std::endl;
+    sif::info << "RwHandler::handleTemperatureReply: CLC mode: "
+            << static_cast<unsigned int>(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
+}

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
+	 * @brief   This function can be used to build commands which do not contain any data apart
-	 *          wheel.
+	 *          from the command id and the CRC.
+	 * @param commandId The command id of the command to build.
 	 */
-	void prepareResetMcuCommand();
+	void prepareSimpleCommand(DeviceCommandId_t id);
-
-	/**
-	 * @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);
 
 	/**
 	 * @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_ */

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<uint8_t> lastResetStatus = lp_var_t<uint8_t>(sid.objectId,
             PoolIds::TM_LAST_RESET_STATUS, this);
-    lp_var_t<uint32_t> mcuTemperature = lp_var_t<uint32_t>(sid.objectId,
+    lp_var_t<int32_t> mcuTemperature = lp_var_t<int32_t>(sid.objectId,
             PoolIds::TM_MCU_TEMPERATURE, this);
     lp_var_t<uint8_t> rwState = lp_var_t<uint8_t>(sid.objectId,
             PoolIds::TM_RW_STATE, this);
@@ -196,7 +200,7 @@ public:
             PoolIds::INVALID_CMD_PACKETS, this);
     lp_var_t<uint32_t> numOfCmdExecutedReplies = lp_var_t<uint32_t>(sid.objectId,
             PoolIds::EXECUTED_REPLIES, this);
-    lp_var_t<uint32_t> numOfCmdeplies = lp_var_t<uint32_t>(sid.objectId,
+    lp_var_t<uint32_t> numOfCmdReplies = lp_var_t<uint32_t>(sid.objectId,
             PoolIds::COMMAND_REPLIES, this);
     lp_var_t<uint32_t> uartNumOfBytesWritten = lp_var_t<uint32_t>(sid.objectId,
             PoolIds::UART_BYTES_WRITTEN, this);