mission/acs/str/StarTrackerHandler.cpp

@@ -55,6 +55,7 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
       blobsSet(this),
       centroidSet(this),
       centroidsSet(this),
+      contrastSet(this),
       strHelper(strHelper),
       paramJsonFile(jsonFileStr),
       powerSwitch(powerSwitch) {
@@ -352,6 +353,7 @@ ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t* id
       if (currentSecondaryTmIter == additionalRequestedTm.end()) {
         currentSecondaryTmIter = additionalRequestedTm.begin();
       }
+      sif::debug << "requesting ID " << *id << std::endl;
       normalState = NormalState::SOLUTION_REQUEST;
       break;
     case NormalState::SOLUTION_REQUEST:
@@ -524,6 +526,9 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
       prepareRequestCentroidsTm();
       return returnvalue::OK;
     }
+    case (startracker::REQ_CONTRAST): {
+      return returnvalue::OK;
+    }
     case (startracker::BOOT): {
       prepareBootCommand();
       return returnvalue::OK;
@@ -809,6 +814,8 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
   this->insertInCommandAndReplyMap(startracker::REQ_CENTROIDS, 3, &centroidsSet,
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
+  this->insertInCommandAndReplyMap(startracker::REQ_CONTRAST, 3, &contrastSet,
+                                   startracker::MAX_FRAME_SIZE * 2 + 2);
 }
 
 ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
@@ -994,6 +1001,12 @@ void StarTrackerHandler::resetSecondaryTmSet() {
       centroidsSet.setValidity(false, true);
     }
   }
+  {
+    PoolReadGuard pg(&histogramSet);
+    if (pg.getReadResult() == returnvalue::OK) {
+      histogramSet.setValidity(false, true);
+    }
+  }
 }
 
 void StarTrackerHandler::bootBootloader() {
@@ -1106,6 +1119,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
       result = handleTm(packet, solutionSet, startracker::SolutionSet::SIZE, "REQ_SOLUTION");
       break;
     }
+    case(startracker::REQ_CONTRAST): {
+      result = handleTm(packet, contrastSet, startracker::ContrastSet::SIZE, "REQ_CONTRAST");
+      break;
+    }
     case (startracker::REQ_AUTO_BLOB): {
       result = handleAutoBlobTm(packet);
       break;
@@ -1532,6 +1549,13 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
   localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_Y_COORDS, new PoolEntry<float>(16));
   localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_MAGNITUDES, new PoolEntry<uint8_t>(16));
 
+  localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_TICKS, new PoolEntry<uint32_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_TIME, new PoolEntry<uint64_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_A, new PoolEntry<uint32_t>(9));
+  localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_B, new PoolEntry<uint32_t>(9));
+  localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_C, new PoolEntry<uint32_t>(9));
+  localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_D, new PoolEntry<uint32_t>(9));
+
   poolManager.subscribeForDiagPeriodicPacket(
       subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0));
   poolManager.subscribeForRegularPeriodicPacket(
@@ -1558,6 +1582,8 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
       subdp::RegularHkPeriodicParams(centroidSet.getSid(), false, 10.0));
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(centroidsSet.getSid(), false, 10.0));
+  poolManager.subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams(contrastSet.getSid(), false, 10.0));
   return returnvalue::OK;
 }
 
@@ -1823,6 +1849,10 @@ ReturnValue_t StarTrackerHandler::scanForTmReply(uint8_t replyId, DeviceCommandI
       *foundId = startracker::REQ_MATCHED_CENTROIDS;
       break;
     }
+    case(startracker::ID::CONTRAST): {
+      *foundId = startracker::REQ_CONTRAST;
+      break;
+    }
     default: {
       sif::debug << "StarTrackerHandler::scanForTmReply: Reply contains invalid reply ID: "
                  << static_cast<unsigned int>(replyId) << std::endl;
@@ -2047,6 +2077,14 @@ ReturnValue_t StarTrackerHandler::prepareRequestCentroidsTm() {
   return returnvalue::OK;
 }
 
+ReturnValue_t StarTrackerHandler::prepareRequestContrastTm() {
+  uint32_t length = 0;
+  arc_tm_pack_contrast_req(commandBuffer, &length);
+  rawPacket = commandBuffer;
+  rawPacketLen = length;
+  return returnvalue::OK;
+}
+
 ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData,
                                                       size_t commandDataLen,
                                                       ArcsecJsonParamBase& paramSet,
@@ -2749,6 +2787,7 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
     case startracker::REQ_BLOBS:
     case startracker::REQ_CENTROID:
     case startracker::REQ_CENTROIDS:
+    case startracker::REQ_CONTRAST:
       if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE) {
         return STARTRACKER_NOT_RUNNING_FIRMWARE;
       }

