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 +164 -13
4 changed files with 164 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 6a6789ae99 - Show all commits

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

@ -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;
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;
ReturnValue_t result = statusFieldCheck(rawFrame);
if (result != returnvalue::OK) {
return result;
}
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;
}

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

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

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

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

2
tmtc

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