added get telemetry command
This commit is contained in:
@ -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
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user