finished STR extensions TMTC

mission/acs/str/StarTrackerHandler.cpp

@@ -50,7 +50,10 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
       logSubscriptionSet(this),
       debugCameraSet(this),
       matchedCentroids(this),
+      blobSet(this),
       blobsSet(this),
+      centroidSet(this),
+      centroidsSet(this),
       strHelper(strHelper),
       paramJsonFile(jsonFileStr),
       powerSwitch(powerSwitch) {
@@ -474,6 +477,12 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
       prepareTimeRequest();
       return returnvalue::OK;
     }
+    case (startracker::REQ_CENTROID): {
+      return returnvalue::OK;
+    }
+    case (startracker::REQ_CENTROIDS): {
+      return returnvalue::OK;
+    }
     case (startracker::BOOT): {
       prepareBootCommand();
       return returnvalue::OK;
@@ -745,10 +754,14 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
   this->insertInCommandAndReplyMap(startracker::REQ_MATCHED_CENTROIDS, 3, &matchedCentroids,
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
-  this->insertInCommandAndReplyMap(startracker::REQ_BLOB, 3, &blobsSet,
+  this->insertInCommandAndReplyMap(startracker::REQ_BLOB, 3, &blobSet,
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
   this->insertInCommandAndReplyMap(startracker::REQ_BLOBS, 3, &blobsSet,
                                    startracker::MAX_FRAME_SIZE * 2 + 2);
+  this->insertInCommandAndReplyMap(startracker::REQ_CENTROID, 3, &centroidSet,
+                                   startracker::MAX_FRAME_SIZE * 2 + 2);
+  this->insertInCommandAndReplyMap(startracker::REQ_CENTROIDS, 3, &centroidsSet,
+                                   startracker::MAX_FRAME_SIZE * 2 + 2);
 }
 
 ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
@@ -1012,6 +1025,14 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
       result = handleBlobsTm(packet);
       break;
     }
+    case (startracker::REQ_CENTROID): {
+      result = handleCentroidTm(packet);
+      break;
+    }
+    case (startracker::REQ_CENTROIDS): {
+      result = handleCentroidsTm(packet);
+      break;
+    }
     case (startracker::REQ_TEMPERATURE): {
       result = handleTm(packet, temperatureSet, startracker::TemperatureSet::SIZE, "REQ_TEMP");
       break;
@@ -1398,6 +1419,17 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
   localDataPoolMap.emplace(startracker::PoolIds::BLOBS_X_COORDS, new PoolEntry<uint16_t>(8));
   localDataPoolMap.emplace(startracker::PoolIds::BLOBS_Y_COORDS, new PoolEntry<uint16_t>(8));
 
+  localDataPoolMap.emplace(startracker::PoolIds::CENTROID_TICKS, new PoolEntry<uint32_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::CENTROID_TIME, new PoolEntry<uint64_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::CENTROID_COUNT, new PoolEntry<uint32_t>());
+
+  localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_TICKS, new PoolEntry<uint32_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_TIME, new PoolEntry<uint64_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_COUNT, new PoolEntry<uint16_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_X_COORDS, new PoolEntry<float>(16));
+  localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_Y_COORDS, new PoolEntry<float>(16));
+  localDataPoolMap.emplace(startracker::PoolIds::CENTROIDS_MAGNITUDES, new PoolEntry<uint8_t>(16));
+
   poolManager.subscribeForDiagPeriodicPacket(
       subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0));
   poolManager.subscribeForRegularPeriodicPacket(
@@ -1412,6 +1444,16 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
       subdp::RegularHkPeriodicParams(histogramSet.getSid(), false, 10.0));
   poolManager.subscribeForRegularPeriodicPacket(
       subdp::RegularHkPeriodicParams(lisaSet.getSid(), false, 10.0));
+  poolManager.subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams(matchedCentroids.getSid(), false, 10.0));
+  poolManager.subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams(blobSet.getSid(), false, 10.0));
+  poolManager.subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams(blobsSet.getSid(), false, 10.0));
+  poolManager.subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams(centroidSet.getSid(), false, 10.0));
+  poolManager.subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams(centroidsSet.getSid(), false, 10.0));
   return returnvalue::OK;
 }
 
@@ -1853,6 +1895,22 @@ ReturnValue_t StarTrackerHandler::prepareRequestBlobsTm() {
   return returnvalue::OK;
 }
 
