added blob tm

mission/acs/str/StarTrackerHandler.cpp

@@ -50,6 +50,7 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
       logSubscriptionSet(this),
       debugCameraSet(this),
       matchedCentroids(this),
+      blobsSet(this),
       strHelper(strHelper),
       paramJsonFile(jsonFileStr),
       powerSwitch(powerSwitch) {
@@ -612,6 +613,14 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
       result = prepareRequestMatchedCentroids();
       return result;
     }
+    case (startracker::REQ_BLOB): {
+      result = prepareRequestBlobTm();
+      return result;
+    }
+    case (startracker::REQ_BLOBS): {
+      result = prepareRequestBlobsTm();
+      return result;
+    }
     case (startracker::REQ_MATCHING): {
       result = prepareRequestMatchingParams();
       return result;
@@ -736,6 +745,10 @@ 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,
+                                   startracker::MAX_FRAME_SIZE * 2 + 2);
+  this->insertInCommandAndReplyMap(startracker::REQ_BLOBS, 3, &blobsSet,
+                                   startracker::MAX_FRAME_SIZE * 2 + 2);
 }
 
 ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
@@ -991,6 +1004,14 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
       result = handleMatchedCentroidTm(packet);
       break;
     }
+    case (startracker::REQ_BLOB): {
+      result = handleBlobTm(packet);
+      break;
+    }
+    case (startracker::REQ_BLOBS): {
+      result = handleBlobsTm(packet);
+      break;
+    }
     case (startracker::REQ_TEMPERATURE): {
       result = handleTm(packet, temperatureSet, startracker::TemperatureSet::SIZE, "REQ_TEMP");
       break;
@@ -1364,6 +1385,14 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
                            new PoolEntry<uint32_t>());
   localDataPoolMap.emplace(startracker::PoolIds::TIME_MATCHED_CENTROIDS, new PoolEntry<uint64_t>());
 
