eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

added get telemetry command

Browse Source
This commit is contained in:
Jakob Meier 2021-06-30 15:07:26 +02:00
parent 480f1a833e
commit 83fa6e7a4b
3 changed files with 155 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

202
mission/devices/RwHandler.cpp
View File

@ -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_REF_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<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() {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::RESET_MCU);
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);
void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) {
commandBuffer[0] = static_cast<uint8_t>(id);
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
}

41
mission/devices/RwHandler.h
View File

@ -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_ */

10
mission/devices/devicedefinitions/RwDefinitions.h
View File

@ -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);