eive-obsw/mission/devices/RwHandler.cpp

567 lines
26 KiB
C++

#include "RwHandler.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw_hal/common/gpio/GpioIF.h>
#include "OBSWConfig.h"
RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
GpioIF* gpioComIF, gpioId_t enableGpio)
: DeviceHandlerBase(objectId, comIF, comCookie),
gpioComIF(gpioComIF),
enableGpio(enableGpio),
statusSet(this),
lastResetStatusSet(this),
tmDataset(this),
rwSpeedActuationSet(*this) {
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::GET_RESET_STATUS;
if (gpioComIF->pullHigh(enableGpio) != returnvalue::OK) {
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high";
}
setMode(_MODE_TO_ON);
}
void RwHandler::doShutDown() {
if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) {
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low";
}
setMode(_MODE_POWER_DOWN);
}
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
switch (internalState) {
case InternalState::GET_RESET_STATUS:
*id = RwDefinitions::GET_LAST_RESET_STATUS;
internalState = InternalState::READ_TEMPERATURE;
break;
case InternalState::READ_TEMPERATURE:
*id = RwDefinitions::GET_TEMPERATURE;
internalState = InternalState::GET_RW_SATUS;
break;
case InternalState::GET_RW_SATUS:
*id = RwDefinitions::GET_RW_STATUS;
internalState = InternalState::GET_RESET_STATUS;
break;
case InternalState::CLEAR_RESET_STATUS:
*id = RwDefinitions::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 (RwDefinitions::RESET_MCU): {
prepareSimpleCommand(deviceCommand);
return returnvalue::OK;
}
case (RwDefinitions::GET_LAST_RESET_STATUS): {
prepareSimpleCommand(deviceCommand);
return returnvalue::OK;
}
case (RwDefinitions::CLEAR_LAST_RESET_STATUS): {
prepareSimpleCommand(deviceCommand);
return returnvalue::OK;
}
case (RwDefinitions::GET_RW_STATUS): {
prepareSimpleCommand(deviceCommand);
return returnvalue::OK;
}
case (RwDefinitions::INIT_RW_CONTROLLER): {
prepareSimpleCommand(deviceCommand);
return returnvalue::OK;
}
case (RwDefinitions::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;
}
// 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;
}
} else {
// Read set rw speed value from local pool
PoolReadGuard pg(&rwSpeedActuationSet);
}
if (ACTUATION_WIRETAPPING) {
int32_t speed;
uint16_t rampTime;
rwSpeedActuationSet.getRwSpeed(speed, rampTime);
sif::debug << "Actuating RW with speed = " << speed << " and rampTime = " << rampTime
<< std::endl;
}
result = checkSpeedAndRampTime();
if (result != returnvalue::OK) {
return result;
}
result = prepareSetSpeedCmd();
return result;
}
case (RwDefinitions::GET_TEMPERATURE): {
prepareSimpleCommand(deviceCommand);
return returnvalue::OK;
}
case (RwDefinitions::GET_TM): {
prepareSimpleCommand(deviceCommand);
return returnvalue::OK;
}
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
return returnvalue::FAILED;
}
void RwHandler::fillCommandAndReplyMap() {
this->insertInCommandMap(RwDefinitions::RESET_MCU);
this->insertInCommandAndReplyMap(RwDefinitions::GET_LAST_RESET_STATUS, 1, &lastResetStatusSet,
RwDefinitions::SIZE_GET_RESET_STATUS);
this->insertInCommandAndReplyMap(RwDefinitions::CLEAR_LAST_RESET_STATUS, 1, nullptr,
RwDefinitions::SIZE_CLEAR_RESET_STATUS);
this->insertInCommandAndReplyMap(RwDefinitions::GET_RW_STATUS, 1, &statusSet,
RwDefinitions::SIZE_GET_RW_STATUS);
this->insertInCommandAndReplyMap(RwDefinitions::INIT_RW_CONTROLLER, 1, nullptr,
RwDefinitions::SIZE_INIT_RW);
this->insertInCommandAndReplyMap(RwDefinitions::GET_TEMPERATURE, 1, nullptr,
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,
DeviceCommandId_t* foundId, size_t* foundLen) {
uint8_t replyByte = *start;
switch (replyByte) {
case (RwDefinitions::GET_LAST_RESET_STATUS): {
*foundLen = RwDefinitions::SIZE_GET_RESET_STATUS;
*foundId = RwDefinitions::GET_LAST_RESET_STATUS;
break;
}
case (RwDefinitions::CLEAR_LAST_RESET_STATUS): {
*foundLen = RwDefinitions::SIZE_CLEAR_RESET_STATUS;
*foundId = RwDefinitions::CLEAR_LAST_RESET_STATUS;
break;
}
case (RwDefinitions::GET_RW_STATUS): {
*foundLen = RwDefinitions::SIZE_GET_RW_STATUS;
*foundId = RwDefinitions::GET_RW_STATUS;
break;
}
case (RwDefinitions::INIT_RW_CONTROLLER): {
*foundLen = RwDefinitions::SIZE_INIT_RW;
*foundId = RwDefinitions::INIT_RW_CONTROLLER;
break;
}
case (RwDefinitions::SET_SPEED): {
*foundLen = RwDefinitions::SIZE_SET_SPEED_REPLY;
*foundId = RwDefinitions::SET_SPEED;
break;
}
case (RwDefinitions::GET_TEMPERATURE): {
*foundLen = RwDefinitions::SIZE_GET_TEMPERATURE_REPLY;
*foundId = RwDefinitions::GET_TEMPERATURE;
break;
}
case (RwDefinitions::GET_TM): {
*foundLen = RwDefinitions::SIZE_GET_TELEMETRY_REPLY;
*foundId = RwDefinitions::GET_TM;
break;
}
default: {
sif::warning << "RwHandler::scanForReply: Reply contains invalid command code" << std::endl;
*foundLen = remainingSize;
return returnvalue::FAILED;
}
}
sizeOfReply = *foundLen;
return returnvalue::OK;
}
ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
/** Check result code */
if (*(packet + 1) == RwDefinitions::STATE_ERROR) {
sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: " << id
<< std::endl;
return EXECUTION_FAILED;
}
/** Received in little endian byte order */
uint16_t replyCrc = *(packet + sizeOfReply - 1) << 8 | *(packet + sizeOfReply - 2);
if (CRC::crc16ccitt(packet, sizeOfReply - 2, 0xFFFF) != replyCrc) {
sif::error << "RwHandler::interpretDeviceReply: cRC error" << std::endl;
return CRC_ERROR;
}
switch (id) {
case (RwDefinitions::GET_LAST_RESET_STATUS): {
handleResetStatusReply(packet);
break;
}
case (RwDefinitions::GET_RW_STATUS): {
handleGetRwStatusReply(packet);
break;
}
case (RwDefinitions::CLEAR_LAST_RESET_STATUS):
case (RwDefinitions::INIT_RW_CONTROLLER):
case (RwDefinitions::SET_SPEED):
// no reply data expected
break;
case (RwDefinitions::GET_TEMPERATURE): {
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;
}
}
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(RwDefinitions::RW_SPEED, &rwSpeed);
localDataPoolMap.emplace(RwDefinitions::RAMP_TIME, &rampTime);
localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::REFERENCE_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::STATE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(RwDefinitions::CLC_MODE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(RwDefinitions::LAST_RESET_STATUS, new PoolEntry<uint8_t>({0}));
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<int32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry<float>({0}));
localDataPoolMap.emplace(RwDefinitions::PRESSURE, new PoolEntry<float>({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<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}));
localDataPoolMap.emplace(RwDefinitions::EXECUTED_REPLIES, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::COMMAND_REPLIES, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_BYTES_WRITTEN, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_BYTES_READ, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_PARITY_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_NOISE_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_FRAME_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::UART_TOTAL_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_WRITTEN, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::SPI_BYTES_READ, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::SPI_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(RwDefinitions::SPI_TOTAL_ERRORS, new PoolEntry<uint32_t>({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<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);
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<uint8_t>(RwDefinitions::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<uint8_t>(crc & 0xFF);
commandBuffer[8] = static_cast<uint8_t>((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(RwDefinitions::RESET_OCCURED, resetStatus, 0);
}
lastResetStatusSet.currentResetStatus = resetStatus;
if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::info << "RwHandler::handleResetStatusReply: Last reset status: "
<< static_cast<unsigned int>(lastResetStatusSet.lastNonClearedResetStatus.value)
<< std::endl;
sif::info << "RwHandler::handleResetStatusReply: Current reset status: "
<< static_cast<unsigned int>(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 == RwDefinitions::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(RwDefinitions::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<unsigned int>(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<unsigned int>(tmDataset.rwState.value) << std::endl;
sif::info << "RwHandler::handleGetTelemetryReply: CLC mode: "
<< static_cast<unsigned int>(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; }