imqt base functions
This commit is contained in:
@ -6,7 +6,8 @@
|
||||
#include <fsfwconfig/OBSWConfig.h>
|
||||
|
||||
IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
|
||||
DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this) {
|
||||
DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this), calMtmMeasurementSet(
|
||||
this), rawMtmMeasurementSet(this) {
|
||||
if (comCookie == NULL) {
|
||||
sif::error << "IMTQHandler: Invalid com cookie" << std::endl;
|
||||
}
|
||||
@ -29,7 +30,28 @@ void IMTQHandler::doShutDown(){
|
||||
|
||||
ReturnValue_t IMTQHandler::buildNormalDeviceCommand(
|
||||
DeviceCommandId_t * id) {
|
||||
*id = IMTQ::GET_ENG_HK_DATA;
|
||||
switch (communicationStep) {
|
||||
case CommunicationStep::GET_ENG_HK_DATA:
|
||||
*id = IMTQ::GET_ENG_HK_DATA;
|
||||
communicationStep = CommunicationStep::START_MTM_MEASUREMENT;
|
||||
break;
|
||||
case CommunicationStep::START_MTM_MEASUREMENT:
|
||||
*id = IMTQ::START_MTM_MEASUREMENT;
|
||||
communicationStep = CommunicationStep::GET_CAL_MTM_MEASUREMENT;
|
||||
break;
|
||||
case CommunicationStep::GET_CAL_MTM_MEASUREMENT:
|
||||
*id = IMTQ::GET_CAL_MTM_MEASUREMENT;
|
||||
communicationStep = CommunicationStep::GET_RAW_MTM_MEASUREMENT;
|
||||
break;
|
||||
case CommunicationStep::GET_RAW_MTM_MEASUREMENT:
|
||||
*id = IMTQ::GET_RAW_MTM_MEASUREMENT;
|
||||
communicationStep = CommunicationStep::GET_ENG_HK_DATA;
|
||||
break;
|
||||
default:
|
||||
sif::debug << "IMTQHandler::buildNormalDeviceCommand: Invalid communication step"
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
return buildCommandFromCommand(*id, NULL, 0);
|
||||
}
|
||||
|
||||
@ -69,6 +91,24 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(
|
||||
rawPacketLen = 1;
|
||||
return RETURN_OK;
|
||||
}
|
||||
case(IMTQ::START_MTM_MEASUREMENT): {
|
||||
commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT;
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = 1;
|
||||
return RETURN_OK;
|
||||
}
|
||||
case(IMTQ::GET_CAL_MTM_MEASUREMENT): {
|
||||
commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT;
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = 1;
|
||||
return RETURN_OK;
|
||||
}
|
||||
case(IMTQ::GET_RAW_MTM_MEASUREMENT): {
|
||||
commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT;
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = 1;
|
||||
return RETURN_OK;
|
||||
}
|
||||
default:
|
||||
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
||||
}
|
||||
@ -81,7 +121,13 @@ void IMTQHandler::fillCommandAndReplyMap() {
|
||||
this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset,
|
||||
IMTQ::SIZE_ENG_HK_DATA_REPLY);
|
||||
this->insertInCommandAndReplyMap(IMTQ::GET_COMMANDED_DIPOLE, 1, nullptr,
|
||||
IMTQ::SIZE_ENG_HK_DATA_REPLY);
|
||||
IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY);
|
||||
this->insertInCommandAndReplyMap(IMTQ::START_MTM_MEASUREMENT, 1, nullptr,
|
||||
IMTQ::SIZE_STATUS_REPLY);
|
||||
this->insertInCommandAndReplyMap(IMTQ::GET_CAL_MTM_MEASUREMENT, 1, &calMtmMeasurementSet,
|
||||
IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT);
|
||||
this->insertInCommandAndReplyMap(IMTQ::GET_RAW_MTM_MEASUREMENT, 1, &rawMtmMeasurementSet,
|
||||
IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT);
|
||||
}
|
||||
|
||||
ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start,
|
||||
@ -94,6 +140,10 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start,
|
||||
*foundLen = IMTQ::SIZE_STATUS_REPLY;
|
||||
*foundId = IMTQ::START_ACTUATION_DIPOLE;
|
||||
break;
|
||||
case(IMTQ::CC::START_MTM_MEASUREMENT):
|
||||
*foundLen = IMTQ::SIZE_STATUS_REPLY;
|
||||
*foundId = IMTQ::START_MTM_MEASUREMENT;
|
||||
break;
|
||||
case(IMTQ::CC::GET_ENG_HK_DATA):
|
||||
*foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY;
|
||||
*foundId = IMTQ::GET_ENG_HK_DATA;
|
||||
@ -102,6 +152,14 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start,
|
||||
*foundLen = IMTQ::SIZE_GET_COMMANDED_DIPOLE_REPLY;
|
||||
*foundId = IMTQ::GET_COMMANDED_DIPOLE;
|
||||
break;
|
||||
case(IMTQ::CC::GET_CAL_MTM_MEASUREMENT):
|
||||
*foundLen = IMTQ::SIZE_GET_CAL_MTM_MEASUREMENT;
|
||||
*foundId = IMTQ::GET_CAL_MTM_MEASUREMENT;
|
||||
break;
|
||||
case(IMTQ::CC::GET_RAW_MTM_MEASUREMENT):
|
||||
*foundLen = IMTQ::SIZE_GET_RAW_MTM_MEASUREMENT;
|
||||
*foundId = IMTQ::GET_RAW_MTM_MEASUREMENT;
|
||||
break;
|
||||
default:
|
||||
sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl;
|
||||
result = IGNORE_REPLY_DATA;
|
||||
@ -124,7 +182,8 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
|
||||
switch (id) {
|
||||
case (IMTQ::START_ACTUATION_DIPOLE):
|
||||
// Replies only the status byte which is already handled with parseStatusByte
|
||||
case (IMTQ::START_MTM_MEASUREMENT):
|
||||
/* Replies only the status byte which is already handled with parseStatusByte */
|
||||
break;
|
||||
case (IMTQ::GET_ENG_HK_DATA):
|
||||
fillEngHkDataset(packet);
|
||||
@ -132,6 +191,12 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id,
|
||||
case (IMTQ::GET_COMMANDED_DIPOLE):
|
||||
handleGetCommandedDipoleReply(packet);
|
||||
break;
|
||||
case (IMTQ::GET_CAL_MTM_MEASUREMENT):
|
||||
fillCalibratedMtmDataset(packet);
|
||||
break;
|
||||
case (IMTQ::GET_RAW_MTM_MEASUREMENT):
|
||||
fillRawMtmDataset(packet);
|
||||
break;
|
||||
default: {
|
||||
sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
|
||||
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
|
||||
@ -152,18 +217,31 @@ uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){
|
||||
ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||
LocalDataPoolManager& poolManager) {
|
||||
|
||||
/** Entries of engineering housekeeping dataset */
|
||||
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::DIGITAL_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT, 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 }));
|
||||
|
||||
/** Entries of calibrated MTM measurement dataset */
|
||||
localDataPoolMap.emplace(IMTQ::MTM_CAL_X, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::MTM_CAL_Y, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::MTM_CAL_Z, new PoolEntry<uint32_t>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry<uint8_t>( { 0 }));
|
||||
|
||||
/** Entries of raw MTM measurement dataset */
|
||||
localDataPoolMap.emplace(IMTQ::MTM_RAW_X, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::MTM_RAW_Y, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::MTM_RAW_Z, new PoolEntry<float>( { 0 }));
|
||||
localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>( { 0 }));
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
@ -200,36 +278,36 @@ ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
|
||||
|
||||
void IMTQHandler::fillEngHkDataset(const uint8_t* packet) {
|
||||
uint8_t offset = 2;
|
||||
engHkDataset.digitalVoltageMv = *(packet + offset + 1) | *(packet + offset);
|
||||
engHkDataset.digitalVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
|
||||
offset += 2;
|
||||
engHkDataset.analogVoltageMv = *(packet + offset + 1) | *(packet + offset);
|
||||
engHkDataset.analogVoltageMv = *(packet + offset + 1) << 8 | *(packet + offset);
|
||||
offset += 2;
|
||||
engHkDataset.digitalCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
|
||||
engHkDataset.digitalCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
engHkDataset.analogCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
|
||||
engHkDataset.analogCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
engHkDataset.coilXcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
|
||||
engHkDataset.coilXCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
engHkDataset.coilYcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
|
||||
engHkDataset.coilYCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
engHkDataset.coilZcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001;
|
||||
engHkDataset.coilZCurrentmA = (*(packet + offset + 1) << 8 | *(packet + offset)) * 0.1;
|
||||
offset += 2;
|
||||
engHkDataset.coilXTemperature = (*(packet + offset + 1) | *(packet + offset));
|
||||
engHkDataset.coilXTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
offset += 2;
|
||||
engHkDataset.coilYTemperature = (*(packet + offset + 1) | *(packet + offset));
|
||||
engHkDataset.coilYTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
offset += 2;
|
||||
engHkDataset.coilZTemperature = (*(packet + offset + 1) | *(packet + offset));
|
||||
engHkDataset.coilZTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
offset += 2;
|
||||
engHkDataset.mcuTemperature = (*(packet + offset + 1) | *(packet + offset));
|
||||
engHkDataset.mcuTemperature = (*(packet + offset + 1) << 8 | *(packet + offset));
|
||||
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_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 digital current: " << engHkDataset.digitalCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ coil X current: " << engHkDataset.coilXCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ coil Y current: " << engHkDataset.coilYCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ coil Z current: " << engHkDataset.coilZCurrentmA << " mA" << std::endl;
|
||||
sif::info << "IMTQ coil X temperature: " << engHkDataset.coilXTemperature << " °C"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ coil Y temperature: " << engHkDataset.coilYTemperature << " °C"
|
||||
@ -245,15 +323,11 @@ void IMTQHandler::setModeNormal() {
|
||||
mode = MODE_NORMAL;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId) {
|
||||
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
|
||||
if (wiretappingMode == RAW) {
|
||||
/* Data already sent in doGetRead() */
|
||||
return result;
|
||||
return;
|
||||
}
|
||||
|
||||
DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
|
||||
@ -267,8 +341,71 @@ void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCom
|
||||
return;
|
||||
}
|
||||
|
||||
result = actionHelper.reportData(queueId, replyId, data, dataSize);
|
||||
ReturnValue_t result = actionHelper.reportData(queueId, replyId, data, dataSize);
|
||||
if (result != RETURN_OK) {
|
||||
sif::debug << "IMTQHandler::handleDeviceTM: Failed to report data" << std::endl;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void IMTQHandler::handleGetCommandedDipoleReply(const uint8_t* packet) {
|
||||
uint8_t tmData[6];
|
||||
/* Switching endianess of received dipole values */
|
||||
tmData[0] = *(packet + 3);
|
||||
tmData[1] = *(packet + 2);
|
||||
tmData[2] = *(packet + 5);
|
||||
tmData[3] = *(packet + 4);
|
||||
tmData[4] = *(packet + 7);
|
||||
tmData[5] = *(packet + 6);
|
||||
handleDeviceTM(tmData, sizeof(tmData), IMTQ::GET_COMMANDED_DIPOLE);
|
||||
}
|
||||
|
||||
void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) {
|
||||
int8_t offset = 2;
|
||||
calMtmMeasurementSet.mtmXnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||
offset += 4;
|
||||
calMtmMeasurementSet.mtmYnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||
offset += 4;
|
||||
calMtmMeasurementSet.mtmZnT = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
| *(packet + offset + 1) << 8 | *(packet + offset);
|
||||
offset += 4;
|
||||
calMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24)
|
||||
| (*(packet + offset + 2) << 16) | (*(packet + offset + 1) << 8) | (*(packet + offset));
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1
|
||||
sif::info << "IMTQ calibrated MTM measurement X: " << calMtmMeasurementSet.mtmXnT << " nT"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ calibrated MTM measurement Y: " << calMtmMeasurementSet.mtmYnT << " nT"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ calibrated MTM measurement Z: " << calMtmMeasurementSet.mtmZnT << " nT"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ coil actuation status during MTM measurement: "
|
||||
<< (unsigned int) calMtmMeasurementSet.coilActuationStatus.value << std::endl;
|
||||
#endif
|
||||
}
|
||||
|
||||
void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) {
|
||||
int8_t offset = 2;
|
||||
rawMtmMeasurementSet.mtmXnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
| *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
|
||||
offset += 4;
|
||||
rawMtmMeasurementSet.mtmYnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
| *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
|
||||
offset += 4;
|
||||
rawMtmMeasurementSet.mtmZnT = (*(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
| *(packet + offset + 1) << 8 | *(packet + offset)) * 7.5;
|
||||
offset += 4;
|
||||
rawMtmMeasurementSet.coilActuationStatus = (*(packet + offset + 3) << 24)
|
||||
| (*(packet + offset + 2) << 16) | (*(packet + offset + 1) << 8) | (*(packet + offset));
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && IMQT_DEBUG == 1
|
||||
sif::info << "IMTQ raw MTM measurement X: " << rawMtmMeasurementSet.mtmXnT << " nT"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ raw MTM measurement Y: " << rawMtmMeasurementSet.mtmYnT << " nT"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ raw MTM measurement Z: " << rawMtmMeasurementSet.mtmZnT << " nT"
|
||||
<< std::endl;
|
||||
sif::info << "IMTQ coil actuation status during MTM measurement: "
|
||||
<< (unsigned int) rawMtmMeasurementSet.coilActuationStatus.value << std::endl;
|
||||
#endif
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user