diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 48c6f521..c97fd21b 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -89,7 +89,7 @@ void AcsController::copyMgmData() { { PoolReadGuard pg(&imtqMgmSet); if (pg.getReadResult() == RETURN_OK) { - std::memcpy(mgmData.imtqCal.value, imtqMgmSet.mgmXyz.value, 3 * sizeof(int32_t)); + std::memcpy(mgmData.imtqRaw.value, imtqMgmSet.mtmRawNt.value, 3 * sizeof(float)); mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value; } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 358b1871..83d9afab 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -43,14 +43,13 @@ class AcsController : public ExtendedControllerBase { MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER); RM3100::Rm3100PrimaryDataset mgm3Rm3100Set = RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER); - IMTQ::CalibratedMtmMeasurementSet imtqMgmSet = - IMTQ::CalibratedMtmMeasurementSet(objects::IMTQ_HANDLER); + IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER); PoolEntry mgm0PoolVec = PoolEntry(3); PoolEntry mgm1PoolVec = PoolEntry(3); PoolEntry mgm2PoolVec = PoolEntry(3); PoolEntry mgm3PoolVec = PoolEntry(3); - PoolEntry imtqMgmPoolVec = PoolEntry(3); + PoolEntry imtqMgmPoolVec = PoolEntry(3); PoolEntry imtqCalActStatus = PoolEntry(); void copyMgmData(); diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index bafe778f..de971cf5 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -53,7 +53,7 @@ class MgmData : public StaticLocalDataSet { lp_vec_t mgm2Lis3 = lp_vec_t(sid.objectId, MGM_2_LIS3_UT, this); lp_vec_t mgm3Rm3100 = lp_vec_t(sid.objectId, MGM_3_RM3100_UT, this); // The IMTQ measurements are in integer nT - lp_vec_t imtqCal = lp_vec_t(sid.objectId, MGM_IMTQ_CAL_NT, this); + lp_vec_t imtqRaw = lp_vec_t(sid.objectId, MGM_IMTQ_CAL_NT, this); lp_var_t actuationCalStatus = lp_var_t(sid.objectId, MGM_IMTQ_CAL_ACT_STATUS, this); diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp index 91e378ca..0d1f72db 100644 --- a/mission/devices/IMTQHandler.cpp +++ b/mission/devices/IMTQHandler.cpp @@ -335,9 +335,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat localDataPoolMap.emplace(IMTQ::ACTUATION_CAL_STATUS, new PoolEntry({0})); /** Entries of raw MTM measurement dataset */ - localDataPoolMap.emplace(IMTQ::MTM_RAW_X, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::MTM_RAW_Y, new PoolEntry({0})); - localDataPoolMap.emplace(IMTQ::MTM_RAW_Z, new PoolEntry({0})); + localDataPoolMap.emplace(IMTQ::MTM_RAW, new PoolEntry(3)); localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry({0})); /** INIT measurements for positive X axis test */ @@ -780,29 +778,45 @@ void IMTQHandler::fillCalibratedMtmDataset(const uint8_t* packet) { void IMTQHandler::fillRawMtmDataset(const uint8_t* packet) { PoolReadGuard rg(&rawMtmMeasurementSet); - 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)); + unsigned int offset = 2; + size_t deSerLen = 16; + const uint8_t* dataStart = packet + offset; + int32_t xRaw = 0; + int32_t yRaw = 0; + int32_t zRaw = 0; + uint32_t coilActStatus = 0; + auto res = + SerializeAdapter::deSerialize(&xRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); + if (res != HasReturnvaluesIF::RETURN_OK) { + return; + } + res = + SerializeAdapter::deSerialize(&yRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); + if (res != HasReturnvaluesIF::RETURN_OK) { + return; + } + res = + SerializeAdapter::deSerialize(&zRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE); + if (res != HasReturnvaluesIF::RETURN_OK) { + return; + } + res = SerializeAdapter::deSerialize(&coilActStatus, &dataStart, &deSerLen, + SerializeIF::Endianness::LITTLE); + if (res != HasReturnvaluesIF::RETURN_OK) { + return; + } + rawMtmMeasurementSet.mtmRawNt[0] = xRaw * 7.5; + rawMtmMeasurementSet.mtmRawNt[1] = yRaw * 7.5; + rawMtmMeasurementSet.mtmRawNt[2] = zRaw * 7.5; + rawMtmMeasurementSet.coilActuationStatus = static_cast(coilActStatus); + rawMtmMeasurementSet.setValidity(true, true); if (debugMode) { #if OBSW_VERBOSE_LEVEL >= 1 - sif::info << "IMTQ raw MTM measurement X: " << rawMtmMeasurementSet.mtmXnT << " nT" + sif::info << "IMTQ raw MTM measurement X: " << rawMtmMeasurementSet.mtmRawNt[0] << " nT" << std::endl; - sif::info << "IMTQ raw MTM measurement Y: " << rawMtmMeasurementSet.mtmYnT << " nT" + sif::info << "IMTQ raw MTM measurement Y: " << rawMtmMeasurementSet.mtmRawNt[1] << " nT" << std::endl; - sif::info << "IMTQ raw MTM measurement Z: " << rawMtmMeasurementSet.mtmZnT << " nT" + sif::info << "IMTQ raw MTM measurement Z: " << rawMtmMeasurementSet.mtmRawNt[2] << " nT" << std::endl; sif::info << "IMTQ coil actuation status during MTM measurement: " << (unsigned int)rawMtmMeasurementSet.coilActuationStatus.value << std::endl; diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h index e6661f84..485be5da 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -117,9 +117,7 @@ enum IMTQPoolIds : lp_id_t { MCU_TEMPERATURE, MGM_CAL_NT, ACTUATION_CAL_STATUS, - MTM_RAW_X, - MTM_RAW_Y, - MTM_RAW_Z, + MTM_RAW, ACTUATION_RAW_STATUS, INIT_POS_X_ERR, @@ -422,9 +420,7 @@ class RawMtmMeasurementSet : public StaticLocalDataSet { RawMtmMeasurementSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, RAW_MTM_SET)) {} /** The unit of all measurements is nT */ - lp_var_t mtmXnT = lp_var_t(sid.objectId, MTM_RAW_X, this); - lp_var_t mtmYnT = lp_var_t(sid.objectId, MTM_RAW_Y, this); - lp_var_t mtmZnT = lp_var_t(sid.objectId, MTM_RAW_Z, this); + lp_vec_t mtmRawNt = lp_vec_t(sid.objectId, MTM_RAW, this); /** 1 if coils were actuating during measurement otherwise 0 */ lp_var_t coilActuationStatus = lp_var_t(sid.objectId, ACTUATION_RAW_STATUS, this);