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

it appears to work well now

Browse Source
This commit is contained in:
Robin Müller
2023-02-17 02:10:08 +01:00
parent 9d59f960a4
commit c6c92e1140
8 changed files with 179 additions and 270 deletions
Download Patch File Download Diff File Expand all files Collapse all files

258
mission/devices/RwHandler.cpp
View File

@ -2,6 +2,7 @@
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw_hal/common/gpio/GpioIF.h>
#include "OBSWConfig.h"
@ -51,26 +52,6 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
*id = rws::REQUEST_ID;
break;
}
// case InternalState::SET_SPEED:
// *id = rws::SET_SPEED;
// internalState = InternalState::GET_RESET_STATUS;
// break;
// case InternalState::GET_RESET_STATUS:
// *id = rws::GET_LAST_RESET_STATUS;
// internalState = InternalState::READ_TEMPERATURE;
// break;
// case InternalState::READ_TEMPERATURE:
// *id = rws::GET_TEMPERATURE;
// internalState = InternalState::GET_RW_SATUS;
// break;
// case InternalState::GET_RW_SATUS:
// *id = rws::GET_RW_STATUS;
// internalState = InternalState::CLEAR_RESET_STATUS;
// break;
// case InternalState::CLEAR_RESET_STATUS:
// *id = rws::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;
@ -88,6 +69,7 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
ReturnValue_t result = returnvalue::OK;
switch (deviceCommand) {
case (rws::SET_SPEED):
case (rws::REQUEST_ID): {
if (commandData != nullptr && commandDataLen != 6) {
sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
@ -148,66 +130,6 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
internalState = InternalState::GET_TM;
return returnvalue::OK;
}
// case (rws::GET_LAST_RESET_STATUS): {
// prepareSimpleCommand(deviceCommand);
// return returnvalue::OK;
// }
// case (rws::CLEAR_LAST_RESET_STATUS): {
// prepareSimpleCommand(deviceCommand);
// return returnvalue::OK;
// }
// case (rws::GET_RW_STATUS): {
// prepareSimpleCommand(deviceCommand);
// return returnvalue::OK;
// }
// case (rws::INIT_RW_CONTROLLER): {
// prepareSimpleCommand(deviceCommand);
// return returnvalue::OK;
// }
// case (rws::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;
// }
//
// {
// PoolReadGuard pg(&rwSpeedActuationSet);
// // 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;
// }
// }
// }
// if (ACTUATION_WIRETAPPING) {
// int32_t speed = 0;
// uint16_t rampTime = 0;
// rwSpeedActuationSet.getRwSpeed(speed, rampTime);
// sif::debug << "Actuating RW " << static_cast<int>(rwIdx) << " with speed = " <<
// speed
// << " and rampTime = " << rampTime << std::endl;
// }
// result = checkSpeedAndRampTime();
// if (result != returnvalue::OK) {
// return result;
// }
// result = prepareSetSpeedCmd();
// return result;
// }
// case (rws::GET_TEMPERATURE): {
// prepareSimpleCommand(deviceCommand);
// return returnvalue::OK;
// }
// case (rws::GET_TM): {
// prepareSimpleCommand(deviceCommand);
// return returnvalue::OK;
// }
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
@ -219,20 +141,9 @@ void RwHandler::fillCommandAndReplyMap() {
insertInReplyMap(rws::REPLY_ID, 5, nullptr, 0, true);
insertInCommandMap(rws::RESET_MCU);
insertInCommandMap(rws::SET_SPEED);
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);
/*
this->insertInCommandAndReplyMap(rws::GET_LAST_RESET_STATUS, 1, &lastResetStatusSet,
rws::SIZE_GET_RESET_STATUS);
this->insertInCommandAndReplyMap(rws::CLEAR_LAST_RESET_STATUS, 1, nullptr,
rws::SIZE_CLEAR_RESET_STATUS);
this->insertInCommandAndReplyMap(rws::GET_RW_STATUS, 1, &statusSet, rws::SIZE_GET_RW_STATUS);
this->insertInCommandAndReplyMap(rws::INIT_RW_CONTROLLER, 1, nullptr, rws::SIZE_INIT_RW);
this->insertInCommandAndReplyMap(rws::GET_TEMPERATURE, 1, nullptr,
rws::SIZE_GET_TEMPERATURE_REPLY);
this->insertInCommandAndReplyMap(rws::SET_SPEED, 1, nullptr, rws::SIZE_SET_SPEED_REPLY);
this->insertInCommandAndReplyMap(rws::GET_TM, 1, &tmDataset, rws::SIZE_GET_TELEMETRY_REPLY);
*/
}
ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize,
@ -240,64 +151,28 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize
if (getMode() == _MODE_WAIT_OFF) {
return IGNORE_FULL_PACKET;
}
// sif::debug << "base mode: " << baseMode << std::endl;
if (remainingSize > 0) {
*foundLen = remainingSize;
*foundId = rws::REPLY_ID;
}
// RwReplies replies(start);
// switch (replyByte) {
// case (rws::GET_LAST_RESET_STATUS): {
// *foundLen = rws::SIZE_GET_RESET_STATUS;
// *foundId = rws::GET_LAST_RESET_STATUS;
// break;
// }
// case (rws::CLEAR_LAST_RESET_STATUS): {
// *foundLen = rws::SIZE_CLEAR_RESET_STATUS;
// *foundId = rws::CLEAR_LAST_RESET_STATUS;
// break;
// }
// case (rws::GET_RW_STATUS): {
// *foundLen = rws::SIZE_GET_RW_STATUS;
// *foundId = rws::GET_RW_STATUS;
// break;
// }
// case (rws::INIT_RW_CONTROLLER): {
// *foundLen = rws::SIZE_INIT_RW;
// *foundId = rws::INIT_RW_CONTROLLER;
// break;
// }
// case (rws::SET_SPEED): {
// *foundLen = rws::SIZE_SET_SPEED_REPLY;
// *foundId = rws::SET_SPEED;
// break;
// }
// case (rws::GET_TEMPERATURE): {
// *foundLen = rws::SIZE_GET_TEMPERATURE_REPLY;
// *foundId = rws::GET_TEMPERATURE;
// break;
// }
// case (rws::GET_TM): {
// *foundLen = rws::SIZE_GET_TELEMETRY_REPLY;
// *foundId = rws::GET_TM;
// break;
// }
// default: {
// sif::warning << "RwHandler::scanForReply: Reply contains invalid command code" <<
// std::endl; *foundLen = remainingSize; return returnvalue::FAILED;
// }
// }
return returnvalue::OK;
}
ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
RwReplies replies(packet);
ReturnValue_t result = returnvalue::OK;
ReturnValue_t status;
auto checkPacket = [&](DeviceCommandId_t id, const uint8_t* packetPtr) {
// 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.
auto packetLen = rws::idToPacketLen(id);
uint16_t replyCrc = (*(packet + packetLen - 1) << 8) | *(packet + packetLen - 2);
if (CRC::crc16ccitt(packet, packetLen - 2, 0xFFFF) != replyCrc) {
// sif::error << "RwHandler::interpretDeviceReply: CRC error for ID " << id << std::endl;
// arrayprinter::print(packetPtr, packetLen);
uint16_t replyCrc;
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;
return CRC_ERROR;
}
if (packetPtr[1] == rws::STATE_ERROR) {
@ -307,35 +182,73 @@ ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_
}
return returnvalue::OK;
};
checkPacket(rws::DeviceCommandId::SET_SPEED, replies.getSetSpeedReply());
if (returnvalue::OK ==
checkPacket(rws::DeviceCommandId::GET_RW_STATUS, replies.getRwStatusReply())) {
handleGetRwStatusReply(replies.getRwStatusReply());
if (replies.wasSetSpeedReplyRead()) {
status = checkPacket(rws::DeviceCommandId::SET_SPEED, replies.getSetSpeedReply());
if (status != returnvalue::OK) {
result = status;
}
}
if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_LAST_RESET_STATUS,
replies.getGetLastResetStatusReply())) {
handleResetStatusReply(replies.getGetLastResetStatusReply());
if (replies.wasRwStatusRead()) {
status = checkPacket(rws::DeviceCommandId::GET_RW_STATUS, replies.getRwStatusReply());
if (status == returnvalue::OK) {
handleGetRwStatusReply(replies.getRwStatusReply());
} else {
result = status;
}
}
checkPacket(rws::DeviceCommandId::CLEAR_LAST_RESET_STATUS,
replies.getClearLastResetStatusReply());
if (returnvalue::OK ==
checkPacket(rws::DeviceCommandId::GET_TEMPERATURE, replies.getReadTemperatureReply())) {
handleTemperatureReply(replies.getReadTemperatureReply());
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;
}
}
if (internalState == InternalState::GET_TM) {
if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_TM, replies.getHkDataReply())) {
handleGetTelemetryReply(replies.getHkDataReply());
if (replies.wasHkDataReplyRead()) {
status = checkPacket(rws::DeviceCommandId::GET_TM, replies.getHkDataReply());
if (status == returnvalue::OK) {
handleGetTelemetryReply(replies.getHkDataReply());
} else {
result = status;
}
internalState = InternalState::DEFAULT;
}
internalState = InternalState::DEFAULT;
}
if (internalState == InternalState::INIT_RW_CONTROLLER) {
checkPacket(rws::DeviceCommandId::INIT_RW_CONTROLLER, replies.getInitRwControllerReply());
internalState = InternalState::DEFAULT;
if (replies.wasInitRwControllerReplyRead()) {
status =
checkPacket(rws::DeviceCommandId::INIT_RW_CONTROLLER, replies.getInitRwControllerReply());
if (status != returnvalue::OK) {
result = status;
}
internalState = InternalState::DEFAULT;
}
}
if (internalState == InternalState::RESET_MCU) {
internalState = InternalState::DEFAULT;
}
return returnvalue::OK;
return result;
}
uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 5000; }
@ -388,17 +301,6 @@ ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataP
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;
@ -416,28 +318,6 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime() {
return returnvalue::OK;
}
/*
ReturnValue_t RwHandler::prepareSetSpeedCmd() {
commandBuffer[0] = static_cast<uint8_t>(rws::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;