mission/acs/str/StarTrackerHandler.h

@@ -212,6 +212,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
   startracker::BlobsSet blobsSet;
   startracker::CentroidSet centroidSet;
   startracker::CentroidsSet centroidsSet;
+  startracker::CentroidsSet contrastSet;
 
   // Pointer to object responsible for uploading and downloading images to/from the star tracker
   StrComHandler* strHelper = nullptr;
@@ -460,6 +461,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
   ReturnValue_t prepareRequestBlobsTm();
   ReturnValue_t prepareRequestCentroidTm();
   ReturnValue_t prepareRequestCentroidsTm();
+  ReturnValue_t prepareRequestContrastTm();
   ReturnValue_t prepareRequestTrackingParams();
   ReturnValue_t prepareRequestValidationParams();
   ReturnValue_t prepareRequestAlgoParams();

mission/acs/str/strHelpers.h

@@ -309,6 +309,13 @@ enum PoolIds : lp_id_t {
   CENTROIDS_X_COORDS,
   CENTROIDS_Y_COORDS,
   CENTROIDS_MAGNITUDES,
+
+  CONTRAST_TICKS,
+  CONTRAST_TIME,
+  CONTRAST_A,
+  CONTRAST_B,
+  CONTRAST_C,
+  CONTRAST_D,
 };
 
 static const DeviceCommandId_t PING_REQUEST = 0;
@@ -327,6 +334,7 @@ static const DeviceCommandId_t IMAGE_PROCESSOR = 19;
 static const DeviceCommandId_t REQ_SOLUTION = 24;
 static const DeviceCommandId_t REQ_TEMPERATURE = 25;
 static const DeviceCommandId_t REQ_HISTOGRAM = 28;
+static constexpr DeviceCommandId_t REQ_CONTRAST = 29;
 static const DeviceCommandId_t LIMITS = 40;
 static const DeviceCommandId_t MOUNTING = 41;
 static const DeviceCommandId_t CAMERA = 42;
@@ -403,6 +411,7 @@ static const uint32_t BLOB_SET_ID = REQ_BLOB;
 static const uint32_t BLOBS_SET_ID = REQ_BLOBS;
 static const uint32_t CENTROID_SET_ID = REQ_CENTROID;
 static const uint32_t CENTROIDS_SET_ID = REQ_CENTROIDS;
+static const uint32_t CONTRAST_SET_ID = REQ_CONTRAST;
 
 /** Max size of unencoded frame */
 static const size_t MAX_FRAME_SIZE = 1200;
@@ -1544,6 +1553,29 @@ class CentroidsSet : public StaticLocalDataSet<10> {
       lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::CENTROIDS_MAGNITUDES, this);
 };
 
+class ContrastSet: public StaticLocalDataSet<8> {
+
+public:
+  static constexpr size_t SIZE = 156;
+
+  ContrastSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CONTRAST_SET_ID) {}
+
+  // Data received from the Centroids Telemetry Set (ID 29)
+  lp_var_t<uint32_t> ticks =
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_TICKS, this);
+  // Unix time stamp
+  lp_var_t<uint64_t> timeUs =
+      lp_var_t<uint64_t>(sid.objectId, PoolIds::CONTRAST_TIME, this);
+  lp_vec_t<uint32_t, 9> contrastA =
+      lp_vec_t<uint32_t, 9>(sid.objectId, PoolIds::CONTRAST_A, this);
+  lp_vec_t<uint32_t, 9> contrastB =
+      lp_vec_t<uint32_t, 9>(sid.objectId, PoolIds::CONTRAST_B, this);
+  lp_vec_t<uint32_t, 9> contrastC =
+      lp_vec_t<uint32_t, 9>(sid.objectId, PoolIds::CONTRAST_C, this);
+  lp_vec_t<uint32_t, 9> contrastD =
+      lp_vec_t<uint32_t, 9>(sid.objectId, PoolIds::CONTRAST_D, this);
+};
+
 /**
  * @brief   Will store the requested algo parameters
  */

tmtc

@@ -1 +1 @@
-Subproject commit d9530271c38f8c81d6c5f3f11c6f3532085141e3
+Subproject commit 5821c60eb4743b014eb4641104bc35213f0b465b