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

STR extensions
Some checks failed
EIVE/eive-obsw/pipeline/head Build queued...
Details
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Robin Müller
2023-10-02 16:11:17 +02:00
parent 7d1d913220
commit 6a6789ae99
4 changed files with 164 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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;
}