Merge pull request 'prep next patch' (#841) from prep_v7.5.3 into main

Reviewed-on: #841

CHANGELOG.md

@@ -16,6 +16,18 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
+# [v7.5.3] 2023-12-19
+
+## Fixed
+
+- Set STR quaternions to invalid in device handler if the solution is not trustworthy.
+
+# [v7.5.2] 2023-12-14
+
+## Fixed
+
+- Fixed faulty scaling within the QUEST algorithm.
+
 # [v7.5.1] 2023-12-13
 
 - `eive-tmtc` v5.12.1

CMakeLists.txt

@@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13)
 
 set(OBSW_VERSION_MAJOR 7)
 set(OBSW_VERSION_MINOR 5)
-set(OBSW_VERSION_REVISION 1)
+set(OBSW_VERSION_REVISION 3)
 
 # set(CMAKE_VERBOSE TRUE)
 

mission/acs/str/StarTrackerHandler.cpp

@@ -1162,7 +1162,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
       break;
     }
     case (startracker::REQ_SOLUTION): {
-      result = handleTm(packet, solutionSet, "REQ_SOLUTION");
+      result = handleSolution(packet);
       break;
     }
     case (startracker::REQ_CONTRAST): {
@@ -2438,6 +2438,36 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat
   return result;
 }
 
+ReturnValue_t StarTrackerHandler::handleSolution(const uint8_t* rawFrame) {
+  ReturnValue_t result = statusFieldCheck(rawFrame);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  PoolReadGuard pg(&solutionSet);
+  if (pg.getReadResult() != returnvalue::OK) {
+    return result;
+  }
+  const uint8_t* reply = rawFrame + TICKS_OFFSET;
+  solutionSet.setValidityBufferGeneration(false);
+  size_t sizeLeft = fullPacketLen;
+  result = solutionSet.deSerialize(&reply, &sizeLeft, SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    sif::warning << "StarTrackerHandler::handleTm: Deserialization failed for solution set: 0x"
+                 << std::hex << std::setw(4) << result << std::dec << std::endl;
+  }
+  solutionSet.setValidityBufferGeneration(true);
+  solutionSet.setValidity(true, true);
+  solutionSet.caliQw.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.caliQx.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.caliQy.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.caliQz.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.trackQw.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.trackQx.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.trackQy.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.trackQz.setValid(solutionSet.isTrustWorthy.value);
+  return result;
+}
+
 ReturnValue_t StarTrackerHandler::handleAutoBlobTm(const uint8_t* rawFrame) {
   ReturnValue_t result = statusFieldCheck(rawFrame);
   if (result != returnvalue::OK) {

mission/acs/str/StarTrackerHandler.h

@@ -527,6 +527,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
   ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset,
                          const char* context);
 
+  ReturnValue_t handleSolution(const uint8_t* rawFrame);
   ReturnValue_t handleAutoBlobTm(const uint8_t* rawFrame);
   ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame);
   ReturnValue_t handleBlobTm(const uint8_t* rawFrame);

mission/controller/acs/AttitudeEstimation.cpp

@@ -77,24 +77,22 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
     qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
     // Rotational Vector Part
     VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
-    VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
-    VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
+    VectorOperations<double>::mulScalar(helperSum, beta, qRotVecPt1, 3);
     VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
     std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
 
-    VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
+    VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 4);
     QuaternionOperations::normalize(qBI, qBI);
   } else {
     // Scalar Part
     qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
     // Rotational Vector Part
     VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
-    VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
-    VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
+    VectorOperations<double>::mulScalar(helperSum, gamma - alpha, qRotVecPt1, 3);
     VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
     std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
 
-    VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
+    VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 4);
     QuaternionOperations::normalize(qBI, qBI);
   }
   // Low Pass