+  localDataPoolMap.emplace(startracker::PoolIds::BLOB_COUNT, new PoolEntry<uint32_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::BLOBS_COUNT, new PoolEntry<uint16_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::BLOBS_COUNT_USED, new PoolEntry<uint16_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::BLOBS_NR_4LINES_SKIPPED,
+                           new PoolEntry<uint16_t>());
+  localDataPoolMap.emplace(startracker::PoolIds::BLOBS_X_COORDS, new PoolEntry<uint16_t>(8));
+  localDataPoolMap.emplace(startracker::PoolIds::BLOBS_Y_COORDS, new PoolEntry<uint16_t>(8));
+
   poolManager.subscribeForDiagPeriodicPacket(
       subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0));
   poolManager.subscribeForRegularPeriodicPacket(
@@ -1803,6 +1832,22 @@ ReturnValue_t StarTrackerHandler::prepareRequestMatchedCentroids() {
   return returnvalue::OK;
 }
 
+ReturnValue_t StarTrackerHandler::prepareRequestBlobTm() {
+  uint32_t length = 0;
+  arc_tm_pack_blob_req(commandBuffer, &length);
+  rawPacket = commandBuffer;
+  rawPacketLen = length;
+  return returnvalue::OK;
+}
+
+ReturnValue_t StarTrackerHandler::prepareRequestBlobsTm() {
+  uint32_t length = 0;
+  arc_tm_pack_blobs_req(commandBuffer, &length);
+  rawPacket = commandBuffer;
+  rawPacketLen = length;
+  return returnvalue::OK;
+}
+
 ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData,
                                                       size_t commandDataLen,
                                                       ArcsecJsonParamBase& paramSet,
@@ -2168,6 +2213,96 @@ ReturnValue_t StarTrackerHandler::handleMatchedCentroidTm(const uint8_t* rawFram
   return returnvalue::OK;
 }
 
+ReturnValue_t StarTrackerHandler::handleBlobTm(const uint8_t* rawFrame) {
+  ReturnValue_t result = statusFieldCheck(rawFrame);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  size_t remainingLen = fullPacketLen;
+  PoolReadGuard pg(&blobsSet);
+  result = pg.getReadResult();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&blobsSet.ticksBlobTm, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&blobsSet.timeUsBlobTm, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&blobsSet.blobCount, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  blobsSet.ticksBlobTm.setValid(true);
+  blobsSet.timeUsBlobTm.setValid(true);
+  blobsSet.blobCount.setValid(true);
+  return returnvalue::OK;
+}
+
+ReturnValue_t StarTrackerHandler::handleBlobsTm(const uint8_t* rawFrame) {
+  ReturnValue_t result = statusFieldCheck(rawFrame);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  size_t remainingLen = fullPacketLen;
+  PoolReadGuard pg(&blobsSet);
+  result = pg.getReadResult();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&blobsSet.ticksBlobsTm, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&blobsSet.timeUsBlobsTm, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&blobsSet.blobsCount, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&blobsSet.blobsCountUsed, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = SerializeAdapter::deSerialize(&blobsSet.nr4LinesSkipped, &rawFrame, &remainingLen,
+                                         SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  for (unsigned idx = 0; idx < 8; idx++) {
+    result = SerializeAdapter::deSerialize(&blobsSet.xCoords[idx], &rawFrame, &remainingLen,
+                                           SerializeIF::Endianness::LITTLE);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+    result = SerializeAdapter::deSerialize(&blobsSet.yCoords[idx], &rawFrame, &remainingLen,
+                                           SerializeIF::Endianness::LITTLE);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+  }
+  blobsSet.ticksBlobsTm.setValid(true);
+  blobsSet.timeUsBlobsTm.setValid(true);
+  blobsSet.blobsCount.setValid(true);
+  blobsSet.blobsCountUsed.setValid(true);
+  blobsSet.nr4LinesSkipped.setValid(true);
+  blobsSet.xCoords.setValid(true);
+  blobsSet.yCoords.setValid(true);
+  return returnvalue::OK;
+}
+
 ReturnValue_t StarTrackerHandler::handleActionReplySet(const uint8_t* rawFrame,
                                                        LocalPoolDataSetBase& dataset, size_t size) {
   ReturnValue_t result = returnvalue::OK;
@@ -2304,6 +2439,8 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
     case startracker::REQ_LOG_SUBSCRIPTION:
     case startracker::REQ_DEBUG_CAMERA:
     case startracker::REQ_MATCHED_CENTROIDS:
+    case startracker::REQ_BLOB:
+    case startracker::REQ_BLOBS:
       if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE) {
         return STARTRACKER_NOT_RUNNING_FIRMWARE;
       }

mission/acs/str/StarTrackerHandler.h

@@ -151,6 +151,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
   static const uint8_t PARAMS_OFFSET = 2;
   static const uint8_t TICKS_OFFSET = 3;
   static const uint8_t TIME_OFFSET = 7;
+  static const uint8_t TM_PARAM_OFFSET = 15;
   static const uint8_t PARAMETER_ID_OFFSET = 1;
   static const uint8_t ACTION_ID_OFFSET = 1;
   static const uint8_t ACTION_DATA_OFFSET = 3;
@@ -205,6 +206,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
   startracker::LogSubscriptionSet logSubscriptionSet;
   startracker::DebugCameraSet debugCameraSet;
   startracker::MatchedCentroidsSet matchedCentroids;
+  startracker::BlobsSet blobsSet;
 
   // Pointer to object responsible for uploading and downloading images to/from the star tracker
   StrComHandler* strHelper = nullptr;
@@ -433,6 +435,8 @@ class StarTrackerHandler : public DeviceHandlerBase {
   ReturnValue_t prepareRequestLisaParams();
   ReturnValue_t prepareRequestMatchingParams();
   ReturnValue_t prepareRequestMatchedCentroids();
+  ReturnValue_t prepareRequestBlobTm();
+  ReturnValue_t prepareRequestBlobsTm();
   ReturnValue_t prepareRequestTrackingParams();
   ReturnValue_t prepareRequestValidationParams();
   ReturnValue_t prepareRequestAlgoParams();
@@ -494,6 +498,8 @@ class StarTrackerHandler : public DeviceHandlerBase {
                          const char* context);
 
   ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame);
+  ReturnValue_t handleBlobTm(const uint8_t* rawFrame);
+  ReturnValue_t handleBlobsTm(const uint8_t* rawFrame);
 
   /**
    * @brief Checks if star tracker is in valid mode for executing the received command.

mission/acs/str/strHelpers.h

@@ -282,6 +282,18 @@ enum PoolIds : lp_id_t {
   MATCHED_CENTROIDS_Y_COORDS,
   MATCHED_CENTROIDS_X_ERRORS,
   MATCHED_CENTROIDS_Y_ERRORS,
+
+  BLOB_TICKS,
+  BLOB_TIME,
+  BLOB_COUNT,
+
+  BLOBS_TICKS,
+  BLOBS_TIME,
+  BLOBS_COUNT,
+  BLOBS_COUNT_USED,
+  BLOBS_NR_4LINES_SKIPPED,
+  BLOBS_X_COORDS,
+  BLOBS_Y_COORDS,
 };
 
 static const DeviceCommandId_t PING_REQUEST = 0;
@@ -338,6 +350,8 @@ 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 constexpr DeviceCommandId_t REQ_BLOB = 90;
+static constexpr DeviceCommandId_t REQ_BLOBS = 91;
 static const DeviceCommandId_t NONE = 0xFFFFFFFF;
 
 static const uint32_t VERSION_SET_ID = REQ_VERSION;
@@ -363,6 +377,7 @@ 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 BLOBS_SET_ID = REQ_BLOBS;
 
 /** Max size of unencoded frame */
 static const size_t MAX_FRAME_SIZE = 1200;
@@ -1421,6 +1436,33 @@ class MatchedCentroidsSet : public StaticLocalDataSet<20> {
  private:
 };
 
+class BlobsSet : public StaticLocalDataSet<20> {
+ public:
+  BlobsSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, BLOBS_SET_ID) {}
+
+ private:
+  // 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);
+  // Unix time stamp
+  lp_var_t<uint64_t> timeUsBlobTm = 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);
+
+  // Ticks timestamp
+  lp_var_t<uint32_t> ticksBlobsTm = 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<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);
+  lp_var_t<uint16_t> nr4LinesSkipped =
+      lp_var_t<uint16_t>(sid.objectId, PoolIds::BLOBS_NR_4LINES_SKIPPED, this);
+  lp_vec_t<uint16_t, 8> xCoords =
+      lp_vec_t<uint16_t, 8>(sid.objectId, PoolIds::BLOBS_X_COORDS, this);
+  lp_vec_t<uint16_t, 8> yCoords =
+      lp_vec_t<uint16_t, 8>(sid.objectId, PoolIds::BLOBS_Y_COORDS, this);
+};
+
 /**
  * @brief   Will store the requested algo parameters
  */