Merge remote-tracking branch 'origin/develop' into irini

mission/devices/IMTQHandler.cpp

@@ -76,48 +76,48 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
       commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_POSITIVE;
       rawPacket = commandBuffer;
       rawPacketLen = 2;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     case (IMTQ::NEG_X_SELF_TEST): {
       commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
       commandBuffer[1] = IMTQ::SELF_TEST_AXIS::X_NEGATIVE;
       rawPacket = commandBuffer;
       rawPacketLen = 2;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     case (IMTQ::POS_Y_SELF_TEST): {
       commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
       commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_POSITIVE;
       rawPacket = commandBuffer;
       rawPacketLen = 2;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     case (IMTQ::NEG_Y_SELF_TEST): {
       commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
       commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Y_NEGATIVE;
       rawPacket = commandBuffer;
       rawPacketLen = 2;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     case (IMTQ::POS_Z_SELF_TEST): {
       commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
       commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_POSITIVE;
       rawPacket = commandBuffer;
       rawPacketLen = 2;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     case (IMTQ::NEG_Z_SELF_TEST): {
       commandBuffer[0] = IMTQ::CC::SELF_TEST_CMD;
       commandBuffer[1] = IMTQ::SELF_TEST_AXIS::Z_NEGATIVE;
       rawPacket = commandBuffer;
       rawPacketLen = 2;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     case (IMTQ::GET_SELF_TEST_RESULT): {
       commandBuffer[0] = IMTQ::CC::GET_SELF_TEST_RESULT;
       rawPacket = commandBuffer;
       rawPacketLen = 1;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     case (IMTQ::START_ACTUATION_DIPOLE): {
       /* IMTQ expects low byte first */
@@ -135,42 +135,42 @@ ReturnValue_t IMTQHandler::buildCommandFromCommand(DeviceCommandId_t deviceComma
       commandBuffer[8] = commandData[6];
       rawPacket = commandBuffer;
       rawPacketLen = 9;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     case (IMTQ::GET_ENG_HK_DATA): {
       commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA;
       rawPacket = commandBuffer;
       rawPacketLen = 1;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     case (IMTQ::GET_COMMANDED_DIPOLE): {
       commandBuffer[0] = IMTQ::CC::GET_COMMANDED_DIPOLE;
       rawPacket = commandBuffer;
       rawPacketLen = 1;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     case (IMTQ::START_MTM_MEASUREMENT): {
       commandBuffer[0] = IMTQ::CC::START_MTM_MEASUREMENT;
       rawPacket = commandBuffer;
       rawPacketLen = 1;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     case (IMTQ::GET_CAL_MTM_MEASUREMENT): {
       commandBuffer[0] = IMTQ::CC::GET_CAL_MTM_MEASUREMENT;
       rawPacket = commandBuffer;
       rawPacketLen = 1;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     case (IMTQ::GET_RAW_MTM_MEASUREMENT): {
       commandBuffer[0] = IMTQ::CC::GET_RAW_MTM_MEASUREMENT;
       rawPacket = commandBuffer;
       rawPacketLen = 1;
-      return RETURN_OK;
+      return returnvalue::OK;
     }
     default:
       return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
   }
-  return HasReturnvaluesIF::RETURN_FAILED;
+  return returnvalue::FAILED;
 }
 
 void IMTQHandler::fillCommandAndReplyMap() {
@@ -198,7 +198,7 @@ void IMTQHandler::fillCommandAndReplyMap() {
 
 ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSize,
                                         DeviceCommandId_t* foundId, size_t* foundLen) {
-  ReturnValue_t result = RETURN_OK;
+  ReturnValue_t result = returnvalue::OK;
 
   switch (*start) {
     case (IMTQ::CC::START_ACTUATION_DIPOLE):
@@ -243,11 +243,11 @@ ReturnValue_t IMTQHandler::scanForReply(const uint8_t* start, size_t remainingSi
 }
 
 ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
-  ReturnValue_t result = RETURN_OK;
+  ReturnValue_t result = returnvalue::OK;
 
   result = parseStatusByte(packet);
 
-  if (result != RETURN_OK) {
+  if (result != returnvalue::OK) {
     return result;
   }
 
@@ -283,7 +283,7 @@ ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, const uint
     }
   }
 
-  return RETURN_OK;
+  return returnvalue::OK;
 }
 
 void IMTQHandler::setNormalDatapoolEntriesInvalid() {}
@@ -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 */
@@ -610,7 +608,7 @@ ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDat
       subdp::DiagnosticsHkPeriodicParams(calMtmMeasurementSet.getSid(), false, 10.0));
   poolManager.subscribeForDiagPeriodicPacket(
       subdp::DiagnosticsHkPeriodicParams(rawMtmMeasurementSet.getSid(), false, 10.0));
-  return HasReturnvaluesIF::RETURN_OK;
+  return returnvalue::OK;
 }
 
 ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
@@ -629,14 +627,14 @@ ReturnValue_t IMTQHandler::getSelfTestCommandId(DeviceCommandId_t* id) {
                  << "command" << std::endl;
       return UNEXPECTED_SELF_TEST_REPLY;
   }
-  return RETURN_OK;
+  return returnvalue::OK;
 }
 
 ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) {
   uint8_t cmdErrorField = *(packet + 1) & 0xF;
   switch (cmdErrorField) {
     case 0:
-      return RETURN_OK;
+      return returnvalue::OK;
     case 1:
       sif::error << "IMTQHandler::parseStatusByte: Command rejected without reason" << std::endl;
       return REJECTED_WITHOUT_REASON;
@@ -730,7 +728,7 @@ void IMTQHandler::handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCom
   }
 
   ReturnValue_t result = actionHelper.reportData(queueId, replyId, data, dataSize);
-  if (result != RETURN_OK) {
+  if (result != returnvalue::OK) {
     sif::debug << "IMTQHandler::handleDeviceTM: Failed to report data" << std::endl;
     return;
   }
@@ -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 != returnvalue::OK) {
+    return;
+  }
+  res =
+      SerializeAdapter::deSerialize(&yRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE);
+  if (res != returnvalue::OK) {
+    return;
+  }
+  res =
+      SerializeAdapter::deSerialize(&zRaw, &dataStart, &deSerLen, SerializeIF::Endianness::LITTLE);
+  if (res != returnvalue::OK) {
+    return;
+  }
+  res = SerializeAdapter::deSerialize(&coilActStatus, &dataStart, &deSerLen,
+                                      SerializeIF::Endianness::LITTLE);
+  if (res != returnvalue::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;
@@ -2215,7 +2229,7 @@ ReturnValue_t IMTQHandler::getSwitches(const uint8_t** switches, uint8_t* number
   if (switcher != power::NO_SWITCH) {
     *numberOfSwitches = 1;
     *switches = &switcher;
-    return RETURN_OK;
+    return returnvalue::OK;
   }
   return DeviceHandlerBase::NO_SWITCH;
 }