STR extensions

mission/acs/str/StarTrackerHandler.cpp

@@ -607,6 +607,10 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
       result = prepareRequestLisaParams();
       return result;
     }
+    case (startracker::REQ_MATCHED_CENTROIDS): {
+      result = prepareRequestMatchedCentroids();
+      return result;
+    }
     case (startracker::REQ_MATCHING): {
       result = prepareRequestMatchingParams();
       return result;
@@ -729,6 +733,8 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
   this->insertInCommandAndReplyMap(startracker::REQ_DEBUG_CAMERA, 3, &debugCameraSet,
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
+  this->insertInCommandAndReplyMap(startracker::REQ_MATCHED_CENTROIDS, 3, &matchedCentroids,
+                                   startracker::MAX_FRAME_SIZE * 2 + 2);
 }
 
 ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
@@ -860,6 +866,16 @@ void StarTrackerHandler::setUpJsonCfgs(JsonConfigs& cfgs, const char* paramJsonF
   JCFG_DONE = true;
 }
 
+ReturnValue_t StarTrackerHandler::statusFieldCheck(const uint8_t* rawFrame) {
+  uint8_t status = startracker::getStatusField(rawFrame);
+  if (status != startracker::STATUS_OK) {
+    sif::warning << "StarTrackerHandler::handleTm: Reply error: "
+                 << static_cast<unsigned int>(status) << std::endl;
+    return REPLY_ERROR;
+  }
+  return returnvalue::OK;
+}
+
 void StarTrackerHandler::bootBootloader() {
   if (internalState == InternalState::IDLE) {
     internalState = InternalState::BOOT_BOOTLOADER;
@@ -896,24 +912,24 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t* start, size_t rema
     }
     case TMTC_ACTIONREPLY: {
       *foundLen = remainingSize;
+      fullPacketLen = remainingSize;
       return scanForActionReply(startracker::getId(start), foundId);
-      break;
     }
     case TMTC_SETPARAMREPLY: {
       *foundLen = remainingSize;
+      fullPacketLen = remainingSize;
       return scanForSetParameterReply(startracker::getId(start), foundId);
-      break;
     }
     case TMTC_PARAMREPLY: {
       *foundLen = remainingSize;
+      fullPacketLen = remainingSize;
       return scanForGetParameterReply(startracker::getId(start), foundId);
-      break;
     }
     case TMTC_TELEMETRYREPLYA:
     case TMTC_TELEMETRYREPLY: {
       *foundLen = remainingSize;
+      fullPacketLen = remainingSize;
       return scanForTmReply(startracker::getId(start), foundId);
-      break;
     }
     default: {
       sif::debug << "StarTrackerHandler::scanForReply: Reply has invalid type id" << std::endl;
@@ -970,6 +986,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
       result = handleTm(packet, solutionSet, startracker::SolutionSet::SIZE, "REQ_SOLUTION");
       break;
     }
+    case (startracker::REQ_MATCHED_CENTROIDS): {
+      result = handleMatchedCentroidTm(packet);
+      break;
+    }
     case (startracker::REQ_TEMPERATURE): {
       result = handleTm(packet, temperatureSet, startracker::TemperatureSet::SIZE, "REQ_TEMP");
       break;
@@ -1032,6 +1052,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
       handleParamRequest(packet, trackingSet, startracker::TrackingSet::SIZE);
       break;
     }
+    case (startracker::REQ_MATCHED_CENTROIDS): {
+      handleParamRequest(packet, matchedCentroids, startracker::MatchedCentroidsSet::SIZE);
+      break;
+    }
     case (startracker::REQ_VALIDATION): {
       handleParamRequest(packet, validationSet, startracker::ValidationSet::SIZE);
       break;
@@ -1328,6 +1352,20 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
   localDataPoolMap.emplace(startracker::DEBUG_CAMERA_TEST, new PoolEntry<uint32_t>({0}));
 
   localDataPoolMap.emplace(startracker::CHKSUM, new PoolEntry<uint32_t>({0}));
+  localDataPoolMap.emplace(startracker::PoolIds::NUM_MATCHED_CENTROIDS, new PoolEntry<uint8_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_STAR_IDS,
+                           new PoolEntry<uint32_t>(16));
+  localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_X_COORDS,
+                           new PoolEntry<float>(16));
+  localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_Y_COORDS,
+                           new PoolEntry<float>(16));
+  localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_X_ERRORS,
+                           new PoolEntry<float>(16));
+  localDataPoolMap.emplace(startracker::PoolIds::MATCHED_CENTROIDS_Y_ERRORS,
+                           new PoolEntry<float>(16));
+  localDataPoolMap.emplace(startracker::PoolIds::TICKS_MATCHED_CENTROIDS,
+                           new PoolEntry<uint32_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::TIME_MATCHED_CENTROIDS, new PoolEntry<uint64_t>());
 
   poolManager.subscribeForDiagPeriodicPacket(
       subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0));
@@ -1760,6 +1798,14 @@ void StarTrackerHandler::prepareHistogramRequest() {
   rawPacketLen = length;
 }
 
