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

ACS IMTQ Update #284

Merged
muellerr merged 8 commits from mueller/acs-imtq-update into develop 2022-08-24 16:56:52 +02:00
Conversation 0 Commits 8 Files Changed 5 +42 -33
5 changed files with 42 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mission/controller/AcsController.cpp
View File

@ -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;
}
}

5
mission/controller/AcsController.h
View File

@ -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<float> mgm0PoolVec = PoolEntry<float>(3);
PoolEntry<float> mgm1PoolVec = PoolEntry<float>(3);
PoolEntry<float> mgm2PoolVec = PoolEntry<float>(3);
PoolEntry<float> mgm3PoolVec = PoolEntry<float>(3);
PoolEntry<int32_t> imtqMgmPoolVec = PoolEntry<int32_t>(3);
PoolEntry<float> imtqMgmPoolVec = PoolEntry<float>(3);
PoolEntry<uint8_t> imtqCalActStatus = PoolEntry<uint8_t>();
void copyMgmData();

2
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -53,7 +53,7 @@ class MgmData : public StaticLocalDataSet<MGM_SET_ENTRIES> {
lp_vec_t<float, 3> mgm2Lis3 = lp_vec_t<float, 3>(sid.objectId, MGM_2_LIS3_UT, this);
lp_vec_t<float, 3> mgm3Rm3100 = lp_vec_t<float, 3>(sid.objectId, MGM_3_RM3100_UT, this);
// The IMTQ measurements are in integer nT
lp_vec_t<int32_t, 3> imtqCal = lp_vec_t<int32_t, 3>(sid.objectId, MGM_IMTQ_CAL_NT, this);
lp_vec_t<float, 3> imtqRaw = lp_vec_t<float, 3>(sid.objectId, MGM_IMTQ_CAL_NT, this);
lp_var_t<uint8_t> actuationCalStatus =
lp_var_t<uint8_t>(sid.objectId, MGM_IMTQ_CAL_ACT_STATUS, this);

58
mission/devices/IMTQHandler.cpp
View File

@ -335,9 +335,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat
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::MTM_RAW, new PoolEntry<float>(3));
localDataPoolMap.emplace(IMTQ::ACTUATION_RAW_STATUS, new PoolEntry<uint8_t>({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<uint8_t>(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;

8
mission/devices/devicedefinitions/IMTQHandlerDefinitions.h
View File

@ -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<CAL_MTM_POOL_ENTRIES> {
RawMtmMeasurementSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, RAW_MTM_SET)) {}
/** The unit of all measurements is nT */
lp_var_t<float> mtmXnT = lp_var_t<float>(sid.objectId, MTM_RAW_X, this);
lp_var_t<float> mtmYnT = lp_var_t<float>(sid.objectId, MTM_RAW_Y, this);
lp_var_t<float> mtmZnT = lp_var_t<float>(sid.objectId, MTM_RAW_Z, this);
lp_vec_t<float, 3> mtmRawNt = lp_vec_t<float, 3>(sid.objectId, MTM_RAW, this);
/** 1 if coils were actuating during measurement otherwise 0 */
lp_var_t<uint8_t> coilActuationStatus =
lp_var_t<uint8_t>(sid.objectId, ACTUATION_RAW_STATUS, this);