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

DHB refactoring complete
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Robin Müller 2023-02-16 14:10:59 +01:00
parent 9fdb41506b
commit 39f83937c5
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
6 changed files with 311 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
linux/devices/RwPollingTask.cpp
View File

@ -20,6 +20,12 @@ RwPollingTask::RwPollingTask(object_id_t objectId, SpiComIF* spiIF)
} }
ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) { ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) {
for (unsigned i = 0; i < 4; i++) {
if (rwCookies[i] == nullptr) {
sif::error << "Invalid RW cookie at index" << i << std::endl;
return returnvalue::FAILED;
}
}
while (true) { while (true) {
ipcLock->lockMutex(); ipcLock->lockMutex();
state = InternalState::IDLE; state = InternalState::IDLE;
@ -31,9 +37,11 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) {
continue; continue;
} }
for (unsigned idx = 0; idx < rwCookies.size(); idx++) { for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
prepareSetSpeedCmd(idx); if (rwCookies[idx]->setSpeed) {
if (writeOneRwCmd(idx, fd) != returnvalue::OK) { prepareSetSpeedCmd(idx);
continue; if (writeOneRwCmd(idx, fd) != returnvalue::OK) {
continue;
}
} }
} }
closeSpi(fd); closeSpi(fd);
@ -57,6 +65,7 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) {
if (writeAndReadAllRws(rws::CLEAR_LAST_RESET_STATUS) != returnvalue::OK) { if (writeAndReadAllRws(rws::CLEAR_LAST_RESET_STATUS) != returnvalue::OK) {
continue; continue;
} }
// TODO: Special requests
} }
return returnvalue::OK; return returnvalue::OK;
@ -84,6 +93,12 @@ ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) {
modeAndSpeedWasSet = true; modeAndSpeedWasSet = true;
} }
auto* rwCookie = dynamic_cast<RwCookie*>(cookie);
if (rwCookie == nullptr) {
return returnvalue::FAILED;
}
rwCookies[rwCookie->rwIdx] = rwCookie;
return returnvalue::OK; return returnvalue::OK;
} }
@ -94,11 +109,15 @@ ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendDa
} }
int32_t speed = 0; int32_t speed = 0;
uint16_t rampTime = 0; uint16_t rampTime = 0;
SerializeAdapter::deSerialize(&speed, &sendData, &sendLen, SerializeIF::Endianness::MACHINE); const uint8_t* currentBuf = sendData;
SerializeAdapter::deSerialize(&rampTime, &sendData, &sendLen, SerializeIF::Endianness::MACHINE); bool setSpeed = currentBuf[0];
currentBuf += 1;
sendLen -= 1;
SerializeAdapter::deSerialize(&speed, &currentBuf, &sendLen, SerializeIF::Endianness::MACHINE);
SerializeAdapter::deSerialize(&rampTime, &currentBuf, &sendLen, SerializeIF::Endianness::MACHINE);
rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE; rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
if (sendLen == 7 and sendData[6] < static_cast<uint8_t>(rws::SpecialRwRequest::NUM_REQUESTS)) { if (sendLen == 8 and sendData[7] < static_cast<uint8_t>(rws::SpecialRwRequest::NUM_REQUESTS)) {
specialRequest = static_cast<rws::SpecialRwRequest>(sendData[6]); specialRequest = static_cast<rws::SpecialRwRequest>(sendData[7]);
} }
RwCookie* rwCookie = dynamic_cast<RwCookie*>(cookie); RwCookie* rwCookie = dynamic_cast<RwCookie*>(cookie);
if (rwCookie == nullptr) { if (rwCookie == nullptr) {
@ -106,6 +125,7 @@ ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendDa
} }
{ {
MutexGuard mg(ipcLock); MutexGuard mg(ipcLock);
rwCookie->setSpeed = setSpeed;
rwCookie->currentRwSpeed = speed; rwCookie->currentRwSpeed = speed;
rwCookie->currentRampTime = rampTime; rwCookie->currentRampTime = rampTime;
rwCookie->specialRequest = specialRequest; rwCookie->specialRequest = specialRequest;
@ -299,6 +319,9 @@ ReturnValue_t RwPollingTask::readAllRws(int fd, DeviceCommandId_t id) {
return result; return result;
} }
for (unsigned idx = 0; idx < rwCookies.size(); idx++) { for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
if (id == rws::SET_SPEED and !rwCookies[idx]->setSpeed) {
continue;
}
if (spiLock == nullptr) { if (spiLock == nullptr) {
sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl; sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
return returnvalue::FAILED; return returnvalue::FAILED;
@ -319,31 +342,31 @@ size_t RwPollingTask::idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx,
switch (id) { switch (id) {
case (rws::GET_RW_STATUS): { case (rws::GET_RW_STATUS): {
*ptr = replies.rwStatusReply; *ptr = replies.rwStatusReply;
return rws::SIZE_GET_RW_STATUS; break;
} }
case (rws::SET_SPEED): { case (rws::SET_SPEED): {
*ptr = replies.setSpeedReply; *ptr = replies.setSpeedReply;
return rws::SIZE_SET_SPEED_REPLY; break;
} }
case (rws::CLEAR_LAST_RESET_STATUS): { case (rws::CLEAR_LAST_RESET_STATUS): {
*ptr = replies.clearLastResetStatusReply; *ptr = replies.clearLastResetStatusReply;
return rws::SIZE_CLEAR_RESET_STATUS; break;
} }
case (rws::GET_LAST_RESET_STATUS): { case (rws::GET_LAST_RESET_STATUS): {
*ptr = replies.getLastResetStatusReply; *ptr = replies.getLastResetStatusReply;
return rws::SIZE_GET_RESET_STATUS; break;
} }
case (rws::GET_TEMPERATURE): { case (rws::GET_TEMPERATURE): {
*ptr = replies.readTemperatureReply; *ptr = replies.readTemperatureReply;
return rws::SIZE_GET_TEMPERATURE_REPLY; break;
} }
case (rws::GET_TM): { case (rws::GET_TM): {
*ptr = replies.hkDataReply; *ptr = replies.hkDataReply;
return rws::SIZE_GET_TELEMETRY_REPLY; break;
} }
case (rws::INIT_RW_CONTROLLER): { case (rws::INIT_RW_CONTROLLER): {
*ptr = replies.initRwControllerReply; *ptr = replies.initRwControllerReply;
return rws::SIZE_INIT_RW; break;
} }
default: { default: {
sif::error << "no reply buffer for rw command " << id << std::endl; sif::error << "no reply buffer for rw command " << id << std::endl;
@ -351,6 +374,7 @@ size_t RwPollingTask::idAndIdxToReadBuffer(DeviceCommandId_t id, uint8_t rwIdx,
return 0; return 0;
} }
} }
return rws::idToPacketLen(id);
} }
// This closes the SPI // This closes the SPI

1
linux/devices/RwPollingTask.h
View File

@ -21,6 +21,7 @@ class RwCookie : public SpiCookie {
private: private:
std::array<uint8_t, REPLY_BUF_LEN> replyBuf{}; std::array<uint8_t, REPLY_BUF_LEN> replyBuf{};
bool setSpeed = true;
int32_t currentRwSpeed = 0; int32_t currentRwSpeed = 0;
uint16_t currentRampTime = 0; uint16_t currentRampTime = 0;
rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE; rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;

344
mission/devices/RwHandler.cpp
View File

@ -27,11 +27,12 @@ RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCooki
RwHandler::~RwHandler() {} RwHandler::~RwHandler() {}
void RwHandler::doStartUp() { void RwHandler::doStartUp() {
internalState = InternalState::GET_RESET_STATUS; internalState = InternalState::DEFAULT;
if (gpioComIF->pullHigh(enableGpio) != returnvalue::OK) { if (gpioComIF->pullHigh(enableGpio) != returnvalue::OK) {
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high"; sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high";
} }
updatePeriodicReply(true, rws::REPLY_ID);
setMode(_MODE_TO_ON); setMode(_MODE_TO_ON);
} }
@ -39,32 +40,37 @@ void RwHandler::doShutDown() {
if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) { if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) {
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low"; sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low";
} }
internalState = InternalState::DEFAULT;
updatePeriodicReply(false, rws::REPLY_ID);
setMode(_MODE_POWER_DOWN); setMode(_MODE_POWER_DOWN);
} }
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
switch (internalState) { switch (internalState) {
case InternalState::SET_SPEED: case InternalState::DEFAULT: {
*id = rws::SET_SPEED; *id = rws::REQUEST_ID;
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; 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: default:
sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid internal step" << std::endl; sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid internal step" << std::endl;
break; break;
@ -82,27 +88,7 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
ReturnValue_t result = returnvalue::OK; ReturnValue_t result = returnvalue::OK;
switch (deviceCommand) { switch (deviceCommand) {
case (rws::RESET_MCU): { case (rws::REQUEST_ID): {
prepareSimpleCommand(deviceCommand);
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) { if (commandData != nullptr && commandDataLen != 6) {
sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with" sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
<< " invalid length" << std::endl; << " invalid length" << std::endl;
@ -133,17 +119,94 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
return result; return result;
} }
result = prepareSetSpeedCmd(); // set speed flag.
return result; commandBuffer[0] = true;
rawPacketLen = 1;
uint8_t* currentCmdBuf = commandBuffer + 1;
rwSpeedActuationSet.serialize(&currentCmdBuf, &rawPacketLen, sizeof(commandBuffer),
SerializeIF::Endianness::MACHINE);
commandBuffer[rawPacketLen] = static_cast<uint8_t>(rws::SpecialRwRequest::REQUEST_NONE);
break;
} }
case (rws::GET_TEMPERATURE): { case (rws::RESET_MCU): {
prepareSimpleCommand(deviceCommand); commandBuffer[0] = false;
commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::RESET_MCU);
internalState = InternalState::RESET_MCU;
return returnvalue::OK;
}
case (rws::INIT_RW_CONTROLLER): {
commandBuffer[0] = false;
commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::INIT_RW_CONTROLLER);
internalState = InternalState::INIT_RW_CONTROLLER;
return returnvalue::OK; return returnvalue::OK;
} }
case (rws::GET_TM): { case (rws::GET_TM): {
prepareSimpleCommand(deviceCommand); commandBuffer[0] = false;
commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::GET_TM);
internalState = InternalState::GET_TM;
return returnvalue::OK; 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: default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
} }
@ -151,7 +214,13 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
} }
void RwHandler::fillCommandAndReplyMap() { void RwHandler::fillCommandAndReplyMap() {
this->insertInCommandMap(rws::RESET_MCU); insertInCommandMap(rws::REQUEST_ID);
insertInReplyMap(rws::REPLY_ID, 5, nullptr, 0, true);
insertInCommandMap(rws::RESET_MCU);
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, this->insertInCommandAndReplyMap(rws::GET_LAST_RESET_STATUS, 1, &lastResetStatusSet,
rws::SIZE_GET_RESET_STATUS); rws::SIZE_GET_RESET_STATUS);
this->insertInCommandAndReplyMap(rws::CLEAR_LAST_RESET_STATUS, 1, nullptr, this->insertInCommandAndReplyMap(rws::CLEAR_LAST_RESET_STATUS, 1, nullptr,
@ -162,103 +231,106 @@ void RwHandler::fillCommandAndReplyMap() {
rws::SIZE_GET_TEMPERATURE_REPLY); rws::SIZE_GET_TEMPERATURE_REPLY);
this->insertInCommandAndReplyMap(rws::SET_SPEED, 1, nullptr, rws::SIZE_SET_SPEED_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); this->insertInCommandAndReplyMap(rws::GET_TM, 1, &tmDataset, rws::SIZE_GET_TELEMETRY_REPLY);
*/
} }
ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize, ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize,
DeviceCommandId_t* foundId, size_t* foundLen) { DeviceCommandId_t* foundId, size_t* foundLen) {
uint8_t replyByte = *start; // uint8_t replyByte = *start;
switch (replyByte) { if (remainingSize > 0) {
case (rws::GET_LAST_RESET_STATUS): { *foundLen = remainingSize;
*foundLen = rws::SIZE_GET_RESET_STATUS; *foundId = rws::REPLY_ID;
*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;
}
} }
// RwReplies replies(start);
sizeOfReply = *foundLen; // 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; return returnvalue::OK;
} }
ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) { ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
/** Check result code */ RwReplies replies(packet);
if (*(packet + 1) == rws::STATE_ERROR) { ReturnValue_t result = returnvalue::OK;
sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: " << id auto checkPacket = [&](DeviceCommandId_t id, const uint8_t* packetPtr) {
<< std::endl; auto packetLen = rws::idToPacketLen(id);
return EXECUTION_FAILED; 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;
return CRC_ERROR;
}
if (packetPtr[1] == rws::STATE_ERROR) {
sif::error << "RwHandler::interpretDeviceReply: Command execution failed. Command id: " << id
<< std::endl;
result = EXECUTION_FAILED;
}
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 (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_LAST_RESET_STATUS,
/** Received in little endian byte order */ replies.getGetLastResetStatusReply())) {
uint16_t replyCrc = *(packet + sizeOfReply - 1) << 8 | *(packet + sizeOfReply - 2); handleResetStatusReply(replies.getGetLastResetStatusReply());
if (CRC::crc16ccitt(packet, sizeOfReply - 2, 0xFFFF) != replyCrc) {
sif::error << "RwHandler::interpretDeviceReply: cRC error" << std::endl;
return CRC_ERROR;
} }
checkPacket(rws::DeviceCommandId::CLEAR_LAST_RESET_STATUS,
switch (id) { replies.getClearLastResetStatusReply());
case (rws::GET_LAST_RESET_STATUS): { if (returnvalue::OK ==
handleResetStatusReply(packet); checkPacket(rws::DeviceCommandId::GET_TEMPERATURE, replies.getReadTemperatureReply())) {
break; handleTemperatureReply(replies.getReadTemperatureReply());
} }
case (rws::GET_RW_STATUS): { if (internalState == InternalState::GET_TM) {
handleGetRwStatusReply(packet); if (returnvalue::OK == checkPacket(rws::DeviceCommandId::GET_TM, replies.getHkDataReply())) {
break; handleGetTelemetryReply(replies.getHkDataReply());
} }
case (rws::CLEAR_LAST_RESET_STATUS): internalState = InternalState::DEFAULT;
case (rws::INIT_RW_CONTROLLER): }
case (rws::SET_SPEED): if (internalState == InternalState::INIT_RW_CONTROLLER) {
// no reply data expected checkPacket(rws::DeviceCommandId::INIT_RW_CONTROLLER, replies.getInitRwControllerReply());
break; internalState = InternalState::DEFAULT;
case (rws::GET_TEMPERATURE): { }
handleTemperatureReply(packet); if (internalState == InternalState::RESET_MCU) {
break; internalState = InternalState::DEFAULT;
}
case (rws::GET_TM): {
handleGetTelemetryReply(packet);
break;
}
default: {
sif::debug << "RwHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
}
} }
return returnvalue::OK; return returnvalue::OK;
} }
@ -312,6 +384,7 @@ ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataP
return returnvalue::OK; return returnvalue::OK;
} }
/*
void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) { void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) {
commandBuffer[0] = static_cast<uint8_t>(id); commandBuffer[0] = static_cast<uint8_t>(id);
uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF); uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
@ -320,6 +393,7 @@ void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) {
rawPacket = commandBuffer; rawPacket = commandBuffer;
rawPacketLen = 3; rawPacketLen = 3;
} }
*/
ReturnValue_t RwHandler::checkSpeedAndRampTime() { ReturnValue_t RwHandler::checkSpeedAndRampTime() {
int32_t speed = 0; int32_t speed = 0;
@ -338,6 +412,7 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime() {
return returnvalue::OK; return returnvalue::OK;
} }
/*
ReturnValue_t RwHandler::prepareSetSpeedCmd() { ReturnValue_t RwHandler::prepareSetSpeedCmd() {
commandBuffer[0] = static_cast<uint8_t>(rws::SET_SPEED); commandBuffer[0] = static_cast<uint8_t>(rws::SET_SPEED);
uint8_t* serPtr = commandBuffer + 1; uint8_t* serPtr = commandBuffer + 1;
@ -357,13 +432,14 @@ ReturnValue_t RwHandler::prepareSetSpeedCmd() {
rawPacketLen = 9; rawPacketLen = 9;
return result; return result;
} }
*/
void RwHandler::handleResetStatusReply(const uint8_t* packet) { void RwHandler::handleResetStatusReply(const uint8_t* packet) {
PoolReadGuard rg(&lastResetStatusSet); PoolReadGuard rg(&lastResetStatusSet);
uint8_t offset = 2; uint8_t offset = 2;
uint8_t resetStatus = packet[offset]; uint8_t resetStatus = packet[offset];
if (resetStatus != 0) { if (resetStatus != 0) {
internalState = InternalState::CLEAR_RESET_STATUS; // internalState = InternalState::CLEAR_RESET_STATUS;
lastResetStatusSet.lastNonClearedResetStatus = resetStatus; lastResetStatusSet.lastNonClearedResetStatus = resetStatus;
triggerEvent(rws::RESET_OCCURED, resetStatus, 0); triggerEvent(rws::RESET_OCCURED, resetStatus, 0);
} }

24
mission/devices/RwHandler.h
View File

@ -79,30 +79,32 @@ class RwHandler : public DeviceHandlerBase {
rws::TmDataset tmDataset; rws::TmDataset tmDataset;
rws::RwSpeedActuationSet rwSpeedActuationSet; rws::RwSpeedActuationSet rwSpeedActuationSet;
uint8_t commandBuffer[rws::MAX_CMD_SIZE]; uint8_t commandBuffer[32];
uint8_t rwIdx; uint8_t rwIdx;
PoolEntry<int32_t> rwSpeed = PoolEntry<int32_t>({0}); PoolEntry<int32_t> rwSpeed = PoolEntry<int32_t>({0});
PoolEntry<uint16_t> rampTime = PoolEntry<uint16_t>({10}); PoolEntry<uint16_t> rampTime = PoolEntry<uint16_t>({10});
enum class InternalState { enum class InternalState {
GET_RESET_STATUS, DEFAULT,
CLEAR_RESET_STATUS, GET_TM,
READ_TEMPERATURE, INIT_RW_CONTROLLER,
SET_SPEED, RESET_MCU,
GET_RW_SATUS // GET_RESET_STATUS,
// CLEAR_RESET_STATUS,
// READ_TEMPERATURE,
// SET_SPEED,
// GET_RW_SATUS
}; };
InternalState internalState = InternalState::GET_RESET_STATUS; InternalState internalState = InternalState::DEFAULT;
size_t sizeOfReply = 0;
/** /**
* @brief This function can be used to build commands which do not contain any data apart * @brief This function can be used to build commands which do not contain any data apart
* from the command id and the CRC. * from the command id and the CRC.
* @param commandId The command id of the command to build. * @param commandId The command id of the command to build.
*/ */
void prepareSimpleCommand(DeviceCommandId_t id); // void prepareSimpleCommand(DeviceCommandId_t id);
/** /**
* @brief This function checks if the receiced speed and ramp time to set are in a valid * @brief This function checks if the receiced speed and ramp time to set are in a valid
@ -116,7 +118,7 @@ class RwHandler : public DeviceHandlerBase {
* an action message or set in the software. * an action message or set in the software.
* @return returnvalue::OK if successful, otherwise error code. * @return returnvalue::OK if successful, otherwise error code.
*/ */
ReturnValue_t prepareSetSpeedCmd(); // ReturnValue_t prepareSetSpeedCmd();
/** /**
* @brief This function writes the last reset status retrieved with the get last reset status * @brief This function writes the last reset status retrieved with the get last reset status

30
mission/devices/devicedefinitions/rwHelpers.cpp
View File

@ -21,4 +21,34 @@ void rws::encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, uint8_t* encode
encodedBuffer[encodedLen++] = rws::FRAME_DELIMITER; encodedBuffer[encodedLen++] = rws::FRAME_DELIMITER;
} }
size_t rws::idToPacketLen(DeviceCommandId_t id) {
switch (id) {
case (rws::GET_RW_STATUS): {
return rws::SIZE_GET_RW_STATUS;
}
case (rws::SET_SPEED): {
return rws::SIZE_SET_SPEED_REPLY;
}
case (rws::CLEAR_LAST_RESET_STATUS): {
return rws::SIZE_CLEAR_RESET_STATUS;
}
case (rws::GET_LAST_RESET_STATUS): {
return rws::SIZE_GET_RESET_STATUS;
}
case (rws::GET_TEMPERATURE): {
return rws::SIZE_GET_TEMPERATURE_REPLY;
}
case (rws::GET_TM): {
return rws::SIZE_GET_TELEMETRY_REPLY;
}
case (rws::INIT_RW_CONTROLLER): {
return rws::SIZE_INIT_RW;
}
default: {
sif::error << "no reply buffer for rw command " << id << std::endl;
return 0;
}
}
}
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ */ #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ */

32
mission/devices/devicedefinitions/rwHelpers.h
View File

@ -13,6 +13,7 @@ namespace rws {
void encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, uint8_t* encodedBuffer, void encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, uint8_t* encodedBuffer,
size_t& encodedLen); size_t& encodedLen);
size_t idToPacketLen(DeviceCommandId_t id);
static const size_t SIZE_GET_RESET_STATUS = 5; static const size_t SIZE_GET_RESET_STATUS = 5;
static const size_t SIZE_CLEAR_RESET_STATUS = 4; static const size_t SIZE_CLEAR_RESET_STATUS = 4;
@ -112,21 +113,26 @@ enum LastResetStatusBitPos : uint8_t {
LOW_POWER_RESET = 5 LOW_POWER_RESET = 5
}; };
static const DeviceCommandId_t RESET_MCU = 1; enum DeviceCommandId : DeviceCommandId_t {
static const DeviceCommandId_t GET_LAST_RESET_STATUS = 2; RESET_MCU = 1,
static const DeviceCommandId_t CLEAR_LAST_RESET_STATUS = 3;
static const DeviceCommandId_t GET_RW_STATUS = 4; GET_LAST_RESET_STATUS = 2,
/** This command is needed to recover from error state */ CLEAR_LAST_RESET_STATUS = 3,
static const DeviceCommandId_t INIT_RW_CONTROLLER = 5; GET_RW_STATUS = 4,
static const DeviceCommandId_t SET_SPEED = 6; INIT_RW_CONTROLLER = 5,
static const DeviceCommandId_t GET_TEMPERATURE = 8; SET_SPEED = 6,
static const DeviceCommandId_t GET_TM = 9; GET_TEMPERATURE = 8,
GET_TM = 9
};
static constexpr DeviceCommandId_t REQUEST_ID = 0x77;
static constexpr DeviceCommandId_t REPLY_ID = 0x78;
enum SetIds : uint32_t { enum SetIds : uint32_t {
TEMPERATURE_SET_ID = GET_TEMPERATURE, TEMPERATURE_SET_ID = DeviceCommandId::GET_TEMPERATURE,
STATUS_SET_ID = GET_RW_STATUS, STATUS_SET_ID = DeviceCommandId::GET_RW_STATUS,
LAST_RESET_ID = GET_LAST_RESET_STATUS, LAST_RESET_ID = DeviceCommandId::GET_LAST_RESET_STATUS,
TM_SET_ID = GET_TM, TM_SET_ID = DeviceCommandId::GET_TM,
SPEED_CMD_SET = 10, SPEED_CMD_SET = 10,
}; };