+ReturnValue_t StarTrackerHandler::prepareRequestMatchedCentroids() {
+  uint32_t length = 0;
+  arc_tm_pack_matchedcentroids_req(commandBuffer, &length);
+  rawPacket = commandBuffer;
+  rawPacketLen = length;
+  return returnvalue::OK;
+}
+
 ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData,
                                                       size_t commandDataLen,
                                                       ArcsecJsonParamBase& paramSet,
@@ -2039,12 +2085,9 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
 
 ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset,
                                            size_t size, const char* context) {
-  ReturnValue_t result = returnvalue::OK;
+  ReturnValue_t result = statusFieldCheck(rawFrame);
-  uint8_t status = startracker::getStatusField(rawFrame);
+  if (result != returnvalue::OK) {
-  if (status != startracker::STATUS_OK) {
+    return result;
-    sif::warning << "StarTrackerHandler::handleTm: Reply error: "
-                 << static_cast<unsigned int>(status) << std::endl;
-    return REPLY_ERROR;
   }
   result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
   if (result != returnvalue::OK) {
@@ -2069,6 +2112,66 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat
   return result;
 }
 
+ReturnValue_t StarTrackerHandler::handleMatchedCentroidTm(const uint8_t* rawFrame) {
+  ReturnValue_t result = statusFieldCheck(rawFrame);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  const uint8_t* reply = rawFrame + TICKS_OFFSET;
+  size_t remainingLen = fullPacketLen;
+  PoolReadGuard pg(&matchedCentroids);
+  result = pg.getReadResult();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&matchedCentroids.ticks, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&matchedCentroids.timeUs, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&matchedCentroids.numberOfMatchedCentroids, &rawFrame,
+                                         &remainingLen, SerializeIF::Endianness::LITTLE);
+
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  // Yeah, we serialize it like that because I can't model anything with that local datapool crap.
+  for (unsigned idx = 0; idx < 16; idx++) {
+    result = SerializeAdapter::deSerialize(&matchedCentroids.starIds[idx], &rawFrame, &remainingLen,
+                                           SerializeIF::Endianness::LITTLE);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+    result = SerializeAdapter::deSerialize(&matchedCentroids.xCoords[idx], &rawFrame, &remainingLen,
+                                           SerializeIF::Endianness::LITTLE);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+    result = SerializeAdapter::deSerialize(&matchedCentroids.yCoords[idx], &rawFrame, &remainingLen,
+                                           SerializeIF::Endianness::LITTLE);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+    result = SerializeAdapter::deSerialize(&matchedCentroids.xErrors[idx], &rawFrame, &remainingLen,
+                                           SerializeIF::Endianness::LITTLE);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+    result = SerializeAdapter::deSerialize(&matchedCentroids.yErrors[idx], &rawFrame, &remainingLen,
+                                           SerializeIF::Endianness::LITTLE);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+  }
+  matchedCentroids.setValidity(true, true);
+  return returnvalue::OK;
+}
+
 ReturnValue_t StarTrackerHandler::handleActionReplySet(const uint8_t* rawFrame,
                                                        LocalPoolDataSetBase& dataset, size_t size) {
   ReturnValue_t result = returnvalue::OK;
@@ -2204,6 +2307,7 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
     case startracker::REQ_SUBSCRIPTION:
     case startracker::REQ_LOG_SUBSCRIPTION:
     case startracker::REQ_DEBUG_CAMERA:
+    case startracker::REQ_MATCHED_CENTROIDS:
       if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE) {
         return STARTRACKER_NOT_RUNNING_FIRMWARE;
       }

mission/acs/str/StarTrackerHandler.h

@@ -204,6 +204,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
   startracker::SubscriptionSet subscriptionSet;
   startracker::LogSubscriptionSet logSubscriptionSet;
   startracker::DebugCameraSet debugCameraSet;
+  startracker::MatchedCentroidsSet matchedCentroids;
 
   // Pointer to object responsible for uploading and downloading images to/from the star tracker
   StrComHandler* strHelper = nullptr;
@@ -297,6 +298,8 @@ class StarTrackerHandler : public DeviceHandlerBase {
 
   const power::Switch_t powerSwitch = power::NO_SWITCH;
 
+  size_t fullPacketLen = 0;
+
   /**
    * @brief   Handles internal state
    */
@@ -429,6 +432,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
   ReturnValue_t prepareRequestCentroidingParams();
   ReturnValue_t prepareRequestLisaParams();
   ReturnValue_t prepareRequestMatchingParams();
+  ReturnValue_t prepareRequestMatchedCentroids();
   ReturnValue_t prepareRequestTrackingParams();
   ReturnValue_t prepareRequestValidationParams();
   ReturnValue_t prepareRequestAlgoParams();
@@ -477,6 +481,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
    */
   void handleStartup(uint8_t tmType, uint8_t parameterId);
 
+  ReturnValue_t statusFieldCheck(const uint8_t* rawFrame);
   /**
    * @brief   Handles telemtry replies and fills the appropriate dataset
    *
@@ -488,6 +493,8 @@ class StarTrackerHandler : public DeviceHandlerBase {
   ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset, size_t size,
                          const char* context);
 
+  ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame);
+
   /**
    * @brief Checks if star tracker is in valid mode for executing the received command.
    *

mission/acs/str/strHelpers.h

@@ -272,7 +272,16 @@ enum PoolIds : lp_id_t {
   LOG_SUBSCRIPTION_LEVEL2,
   LOG_SUBSCRIPTION_MODULE2,
   DEBUG_CAMERA_TIMING,
-  DEBUG_CAMERA_TEST
+  DEBUG_CAMERA_TEST,
+
+  TICKS_MATCHED_CENTROIDS,
+  TIME_MATCHED_CENTROIDS,
+  NUM_MATCHED_CENTROIDS,
+  MATCHED_CENTROIDS_STAR_IDS,
+  MATCHED_CENTROIDS_X_COORDS,
+  MATCHED_CENTROIDS_Y_COORDS,
+  MATCHED_CENTROIDS_X_ERRORS,
+  MATCHED_CENTROIDS_Y_ERRORS,
 };
 
 static const DeviceCommandId_t PING_REQUEST = 0;
@@ -328,6 +337,7 @@ static const DeviceCommandId_t DISABLE_TIMESTAMP_GENERATION = 85;
 static const DeviceCommandId_t ENABLE_TIMESTAMP_GENERATION = 86;
 static constexpr DeviceCommandId_t SET_TIME_FROM_SYS_TIME = 87;
 static constexpr DeviceCommandId_t AUTO_THRESHOLD = 88;
+static constexpr DeviceCommandId_t REQ_MATCHED_CENTROIDS = 89;
 static const DeviceCommandId_t NONE = 0xFFFFFFFF;
 
 static const uint32_t VERSION_SET_ID = REQ_VERSION;
@@ -352,6 +362,7 @@ static const uint32_t ALGO_SET_ID = REQ_ALGO;
 static const uint32_t SUBSCRIPTION_SET_ID = REQ_SUBSCRIPTION;
 static const uint32_t LOG_SUBSCRIPTION_SET_ID = REQ_LOG_SUBSCRIPTION;
 static const uint32_t DEBUG_CAMERA_SET_ID = REQ_DEBUG_CAMERA;
+static const uint32_t MATCHED_CENTROIDS_SET_ID = REQ_MATCHED_CENTROIDS;
 
 /** Max size of unencoded frame */
 static const size_t MAX_FRAME_SIZE = 1200;
@@ -666,7 +677,7 @@ class SolutionSet : public StaticLocalDataSet<SOLUTION_SET_ENTRIES> {
 
   // Ticks timestamp
   lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::TICKS_SOLUTION_SET, this);
-  /// Unix time stamp
+  // Unix time stamp
   lp_var_t<uint64_t> timeUs = lp_var_t<uint64_t>(sid.objectId, PoolIds::TIME_SOLUTION_SET, this);
   // Calibrated quaternion (takes into account the mounting quaternion), typically same as
   // track q values
@@ -1381,6 +1392,35 @@ class ValidationSet : public StaticLocalDataSet<VALIDATION_SET_ENTRIES> {
   }
 };
 
+class MatchedCentroidsSet : public StaticLocalDataSet<20> {
+ public:
+  static constexpr size_t SIZE = 321;
+  MatchedCentroidsSet(HasLocalDataPoolIF* owner)
+      : StaticLocalDataSet(owner, MATCHED_CENTROIDS_SET_ID) {}
+  MatchedCentroidsSet(object_id_t objectId)
+      : StaticLocalDataSet(sid_t(objectId, MATCHED_CENTROIDS_SET_ID)) {}
+  // Ticks timestamp
+  lp_var_t<uint32_t> ticks =
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::TICKS_MATCHED_CENTROIDS, this);
+  // Unix time stamp
+  lp_var_t<uint64_t> timeUs =
+      lp_var_t<uint64_t>(sid.objectId, PoolIds::TIME_MATCHED_CENTROIDS, this);
+  lp_var_t<uint8_t> numberOfMatchedCentroids =
+      lp_var_t<uint8_t>(sid.objectId, PoolIds::NUM_MATCHED_CENTROIDS, this);
+  lp_vec_t<uint32_t, 16> starIds =
+      lp_vec_t<uint32_t, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_STAR_IDS);
+  lp_vec_t<float, 16> xCoords =
+      lp_vec_t<float, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_X_COORDS);
+  lp_vec_t<float, 16> yCoords =
+      lp_vec_t<float, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_Y_COORDS);
+  lp_vec_t<float, 16> xErrors =
+      lp_vec_t<float, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_X_ERRORS);
+  lp_vec_t<float, 16> yErrors =
+      lp_vec_t<float, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_Y_ERRORS);
+
+ private:
+};
+
 /**
  * @brief   Will store the requested algo parameters
  */

tmtc

@@ -1 +1 @@
-Subproject commit 22a42108b431bafa707de2b3a24cad9de95b70d0
+Subproject commit 783bdd297a931a96c8b077ec2c2456a203b23ffc