+ReturnValue_t StarTrackerHandler::prepareRequestCentroidTm() {
+  uint32_t length = 0;
+  arc_tm_pack_centroid_req(commandBuffer, &length);
+  rawPacket = commandBuffer;
+  rawPacketLen = length;
+  return returnvalue::OK;
+}
+
+ReturnValue_t StarTrackerHandler::prepareRequestCentroidsTm() {
+  uint32_t length = 0;
+  arc_tm_pack_centroids_req(commandBuffer, &length);
+  rawPacket = commandBuffer;
+  rawPacketLen = length;
+  return returnvalue::OK;
+}
+
 ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData,
                                                       size_t commandDataLen,
                                                       ArcsecJsonParamBase& paramSet,
@@ -2229,24 +2287,22 @@ ReturnValue_t StarTrackerHandler::handleBlobTm(const uint8_t* rawFrame) {
   if (result != returnvalue::OK) {
     return result;
   }
-  result = SerializeAdapter::deSerialize(&blobsSet.ticksBlobTm, &rawFrame, &remainingLen,
+  result = SerializeAdapter::deSerialize(&blobSet.ticks, &rawFrame, &remainingLen,
                                          SerializeIF::Endianness::LITTLE);
   if (result != returnvalue::OK) {
     return result;
   }
-  result = SerializeAdapter::deSerialize(&blobsSet.timeUsBlobTm, &rawFrame, &remainingLen,
+  result = SerializeAdapter::deSerialize(&blobSet.timeUs, &rawFrame, &remainingLen,
                                          SerializeIF::Endianness::LITTLE);
   if (result != returnvalue::OK) {
     return result;
   }
-  result = SerializeAdapter::deSerialize(&blobsSet.blobCount, &rawFrame, &remainingLen,
+  result = SerializeAdapter::deSerialize(&blobSet.blobCount, &rawFrame, &remainingLen,
                                          SerializeIF::Endianness::LITTLE);
   if (result != returnvalue::OK) {
     return result;
   }
-  blobsSet.ticksBlobTm.setValid(true);
+  blobSet.setValidity(true, true);
-  blobsSet.timeUsBlobTm.setValid(true);
-  blobsSet.blobCount.setValid(true);
   return returnvalue::OK;
 }
 
@@ -2261,12 +2317,12 @@ ReturnValue_t StarTrackerHandler::handleBlobsTm(const uint8_t* rawFrame) {
   if (result != returnvalue::OK) {
     return result;
   }
-  result = SerializeAdapter::deSerialize(&blobsSet.ticksBlobsTm, &rawFrame, &remainingLen,
+  result = SerializeAdapter::deSerialize(&blobsSet.ticks, &rawFrame, &remainingLen,
                                          SerializeIF::Endianness::LITTLE);
   if (result != returnvalue::OK) {
     return result;
   }
-  result = SerializeAdapter::deSerialize(&blobsSet.timeUsBlobsTm, &rawFrame, &remainingLen,
+  result = SerializeAdapter::deSerialize(&blobsSet.timeUs, &rawFrame, &remainingLen,
                                          SerializeIF::Endianness::LITTLE);
   if (result != returnvalue::OK) {
     return result;
@@ -2298,13 +2354,86 @@ ReturnValue_t StarTrackerHandler::handleBlobsTm(const uint8_t* rawFrame) {
       return result;
     }
   }
-  blobsSet.ticksBlobsTm.setValid(true);
+  blobsSet.setValidity(true, true);
-  blobsSet.timeUsBlobsTm.setValid(true);
+  return returnvalue::OK;
-  blobsSet.blobsCount.setValid(true);
+}
-  blobsSet.blobsCountUsed.setValid(true);
+
-  blobsSet.nr4LinesSkipped.setValid(true);
+ReturnValue_t StarTrackerHandler::handleCentroidTm(const uint8_t* rawFrame) {
-  blobsSet.xCoords.setValid(true);
+  ReturnValue_t result = statusFieldCheck(rawFrame);
-  blobsSet.yCoords.setValid(true);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  size_t remainingLen = fullPacketLen;
+  PoolReadGuard pg(&centroidsSet);
+  result = pg.getReadResult();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+
+  result = SerializeAdapter::deSerialize(&centroidSet.ticks, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&centroidSet.timeUs, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&centroidSet.centroidCount, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  centroidSet.setValidity(true, true);
+  return returnvalue::OK;
+}
+
+ReturnValue_t StarTrackerHandler::handleCentroidsTm(const uint8_t* rawFrame) {
+  ReturnValue_t result = statusFieldCheck(rawFrame);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  size_t remainingLen = fullPacketLen;
+  PoolReadGuard pg(&centroidsSet);
+  result = pg.getReadResult();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+
+  result = SerializeAdapter::deSerialize(&centroidsSet.ticksCentroidsTm, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&centroidsSet.timeUsCentroidsTm, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&centroidsSet.centroidsCount, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  for (unsigned idx = 0; idx < 16; idx++) {
+    result = SerializeAdapter::deSerialize(&centroidsSet.centroidsXCoords[idx], &rawFrame,
+                                           &remainingLen, SerializeIF::Endianness::LITTLE);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+    result = SerializeAdapter::deSerialize(&centroidsSet.centroidsYCoords[idx], &rawFrame,
+                                           &remainingLen, SerializeIF::Endianness::LITTLE);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+    result = SerializeAdapter::deSerialize(&centroidsSet.centroidsMagnitudes[idx], &rawFrame,
+                                           &remainingLen, SerializeIF::Endianness::LITTLE);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+  }
+  centroidsSet.setValidity(true, true);
   return returnvalue::OK;
 }
 
@@ -2446,6 +2575,8 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
     case startracker::REQ_MATCHED_CENTROIDS:
     case startracker::REQ_BLOB:
     case startracker::REQ_BLOBS:
+    case startracker::REQ_CENTROID:
+    case startracker::REQ_CENTROIDS:
       if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE) {
         return STARTRACKER_NOT_RUNNING_FIRMWARE;
       }

mission/acs/str/StarTrackerHandler.h

@@ -206,7 +206,10 @@ class StarTrackerHandler : public DeviceHandlerBase {
   startracker::LogSubscriptionSet logSubscriptionSet;
   startracker::DebugCameraSet debugCameraSet;
   startracker::MatchedCentroidsSet matchedCentroids;
+  startracker::BlobSet blobSet;
   startracker::BlobsSet blobsSet;
+  startracker::CentroidSet centroidSet;
+  startracker::CentroidsSet centroidsSet;
 
   // Pointer to object responsible for uploading and downloading images to/from the star tracker
   StrComHandler* strHelper = nullptr;
@@ -437,6 +440,8 @@ class StarTrackerHandler : public DeviceHandlerBase {
   ReturnValue_t prepareRequestMatchedCentroids();
   ReturnValue_t prepareRequestBlobTm();
   ReturnValue_t prepareRequestBlobsTm();
+  ReturnValue_t prepareRequestCentroidTm();
+  ReturnValue_t prepareRequestCentroidsTm();
   ReturnValue_t prepareRequestTrackingParams();
   ReturnValue_t prepareRequestValidationParams();
   ReturnValue_t prepareRequestAlgoParams();
@@ -500,6 +505,8 @@ class StarTrackerHandler : public DeviceHandlerBase {
   ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame);
   ReturnValue_t handleBlobTm(const uint8_t* rawFrame);
   ReturnValue_t handleBlobsTm(const uint8_t* rawFrame);
+  ReturnValue_t handleCentroidTm(const uint8_t* rawFrame);
+  ReturnValue_t handleCentroidsTm(const uint8_t* rawFrame);
 
   /**
    * @brief Checks if star tracker is in valid mode for executing the received command.

mission/acs/str/strHelpers.h

@@ -294,6 +294,17 @@ enum PoolIds : lp_id_t {
   BLOBS_NR_4LINES_SKIPPED,
   BLOBS_X_COORDS,
   BLOBS_Y_COORDS,
+
+  CENTROID_TICKS,
+  CENTROID_TIME,
+  CENTROID_COUNT,
+
+  CENTROIDS_TICKS,
+  CENTROIDS_TIME,
+  CENTROIDS_COUNT,
+  CENTROIDS_X_COORDS,
+  CENTROIDS_Y_COORDS,
+  CENTROIDS_MAGNITUDES,
 };
 
 static const DeviceCommandId_t PING_REQUEST = 0;
@@ -352,6 +363,8 @@ static constexpr DeviceCommandId_t AUTO_THRESHOLD = 88;
 static constexpr DeviceCommandId_t REQ_MATCHED_CENTROIDS = 89;
 static constexpr DeviceCommandId_t REQ_BLOB = 90;
 static constexpr DeviceCommandId_t REQ_BLOBS = 91;
+static constexpr DeviceCommandId_t REQ_CENTROID = 92;
+static constexpr DeviceCommandId_t REQ_CENTROIDS = 93;
 static const DeviceCommandId_t NONE = 0xFFFFFFFF;
 
 static const uint32_t VERSION_SET_ID = REQ_VERSION;
@@ -377,7 +390,10 @@ 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;
+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;
 
 /** Max size of unencoded frame */
 static const size_t MAX_FRAME_SIZE = 1200;
@@ -1436,21 +1452,23 @@ class MatchedCentroidsSet : public StaticLocalDataSet<20> {
  private:
 };
 
-class BlobsSet : public StaticLocalDataSet<20> {
+class BlobSet : public StaticLocalDataSet<5> {
  public:
-  BlobsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOBS_SET_ID) {}
+  BlobSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOB_SET_ID) {}
-
   // The blob count received from the Blob Telemetry Set (ID 25)
   // Ticks timestamp
-  lp_var_t<uint32_t> ticksBlobTm = lp_var_t<uint32_t>(sid.objectId, PoolIds::BLOB_TICKS, this);
+  lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::BLOB_TICKS, this);
   // Unix time stamp
-  lp_var_t<uint64_t> timeUsBlobTm = lp_var_t<uint64_t>(sid.objectId, PoolIds::BLOB_TIME, this);
+  lp_var_t<uint64_t> timeUs = lp_var_t<uint64_t>(sid.objectId, PoolIds::BLOB_TIME, this);
   lp_var_t<uint32_t> blobCount = lp_var_t<uint32_t>(sid.objectId, PoolIds::BLOB_COUNT, this);
-
+};
+class BlobsSet : public StaticLocalDataSet<10> {
+ public:
+  BlobsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOBS_SET_ID) {}
   // Ticks timestamp
-  lp_var_t<uint32_t> ticksBlobsTm = lp_var_t<uint32_t>(sid.objectId, PoolIds::BLOBS_TICKS, this);
+  lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::BLOBS_TICKS, this);
   // Unix time stamp
-  lp_var_t<uint64_t> timeUsBlobsTm = lp_var_t<uint64_t>(sid.objectId, PoolIds::BLOBS_TIME, this);
+  lp_var_t<uint64_t> timeUs = lp_var_t<uint64_t>(sid.objectId, PoolIds::BLOBS_TIME, this);
   lp_var_t<uint16_t> blobsCount = lp_var_t<uint16_t>(sid.objectId, PoolIds::BLOBS_COUNT, this);
   lp_var_t<uint16_t> blobsCountUsed =
       lp_var_t<uint16_t>(sid.objectId, PoolIds::BLOBS_COUNT_USED, this);
@@ -1462,6 +1480,40 @@ class BlobsSet : public StaticLocalDataSet<20> {
       lp_vec_t<uint16_t, 8>(sid.objectId, PoolIds::BLOBS_Y_COORDS, this);
 };
 
+class CentroidSet : public StaticLocalDataSet<5> {
+ public:
+  CentroidSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CENTROID_SET_ID) {}
+
+  // Data received from the Centroid Telemetry Set (ID 26)
+  // Ticks timestamp
+  lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::CENTROID_TICKS, this);
+  // Unix time stamp
+  lp_var_t<uint64_t> timeUs = lp_var_t<uint64_t>(sid.objectId, PoolIds::CENTROID_TIME, this);
+  // The centroid count received from the Centroid Telemetry Set (ID 26)
+  lp_var_t<uint32_t> centroidCount =
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::CENTROID_COUNT, this);
+};
+
+class CentroidsSet : public StaticLocalDataSet<10> {
+ public:
+  CentroidsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CENTROIDS_SET_ID) {}
+
+  // Data received from the Centroids Telemetry Set (ID 37)
+  lp_var_t<uint32_t> ticksCentroidsTm =
+      lp_var_t<uint32_t>(sid.objectId, PoolIds::CENTROIDS_TICKS, this);
+  // Unix time stamp
+  lp_var_t<uint64_t> timeUsCentroidsTm =
+      lp_var_t<uint64_t>(sid.objectId, PoolIds::CENTROIDS_TIME, this);
+  lp_var_t<uint16_t> centroidsCount =
+      lp_var_t<uint16_t>(sid.objectId, PoolIds::CENTROIDS_COUNT, this);
+  lp_vec_t<float, 16> centroidsXCoords =
+      lp_vec_t<float, 16>(sid.objectId, PoolIds::CENTROIDS_X_COORDS, this);
+  lp_vec_t<float, 16> centroidsYCoords =
+      lp_vec_t<float, 16>(sid.objectId, PoolIds::CENTROIDS_Y_COORDS, this);
+  lp_vec_t<uint8_t, 16> centroidsMagnitudes =
+      lp_vec_t<uint8_t, 16>(sid.objectId, PoolIds::CENTROIDS_MAGNITUDES, this);
+};
+
 /**
  * @brief   Will store the requested algo parameters
  */