eive-obsw/mission/acs/RwHandler.cpp

599 lines
26 KiB
C++
Raw Normal View History

2021-06-21 09:50:26 +02:00
#include "RwHandler.h"
#include <fsfw/datapool/PoolReadGuard.h>
2022-01-17 15:58:27 +01:00
#include <fsfw/globalfunctions/CRC.h>
2023-02-17 02:10:08 +01:00
#include <fsfw/globalfunctions/arrayprinter.h>
2022-04-22 11:29:51 +02:00
#include <fsfw_hal/common/gpio/GpioIF.h>
2023-04-07 17:02:37 +02:00
#include <mission/acs/defs.h>
2021-06-21 09:50:26 +02:00
2022-01-17 15:58:27 +01:00
#include "OBSWConfig.h"
2021-06-21 09:50:26 +02:00
2022-01-17 15:58:27 +01:00
RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
2023-02-14 18:41:43 +01:00
GpioIF* gpioComIF, gpioId_t enableGpio, uint8_t rwIdx)
2022-01-17 15:58:27 +01:00
: DeviceHandlerBase(objectId, comIF, comCookie),
gpioComIF(gpioComIF),
enableGpio(enableGpio),
statusSet(this),
lastResetStatusSet(this),
2023-02-10 13:16:50 +01:00
tmDataset(this),
2023-02-14 18:41:43 +01:00
rwSpeedActuationSet(*this),
rwIdx(rwIdx) {
2022-04-22 11:29:51 +02:00
if (comCookie == nullptr) {
2022-01-17 15:58:27 +01:00
sif::error << "RwHandler: Invalid com cookie" << std::endl;
}
2022-04-22 11:29:51 +02:00
if (gpioComIF == nullptr) {
2022-01-17 15:58:27 +01:00
sif::error << "RwHandler: Invalid gpio communication interface" << std::endl;
}
2023-04-07 17:02:37 +02:00
currentRequest.rwIdx = rwIdx;
2021-06-21 09:50:26 +02:00
}
2023-04-07 17:02:37 +02:00
2022-01-17 15:58:27 +01:00
RwHandler::~RwHandler() {}
2021-06-29 16:08:20 +02:00
2022-01-17 15:58:27 +01:00
void RwHandler::doStartUp() {
2023-02-16 14:10:59 +01:00
internalState = InternalState::DEFAULT;
2021-06-29 16:08:20 +02:00
2022-08-24 17:27:47 +02:00
if (gpioComIF->pullHigh(enableGpio) != returnvalue::OK) {
2023-04-07 11:37:17 +02:00
sif::error << "RW Handler " << rwIdx << ": Failed to pull ENABLE pin high for startup";
2022-01-17 15:58:27 +01:00
}
2023-04-07 17:02:37 +02:00
currentRequest.mode = acs::SimpleSensorMode::NORMAL;
2023-02-16 14:10:59 +01:00
updatePeriodicReply(true, rws::REPLY_ID);
2023-03-26 15:28:00 +02:00
statusSet.setReportingEnabled(true);
2022-01-17 15:58:27 +01:00
setMode(_MODE_TO_ON);
2021-06-21 09:50:26 +02:00
}
void RwHandler::doShutDown() {
2023-04-07 11:37:17 +02:00
if (internalState != InternalState::SHUTDOWN) {
internalState = InternalState::SHUTDOWN;
2023-04-07 17:02:37 +02:00
shutdownState = ShutdownState::SET_SPEED_ZERO;
2023-04-07 11:37:17 +02:00
offTransitionCountdown.resetTimer();
2022-01-17 15:58:27 +01:00
}
2023-04-07 17:02:37 +02:00
if ((internalState == InternalState::SHUTDOWN) and
(shutdownState == ShutdownState::DONE or offTransitionCountdown.hasTimedOut())) {
2023-04-07 11:37:17 +02:00
if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) {
sif::error << "RW Handler " << rwIdx << ": Failed to pull ENABLE pin low for shutdown";
}
updatePeriodicReply(false, rws::REPLY_ID);
{
PoolReadGuard pg(&statusSet);
statusSet.currSpeed = 0.0;
statusSet.referenceSpeed = 0.0;
statusSet.state = 0;
statusSet.setValidity(false, true);
statusSet.setReportingEnabled(false);
}
{
PoolReadGuard pg(&tmDataset);
tmDataset.setValidity(false, true);
}
internalState = InternalState::DEFAULT;
2023-04-07 17:02:37 +02:00
shutdownState = ShutdownState::NONE;
2023-04-07 11:37:17 +02:00
// The power switch is handled by the assembly, so we can go off here directly.
setMode(MODE_OFF);
}
2021-06-21 09:50:26 +02:00
}
2022-01-17 15:58:27 +01:00
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
switch (internalState) {
2023-02-16 14:10:59 +01:00
case InternalState::DEFAULT: {
*id = rws::REQUEST_ID;
break;
2023-02-16 14:10:59 +01:00
}
2021-06-21 09:50:26 +02:00
default:
2022-01-17 15:58:27 +01:00
sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid internal step" << std::endl;
break;
}
return buildCommandFromCommand(*id, NULL, 0);
2021-06-21 09:50:26 +02:00
}
2022-01-17 15:58:27 +01:00
ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
2023-04-07 11:37:17 +02:00
if (internalState == InternalState::SHUTDOWN) {
2023-04-07 17:02:37 +02:00
if (shutdownState == ShutdownState::SET_SPEED_ZERO) {
{
PoolReadGuard pg(&rwSpeedActuationSet);
rwSpeedActuationSet.setRwSpeed(0, 10);
}
*id = rws::REQUEST_ID;
return buildCommandFromCommand(*id, nullptr, 0);
} else if (shutdownState == ShutdownState::STOP_POLLING) {
currentRequest.mode = acs::SimpleSensorMode::OFF;
*id = rws::REQUEST_ID;
return buildCommandFromCommand(*id, nullptr, 0);
2023-04-07 11:37:17 +02:00
}
}
2022-01-17 15:58:27 +01:00
return NOTHING_TO_SEND;
2021-06-21 09:50:26 +02:00
}
ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
2022-01-17 15:58:27 +01:00
const uint8_t* commandData,
size_t commandDataLen) {
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::OK;
2021-06-21 09:50:26 +02:00
2022-01-17 15:58:27 +01:00
switch (deviceCommand) {
2023-02-17 02:10:08 +01:00
case (rws::SET_SPEED):
2023-02-16 14:10:59 +01:00
case (rws::REQUEST_ID): {
2023-02-10 13:16:50 +01:00
if (commandData != nullptr && commandDataLen != 6) {
2022-01-17 15:58:27 +01:00
sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
<< " invalid length" << std::endl;
return SET_SPEED_COMMAND_INVALID_LENGTH;
}
2023-02-14 18:41:43 +01:00
{
2023-02-10 13:16:50 +01:00
PoolReadGuard pg(&rwSpeedActuationSet);
2023-02-14 18:41:43 +01:00
// 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;
}
}
2023-02-10 13:16:50 +01:00
}
if (ACTUATION_WIRETAPPING) {
2023-02-14 18:41:43 +01:00
int32_t speed = 0;
uint16_t rampTime = 0;
2023-02-10 13:16:50 +01:00
rwSpeedActuationSet.getRwSpeed(speed, rampTime);
2023-02-14 18:41:43 +01:00
sif::debug << "Actuating RW " << static_cast<int>(rwIdx) << " with speed = " << speed
<< " and rampTime = " << rampTime << std::endl;
2023-02-10 13:16:50 +01:00
}
result = checkSpeedAndRampTime();
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
2021-06-21 09:50:26 +02:00
return result;
2022-01-17 15:58:27 +01:00
}
2023-02-16 14:10:59 +01:00
// set speed flag.
2023-04-07 17:02:37 +02:00
currentRequest.setSpeed = true;
rwSpeedActuationSet.getRwSpeed(currentRequest.currentRwSpeed, currentRequest.currentRampTime);
currentRequest.specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
rawPacket = reinterpret_cast<uint8_t*>(&currentRequest);
rawPacketLen = sizeof(rws::RwRequest);
2023-02-16 17:57:21 +01:00
return returnvalue::OK;
2023-02-16 14:10:59 +01:00
}
case (rws::RESET_MCU): {
2023-04-07 17:02:37 +02:00
currentRequest.setSpeed = false;
currentRequest.specialRequest = rws::SpecialRwRequest::RESET_MCU;
2023-02-16 14:10:59 +01:00
internalState = InternalState::RESET_MCU;
2023-04-07 17:02:37 +02:00
rawPacket = reinterpret_cast<uint8_t*>(&currentRequest);
rawPacketLen = sizeof(rws::RwRequest);
2023-02-16 14:10:59 +01:00
return returnvalue::OK;
2021-06-21 09:50:26 +02:00
}
2023-02-16 14:10:59 +01:00
case (rws::INIT_RW_CONTROLLER): {
2023-04-07 17:02:37 +02:00
currentRequest.setSpeed = false;
currentRequest.specialRequest = rws::SpecialRwRequest::INIT_RW_CONTROLLER;
2023-02-16 14:10:59 +01:00
internalState = InternalState::INIT_RW_CONTROLLER;
2023-04-07 17:02:37 +02:00
rawPacket = reinterpret_cast<uint8_t*>(&currentRequest);
rawPacketLen = sizeof(rws::RwRequest);
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2021-06-21 09:50:26 +02:00
}
2023-02-15 17:02:22 +01:00
case (rws::GET_TM): {
2023-04-07 17:02:37 +02:00
currentRequest.setSpeed = false;
currentRequest.specialRequest = rws::SpecialRwRequest::GET_TM;
2023-02-16 14:10:59 +01:00
internalState = InternalState::GET_TM;
2023-04-07 17:02:37 +02:00
rawPacket = reinterpret_cast<uint8_t*>(&currentRequest);
rawPacketLen = sizeof(rws::RwRequest);
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2021-06-30 08:21:14 +02:00
}
2021-06-21 09:50:26 +02:00
default:
2022-01-17 15:58:27 +01:00
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
2022-08-24 17:27:47 +02:00
return returnvalue::FAILED;
2021-06-21 09:50:26 +02:00
}
void RwHandler::fillCommandAndReplyMap() {
2023-02-16 14:10:59 +01:00
insertInCommandMap(rws::REQUEST_ID);
insertInReplyMap(rws::REPLY_ID, 5, nullptr, 0, true);
insertInCommandMap(rws::RESET_MCU);
2023-02-17 02:10:08 +01:00
insertInCommandMap(rws::SET_SPEED);
2023-02-16 14:10:59 +01:00
insertInCommandAndReplyMap(rws::INIT_RW_CONTROLLER, 5, nullptr, 0, false, true, rws::REPLY_ID);
insertInCommandAndReplyMap(rws::GET_TM, 5, nullptr, 0, false, true, rws::REPLY_ID);
2021-06-21 09:50:26 +02:00
}
2022-01-17 15:58:27 +01:00
ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize,
DeviceCommandId_t* foundId, size_t* foundLen) {
2023-02-17 02:39:00 +01:00
if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
2023-02-16 17:57:21 +01:00
return IGNORE_FULL_PACKET;
}
2023-02-16 14:10:59 +01:00
if (remainingSize > 0) {
*foundLen = remainingSize;
*foundId = rws::REPLY_ID;
2022-01-17 15:58:27 +01:00
}
2023-04-07 17:02:37 +02:00
if (internalState == InternalState::SHUTDOWN and shutdownState == ShutdownState::STOP_POLLING) {
shutdownState = ShutdownState::DONE;
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2021-06-21 09:50:26 +02:00
}
2022-01-17 15:58:27 +01:00
ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
2023-02-16 14:10:59 +01:00
RwReplies replies(packet);
ReturnValue_t result = returnvalue::OK;
2023-02-17 02:10:08 +01:00
ReturnValue_t status;
2023-02-17 12:19:53 +01:00
auto checkPacket = [&](DeviceCommandId_t currentId, const uint8_t* packetPtr) {
2023-02-17 02:10:08 +01:00
// This is just the packet length of the frame from the RW. The actual
// data is one more flag byte to show whether the value was read at least once.
2023-02-17 12:19:53 +01:00
auto packetLen = rws::idToPacketLen(currentId);
2023-02-17 02:10:08 +01:00
// arrayprinter::print(packetPtr, packetLen);
2023-02-18 13:12:44 +01:00
uint16_t replyCrc = 0;
2023-02-17 02:10:08 +01:00
size_t dummy = 0;
SerializeAdapter::deSerialize(&replyCrc, packetPtr + packetLen - 2, &dummy,
SerializeIF::Endianness::LITTLE);
if (CRC::crc16ccitt(packetPtr, packetLen - 2) != replyCrc) {
sif::error << "RwHandler::interpretDeviceReply: CRC error for ID " << id << std::endl;
2023-02-16 14:10:59 +01:00
return CRC_ERROR;
2021-06-21 09:50:26 +02:00
}
2023-02-16 14:10:59 +01:00
if (packetPtr[1] == rws::STATE_ERROR) {
sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: " << id
<< std::endl;
result = EXECUTION_FAILED;
2021-06-30 15:07:26 +02:00
}
2023-02-16 14:10:59 +01:00
return returnvalue::OK;
};
2023-02-17 02:10:08 +01:00
if (replies.wasSetSpeedReplyRead()) {
status = checkPacket(rws::DeviceCommandId::SET_SPEED, replies.getSetSpeedReply());
if (status != returnvalue::OK) {
result = status;
}
2023-02-16 14:10:59 +01:00
}
2023-02-17 02:10:08 +01:00
if (replies.wasRwStatusRead()) {
status = checkPacket(rws::DeviceCommandId::GET_RW_STATUS, replies.getRwStatusReply());
if (status == returnvalue::OK) {
handleGetRwStatusReply(replies.getRwStatusReply());
} else {
result = status;
}
2023-02-16 14:10:59 +01:00
}
2023-02-17 02:10:08 +01:00
if (replies.wasGetLastStatusReplyRead()) {
status = checkPacket(rws::DeviceCommandId::GET_LAST_RESET_STATUS,
replies.getGetLastResetStatusReply());
if (status == returnvalue::OK) {
handleResetStatusReply(replies.getGetLastResetStatusReply());
} else {
result = status;
}
}
if (replies.wasClearLastRsetStatusReplyRead()) {
status = checkPacket(rws::DeviceCommandId::CLEAR_LAST_RESET_STATUS,
replies.getClearLastResetStatusReply());
if (status != returnvalue::OK) {
result = status;
}
}
if (replies.wasReadTemperatureReplySet()) {
status = checkPacket(rws::DeviceCommandId::GET_TEMPERATURE, replies.getReadTemperatureReply());
if (status == returnvalue::OK) {
handleTemperatureReply(replies.getReadTemperatureReply());
} else {
result = status;
}
2023-02-16 14:10:59 +01:00
}
if (internalState == InternalState::GET_TM) {
2023-02-17 02:10:08 +01:00
if (replies.wasHkDataReplyRead()) {
status = checkPacket(rws::DeviceCommandId::GET_TM, replies.getHkDataReply());
if (status == returnvalue::OK) {
handleGetTelemetryReply(replies.getHkDataReply());
} else {
result = status;
}
internalState = InternalState::DEFAULT;
2021-06-21 09:50:26 +02:00
}
2023-02-16 14:10:59 +01:00
}
if (internalState == InternalState::INIT_RW_CONTROLLER) {
2023-02-17 02:10:08 +01:00
if (replies.wasInitRwControllerReplyRead()) {
status =
checkPacket(rws::DeviceCommandId::INIT_RW_CONTROLLER, replies.getInitRwControllerReply());
if (status != returnvalue::OK) {
result = status;
}
internalState = InternalState::DEFAULT;
}
2023-02-16 14:10:59 +01:00
}
if (internalState == InternalState::RESET_MCU) {
internalState = InternalState::DEFAULT;
2022-01-17 15:58:27 +01:00
}
2023-02-17 02:10:08 +01:00
return result;
2021-06-21 09:50:26 +02:00
}
2022-01-17 15:58:27 +01:00
uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }
2021-06-21 09:50:26 +02:00
ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
2022-01-17 15:58:27 +01:00
LocalDataPoolManager& poolManager) {
2023-02-15 17:02:22 +01:00
localDataPoolMap.emplace(rws::RW_SPEED, &rwSpeed);
localDataPoolMap.emplace(rws::RAMP_TIME, &rampTime);
localDataPoolMap.emplace(rws::TEMPERATURE_C, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(rws::CURR_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(rws::REFERENCE_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(rws::STATE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::CLC_MODE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::LAST_RESET_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::CURRRENT_RESET_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::TM_LAST_RESET_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::TM_MCU_TEMPERATURE, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(rws::PRESSURE_SENSOR_TEMPERATURE, new PoolEntry<float>({0}));
localDataPoolMap.emplace(rws::PRESSURE, new PoolEntry<float>({0}));
localDataPoolMap.emplace(rws::TM_RW_STATE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::TM_CLC_MODE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(rws::TM_RW_CURR_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(rws::TM_RW_REF_SPEED, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(rws::INVALID_CRC_PACKETS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::INVALID_LEN_PACKETS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::INVALID_CMD_PACKETS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::EXECUTED_REPLIES, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::COMMAND_REPLIES, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_BYTES_WRITTEN, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_BYTES_READ, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_PARITY_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_NOISE_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_FRAME_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::UART_TOTAL_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::SPI_BYTES_WRITTEN, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::SPI_BYTES_READ, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::SPI_REG_OVERRUN_ERRORS, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(rws::SPI_TOTAL_ERRORS, new PoolEntry<uint32_t>({0}));
2022-08-15 11:57:57 +02:00
poolManager.subscribeForDiagPeriodicPacket(
2023-03-26 15:28:00 +02:00
subdp::DiagnosticsHkPeriodicParams(statusSet.getSid(), false, 12.0));
2022-08-15 11:57:57 +02:00
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(tmDataset.getSid(), false, 30.0));
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(lastResetStatusSet.getSid(), false, 30.0));
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2021-06-21 09:50:26 +02:00
}
2023-02-10 13:16:50 +01:00
ReturnValue_t RwHandler::checkSpeedAndRampTime() {
int32_t speed = 0;
uint16_t rampTime = 0;
rwSpeedActuationSet.getRwSpeed(speed, rampTime);
2022-01-17 15:58:27 +01:00
if ((speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) && (speed != 0)) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl;
return INVALID_SPEED;
}
2021-06-21 09:50:26 +02:00
if (rampTime < 10 || rampTime > 20000) {
2022-01-17 15:58:27 +01:00
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time" << std::endl;
return INVALID_RAMP_TIME;
}
2021-06-21 09:50:26 +02:00
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2021-06-21 09:50:26 +02:00
}
void RwHandler::handleResetStatusReply(const uint8_t* packet) {
2022-01-17 15:58:27 +01:00
PoolReadGuard rg(&lastResetStatusSet);
uint8_t offset = 2;
uint8_t resetStatus = packet[offset];
2022-05-11 15:45:38 +02:00
if (resetStatus != 0) {
2023-02-16 14:10:59 +01:00
// internalState = InternalState::CLEAR_RESET_STATUS;
lastResetStatusSet.lastNonClearedResetStatus = resetStatus;
2023-02-15 17:02:22 +01:00
triggerEvent(rws::RESET_OCCURED, resetStatus, 0);
2022-01-17 15:58:27 +01:00
}
2022-05-05 13:40:43 +02:00
lastResetStatusSet.currentResetStatus = resetStatus;
2022-04-26 10:37:25 +02:00
if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
sif::info << "RwHandler::handleResetStatusReply: Last reset status: "
2022-05-05 13:40:43 +02:00
<< static_cast<unsigned int>(lastResetStatusSet.lastNonClearedResetStatus.value)
<< std::endl;
2022-04-26 10:37:25 +02:00
sif::info << "RwHandler::handleResetStatusReply: Current reset status: "
2022-05-05 13:40:43 +02:00
<< static_cast<unsigned int>(lastResetStatusSet.currentResetStatus.value)
2022-04-26 10:37:25 +02:00
<< std::endl;
#endif
2022-04-26 10:37:25 +02:00
}
}
2021-06-21 09:50:26 +02:00
void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
2022-05-10 18:34:39 +02:00
PoolReadGuard rg0(&statusSet);
PoolReadGuard rg1(&tmDataset);
2022-01-17 15:58:27 +01:00
uint8_t offset = 2;
statusSet.currSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
2022-05-10 18:34:39 +02:00
tmDataset.rwCurrSpeed = statusSet.currSpeed;
tmDataset.rwCurrSpeed.setValid(true);
2022-01-17 15:58:27 +01:00
offset += 4;
statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
2022-05-10 18:34:39 +02:00
tmDataset.rwRefSpeed = statusSet.referenceSpeed;
tmDataset.rwRefSpeed.setValid(true);
2022-01-17 15:58:27 +01:00
offset += 4;
statusSet.state = *(packet + offset);
2022-05-10 18:34:39 +02:00
tmDataset.rwState = statusSet.state;
tmDataset.rwState.setValid(true);
2022-01-17 15:58:27 +01:00
offset += 1;
statusSet.clcMode = *(packet + offset);
2022-05-10 18:34:39 +02:00
tmDataset.rwClcMode = statusSet.clcMode;
tmDataset.rwClcMode.setValid(true);
statusSet.setValidity(true, true);
2022-01-17 15:58:27 +01:00
2023-04-07 17:02:37 +02:00
if (internalState == InternalState::SHUTDOWN and std::abs(tmDataset.rwCurrSpeed.value) <= 2 and
shutdownState == ShutdownState::SET_SPEED_ZERO) {
2023-04-07 11:37:17 +02:00
// Finish transition to off.
2023-04-07 17:02:37 +02:00
shutdownState = ShutdownState::STOP_POLLING;
2023-04-07 11:37:17 +02:00
}
2023-02-15 17:02:22 +01:00
if (statusSet.state == rws::STATE_ERROR) {
2023-03-08 18:09:50 +01:00
// Trigger FDIR reaction, first recovery, then faulty if it doesnt fix the issue.
triggerEvent(DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT, statusSet.state.value, 0);
2022-01-17 15:58:27 +01:00
sif::error << "RwHandler::handleGetRwStatusReply: Reaction wheel in error state" << std::endl;
}
2021-06-29 16:08:20 +02:00
2022-04-26 10:37:25 +02:00
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;
2021-06-21 09:50:26 +02:00
#endif
2022-04-26 10:37:25 +02:00
}
2021-06-21 09:50:26 +02:00
}
void RwHandler::handleTemperatureReply(const uint8_t* packet) {
2022-05-10 18:34:39 +02:00
PoolReadGuard rg(&statusSet);
2022-01-17 15:58:27 +01:00
uint8_t offset = 2;
2022-05-10 18:34:39 +02:00
statusSet.temperatureCelcius = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 |
*(packet + offset + 1) << 8 | *(packet + offset);
2022-04-26 10:37:25 +02:00
if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
2022-05-10 18:34:39 +02:00
sif::info << "RwHandler::handleTemperatureReply: Temperature: " << statusSet.temperatureCelcius
<< " °C" << std::endl;
2021-06-21 09:50:26 +02:00
#endif
2022-04-26 10:37:25 +02:00
}
2021-06-21 09:50:26 +02:00
}
2021-06-30 15:07:26 +02:00
2023-02-16 16:54:58 +01:00
LocalPoolDataSetBase* RwHandler::getDataSetHandle(sid_t sid) {
switch (sid.ownerSetId) {
case (rws::SetIds::STATUS_SET_ID): {
return &statusSet;
}
case (rws::SetIds::LAST_RESET_ID): {
return &lastResetStatusSet;
}
case (rws::SetIds::SPEED_CMD_SET): {
return &rwSpeedActuationSet;
}
case (rws::SetIds::TM_SET_ID): {
return &tmDataset;
}
}
return nullptr;
}
2021-06-30 15:07:26 +02:00
void RwHandler::handleGetTelemetryReply(const uint8_t* packet) {
2022-01-17 15:58:27 +01:00
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);
2022-04-26 10:37:25 +02:00
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;
2021-06-30 15:07:26 +02:00
#endif
2022-04-26 10:37:25 +02:00
}
2021-06-30 15:07:26 +02:00
}
2022-04-26 10:37:25 +02:00
void RwHandler::setDebugMode(bool enable) { this->debugMode = enable; }