eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

STR extensions #798

Merged
muellerr merged 32 commits from str-extensions into main 2023-10-27 13:50:13 +02:00
Conversation 1 Commits 32 Files Changed 5 +214 -24
3 changed files with 214 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit cc1be69764 - Show all commits

163
mission/acs/str/StarTrackerHandler.cpp
View File

@ -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);
blobsSet.timeUsBlobTm.setValid(true);
blobsSet.blobCount.setValid(true);
blobSet.setValidity(true, 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.timeUsBlobsTm.setValid(true);
blobsSet.blobsCount.setValid(true);
blobsSet.blobsCountUsed.setValid(true);
blobsSet.nr4LinesSkipped.setValid(true);
blobsSet.xCoords.setValid(true);
blobsSet.yCoords.setValid(true);
blobsSet.setValidity(true, true);
return returnvalue::OK;
}
ReturnValue_t StarTrackerHandler::handleCentroidTm(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(&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;
}

7
mission/acs/str/StarTrackerHandler.h
View File

@ -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.

68
mission/acs/str/strHelpers.h
View File

@ -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
*/