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

plocHandler wip

Browse Source
This commit is contained in:
Jakob Meier
2021-04-11 12:04:13 +02:00
parent 699b4a0eb9
commit 400f60c7be
11 changed files with 365 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

262
mission/devices/PlocHandler.cpp
View File

@ -6,7 +6,7 @@
#include <fsfwconfig/OBSWConfig.h>
PlocHandler::PlocHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this) {
DeviceHandlerBase(objectId, comIF, comCookie) {
if (comCookie == NULL) {
sif::error << "PlocHandler: Invalid com cookie" << std::endl;
}
@ -28,8 +28,7 @@ void PlocHandler::doShutDown(){
ReturnValue_t PlocHandler::buildNormalDeviceCommand(
DeviceCommandId_t * id) {
*id = IMTQ::GET_ENG_HK_DATA;
return buildCommandFromCommand(*id, NULL, 0);
return RETURN_OK;
}
ReturnValue_t PlocHandler::buildTransitionDeviceCommand(
@ -41,34 +40,37 @@ ReturnValue_t PlocHandler::buildCommandFromCommand(
DeviceCommandId_t deviceCommand, const uint8_t * commandData,
size_t commandDataLen) {
switch(deviceCommand) {
case(IMTQ::GET_ENG_HK_DATA): {
commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
rawPacket = commandBuffer;
case(PLOC::TC_MEM_WRITE): {
const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16
| *(commandData + 2) << 8 | *(commandData + 3);
const uint32_t memoryData = *(commandData + 4) << 24 | *(commandData + 5) << 16
| *(commandData + 6) << 8 | *(commandData + 7);
PLOC::TcMemWrite tcMemWrite(memoryAddress, memoryData);
rawPacket = tcMemWrite.getWholeData();
rawPacketLen = tcMemWrite.getFullSize();
rememberCommandId = PLOC::TC_MEM_WRITE;
return RETURN_OK;
}
case(IMTQ::START_ACTUATION_DIPOLE): {
/* IMTQ expects low byte first */
commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE;
commandBuffer[1] = *(commandData + 1);
commandBuffer[2] = *(commandData);
commandBuffer[3] = *(commandData + 3);
commandBuffer[4] = *(commandData + 2);
commandBuffer[5] = *(commandData + 5);
commandBuffer[6] = *(commandData + 4);
commandBuffer[7] = *(commandData + 7);
commandBuffer[8] = *(commandData + 6);
rawPacket = commandBuffer;
case(PLOC::TC_MEM_READ): {
const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16
| *(commandData + 2) << 8 | *(commandData + 3);
PLOC::TcMemRead tcMemRead(memoryAddress);
rawPacket = tcMemRead.getWholeData();
rawPacketLen = tcMemRead.getFullSize();
rememberCommandId = PLOC::TC_MEM_READ;
return RETURN_OK;
}
default:
sif::debug << "PlocHandler::buildCommandFromCommand: Command not implemented" << std::endl;
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
return HasReturnvaluesIF::RETURN_FAILED;
}
void PlocHandler::fillCommandAndReplyMap() {
this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
IMTQ::SIZE_ENG_HK_DATA_REPLY, false, true, IMTQ::SIZE_ENG_HK_DATA_REPLY);
this->insertInCommandAndReplyMap(PLOC::TC_MEM_READ, 1, nullptr, PLOC::SIZE_ACK_REPORT);
this->insertInReplyMap(PLOC::ACK_SUCCESS, 1);
this->insertInReplyMap(PLOC::ACK_FAILURE, 1);
}
ReturnValue_t PlocHandler::scanForReply(const uint8_t *start,
@ -76,34 +78,51 @@ ReturnValue_t PlocHandler::scanForReply(const uint8_t *start,
ReturnValue_t result = RETURN_OK;
switch(*start) {
case(IMTQ::CC::GET_ENG_HK_DATA):
*foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY;
*foundId = IMTQ::GET_ENG_HK_DATA;
*foundId = *(start) << 8 | *(start + 1) & APID_MASK;
switch(*foundId) {
case(PLOC::ACK_SUCCESS):
*foundLen = PLOC::SIZE_ACK_REPORT;
break;
case(PLOC::ACK_FAILURE):
*foundLen = PLOC::SIZE_ACK_REPORT;
break;
default:
sif::debug << "PlocHandler::scanForReply: Reply contains invalid command code" << std::endl;
sif::debug << "PlocHandler::scanForReply: Reply has invalid apid" << std::endl;
result = IGNORE_REPLY_DATA;
break;
}
result = verifyPacket(start, foundLen);
return result;
}
ReturnValue_t PlocHandler::verifyPacket(const uint8_t* start, size_t* foundLen) {
uint16_t receivedCrc = *(start + *foundLen - 2) << 8 | *(start + *foundlen - 1);
if (receivedCrc != CRC::crc16ccitt(start, *foundLen, 0)) {
return CRC_FAILURE;
}
return RETURN_OK;
}
ReturnValue_t PlocHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
ReturnValue_t result = RETURN_OK;
result = parseStatusByte(packet);
if (result != RETURN_OK) {
return result;
}
switch (id) {
case (IMTQ::GET_ENG_HK_DATA):
fillEngHkDataset(packet);
case (PLOC::ACK_SUCCESS):
receiveTm();
receiveExecutionReport();
break;
case (PLOC::ACK_FAILURE):
//TODO: Interpretation of status field in ack reply.
sif::error << "PlocHandler::interpretDeviceReply: Received ack failure reply" << std::endl;
triggerEvent(ACK_FAILURE, rememberCommandId);
break;
default: {
sif::debug << "PlocHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
@ -114,71 +133,116 @@ ReturnValue_t PlocHandler::interpretDeviceReply(DeviceCommandId_t id,
return RETURN_OK;
}
ReturnValue_t PlocHandler::parseStatusByte(const uint8_t* packet) {
uint8_t cmdErrorField = *(packet + 1) & 0xF;
switch (cmdErrorField) {
case 0:
return RETURN_OK;
case 1:
return REJECTED_WITHOUT_REASON;
case 2:
return INVALID_COMMAND_CODE;
case 3:
return PARAMETER_MISSING;
case 4:
return PARAMETER_INVALID;
case 5:
return CC_UNAVAILABLE;
case 7:
return INTERNAL_PROCESSING_ERROR;
ReturnValue_t PlocHandler::receiveTm() {
switch (rememberCommandId) {
case (PLOC::TC_MEM_WRITE):
break;
case (PLOC::TC_MEM_READ):
receiveTmMemoryReadReport();
break;
default:
sif::error << "PlocHandler::parseStatusByte: CMD Error field contains unknown error code "
<< cmdErrorField << std::endl;
return CMD_ERR_UNKNOWN;
sif::debug << "PlocHandler::receiveTm: Rembered unknown command id" << std::endl;
break;
}
}
void PlocHandler::fillEngHkDataset(const uint8_t* packet) {
uint8_t offset = 2;
engHkDataset.digitalVoltageMv = *(packet + offset + 1) | *(packet + offset);
offset += 2;
engHkDataset.analogVoltageMv = *(packet + offset + 1) | *(packet + offset);
offset += 2;
engHkDataset.digitalCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
offset += 2;
engHkDataset.analogCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
offset += 2;
engHkDataset.coilXcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
offset += 2;
engHkDataset.coilYcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
offset += 2;
engHkDataset.coilZcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
offset += 2;
engHkDataset.coilXTemperature = (*(packet + offset + 1) | *(packet + offset));
offset += 2;
engHkDataset.coilYTemperature = (*(packet + offset + 1) | *(packet + offset));
offset += 2;
engHkDataset.coilZTemperature = (*(packet + offset + 1) | *(packet + offset));
offset += 2;
engHkDataset.mcuTemperature = (*(packet + offset + 1) | *(packet + offset));
void PlocHandler::receiveExecutionReport() {
#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1
sif::info << "IMTQ digital voltage: " << engHkDataset.digitalVoltageMv << " mV" << std::endl;
sif::info << "IMTQ analog voltage: " << engHkDataset.analogVoltageMv << " mV" << std::endl;
sif::info << "IMTQ digital current: " << engHkDataset.digitalCurrentA << " A" << std::endl;
sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentA << " A" << std::endl;
sif::info << "IMTQ coil X current: " << engHkDataset.coilXcurrentA << " A" << std::endl;
sif::info << "IMTQ coil Y current: " << engHkDataset.coilYcurrentA << " A" << std::endl;
sif::info << "IMTQ coil Z current: " << engHkDataset.coilZcurrentA << " A" << std::endl;
sif::info << "IMTQ coil X temperature: " << engHkDataset.coilXTemperature << " °C"
<< std::endl;
sif::info << "IMTQ coil Y temperature: " << engHkDataset.coilYTemperature << " °C"
<< std::endl;
sif::info << "IMTQ coil Z temperature: " << engHkDataset.coilZTemperature << " °C"
<< std::endl;
sif::info << "IMTQ coil MCU temperature: " << engHkDataset.mcuTemperature << " °C"
<< std::endl;
#endif
size_t receivedDataLen = 0;
uint8_t *receivedData = nullptr;
communicationInterface->requestReceiveMessage(comCookie, PLOC::SIZE_EXE_REPORT);
communicationInterface->readReceivedMessage(comCookie, &receivedData, &receivedDataLen);
if(!verifyPacket(receivedData, receivedDataLen)) {
replyRawData(data, len, defaultRawReceiver);
triggerEvent(EXE_RPT_INVALID_CRC);
sif::error << "PlocHandler::receiveExecutionReport: Execution report has invalid crc"
<< std::endl;
return;
}
handleExecutionReport(receivedData, receivedDataLen);
}
void PlocHandler::handleExecutionReport(const uint8_t* receivedData, size_t receivedDataLen) {
uint16_t apid = *(start) << 8 | *(start + 1) & APID_MASK;
switch (apid) {
case (PLOC::APID_EXE_SUCCESS): {
return;
}
case (PLOC::APID_EXE_FAILURE): {
//TODO: Interpretation of status field in execution report
sif::error << "PlocHandler::handleExecutionReport: Received execution failure report"
<< std::endl;
triggerEvent(EXE_FAILURE, rememberCommandId);
return;
}
}
return;
}
void PlocHandler::receiveTmMemoryReadReport() {
size_t receivedDataLen = 0;
uint8_t *receivedData = nullptr;
/* Receiving the telemetry packet */
ReturnValue_t result = communicationInterface->requestReceiveMessage(comCookie,
PLOC::SIZE_TM_MEM_READ_REPORT);
if (result != RETURN_OK) {
sif::error << "PlocHandler::receiveExecutionReport: Failed to request memory read telemetry "
<< std::endl;
triggerEvent(REQUESTING_TM_READ_REPORT_FAILED, result);
return;
}
result = communicationInterface->readReceivedMessage(comCookie, &receivedData, &receivedDataLen);
if (result != RETURN_OK) {
sif::error << "PlocHandler::receiveExecutionReport: Failed to request memory read telemetry "
<< std::endl;
return;
}
uint16_t apid = *(start) << 8 | *(start + 1) & APID_MASK;
if (apid != PLOC::APID_TM_READ_REPORT) {
sif::error << "PlocHandler::receiveTmReadReport: Tm read report has invalid apid"
<< std::endl;
return;
}
if(!verifyPacket(receivedData, receivedDataLen)) {
replyRawData(data, len, defaultRawReceiver);
triggerEvent(TM_READ_RPT_INVALID_CRC);
sif::error << "PlocHandler::receiveTmReadReport: TM read report has invalid crc"
<< std::endl;
return;
}
handleDeviceTM(receivedData, receivedDataLen, PLOC::TC_MEM_READ);
}
void PlocHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
ReturnValue_t result = RETURN_OK;
DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
if (iter == deviceReplyMap.end()) {
sif::debug << "PlocHandler::handleDeviceTM: Unknown reply id" << std::endl;
return;
}
MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;
if (queueId == NO_COMMANDER) {
return;
}
result = actionHelper.reportData(queueId, replyId, dataSet);
if (result != RETURN_OK) {
sif::debug << "PlocHandler::handleDeviceTM: Failed to report data" << std::endl;
}
}
void PlocHandler::setNormalDatapoolEntriesInvalid(){
@ -192,18 +256,6 @@ uint32_t PlocHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
ReturnValue_t PlocHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::ANALOG_VOLTAGE_MV, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::DIGITAL_CURRENT_A, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT_A, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT_A, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT_A, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT_A, new PoolEntry<float>( { 0 }));
localDataPoolMap.emplace(IMTQ::COIL_X_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::COIL_Y_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry<uint16_t>( { 0 }));
return HasReturnvaluesIF::RETURN_OK;
}