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

Merge branch 'main' into persistent-tle-store
All checks were successful
EIVE/eive-obsw/pipeline/pr-dev-7.5.0 This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-11-29 11:14:19 +01:00
parent 1d51bfba3d 49a87224e7
commit 3b4ff437c0
112 changed files with 4234 additions and 1332 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
mission/acs/SusHandler.cpp
View File

@ -97,7 +97,7 @@ ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8
// appear sometimes for the SUS device: Allow invalid message up to a certain threshold
// before triggering FDIR reactions.
if (reply->tempRaw == 0xfff and not waitingForRecovery) {
if (invalidMsgCounter == 0) {
if (invalidMsgCounter == 0 and invalidMsgPeriodCounter == 0) {
triggerEvent(TEMPERATURE_ALL_ONES_START);
} else if (invalidMsgCounter == susMax1227::MAX_INVALID_MSG_COUNT) {
triggerEvent(DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT);
@ -110,8 +110,17 @@ ReturnValue_t SusHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8
return returnvalue::OK;
}
if (invalidMsgCounter > 0) {
triggerEvent(TEMPERATURE_ALL_ONES_RECOVERY, invalidMsgCounter);
invalidMsgPeriodCounter++;
if (invalidMsgCounter > invalidMsgCounterMax) {
invalidMsgCounterMax = invalidMsgCounter;
}
invalidMsgCounter = 0;
invalidMsgCountdown.resetTimer();
}
if (invalidMsgCountdown.hasTimedOut() and invalidMsgPeriodCounter > 0) {
triggerEvent(TEMPERATURE_ALL_ONES_RECOVERY, invalidMsgPeriodCounter, invalidMsgCounterMax);
invalidMsgPeriodCounter = 0;
invalidMsgCounterMax = 0;
}
dataset.setValidity(true, true);
dataset.tempC = max1227::getTemperature(reply->tempRaw);

10
mission/acs/SusHandler.h
View File

@ -20,8 +20,9 @@ class SusHandler : public DeviceHandlerBase {
//! [EXPORT] : [COMMENT] Detected invalid values, starting invalid message counting
static constexpr Event TEMPERATURE_ALL_ONES_START =
event::makeEvent(SUBSYSTEM_ID, 0, severity::MEDIUM);
//! [EXPORT] : [COMMENT] Detected valid values again, resetting invalid message counter.
//! P1: Invalid message counter.
//! [EXPORT] : [COMMENT] Detected valid values for a prolonged time again, resetting all counters.
//! P1: Number of periods with invalid messages.
//! P2: Maximum invalid message counter.
static constexpr Event TEMPERATURE_ALL_ONES_RECOVERY =
event::makeEvent(SUBSYSTEM_ID, 1, severity::INFO);
@ -54,8 +55,13 @@ class SusHandler : public DeviceHandlerBase {
susMax1227::SusDataset dataset;
acs::SusRequest request{};
uint8_t susIdx;
// After 1 minute, trigger the event for the invalid messages.
Countdown invalidMsgCountdown = Countdown(60000);
bool waitingForRecovery = true;
uint32_t invalidMsgCounter = 0;
uint32_t invalidMsgCounterMax = 0;
uint32_t invalidMsgPeriodCounter = 0;
uint32_t transitionDelay = 1000;
bool goToNormalMode = false;

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

@ -49,6 +49,13 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
subscriptionSet(this),
logSubscriptionSet(this),
debugCameraSet(this),
autoBlobSet(this),
matchedCentroidsSet(this),
blobSet(this),
blobsSet(this),
centroidSet(this),
centroidsSet(this),
contrastSet(this),
strHelper(strHelper),
paramJsonFile(jsonFileStr),
powerSwitch(powerSwitch) {
@ -59,6 +66,8 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
sif::error << "StarTrackerHandler: Invalid str image loader" << std::endl;
}
eventQueue = QueueFactory::instance()->createMessageQueue(EventMessage::EVENT_MESSAGE_SIZE * 5);
additionalRequestedTm.emplace(startracker::REQ_TEMPERATURE);
currentSecondaryTmIter = additionalRequestedTm.begin();
}
StarTrackerHandler::~StarTrackerHandler() {}
@ -165,6 +174,35 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu
ReturnValue_t result = returnvalue::OK;
switch (actionId) {
case (startracker::ADD_SECONDARY_TM_TO_NORMAL_MODE): {
if (size < 4) {
return HasActionsIF::INVALID_PARAMETERS;
}
DeviceCommandId_t idToAdd;
result =
SerializeAdapter::deSerialize(&idToAdd, data, &size, SerializeIF::Endianness::NETWORK);
if (result != returnvalue::OK) {
return result;
}
addSecondaryTmForNormalMode(idToAdd);
return EXECUTION_FINISHED;
}
case (startracker::RESET_SECONDARY_TM_SET): {
resetSecondaryTmSet();
return EXECUTION_FINISHED;
}
case (startracker::READ_SECONDARY_TM_SET): {
std::vector<uint8_t> dataVec(additionalRequestedTm.size() * 4);
unsigned idx = 0;
size_t serLen = 0;
for (const auto& cmd : additionalRequestedTm) {
SerializeAdapter::serialize(&cmd, dataVec.data() + idx * 4, &serLen, dataVec.size(),
SerializeIF::Endianness::NETWORK);
idx++;
}
actionHelper.reportData(commandedBy, actionId, dataVec.data(), dataVec.size());
return EXECUTION_FINISHED;
}
case (startracker::STOP_IMAGE_LOADER): {
strHelper->stopProcess();
return EXECUTION_FINISHED;
@ -306,13 +344,20 @@ ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t* id
return NOTHING_TO_SEND;
}
switch (normalState) {
case NormalState::TEMPERATURE_REQUEST:
*id = startracker::REQ_TEMPERATURE;
case NormalState::SECONDARY_REQUEST:
if (additionalRequestedTm.size() == 0) {
break;
}
*id = *currentSecondaryTmIter;
currentSecondaryTmIter++;
if (currentSecondaryTmIter == additionalRequestedTm.end()) {
currentSecondaryTmIter = additionalRequestedTm.begin();
}
normalState = NormalState::SOLUTION_REQUEST;
break;
case NormalState::SOLUTION_REQUEST:
*id = startracker::REQ_SOLUTION;
normalState = NormalState::TEMPERATURE_REQUEST;
normalState = NormalState::SECONDARY_REQUEST;
break;
default:
sif::debug << "StarTrackerHandler::buildNormalDeviceCommand: Invalid normal step"
@ -472,6 +517,18 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
prepareTimeRequest();
return returnvalue::OK;
}
case (startracker::REQ_CENTROID): {
prepareRequestCentroidTm();
return returnvalue::OK;
}
case (startracker::REQ_CENTROIDS): {
prepareRequestCentroidsTm();
return returnvalue::OK;
}
case (startracker::REQ_CONTRAST): {
prepareRequestContrastTm();
return returnvalue::OK;
}
case (startracker::BOOT): {
prepareBootCommand();
return returnvalue::OK;
@ -607,6 +664,22 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
result = prepareRequestLisaParams();
return result;
}
case (startracker::REQ_MATCHED_CENTROIDS): {
result = prepareRequestMatchedCentroidsTm();
return result;
}
case (startracker::REQ_BLOB): {
result = prepareRequestBlobTm();
return result;
}
case (startracker::REQ_BLOBS): {
result = prepareRequestBlobsTm();
return result;
}
case (startracker::REQ_AUTO_BLOB): {
result = prepareRequestAutoBlobTm();
return returnvalue::OK;
}
case (startracker::REQ_MATCHING): {
result = prepareRequestMatchingParams();
return result;
@ -729,6 +802,20 @@ 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_AUTO_BLOB, 3, &autoBlobSet,
startracker::MAX_FRAME_SIZE * 2 + 2);
this->insertInCommandAndReplyMap(startracker::REQ_MATCHED_CENTROIDS, 3, &matchedCentroidsSet,
startracker::MAX_FRAME_SIZE * 2 + 2);
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);
this->insertInCommandAndReplyMap(startracker::REQ_CONTRAST, 3, &contrastSet,
startracker::MAX_FRAME_SIZE * 2 + 2);
}
ReturnValue_t StarTrackerHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
@ -860,6 +947,74 @@ 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::addSecondaryTmForNormalMode(DeviceCommandId_t cmd) {
additionalRequestedTm.emplace(cmd);
}
void StarTrackerHandler::resetSecondaryTmSet() {
additionalRequestedTm.clear();
additionalRequestedTm.emplace(startracker::REQ_TEMPERATURE);
currentSecondaryTmIter = additionalRequestedTm.begin();
{
PoolReadGuard pg(&autoBlobSet);
if (pg.getReadResult() == returnvalue::OK) {
autoBlobSet.setValidity(false, true);
}
}
{
PoolReadGuard pg(&matchedCentroidsSet);
if (pg.getReadResult() == returnvalue::OK) {
matchedCentroidsSet.setValidity(false, true);
}
}
{
PoolReadGuard pg(&blobSet);
if (pg.getReadResult() == returnvalue::OK) {
blobSet.setValidity(false, true);
}
}
{
PoolReadGuard pg(&blobsSet);
if (pg.getReadResult() == returnvalue::OK) {
blobsSet.setValidity(false, true);
}
}
{
PoolReadGuard pg(&centroidSet);
if (pg.getReadResult() == returnvalue::OK) {
centroidSet.setValidity(false, true);
}
}
{
PoolReadGuard pg(&contrastSet);
if (pg.getReadResult() == returnvalue::OK) {
contrastSet.setValidity(false, true);
}
}
{
PoolReadGuard pg(&centroidsSet);
if (pg.getReadResult() == returnvalue::OK) {
centroidsSet.setValidity(false, true);
}
}
{
PoolReadGuard pg(&histogramSet);
if (pg.getReadResult() == returnvalue::OK) {
histogramSet.setValidity(false, true);
}
}
}
void StarTrackerHandler::bootBootloader() {
if (internalState == InternalState::IDLE) {
internalState = InternalState::BOOT_BOOTLOADER;
@ -896,24 +1051,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;
@ -933,7 +1088,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
break;
}
case (startracker::REQ_TIME): {
result = handleTm(packet, timeSet, startracker::TimeSet::SIZE, "REQ_TIME");
result = handleTm(packet, timeSet, "REQ_TIME");
break;
}
case (startracker::PING_REQUEST): {
@ -948,7 +1103,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
break;
}
case (startracker::REQ_VERSION): {
result = handleTm(packet, versionSet, startracker::VersionSet::SIZE, "REQ_VERSION");
result = handleTm(packet, versionSet, "REQ_VERSION");
if (result != returnvalue::OK) {
return result;
}
@ -959,23 +1114,51 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
break;
}
case (startracker::REQ_INTERFACE): {
result = handleTm(packet, interfaceSet, startracker::InterfaceSet::SIZE, "REQ_INTERFACE");
result = handleTm(packet, interfaceSet, "REQ_INTERFACE");
break;
}
case (startracker::REQ_POWER): {
result = handleTm(packet, powerSet, startracker::PowerSet::SIZE, "REQ_POWER");
result = handleTm(packet, powerSet, "REQ_POWER");
break;
}
case (startracker::REQ_SOLUTION): {
result = handleTm(packet, solutionSet, startracker::SolutionSet::SIZE, "REQ_SOLUTION");
result = handleTm(packet, solutionSet, "REQ_SOLUTION");
break;
}
case (startracker::REQ_CONTRAST): {
result = handleTm(packet, contrastSet, "REQ_CONTRAST");
break;
}
case (startracker::REQ_AUTO_BLOB): {
result = handleAutoBlobTm(packet);
break;
}
case (startracker::REQ_MATCHED_CENTROIDS): {
result = handleMatchedCentroidTm(packet);
break;
}
case (startracker::REQ_BLOB): {
result = handleBlobTm(packet);
break;
}
case (startracker::REQ_BLOBS): {
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");
result = handleTm(packet, temperatureSet, "REQ_TEMP");
break;
}
case (startracker::REQ_HISTOGRAM): {
result = handleTm(packet, histogramSet, startracker::HistogramSet::SIZE, "REQ_HISTO");
result = handleTm(packet, histogramSet, "REQ_HISTO");
break;
}
case (startracker::SUBSCRIPTION):
@ -1329,6 +1512,56 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
localDataPoolMap.emplace(startracker::CHKSUM, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(startracker::TICKS_AUTO_BLOB, new PoolEntry<uint32_t>());
localDataPoolMap.emplace(startracker::TIME_AUTO_BLOB, new PoolEntry<uint64_t>());
localDataPoolMap.emplace(startracker::AUTO_BLOB_THRESHOLD, new PoolEntry<float>());
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>());
localDataPoolMap.emplace(startracker::PoolIds::BLOB_TICKS, new PoolEntry<uint32_t>());
localDataPoolMap.emplace(startracker::PoolIds::BLOB_TIME, new PoolEntry<uint64_t>());
localDataPoolMap.emplace(startracker::PoolIds::BLOB_COUNT, new PoolEntry<uint32_t>());
localDataPoolMap.emplace(startracker::PoolIds::BLOBS_TICKS, new PoolEntry<uint32_t>());
localDataPoolMap.emplace(startracker::PoolIds::BLOBS_TIME, new PoolEntry<uint64_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));
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));
localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_TICKS, new PoolEntry<uint32_t>());
localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_TIME, new PoolEntry<uint64_t>());
localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_A, new PoolEntry<uint32_t>(9));
localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_B, new PoolEntry<uint32_t>(9));
localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_C, new PoolEntry<uint32_t>(9));
localDataPoolMap.emplace(startracker::PoolIds::CONTRAST_D, new PoolEntry<uint32_t>(9));
poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(temperatureSet.getSid(), false, 10.0));
poolManager.subscribeForRegularPeriodicPacket(
@ -1343,6 +1576,20 @@ 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(autoBlobSet.getSid(), false, 10.0));
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(matchedCentroidsSet.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));
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(contrastSet.getSid(), false, 10.0));
return returnvalue::OK;
}
@ -1584,8 +1831,36 @@ ReturnValue_t StarTrackerHandler::scanForTmReply(uint8_t replyId, DeviceCommandI
*foundId = startracker::REQ_SOLUTION;
break;
}
case (startracker::ID::BLOB): {
*foundId = startracker::REQ_BLOB;
break;
}
case (startracker::ID::BLOBS): {
*foundId = startracker::REQ_BLOBS;
break;
}
case (startracker::ID::CENTROID): {
*foundId = startracker::REQ_CENTROID;
break;
}
case (startracker::ID::CENTROIDS): {
*foundId = startracker::REQ_CENTROIDS;
break;
}
case (startracker::ID::AUTO_BLOB): {
*foundId = startracker::REQ_AUTO_BLOB;
break;
}
case (startracker::ID::MATCHED_CENTROIDS): {
*foundId = startracker::REQ_MATCHED_CENTROIDS;
break;
}
case (startracker::ID::CONTRAST): {
*foundId = startracker::REQ_CONTRAST;
break;
}
default: {
sif::debug << "StarTrackerHandler::scanForTmReply: Reply contains invalid reply id: "
sif::debug << "StarTrackerHandler::scanForTmReply: Reply contains invalid reply ID: "
<< static_cast<unsigned int>(replyId) << std::endl;
return returnvalue::FAILED;
break;
@ -1760,6 +2035,62 @@ void StarTrackerHandler::prepareHistogramRequest() {
rawPacketLen = length;
}
ReturnValue_t StarTrackerHandler::prepareRequestAutoBlobTm() {
uint32_t length = 0;
arc_tm_pack_autoblob_req(commandBuffer, &length);
rawPacket = commandBuffer;
rawPacketLen = length;
return returnvalue::OK;
}
ReturnValue_t StarTrackerHandler::prepareRequestMatchedCentroidsTm() {
uint32_t length = 0;
arc_tm_pack_matchedcentroids_req(commandBuffer, &length);
rawPacket = commandBuffer;
rawPacketLen = length;
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::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::prepareRequestContrastTm() {
uint32_t length = 0;
arc_tm_pack_contrast_req(commandBuffer, &length);
rawPacket = commandBuffer;
rawPacketLen = length;
return returnvalue::OK;
}
ReturnValue_t StarTrackerHandler::prepareParamCommand(const uint8_t* commandData,
size_t commandDataLen,
ArcsecJsonParamBase& paramSet,
@ -2038,13 +2369,10 @@ 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;
const char* context) {
ReturnValue_t result = statusFieldCheck(rawFrame);
if (result != returnvalue::OK) {
return result;
}
result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT);
if (result != returnvalue::OK) {
@ -2052,10 +2380,11 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat
}
const uint8_t* reply = rawFrame + TICKS_OFFSET;
dataset.setValidityBufferGeneration(false);
result = dataset.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE);
size_t sizeLeft = fullPacketLen;
result = dataset.deSerialize(&reply, &sizeLeft, SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
sif::warning << "StarTrackerHandler::handleTm: Deserialization failed for " << context
<< std::endl;
sif::warning << "StarTrackerHandler::handleTm: Deserialization failed for " << context << ": 0x"
<< std::hex << std::setw(4) << result << std::dec << std::endl;
}
dataset.setValidityBufferGeneration(true);
dataset.setValidity(true, true);
@ -2069,6 +2398,262 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat
return result;
}
ReturnValue_t StarTrackerHandler::handleAutoBlobTm(const uint8_t* rawFrame) {
ReturnValue_t result = statusFieldCheck(rawFrame);
if (result != returnvalue::OK) {
return result;
}
rawFrame += TICKS_OFFSET;
size_t remainingLen = fullPacketLen;
PoolReadGuard pg(&autoBlobSet);
result = pg.getReadResult();
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&autoBlobSet.ticks, &rawFrame, &remainingLen,
SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&autoBlobSet.timeUs, &rawFrame, &remainingLen,
SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&autoBlobSet.threshold, &rawFrame, &remainingLen,
SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
autoBlobSet.setValidity(true, true);
return returnvalue::OK;
}
ReturnValue_t StarTrackerHandler::handleMatchedCentroidTm(const uint8_t* rawFrame) {
ReturnValue_t result = statusFieldCheck(rawFrame);
if (result != returnvalue::OK) {
return result;
}
rawFrame += TICKS_OFFSET;
size_t remainingLen = fullPacketLen;
PoolReadGuard pg(&matchedCentroidsSet);
result = pg.getReadResult();
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&matchedCentroidsSet.ticks, &rawFrame, &remainingLen,
SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&matchedCentroidsSet.timeUs, &rawFrame, &remainingLen,
SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&matchedCentroidsSet.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(&matchedCentroidsSet.starIds[idx], &rawFrame,
&remainingLen, SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&matchedCentroidsSet.xCoords[idx], &rawFrame,
&remainingLen, SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&matchedCentroidsSet.yCoords[idx], &rawFrame,
&remainingLen, SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&matchedCentroidsSet.xErrors[idx], &rawFrame,
&remainingLen, SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&matchedCentroidsSet.yErrors[idx], &rawFrame,
&remainingLen, SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
}
matchedCentroidsSet.setValidity(true, true);
return returnvalue::OK;
}
ReturnValue_t StarTrackerHandler::handleBlobTm(const uint8_t* rawFrame) {
ReturnValue_t result = statusFieldCheck(rawFrame);
if (result != returnvalue::OK) {
return result;
}
rawFrame += TICKS_OFFSET;
size_t remainingLen = fullPacketLen;
PoolReadGuard pg(&blobsSet);
result = pg.getReadResult();
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&blobSet.ticks, &rawFrame, &remainingLen,
SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&blobSet.timeUs, &rawFrame, &remainingLen,
SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&blobSet.blobCount, &rawFrame, &remainingLen,
SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
blobSet.setValidity(true, true);
return returnvalue::OK;
}
ReturnValue_t StarTrackerHandler::handleBlobsTm(const uint8_t* rawFrame) {
ReturnValue_t result = statusFieldCheck(rawFrame);
if (result != returnvalue::OK) {
return result;
}
rawFrame += TICKS_OFFSET;
size_t remainingLen = fullPacketLen;
PoolReadGuard pg(&blobsSet);
result = pg.getReadResult();
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&blobsSet.ticks, &rawFrame, &remainingLen,
SerializeIF::Endianness::LITTLE);
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&blobsSet.timeUs, &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.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;
}
rawFrame += TICKS_OFFSET;
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;
}
rawFrame += TICKS_OFFSET;
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;
}
ReturnValue_t StarTrackerHandler::handleActionReplySet(const uint8_t* rawFrame,
LocalPoolDataSetBase& dataset, size_t size) {
ReturnValue_t result = returnvalue::OK;
@ -2204,10 +2789,17 @@ 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:
case startracker::REQ_BLOB:
case startracker::REQ_BLOBS:
case startracker::REQ_CENTROID:
case startracker::REQ_CENTROIDS:
case startracker::REQ_CONTRAST: {
if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE) {
return STARTRACKER_NOT_RUNNING_FIRMWARE;
}
break;
}
case startracker::FIRMWARE_UPDATE:
case startracker::FLASH_READ:
if (getMode() != MODE_ON or getSubmode() != startracker::Program::BOOTLOADER) {

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

@ -8,6 +8,7 @@
#include <mission/acs/str/strJsonCommands.h>
#include <thirdparty/sagittactl/wire/common/genericstructs.h>
#include <set>
#include <thread>
#include "OBSWConfig.h"
@ -151,6 +152,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;
@ -204,6 +206,13 @@ class StarTrackerHandler : public DeviceHandlerBase {
startracker::SubscriptionSet subscriptionSet;
startracker::LogSubscriptionSet logSubscriptionSet;
startracker::DebugCameraSet debugCameraSet;
startracker::AutoBlobSet autoBlobSet;
startracker::MatchedCentroidsSet matchedCentroidsSet;
startracker::BlobSet blobSet;
startracker::BlobsSet blobsSet;
startracker::CentroidSet centroidSet;
startracker::CentroidsSet centroidsSet;
startracker::ContrastSet contrastSet;
// Pointer to object responsible for uploading and downloading images to/from the star tracker
StrComHandler* strHelper = nullptr;
@ -239,9 +248,9 @@ class StarTrackerHandler : public DeviceHandlerBase {
std::string paramJsonFile;
enum class NormalState { TEMPERATURE_REQUEST, SOLUTION_REQUEST };
enum class NormalState { SECONDARY_REQUEST, SOLUTION_REQUEST };
NormalState normalState = NormalState::TEMPERATURE_REQUEST;
NormalState normalState = NormalState::SECONDARY_REQUEST;
enum class StartupState {
IDLE,
@ -297,6 +306,11 @@ class StarTrackerHandler : public DeviceHandlerBase {
const power::Switch_t powerSwitch = power::NO_SWITCH;
size_t fullPacketLen = 0;
std::set<DeviceCommandId_t> additionalRequestedTm{};
std::set<DeviceCommandId_t>::iterator currentSecondaryTmIter;
/**
* @brief Handles internal state
*/
@ -345,6 +359,18 @@ class StarTrackerHandler : public DeviceHandlerBase {
*/
ReturnValue_t executeFlashReadCommand(const uint8_t* commandData, size_t commandDataLen);
/**
* Add a TM request to the list of telemetry which will be polled in the secondary step of
* the device communication.
* @param cmd
*/
void addSecondaryTmForNormalMode(DeviceCommandId_t cmd);
/**
* Reset the secondary set, which will only contain a TEMPERATURE set request after the reset.
*/
void resetSecondaryTmSet();
/**
* @brief Fills command buffer with data to boot image (works only when star tracker is
* in bootloader mode).
@ -429,6 +455,13 @@ class StarTrackerHandler : public DeviceHandlerBase {
ReturnValue_t prepareRequestCentroidingParams();
ReturnValue_t prepareRequestLisaParams();
ReturnValue_t prepareRequestMatchingParams();
ReturnValue_t prepareRequestAutoBlobTm();
ReturnValue_t prepareRequestMatchedCentroidsTm();
ReturnValue_t prepareRequestBlobTm();
ReturnValue_t prepareRequestBlobsTm();
ReturnValue_t prepareRequestCentroidTm();
ReturnValue_t prepareRequestCentroidsTm();
ReturnValue_t prepareRequestContrastTm();
ReturnValue_t prepareRequestTrackingParams();
ReturnValue_t prepareRequestValidationParams();
ReturnValue_t prepareRequestAlgoParams();
@ -477,6 +510,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
*
@ -485,9 +519,16 @@ class StarTrackerHandler : public DeviceHandlerBase {
*
* @return returnvalue::OK if successful, otherwise error return value
*/
ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset, size_t size,
ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset,
const char* context);
ReturnValue_t handleAutoBlobTm(const uint8_t* rawFrame);
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.
*

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

@ -272,7 +272,50 @@ enum PoolIds : lp_id_t {
LOG_SUBSCRIPTION_LEVEL2,
LOG_SUBSCRIPTION_MODULE2,
DEBUG_CAMERA_TIMING,
DEBUG_CAMERA_TEST
DEBUG_CAMERA_TEST,
TICKS_AUTO_BLOB,
TIME_AUTO_BLOB,
AUTO_BLOB_THRESHOLD,
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,
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,
CENTROID_TICKS,
CENTROID_TIME,
CENTROID_COUNT,
CENTROIDS_TICKS,
CENTROIDS_TIME,
CENTROIDS_COUNT,
CENTROIDS_X_COORDS,
CENTROIDS_Y_COORDS,
CENTROIDS_MAGNITUDES,
CONTRAST_TICKS,
CONTRAST_TIME,
CONTRAST_A,
CONTRAST_B,
CONTRAST_C,
CONTRAST_D,
};
static const DeviceCommandId_t PING_REQUEST = 0;
@ -291,6 +334,7 @@ static const DeviceCommandId_t IMAGE_PROCESSOR = 19;
static const DeviceCommandId_t REQ_SOLUTION = 24;
static const DeviceCommandId_t REQ_TEMPERATURE = 25;
static const DeviceCommandId_t REQ_HISTOGRAM = 28;
static constexpr DeviceCommandId_t REQ_CONTRAST = 29;
static const DeviceCommandId_t LIMITS = 40;
static const DeviceCommandId_t MOUNTING = 41;
static const DeviceCommandId_t CAMERA = 42;
@ -328,6 +372,15 @@ 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_AUTO_BLOB = 89;
static constexpr DeviceCommandId_t REQ_MATCHED_CENTROIDS = 90;
static constexpr DeviceCommandId_t REQ_BLOB = 91;
static constexpr DeviceCommandId_t REQ_BLOBS = 92;
static constexpr DeviceCommandId_t REQ_CENTROID = 93;
static constexpr DeviceCommandId_t REQ_CENTROIDS = 94;
static constexpr DeviceCommandId_t ADD_SECONDARY_TM_TO_NORMAL_MODE = 95;
static constexpr DeviceCommandId_t RESET_SECONDARY_TM_SET = 96;
static constexpr DeviceCommandId_t READ_SECONDARY_TM_SET = 97;
static const DeviceCommandId_t NONE = 0xFFFFFFFF;
static const uint32_t VERSION_SET_ID = REQ_VERSION;
@ -352,6 +405,13 @@ 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 AUTO_BLOB_SET_ID = REQ_AUTO_BLOB;
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;
static const uint32_t CONTRAST_SET_ID = REQ_CONTRAST;
/** Max size of unencoded frame */
static const size_t MAX_FRAME_SIZE = 1200;
@ -412,6 +472,12 @@ static const uint8_t LOG_LEVEL = 3;
static const uint8_t LOG_SUBSCRIPTION = 19;
static const uint8_t DEBUG_CAMERA = 20;
static const uint8_t AUTO_THRESHOLD = 23;
static constexpr uint8_t BLOB = 25;
static constexpr uint8_t BLOBS = 36;
static constexpr uint8_t CENTROID = 26;
static constexpr uint8_t CENTROIDS = 37;
static constexpr uint8_t AUTO_BLOB = 39;
static constexpr uint8_t MATCHED_CENTROIDS = 40;
} // namespace ID
namespace Program {
@ -474,8 +540,6 @@ static const uint32_t FLASH_REGION_SIZE = 0x20000;
*/
class TemperatureSet : public StaticLocalDataSet<TEMPERATURE_SET_ENTRIES> {
public:
static const size_t SIZE = 24;
TemperatureSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TEMPERATURE_SET_ID) {}
TemperatureSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, TEMPERATURE_SET_ID)) {}
@ -505,8 +569,6 @@ class TemperatureSet : public StaticLocalDataSet<TEMPERATURE_SET_ENTRIES> {
*/
class VersionSet : public StaticLocalDataSet<VERSION_SET_ENTRIES> {
public:
static const size_t SIZE = 15;
VersionSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, VERSION_SET_ID) {}
VersionSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, VERSION_SET_ID)) {}
@ -536,8 +598,6 @@ class VersionSet : public StaticLocalDataSet<VERSION_SET_ENTRIES> {
*/
class InterfaceSet : public StaticLocalDataSet<INTERFACE_SET_ENTRIES> {
public:
static const size_t SIZE = 20;
InterfaceSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, REQ_INTERFACE) {}
InterfaceSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, REQ_INTERFACE)) {}
@ -565,8 +625,6 @@ class InterfaceSet : public StaticLocalDataSet<INTERFACE_SET_ENTRIES> {
*/
class PowerSet : public StaticLocalDataSet<POWER_SET_ENTRIES> {
public:
static const size_t SIZE = 76;
PowerSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, REQ_INTERFACE) {}
PowerSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, REQ_INTERFACE)) {}
@ -630,8 +688,6 @@ class PowerSet : public StaticLocalDataSet<POWER_SET_ENTRIES> {
*/
class TimeSet : public StaticLocalDataSet<TIME_SET_ENTRIES> {
public:
static const size_t SIZE = 24;
TimeSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TIME_SET_ID) {}
TimeSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, TIME_SET_ID)) {}
@ -658,15 +714,13 @@ class TimeSet : public StaticLocalDataSet<TIME_SET_ENTRIES> {
*/
class SolutionSet : public StaticLocalDataSet<SOLUTION_SET_ENTRIES> {
public:
static const size_t SIZE = 79;
SolutionSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, SOLUTION_SET_ID) {}
SolutionSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, SOLUTION_SET_ID)) {}
// 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 +1435,123 @@ class ValidationSet : public StaticLocalDataSet<VALIDATION_SET_ENTRIES> {
}
};
class AutoBlobSet : public StaticLocalDataSet<3> {
public:
AutoBlobSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, AUTO_BLOB_SET_ID) {}
// Ticks timestamp
lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::TICKS_AUTO_BLOB, this);
// Unix time stamp
lp_var_t<uint64_t> timeUs = lp_var_t<uint64_t>(sid.objectId, PoolIds::TIME_AUTO_BLOB, this);
lp_var_t<float> threshold = lp_var_t<float>(sid.objectId, PoolIds::AUTO_BLOB_THRESHOLD, this);
private:
};
class MatchedCentroidsSet : public StaticLocalDataSet<20> {
public:
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, this);
lp_vec_t<float, 16> xCoords =
lp_vec_t<float, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_X_COORDS, this);
lp_vec_t<float, 16> yCoords =
lp_vec_t<float, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_Y_COORDS, this);
lp_vec_t<float, 16> xErrors =
lp_vec_t<float, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_X_ERRORS, this);
lp_vec_t<float, 16> yErrors =
lp_vec_t<float, 16>(sid.objectId, PoolIds::MATCHED_CENTROIDS_Y_ERRORS, this);
private:
};
class BlobSet : public StaticLocalDataSet<5> {
public:
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> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::BLOB_TICKS, this);
// Unix time stamp
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> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::BLOBS_TICKS, this);
// Unix time stamp
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);
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);
};
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);
};
class ContrastSet : public StaticLocalDataSet<8> {
public:
ContrastSet(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, CONTRAST_SET_ID) {}
// Data received from the Centroids Telemetry Set (ID 29)
lp_var_t<uint32_t> ticks = lp_var_t<uint32_t>(sid.objectId, PoolIds::CONTRAST_TICKS, this);
// Unix time stamp
lp_var_t<uint64_t> timeUs = lp_var_t<uint64_t>(sid.objectId, PoolIds::CONTRAST_TIME, this);
lp_vec_t<uint32_t, 9> contrastA = lp_vec_t<uint32_t, 9>(sid.objectId, PoolIds::CONTRAST_A, this);
lp_vec_t<uint32_t, 9> contrastB = lp_vec_t<uint32_t, 9>(sid.objectId, PoolIds::CONTRAST_B, this);
lp_vec_t<uint32_t, 9> contrastC = lp_vec_t<uint32_t, 9>(sid.objectId, PoolIds::CONTRAST_C, this);
lp_vec_t<uint32_t, 9> contrastD = lp_vec_t<uint32_t, 9>(sid.objectId, PoolIds::CONTRAST_D, this);
};
/**
* @brief Will store the requested algo parameters
*/

2
mission/cfdp/CMakeLists.txt
View File

@ -1 +1 @@
target_sources(${LIB_EIVE_MISSION} PRIVATE CfdpHandler.cpp)
target_sources(${LIB_EIVE_MISSION} PRIVATE CfdpHandler.cpp CfdpUser.cpp)

38
mission/cfdp/CfdpFaultHandler.h Normal file
View File

@ -0,0 +1,38 @@
#ifndef MISSION_CFDP_CFDPFAULTHANDLER_H_
#define MISSION_CFDP_CFDPFAULTHANDLER_H_
#include "defs.h"
#include "fsfw/cfdp.h"
namespace cfdp {
class EiveFaultHandler : public cfdp::FaultHandlerBase, public SystemObject {
public:
EiveFaultHandler(object_id_t objectId) : SystemObject(objectId) {}
void noticeOfSuspensionCb(cfdp::TransactionId& id, cfdp::ConditionCode code) override {
sif::warning << "Notice of suspension detected for transaction " << id
<< " with condition code: " << cfdp::getConditionCodeString(code) << std::endl;
triggerEvent(cfdp::FAULT_HANDLER_TRIGGERED, FaultHandlerCode::NOTICE_OF_SUSPENSION, code);
}
void noticeOfCancellationCb(cfdp::TransactionId& id, cfdp::ConditionCode code) override {
sif::warning << "Notice of suspension detected for transaction " << id
<< " with condition code: " << cfdp::getConditionCodeString(code) << std::endl;
triggerEvent(cfdp::FAULT_HANDLER_TRIGGERED, FaultHandlerCode::NOTICE_OF_CANCELLATION, code);
}
void abandonCb(cfdp::TransactionId& id, cfdp::ConditionCode code) override {
sif::warning << "Transaction " << id
<< " was abandoned, condition code : " << cfdp::getConditionCodeString(code)
<< std::endl;
triggerEvent(cfdp::FAULT_HANDLER_TRIGGERED, FaultHandlerCode::ABANDON_TRANSACTION, code);
}
void ignoreCb(cfdp::TransactionId& id, cfdp::ConditionCode code) override {
sif::warning << "Fault ignored for transaction " << id
<< ", condition code: " << cfdp::getConditionCodeString(code) << std::endl;
triggerEvent(cfdp::FAULT_HANDLER_TRIGGERED, FaultHandlerCode::IGNORE_ERROR, code);
}
};
} // namespace cfdp
#endif /* MISSION_CFDP_CFDPFAULTHANDLER_H_ */

185
mission/cfdp/CfdpHandler.cpp
View File

@ -1,25 +1,36 @@
#include "CfdpHandler.h"
#include <fsfw/cfdp/CfdpMessage.h>
#include <fsfw/ipc/CommandMessage.h>
#include "eive/definitions.h"
#include "fsfw/cfdp/pdu/AckPduReader.h"
#include "fsfw/cfdp/pdu/PduHeaderReader.h"
#include "fsfw/globalfunctions/arrayprinter.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/tasks/TaskFactory.h"
#include "fsfw/tmtcservices/TmTcMessage.h"
#include "mission/sysDefs.h"
using namespace returnvalue;
using namespace cfdp;
CfdpHandler::CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerCfg& cfdpCfg)
: SystemObject(fsfwParams.objectId),
msgQueue(fsfwParams.msgQueue),
destHandler(
DestHandlerParams(LocalEntityCfg(cfdpCfg.id, cfdpCfg.indicCfg, cfdpCfg.faultHandler),
cfdpCfg.userHandler, cfdpCfg.remoteCfgProvider, cfdpCfg.packetInfoList,
cfdpCfg.lostSegmentsList),
FsfwParams(fsfwParams.packetDest, nullptr, this, fsfwParams.tcStore,
fsfwParams.tmStore)) {
destHandler.setMsgQueue(msgQueue);
}
CfdpHandler::CfdpHandler(const FsfwHandlerParams& fsfwHandlerParams, const CfdpHandlerCfg& cfdpCfg,
const std::atomic_bool& throttleSignal)
: SystemObject(fsfwHandlerParams.objectId),
pduQueue(fsfwHandlerParams.tmtcQueue),
cfdpRequestQueue(fsfwHandlerParams.cfdpQueue),
localCfg(cfdpCfg.id, cfdpCfg.indicCfg, cfdpCfg.faultHandler),
remoteCfgProvider(cfdpCfg.remoteCfgProvider),
fsfwParams(fsfwHandlerParams.packetDest, &fsfwHandlerParams.tmtcQueue, this,
fsfwHandlerParams.tcStore, fsfwHandlerParams.tmStore),
destHandler(DestHandlerParams(localCfg, cfdpCfg.userHandler, cfdpCfg.remoteCfgProvider,
cfdpCfg.packetInfoList, cfdpCfg.lostSegmentsList),
this->fsfwParams),
srcHandler(SourceHandlerParams(localCfg, cfdpCfg.userHandler, seqCntProvider),
this->fsfwParams),
ipcStore(fsfwHandlerParams.ipcStore),
throttleSignal(throttleSignal) {}
[[nodiscard]] const char* CfdpHandler::getName() const { return "CFDP Handler"; }
@ -27,7 +38,7 @@ CfdpHandler::CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerC
return destHandler.getDestHandlerParams().cfg.localId.getValue();
}
[[nodiscard]] MessageQueueId_t CfdpHandler::getRequestQueue() const { return msgQueue.getId(); }
[[nodiscard]] MessageQueueId_t CfdpHandler::getRequestQueue() const { return pduQueue.getId(); }
ReturnValue_t CfdpHandler::initialize() {
ReturnValue_t result = destHandler.initialize();
@ -40,29 +51,63 @@ ReturnValue_t CfdpHandler::initialize() {
return SystemObject::initialize();
}
ReturnValue_t CfdpHandler::performOperation(uint8_t operationCode) {
// TODO: Receive TC packets and route them to source and dest handler, depending on which is
// correct or more appropriate
ReturnValue_t status;
ReturnValue_t result = OK;
TmTcMessage tmtcMsg;
for (status = msgQueue.receiveMessage(&tmtcMsg); status == returnvalue::OK;
status = msgQueue.receiveMessage(&tmtcMsg)) {
result = handleCfdpPacket(tmtcMsg);
[[noreturn]] ReturnValue_t CfdpHandler::performOperation(uint8_t operationCode) {
while (true) {
bool shortDelay = false;
ReturnValue_t result = handlePduPacketMessages();
if (result != OK) {
status = result;
}
result = handleCfdpMessages();
if (result != OK) {
}
uint32_t fsmCount = 1;
const DestHandler::FsmResult& destResult = destHandler.stateMachine();
while (destResult.callStatus == CallStatus::CALL_AGAIN) {
if (fsmCount == config::CFDP_MAX_FSM_CALL_COUNT_DEST_HANDLER) {
shortDelay = true;
break;
}
destHandler.stateMachine();
fsmCount++;
}
fsmCount = 1;
throttlePeriodOngoing = throttleSignal;
// CFDP can be throttled by the slowest live TM handler to handle back pressure in a sensible
// way without requiring huge amounts of memory for large files.
if (!throttlePeriodOngoing) {
const SourceHandler::FsmResult& srcResult = srcHandler.stateMachine();
if (srcResult.packetsSent > 0) {
signals::CFDP_MSG_COUNTER.fetch_add(srcResult.packetsSent, std::memory_order_relaxed);
}
while (srcResult.callStatus == CallStatus::CALL_AGAIN) {
// Limit number of messages.
if (fsmCount == config::CFDP_MAX_FSM_CALL_COUNT_SRC_HANDLER) {
shortDelay = true;
break;
}
srcHandler.stateMachine();
if (srcResult.packetsSent > 0) {
signals::CFDP_MSG_COUNTER.fetch_add(srcResult.packetsSent, std::memory_order_relaxed);
}
if (srcResult.result == cfdp::TM_STORE_FULL) {
sif::warning << "CFDP Source Handler: TM store is full" << std::endl;
} else if (srcResult.result == cfdp::TARGET_MSG_QUEUE_FULL) {
sif::warning << "CFDP Source Handler: TM queue is full" << std::endl;
}
fsmCount++;
}
}
if (shortDelay) {
TaskFactory::delayTask(config::CFDP_SHORT_DELAY_MS);
continue;
}
TaskFactory::delayTask(config::CFDP_REGULAR_DELAY_MS);
}
auto& fsmRes = destHandler.performStateMachine();
// TODO: Error handling?
while (fsmRes.callStatus == CallStatus::CALL_AGAIN) {
destHandler.performStateMachine();
// TODO: Error handling?
}
return status;
}
ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) {
ReturnValue_t CfdpHandler::handlePduPacket(TmTcMessage& msg) {
auto accessorPair = tcStore->getData(msg.getStorageId());
if (accessorPair.first != OK) {
return accessorPair.first;
@ -102,16 +147,19 @@ ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) {
auto passToDestHandler = [&]() {
accessorPair.second.release();
PacketInfo info(type, msg.getStorageId(), directive);
result = destHandler.passPacket(info);
return destHandler.passPacket(info);
};
auto passToSourceHandler = [&]() {
accessorPair.second.release();
PacketInfo info(type, msg.getStorageId(), directive);
// Implement this function.
// result = srcHandler.passPacket(info);
};
if (directive == FileDirective::METADATA or directive == FileDirective::EOF_DIRECTIVE or
directive == FileDirective::PROMPT) {
// Section b) of 4.5.3: These PDUs should always be targeted towards the file receiver a.k.a.
// the destination handler
passToDestHandler();
return passToDestHandler();
} else if (directive == FileDirective::FINISH or directive == FileDirective::NAK or
directive == FileDirective::KEEP_ALIVE) {
// Section c) of 4.5.3: These PDUs should always be targeted towards the file sender a.k.a.
@ -128,9 +176,78 @@ ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) {
if (ackedDirective == FileDirective::EOF_DIRECTIVE) {
passToSourceHandler();
} else if (ackedDirective == FileDirective::FINISH) {
passToDestHandler();
return passToDestHandler();
}
}
}
return result;
}
ReturnValue_t CfdpHandler::handleCfdpRequest(CommandMessage& msg) {
if (msg.getCommand() == CfdpMessage::PUT_REQUEST) {
sif::info << "Received CFDP put request" << std::endl;
if (srcHandler.getState() != CfdpState::IDLE) {
if (putRequestQueue.full()) {
// TODO: Trigger event and discard request. Queue is full, too many requests.
return FAILED;
}
putRequestQueue.push(CfdpMessage::getStoreId(&msg));
} else {
PutRequest putRequest;
auto accessorPair = ipcStore.getData(CfdpMessage::getStoreId(&msg));
const uint8_t* dataPtr = accessorPair.second.data();
size_t dataSize = accessorPair.second.size();
ReturnValue_t result =
putRequest.deSerialize(&dataPtr, &dataSize, SerializeIF::Endianness::MACHINE);
if (result != OK) {
return result;
}
RemoteEntityCfg* remoteCfg;
remoteCfgProvider.getRemoteCfg(putRequest.getDestId(), &remoteCfg);
if (remoteCfg == nullptr) {
sif::error << "CfdpHandler: No remote configuration found for destination ID "
<< putRequest.getDestId() << std::endl;
// TODO: Trigger event
return FAILED;
}
sif::info << "Starting file copy operation for source file "
<< putRequest.getSourceName().getString() << " and dest file "
<< putRequest.getDestName().getString() << std::endl;
return srcHandler.transactionStart(putRequest, *remoteCfg);
}
}
return OK;
}
ReturnValue_t CfdpHandler::handlePduPacketMessages() {
ReturnValue_t status;
ReturnValue_t result = OK;
TmTcMessage pduMsg;
for (status = pduQueue.receiveMessage(&pduMsg); status == returnvalue::OK;
status = pduQueue.receiveMessage(&pduMsg)) {
result = handlePduPacket(pduMsg);
if (result != OK) {
// TODO: Maybe add printout with context specific information?
status = result;
}
}
return status;
}
ReturnValue_t CfdpHandler::handleCfdpMessages() {
ReturnValue_t status;
ReturnValue_t result;
CommandMessage cfdpMsg;
for (status = cfdpRequestQueue.receiveMessage(&cfdpMsg); status == returnvalue::OK;
status = cfdpRequestQueue.receiveMessage(&cfdpMsg)) {
result = handleCfdpRequest(cfdpMsg);
if (result != OK) {
sif::warning << "Handling CFDP request failed with code 0x" << std::setw(4) << std::hex
<< result << std::dec << std::endl;
triggerEvent(cfdp::events::HANDLING_CFDP_REQUEST_FAILED, 0, result);
// TODO: Maybe add printout with context specific information?
status = result;
}
}
return status;
}

43
mission/cfdp/CfdpHandler.h
View File

@ -1,6 +1,11 @@
#ifndef FSFW_EXAMPLE_HOSTED_CFDPHANDLER_H
#define FSFW_EXAMPLE_HOSTED_CFDPHANDLER_H
#include <etl/queue.h>
#include <fsfw/cfdp/handler/SourceHandler.h>
#include <fsfw/ipc/CommandMessage.h>
#include <fsfw/timemanager/Countdown.h>
#include <utility>
#include "fsfw/cfdp/handler/DestHandler.h"
@ -8,22 +13,29 @@
#include "fsfw/tasks/ExecutableObjectIF.h"
#include "fsfw/tmtcservices/AcceptsTelecommandsIF.h"
#include "fsfw/tmtcservices/TmTcMessage.h"
#include "fsfw/util/SeqCountProvider.h"
struct FsfwHandlerParams {
FsfwHandlerParams(object_id_t objectId, HasFileSystemIF& vfs, AcceptsTelemetryIF& packetDest,
StorageManagerIF& tcStore, StorageManagerIF& tmStore, MessageQueueIF& msgQueue)
StorageManagerIF& tcStore, StorageManagerIF& tmStore,
StorageManagerIF& ipcStore, MessageQueueIF& tmtcQueue,
MessageQueueIF& cfdpQueue)
: objectId(objectId),
vfs(vfs),
packetDest(packetDest),
tcStore(tcStore),
tmStore(tmStore),
msgQueue(msgQueue) {}
ipcStore(ipcStore),
tmtcQueue(tmtcQueue),
cfdpQueue(cfdpQueue) {}
object_id_t objectId{};
HasFileSystemIF& vfs;
AcceptsTelemetryIF& packetDest;
StorageManagerIF& tcStore;
StorageManagerIF& tmStore;
MessageQueueIF& msgQueue;
StorageManagerIF& ipcStore;
MessageQueueIF& tmtcQueue;
MessageQueueIF& cfdpQueue;
};
struct CfdpHandlerCfg {
@ -50,22 +62,39 @@ struct CfdpHandlerCfg {
class CfdpHandler : public SystemObject, public ExecutableObjectIF, public AcceptsTelecommandsIF {
public:
explicit CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerCfg& cfdpCfg);
explicit CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerCfg& cfdpCfg,
const std::atomic_bool& throttleSignal);
[[nodiscard]] const char* getName() const override;
[[nodiscard]] uint32_t getIdentifier() const override;
[[nodiscard]] MessageQueueId_t getRequestQueue() const override;
ReturnValue_t initialize() override;
ReturnValue_t performOperation(uint8_t operationCode) override;
[[noreturn]] ReturnValue_t performOperation(uint8_t operationCode) override;
private:
MessageQueueIF& msgQueue;
MessageQueueIF& pduQueue;
MessageQueueIF& cfdpRequestQueue;
bool throttlePeriodOngoing = false;
cfdp::LocalEntityCfg localCfg;
cfdp::RemoteConfigTableIF& remoteCfgProvider;
cfdp::FsfwParams fsfwParams;
SeqCountProviderU16 seqCntProvider;
cfdp::DestHandler destHandler;
cfdp::SourceHandler srcHandler;
etl::queue<store_address_t, 16> putRequestQueue;
StorageManagerIF& ipcStore;
StorageManagerIF* tcStore = nullptr;
StorageManagerIF* tmStore = nullptr;
ReturnValue_t handleCfdpPacket(TmTcMessage& msg);
const std::atomic_bool& throttleSignal;
ReturnValue_t handlePduPacketMessages();
ReturnValue_t handlePduPacket(TmTcMessage& msg);
ReturnValue_t handleCfdpRequest(CommandMessage& msg);
ReturnValue_t handleCfdpMessages();
};
#endif // FSFW_EXAMPLE_HOSTED_CFDPHANDLER_H

51
mission/cfdp/CfdpUser.cpp Normal file
View File

@ -0,0 +1,51 @@
#include "CfdpUser.h"
#include <fsfw/ipc/QueueFactory.h>
using namespace returnvalue;
namespace cfdp {
EiveUserHandler::EiveUserHandler(HasFileSystemIF& vfs, StorageManagerIF& ipcStore,
MessageQueueId_t cfdpRequestId)
: cfdp::UserBase(vfs), userQueue(QueueFactory::instance()->createMessageQueue(10)) {
if (userQueue == nullptr) {
sif::error << "EiveUserHandler: Queue creation failed" << std::endl;
return;
}
userQueue->setDefaultDestination(cfdpRequestId);
reservedMsgParser = new ReservedMessageParser(ipcStore, *userQueue, cfdpRequestId);
}
EiveUserHandler::~EiveUserHandler() { QueueFactory::instance()->deleteMessageQueue(userQueue); }
void EiveUserHandler::transactionIndication(const cfdp::TransactionId& id) {}
void EiveUserHandler::eofSentIndication(const cfdp::TransactionId& id) {}
void EiveUserHandler::transactionFinishedIndication(const cfdp::TransactionFinishedParams& params) {
sif::info << "File transaction finished for transaction with " << params.id << std::endl;
}
void EiveUserHandler::metadataRecvdIndication(const cfdp::MetadataRecvdParams& params) {
sif::info << "Metadata received for transaction with " << params.id << std::endl;
if (params.numberOfMsgsToUser > 0 and params.msgsToUserArray != nullptr) {
ReturnValue_t result =
reservedMsgParser->parse(params.msgsToUserArray, params.numberOfMsgsToUser);
if (result != OK) {
sif::warning << "EiveUserHandler: Parsing reserved CFDP messages failed" << std::endl;
}
}
}
void EiveUserHandler::fileSegmentRecvdIndication(const cfdp::FileSegmentRecvdParams& params) {}
void EiveUserHandler::reportIndication(const cfdp::TransactionId& id,
cfdp::StatusReportIF& report) {}
void EiveUserHandler::suspendedIndication(const cfdp::TransactionId& id, cfdp::ConditionCode code) {
}
void EiveUserHandler::resumedIndication(const cfdp::TransactionId& id, size_t progress) {}
void EiveUserHandler::faultIndication(const cfdp::TransactionId& id, cfdp::ConditionCode code,
size_t progress) {}
void EiveUserHandler::abandonedIndication(const cfdp::TransactionId& id, cfdp::ConditionCode code,
size_t progress) {}
void EiveUserHandler::eofRecvIndication(const cfdp::TransactionId& id) {
sif::info << "EOF PDU received for transaction with " << id << std::endl;
}
} // namespace cfdp

37
mission/cfdp/CfdpUser.h Normal file
View File

@ -0,0 +1,37 @@
#ifndef MISSION_CFDP_CFDPUSER_H_
#define MISSION_CFDP_CFDPUSER_H_
#include <fsfw/cfdp/handler/ReservedMessageParser.h>
#include <fsfw/cfdp/handler/UserBase.h>
namespace cfdp {
class EiveUserHandler : public cfdp::UserBase {
public:
explicit EiveUserHandler(HasFileSystemIF& vfs, StorageManagerIF& ipcStore,
MessageQueueId_t cfdpRequestId);
virtual ~EiveUserHandler();
void transactionIndication(const cfdp::TransactionId& id) override;
void eofSentIndication(const cfdp::TransactionId& id) override;
void transactionFinishedIndication(const cfdp::TransactionFinishedParams& params) override;
void metadataRecvdIndication(const cfdp::MetadataRecvdParams& params) override;
void fileSegmentRecvdIndication(const cfdp::FileSegmentRecvdParams& params) override;
void reportIndication(const cfdp::TransactionId& id, cfdp::StatusReportIF& report) override;
void suspendedIndication(const cfdp::TransactionId& id, cfdp::ConditionCode code);
void resumedIndication(const cfdp::TransactionId& id, size_t progress) override;
void faultIndication(const cfdp::TransactionId& id, cfdp::ConditionCode code,
size_t progress) override;
void abandonedIndication(const cfdp::TransactionId& id, cfdp::ConditionCode code,
size_t progress) override;
void eofRecvIndication(const cfdp::TransactionId& id) override;
private:
MessageQueueIF* userQueue;
ReservedMessageParser* reservedMsgParser;
};
} // namespace cfdp
#endif /* MISSION_CFDP_CFDPUSER_H_ */

57
mission/cfdp/Config.h
View File

@ -1,57 +0,0 @@
#ifndef MISSION_CFDP_CONFIG_H_
#define MISSION_CFDP_CONFIG_H_
#include "fsfw/cfdp.h"
namespace cfdp {
class EiveUserHandler : public cfdp::UserBase {
public:
explicit EiveUserHandler(HasFileSystemIF& vfs) : cfdp::UserBase(vfs) {}
virtual ~EiveUserHandler() = default;
void transactionIndication(const cfdp::TransactionId& id) override {}
void eofSentIndication(const cfdp::TransactionId& id) override {}
void transactionFinishedIndication(const cfdp::TransactionFinishedParams& params) override {
sif::info << "File transaction finished for transaction with " << params.id << std::endl;
}
void metadataRecvdIndication(const cfdp::MetadataRecvdParams& params) override {
sif::info << "Metadata received for transaction with " << params.id << std::endl;
}
void fileSegmentRecvdIndication(const cfdp::FileSegmentRecvdParams& params) override {}
void reportIndication(const cfdp::TransactionId& id, cfdp::StatusReportIF& report) override {}
void suspendedIndication(const cfdp::TransactionId& id, cfdp::ConditionCode code) override {}
void resumedIndication(const cfdp::TransactionId& id, size_t progress) override {}
void faultIndication(const cfdp::TransactionId& id, cfdp::ConditionCode code,
size_t progress) override {}
void abandonedIndication(const cfdp::TransactionId& id, cfdp::ConditionCode code,
size_t progress) override {}
void eofRecvIndication(const cfdp::TransactionId& id) override {
sif::info << "EOF PDU received for transaction with " << id << std::endl;
}
};
class EiveFaultHandler : public cfdp::FaultHandlerBase {
public:
void noticeOfSuspensionCb(cfdp::TransactionId& id, cfdp::ConditionCode code) override {
sif::warning << "Notice of suspension detected for transaction " << id
<< " with condition code: " << cfdp::getConditionCodeString(code) << std::endl;
}
void noticeOfCancellationCb(cfdp::TransactionId& id, cfdp::ConditionCode code) override {
sif::warning << "Notice of suspension detected for transaction " << id
<< " with condition code: " << cfdp::getConditionCodeString(code) << std::endl;
}
void abandonCb(cfdp::TransactionId& id, cfdp::ConditionCode code) override {
sif::warning << "Transaction " << id
<< " was abandoned, condition code : " << cfdp::getConditionCodeString(code)
<< std::endl;
}
void ignoreCb(cfdp::TransactionId& id, cfdp::ConditionCode code) override {
sif::warning << "Fault ignored for transaction " << id
<< ", condition code: " << cfdp::getConditionCodeString(code) << std::endl;
}
};
} // namespace cfdp
#endif /* MISSION_CFDP_CONFIG_H_ */

16
mission/cfdp/defs.h Normal file
View File

@ -0,0 +1,16 @@
#ifndef MISSION_CFDP_DEFS_H_
#define MISSION_CFDP_DEFS_H_
#include "eive/eventSubsystemIds.h"
#include "fsfw/events/Event.h"
namespace cfdp {
static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::CFDP_APP;
//! [EXPORT] : [COMMENT] P1: CFDP fault handler code. P2: CFDP condition code.
static constexpr Event FAULT_HANDLER_TRIGGERED = event::makeEvent(SUBSYSTEM_ID, 0, severity::LOW);
} // namespace cfdp
#endif /* MISSION_CFDP_DEFS_H_ */

156
mission/com/LiveTmTask.cpp
View File

@ -5,8 +5,15 @@
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>
#include "mission/sysDefs.h"
static constexpr bool DEBUG_TM_QUEUE_SPEED = false;
std::atomic_bool signals::CFDP_CHANNEL_THROTTLE_SIGNAL = false;
std::atomic_uint32_t signals::CFDP_MSG_COUNTER = 0;
LiveTmTask::LiveTmTask(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel,
VirtualChannelWithQueue& channel, const std::atomic_bool& ptmeLocked)
VirtualChannel& channel, const std::atomic_bool& ptmeLocked,
uint32_t regularTmQueueDepth, uint32_t cfdpQueueDepth)
: SystemObject(objectId),
modeHelper(this),
pusFunnel(pusFunnel),
@ -14,17 +21,47 @@ LiveTmTask::LiveTmTask(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunne
channel(channel),
ptmeLocked(ptmeLocked) {
requestQueue = QueueFactory::instance()->createMessageQueue();
cfdpTmQueue = QueueFactory::instance()->createMessageQueue(cfdpQueueDepth);
regularTmQueue = QueueFactory::instance()->createMessageQueue(regularTmQueueDepth);
}
ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) {
readCommandQueue();
bool handledTm;
ReturnValue_t result;
uint32_t consecutiveRegularCounter = 0;
uint32_t consecutiveCfdpCounter = 0;
bool isCfdp = false;
while (true) {
// The funnel tasks are scheduled here directly as well.
ReturnValue_t result = channel.handleNextTm(!ptmeLocked);
if (result == DirectTmSinkIF::IS_BUSY) {
sif::error << "Lost live TM, PAPB busy" << std::endl;
isCfdp = false;
// TODO: Must read CFDP TM queue and regular TM queue and forward them. Handle regular queue
// first.
handledTm = false;
updateBusyFlag();
if (!channelIsBusy) {
result = handleRegularTmQueue();
if (result == MessageQueueIF::EMPTY) {
result = handleCfdpTmQueue();
isCfdp = true;
}
if (result == returnvalue::OK) {
handledTm = true;
if (DEBUG_TM_QUEUE_SPEED) {
if (isCfdp) {
consecutiveCfdpCounter++;
} else {
consecutiveRegularCounter++;
}
}
} else if (result != MessageQueueIF::EMPTY) {
sif::warning << "LiveTmTask: TM queue failure, returncode 0x" << std::hex << std::setw(4)
<< result << std::dec << std::endl;
}
}
if (result == MessageQueueIF::EMPTY) {
cfdpBackpressureHandling();
if (!handledTm) {
if (tmFunnelCd.hasTimedOut()) {
pusFunnel.performOperation(0);
cfdpFunnel.performOperation(0);
@ -32,10 +69,19 @@ ReturnValue_t LiveTmTask::performOperation(uint8_t opCode) {
}
// Read command queue during idle times.
readCommandQueue();
if (DEBUG_TM_QUEUE_SPEED) {
if (consecutiveCfdpCounter > 0) {
sif::debug << "Consecutive CFDP TM handled: " << consecutiveCfdpCounter << std::endl;
}
if (consecutiveRegularCounter > 0) {
sif::debug << "Consecutive regular TM handled: " << consecutiveRegularCounter
<< std::endl;
}
consecutiveRegularCounter = 0;
consecutiveCfdpCounter = 0;
}
// 40 ms IDLE delay. Might tweak this in the future.
TaskFactory::delayTask(40);
} else {
packetCounter++;
}
}
}
@ -94,9 +140,103 @@ void LiveTmTask::readCommandQueue(void) {
}
}
ReturnValue_t LiveTmTask::handleRegularTmQueue() {
return handleGenericTmQueue(*regularTmQueue, false);
}
ReturnValue_t LiveTmTask::handleCfdpTmQueue() { return handleGenericTmQueue(*cfdpTmQueue, true); }
ReturnValue_t LiveTmTask::handleGenericTmQueue(MessageQueueIF& queue, bool isCfdp) {
TmTcMessage message;
ReturnValue_t result = queue.receiveMessage(&message);
if (result == MessageQueueIF::EMPTY) {
return result;
}
if (isCfdp and signals::CFDP_MSG_COUNTER > 0) {
signals::CFDP_MSG_COUNTER--;
}
if (DEBUG_CFDP_TO_LIVE_TM_TASK and signals::CFDP_MSG_COUNTER > 0) {
sif::debug << "LiveTmTask: CFDP message counter: " << signals::CFDP_MSG_COUNTER << std::endl;
}
store_address_t storeId = message.getStorageId();
const uint8_t* data = nullptr;
size_t size = 0;
result = tmStore->getData(storeId, &data, &size);
if (result != returnvalue::OK) {
sif::warning << "VirtualChannel::performOperation: Failed to read data from TM store"
<< std::endl;
tmStore->deleteData(storeId);
return result;
}
if (!ptmeLocked) {
size_t writtenSize = 0;
result = channel.write(data, size, writtenSize);
if (result == DirectTmSinkIF::PARTIALLY_WRITTEN) {
result = channel.handleWriteCompletionSynchronously(writtenSize, 200);
if (result != returnvalue::OK) {
// TODO: Event? Might lead to dangerous spam though..
sif::warning << "LiveTmTask: Synchronous write of last segment failed with code 0x"
<< std::setw(4) << std::hex << result << std::dec << std::endl;
}
} else if (result != returnvalue::OK) {
sif::error << "LiveTmTask: Channel write failed with code 0x" << std::hex << std::setw(4)
<< result << std::dec << std::endl;
}
}
// Try delete in any case, ignore failures (which should not happen), it is more important to
// propagate write errors.
tmStore->deleteData(storeId);
return result;
}
void LiveTmTask::throttleCfdp() {
throttlePeriodOngoing = true;
signals::CFDP_CHANNEL_THROTTLE_SIGNAL = true;
if (DEBUG_CFDP_TO_LIVE_TM_TASK) {
sif::debug << "Throttling CFDP" << std::endl;
}
}
void LiveTmTask::releaseCfdp() {
throttlePeriodOngoing = false;
signals::CFDP_CHANNEL_THROTTLE_SIGNAL = false;
if (DEBUG_CFDP_TO_LIVE_TM_TASK) {
sif::debug << "Releasing CFDP" << std::endl;
}
}
void LiveTmTask::updateBusyFlag() {
// We cache this as a member, because the busy bit can toggle very quickly..
channelIsBusy = channel.isBusy();
}
void LiveTmTask::cfdpBackpressureHandling() {
if (channelIsBusy and !throttlePeriodOngoing) {
// Throttle CFDP packet creator. It is by far the most relevant data creator, so throttling
// it is the easiest way to handle back pressure for now in a sensible way.
if (signals::CFDP_MSG_COUNTER >= (config::LIVE_CHANNEL_CFDP_QUEUE_SIZE / 2)) {
throttleCfdp();
}
} else if (!channelIsBusy and throttlePeriodOngoing) {
// Half full/empty flow control: Release the CFDP is the queue is empty enough.
if (signals::CFDP_MSG_COUNTER <= (config::LIVE_CHANNEL_CFDP_QUEUE_SIZE / 4)) {
releaseCfdp();
}
}
}
ModeTreeChildIF& LiveTmTask::getModeTreeChildIF() { return *this; }
ReturnValue_t LiveTmTask::initialize() {
modeHelper.initialize();
tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
if (tmStore == nullptr) {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
return returnvalue::OK;
}
MessageQueueId_t LiveTmTask::getNormalLiveQueueId() const { return regularTmQueue->getId(); }
MessageQueueId_t LiveTmTask::getCfdpLiveQueueId() const { return cfdpTmQueue->getId(); }

28
mission/com/LiveTmTask.h
View File

@ -11,6 +11,12 @@
#include <mission/tmtc/CfdpTmFunnel.h>
#include <mission/tmtc/PusTmFunnel.h>
#include <cstdint>
#include "eive/definitions.h"
static constexpr bool DEBUG_CFDP_TO_LIVE_TM_TASK = false;
class LiveTmTask : public SystemObject,
public HasModesIF,
public ExecutableObjectIF,
@ -18,35 +24,51 @@ class LiveTmTask : public SystemObject,
public ModeTreeConnectionIF {
public:
LiveTmTask(object_id_t objectId, PusTmFunnel& pusFunnel, CfdpTmFunnel& cfdpFunnel,
VirtualChannelWithQueue& channel, const std::atomic_bool& ptmeLocked);
VirtualChannel& channel, const std::atomic_bool& ptmeLocked,
uint32_t regularTmQueueDepth, uint32_t cfdpQueueDepth);
MessageQueueId_t getNormalLiveQueueId() const;
MessageQueueId_t getCfdpLiveQueueId() const;
ReturnValue_t performOperation(uint8_t opCode) override;
ReturnValue_t initialize() override;
ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) override;
private:
MessageQueueIF* requestQueue;
MessageQueueIF* cfdpTmQueue;
MessageQueueIF* regularTmQueue;
StorageManagerIF* tmStore = nullptr;
ModeHelper modeHelper;
Mode_t mode = HasModesIF::MODE_OFF;
Countdown tmFunnelCd = Countdown(100);
PusTmFunnel& pusFunnel;
CfdpTmFunnel& cfdpFunnel;
VirtualChannelWithQueue& channel;
uint32_t packetCounter = 0;
VirtualChannel& channel;
const std::atomic_bool& ptmeLocked;
bool throttlePeriodOngoing = false;
bool channelIsBusy = false;
void readCommandQueue(void);
ReturnValue_t handleRegularTmQueue();
ReturnValue_t handleCfdpTmQueue();
ReturnValue_t handleGenericTmQueue(MessageQueueIF& queue, bool isCfdp);
MessageQueueId_t getCommandQueue() const override;
void getMode(Mode_t* mode, Submode_t* submode) override;
void cfdpBackpressureHandling();
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t* msToReachTheMode) override;
void startTransition(Mode_t mode, Submode_t submode) override;
void announceMode(bool recursive) override;
void throttleCfdp();
void releaseCfdp();
void updateBusyFlag();
object_id_t getObjectId() const override;
const HasHealthIF* getOptHealthIF() const override;

12
mission/com/TmStoreTaskBase.cpp
View File

@ -138,8 +138,16 @@ ReturnValue_t TmStoreTaskBase::performDump(PersistentTmStoreWithTmQueue& store,
return result;
}
dumpedLen = tmReader.getFullPacketLen();
result = channel.write(tmReader.getFullData(), dumpedLen);
if (result == DirectTmSinkIF::IS_BUSY) {
size_t writtenSize = 0;
result = channel.write(tmReader.getFullData(), dumpedLen, writtenSize);
if (result == VirtualChannelIF::PARTIALLY_WRITTEN) {
result = channel.handleWriteCompletionSynchronously(writtenSize, 200);
if (result != returnvalue::OK) {
// TODO: Event? Might lead to dangerous spam though..
sif::warning << "PersistentTmStore: Synchronous write of last segment failed with code 0x"
<< std::setw(4) << std::hex << result << std::dec << std::endl;
}
} else if (result == DirectTmSinkIF::IS_BUSY) {
sif::warning << "PersistentTmStore: Unexpected VC channel busy" << std::endl;
} else if (result != returnvalue::OK) {
sif::warning << "PersistentTmStore: Unexpected VC channel write failure" << std::endl;

65
mission/com/VirtualChannel.cpp
View File

@ -1,25 +1,78 @@
#include "VirtualChannel.h"
#include <fsfw/tasks/TaskFactory.h>
VirtualChannel::VirtualChannel(object_id_t objectId, uint8_t vcId, const char* vcName, PtmeIF& ptme,
const std::atomic_bool& txOn)
: SystemObject(objectId), ptme(ptme), vcId(vcId), vcName(vcName), txOn(txOn) {}
ReturnValue_t VirtualChannel::initialize() { return returnvalue::OK; }
ReturnValue_t VirtualChannel::sendNextTm(const uint8_t* data, size_t size) {
return write(data, size);
ReturnValue_t VirtualChannel::sendNextTm(const uint8_t* data, size_t size, size_t& writtenSize) {
return write(data, size, writtenSize);
}
ReturnValue_t VirtualChannel::write(const uint8_t* data, size_t size) {
return ptme.writeToVc(vcId, data, size);
ReturnValue_t VirtualChannel::write(const uint8_t* data, size_t size, size_t& writtenSize) {
if (!ptme.containsVc(vcId)) {
return CHANNEL_DOES_NOT_EXIST;
}
return ptme.getVirtChannel(vcId)->write(data, size, writtenSize);
}
uint8_t VirtualChannel::getVcid() const { return vcId; }
ReturnValue_t VirtualChannel::advanceWrite(size_t& writtenSize) {
if (!ptme.containsVc(vcId)) {
return CHANNEL_DOES_NOT_EXIST;
}
return ptme.getVirtChannel(vcId)->advanceWrite(writtenSize);
}
bool VirtualChannel::writeActive() const {
if (!ptme.containsVc(vcId)) {
return CHANNEL_DOES_NOT_EXIST;
}
return ptme.getVirtChannel(vcId)->writeActive();
}
const char* VirtualChannel::getName() const { return vcName.c_str(); }
bool VirtualChannel::isBusy() const { return ptme.isBusy(vcId); }
bool VirtualChannel::isBusy() const {
if (!ptme.containsVc(vcId)) {
return CHANNEL_DOES_NOT_EXIST;
}
return ptme.getVirtChannel(vcId)->isBusy();
}
void VirtualChannel::cancelTransfer() { ptme.cancelTransfer(vcId); }
void VirtualChannel::cancelTransfer() {
if (!ptme.containsVc(vcId)) {
return;
}
ptme.getVirtChannel(vcId)->cancelTransfer();
}
bool VirtualChannel::isTxOn() const { return txOn; }
ReturnValue_t VirtualChannel::handleWriteCompletionSynchronously(size_t& writtenSize,
unsigned maxCompletionTimeMs) {
unsigned delayMs = 0;
while (true) {
if (isBusy()) {
if (delayMs >= maxCompletionTimeMs) {
break;
}
TaskFactory::delayTask(10);
delayMs += 10;
continue;
}
ReturnValue_t result = advanceWrite(writtenSize);
if (result == returnvalue::OK) {
// Transfer complete
return result;
} else if (result != PARTIALLY_WRITTEN) {
// Some error where we can not or should not continue the transfer.
return result;
}
}
return returnvalue::FAILED;
}

12
mission/com/VirtualChannel.h
View File

@ -15,6 +15,10 @@
*/
class VirtualChannel : public SystemObject, public VirtualChannelIF {
public:
static constexpr uint8_t CLASS_ID = CLASS_ID::VIRTUAL_CHANNEL;
static constexpr ReturnValue_t CHANNEL_DOES_NOT_EXIST = returnvalue::makeCode(CLASS_ID, 0);
/**
* @brief Constructor
*
@ -25,9 +29,13 @@ class VirtualChannel : public SystemObject, public VirtualChannelIF {
const std::atomic_bool& linkStateProvider);
ReturnValue_t initialize() override;
ReturnValue_t sendNextTm(const uint8_t* data, size_t size);
ReturnValue_t sendNextTm(const uint8_t* data, size_t size, size_t& writtenSize);
bool isBusy() const override;
ReturnValue_t write(const uint8_t* data, size_t size) override;
ReturnValue_t write(const uint8_t* data, size_t size, size_t& writtenSize) override;
ReturnValue_t advanceWrite(size_t& writtenSize) override;
ReturnValue_t handleWriteCompletionSynchronously(size_t& writtenSize,
unsigned maxCompletionTimeMs);
bool writeActive() const override;
void cancelTransfer() override;
uint8_t getVcid() const;
bool isTxOn() const;

13
mission/com/VirtualChannelWithQueue.cpp
View File

@ -36,8 +36,19 @@ ReturnValue_t VirtualChannelWithQueue::handleNextTm(bool performWriteOp) {
return result;
}
// TODO: Hnadle partial write handling
size_t writtenSize = 0;
if (performWriteOp) {
result = write(data, size);
result = write(data, size, writtenSize);
if (result == PARTIALLY_WRITTEN) {
result = handleWriteCompletionSynchronously(writtenSize, 200);
if (result != returnvalue::OK) {
// TODO: Event? Might lead to dangerous spam though..
sif::warning
<< "VirtualChannelWithQueue: Synchronous write of last segment failed with code 0x"
<< std::setw(4) << std::hex << result << std::dec << std::endl;
}
}
}
// Try delete in any case, ignore failures (which should not happen), it is more important to
// propagate write errors.

5
mission/controller/CMakeLists.txt
View File

@ -1,6 +1,7 @@
if(TGT_BSP MATCHES "arm/q7s" OR TGT_BSP MATCHES "")
target_sources(${LIB_EIVE_MISSION} PRIVATE ThermalController.cpp
AcsController.cpp)
target_sources(
${LIB_EIVE_MISSION} PRIVATE ThermalController.cpp AcsController.cpp
PowerController.cpp)
endif()
add_subdirectory(acs)

373
mission/controller/PowerController.cpp Normal file
View File

@ -0,0 +1,373 @@
#include <mission/controller/PowerController.h>
PowerController::PowerController(object_id_t objectId, bool enableHkSets)
: ExtendedControllerBase(objectId),
enableHkSets(enableHkSets),
parameterHelper(this),
pwrCtrlCoreHk(this),
enablePl(this) {}
ReturnValue_t PowerController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
return ExtendedControllerBase::initialize();
}
ReturnValue_t PowerController::handleCommandMessage(CommandMessage *message) {
ReturnValue_t result = actionHelper.handleActionMessage(message);
if (result == returnvalue::OK) {
return result;
}
result = parameterHelper.handleParameterMessage(message);
if (result == returnvalue::OK) {
return result;
}
return result;
}
MessageQueueId_t PowerController::getCommandQueue() const { return commandQueue->getId(); }
ReturnValue_t PowerController::getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper *parameterWrapper,
const ParameterWrapper *newValues,
uint16_t startAtIndex) {
switch (domainId) {
case 0x0: // direct members
switch (parameterId) {
case 0x0:
parameterWrapper->set(batteryInternalResistance);
break;
case 0x1:
parameterWrapper->set(batteryMaximumCapacity);
break;
case 0x2: {
float oldCoulombCounterVoltageUpperThreshold = coulombCounterVoltageUpperThreshold;
ReturnValue_t result = newValues->getElement(&coulombCounterVoltageUpperThreshold);
if (result != returnvalue::OK) {
coulombCounterVoltageUpperThreshold = oldCoulombCounterVoltageUpperThreshold;
return result;
}
result = calculateCoulombCounterChargeUpperThreshold();
if (result != returnvalue::OK) {
coulombCounterVoltageUpperThreshold = oldCoulombCounterVoltageUpperThreshold;
return result;
}
parameterWrapper->set(coulombCounterVoltageUpperThreshold);
break;
}
case 0x3:
parameterWrapper->set(maxAllowedTimeDiff);
break;
case 0x4:
parameterWrapper->set(payloadOpLimitOn);
break;
case 0x5:
parameterWrapper->set(payloadOpLimitLow);
break;
case 0x6:
parameterWrapper->set(higherModesLimit);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
default:
return INVALID_DOMAIN_ID;
};
return returnvalue::OK;
}
void PowerController::performControlOperation() {
switch (internalState) {
case InternalState::STARTUP: {
initialCountdown.resetTimer();
internalState = InternalState::INITIAL_DELAY;
return;
}
case InternalState::INITIAL_DELAY: {
if (initialCountdown.hasTimedOut()) {
internalState = InternalState::INIT;
}
return;
}
case InternalState::INIT: {
ReturnValue_t result = calculateCoulombCounterChargeUpperThreshold();
if (result == returnvalue::OK) {
internalState = InternalState::READY;
}
return;
}
case InternalState::READY: {
if (mode != MODE_NORMAL) {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
enablePl.setValidity(false, true);
}
}
if (mode != MODE_OFF) {
calculateStateOfCharge();
if (mode == MODE_NORMAL) {
watchStateOfCharge();
}
}
break;
}
default:
break;
}
}
ReturnValue_t PowerController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(pwrctrl::PoolIds::TOTAL_BATTERY_CURRENT, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(pwrctrl::PoolIds::OPEN_CIRCUIT_VOLTAGE_CHARGE,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(pwrctrl::PoolIds::COULOMB_COUNTER_CHARGE, new PoolEntry<float>({0.0}));
poolManager.subscribeForRegularPeriodicPacket({pwrCtrlCoreHk.getSid(), enableHkSets, 60.0});
localDataPoolMap.emplace(pwrctrl::PoolIds::PAYLOAD_FLAG, new PoolEntry<uint8_t>({false}));
poolManager.subscribeForRegularPeriodicPacket({enablePl.getSid(), false, 60.0});
return returnvalue::OK;
}
LocalPoolDataSetBase *PowerController::getDataSetHandle(sid_t sid) {
switch (sid.ownerSetId) {
case pwrctrl::CORE_HK:
return &pwrCtrlCoreHk;
case pwrctrl::ENABLE_PL:
return &enablePl;
default:
return nullptr;
}
return nullptr;
}
ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t *msToReachTheMode) {
if (mode == MODE_OFF or mode == MODE_ON or mode == MODE_NORMAL) {
if (submode == SUBMODE_NONE) {
return returnvalue::OK;
} else {
return INVALID_SUBMODE;
}
}
return INVALID_MODE;
}
void PowerController::calculateStateOfCharge() {
// get time
Clock::getClock_timeval(&now);
// update EPS HK values
ReturnValue_t result = updateEpsData();
if (result != returnvalue::OK) {
triggerEvent(power::DATASET_READ_FAILED);
sif::error << "Power Controller::Reading of Datasets has failed" << std::endl;
{
PoolReadGuard pg(&pwrCtrlCoreHk);
if (pg.getReadResult() == returnvalue::OK) {
pwrCtrlCoreHk.totalBatteryCurrent.value = INVALID_TOTAL_BATTERY_CURRENT;
pwrCtrlCoreHk.openCircuitVoltageCharge.value = INVALID_SOC;
pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
pwrCtrlCoreHk.setValidity(false, true);
}
}
// store time for next run
oldTime = now;
return;
}
// calculate total battery current
iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.dischargeCurrent.value;
result = calculateOpenCircuitVoltageCharge();
if (result != returnvalue::OK) {
// notifying events have already been triggered
{
PoolReadGuard pg(&pwrCtrlCoreHk);
if (pg.getReadResult() == returnvalue::OK) {
pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
pwrCtrlCoreHk.totalBatteryCurrent.setValid(true);
pwrCtrlCoreHk.openCircuitVoltageCharge.value = INVALID_SOC;
pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(false);
pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
}
}
// store time for next run
oldTime = now;
return;
}
result = calculateCoulombCounterCharge();
if (result != returnvalue::OK) {
// notifying events have already been triggered
{
PoolReadGuard pg(&pwrCtrlCoreHk);
if (pg.getReadResult() == returnvalue::OK) {
pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
pwrCtrlCoreHk.totalBatteryCurrent.setValid(true);
pwrCtrlCoreHk.openCircuitVoltageCharge.value =
charge2stateOfCharge(openCircuitVoltageCharge, false);
pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(true);
pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
}
}
// store time for next run
oldTime = now;
return;
}
// commit to dataset
{
PoolReadGuard pg(&pwrCtrlCoreHk);
if (pg.getReadResult() == returnvalue::OK) {
pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
pwrCtrlCoreHk.openCircuitVoltageCharge.value =
charge2stateOfCharge(openCircuitVoltageCharge, false);
pwrCtrlCoreHk.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge, true);
pwrCtrlCoreHk.setValidity(true, true);
}
}
// store time for next run
oldTime = now;
}
void PowerController::watchStateOfCharge() {
if (pwrCtrlCoreHk.coulombCounterCharge.isValid()) {
if (pwrCtrlCoreHk.coulombCounterCharge.value < payloadOpLimitOn) {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
enablePl.plUseAllowed.value = false;
enablePl.setValidity(true, true);
}
} else {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
enablePl.plUseAllowed.value = true;
enablePl.setValidity(true, true);
}
}
if (not pwrLvlLowFlag and pwrCtrlCoreHk.coulombCounterCharge.value < payloadOpLimitLow) {
triggerEvent(power::POWER_LEVEL_LOW);
pwrLvlLowFlag = true;
} else if (pwrLvlLowFlag and pwrCtrlCoreHk.coulombCounterCharge.value > payloadOpLimitLow) {
pwrLvlLowFlag = false;
}
if (not pwrLvlCriticalFlag and pwrCtrlCoreHk.coulombCounterCharge.value < higherModesLimit) {
triggerEvent(power::POWER_LEVEL_CRITICAL);
pwrLvlCriticalFlag = true;
} else if (pwrLvlCriticalFlag and pwrCtrlCoreHk.coulombCounterCharge.value > higherModesLimit) {
pwrLvlCriticalFlag = false;
}
} else {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
enablePl.plUseAllowed.value = false;
enablePl.setValidity(true, true);
}
}
}
ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
float vBatCorrected =
(bpxBatteryHk.battVoltage.value - iBat * batteryInternalResistance) * CONVERT_FROM_MILLI;
uint8_t lookUpTableIdx = LOOK_UP_TABLE_MAX_IDX;
ReturnValue_t result = lookUpTableOcvIdxFinder(vBatCorrected, lookUpTableIdx, false);
if (result != returnvalue::OK) {
return result;
}
openCircuitVoltageCharge = linearInterpolation(
vBatCorrected, lookUpTableOcv[1][lookUpTableIdx], lookUpTableOcv[1][lookUpTableIdx + 1],
lookUpTableOcv[0][lookUpTableIdx], lookUpTableOcv[0][lookUpTableIdx + 1]);
return returnvalue::OK;
}
ReturnValue_t PowerController::calculateCoulombCounterCharge() {
double timeDiff = timevalOperations::toDouble(now - oldTime);
if (timeDiff > maxAllowedTimeDiff) {
// should not be a permanent state so no spam protection required
triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDiff * 10));
sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDiff
<< std::endl;
return returnvalue::FAILED;
}
if (not pwrCtrlCoreHk.coulombCounterCharge.isValid()) {
coulombCounterCharge = openCircuitVoltageCharge;
} else {
coulombCounterCharge =
coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDiff * SECONDS_TO_HOURS;
if (coulombCounterCharge >= coulombCounterChargeUpperThreshold) {
coulombCounterCharge = coulombCounterChargeUpperThreshold;
}
}
return returnvalue::OK;
}
ReturnValue_t PowerController::updateEpsData() {
std::vector<ReturnValue_t> results;
{
PoolReadGuard pgBat(&bpxBatteryHk);
results.push_back(pgBat.getReadResult());
}
{
PoolReadGuard pgP60(&p60CoreHk);
results.push_back(pgP60.getReadResult());
}
for (const auto &result : results) {
if (result != returnvalue::OK) {
return result;
}
}
return returnvalue::OK;
}
float PowerController::charge2stateOfCharge(float capacity, bool coulombCounter) {
if (coulombCounter) {
return capacity / coulombCounterChargeUpperThreshold;
}
return capacity / batteryMaximumCapacity;
}
float PowerController::linearInterpolation(float x, float x0, float x1, float y0, float y1) {
return y0 + (x - x0) * (y1 - y0) / (x1 - x0);
}
ReturnValue_t PowerController::lookUpTableOcvIdxFinder(float voltage, uint8_t &idx, bool paramCmd) {
if (voltage >= lookUpTableOcv[1][99]) {
if (not voltageOutOfBoundsFlag and not paramCmd) {
triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 0, static_cast<uint32_t>(voltage * 10));
voltageOutOfBoundsFlag = true;
}
sif::error << "Power Controller::Voltage is too high: " << voltage << std::endl;
return returnvalue::FAILED;
} else if (voltage <= lookUpTableOcv[1][0]) {
if (not voltageOutOfBoundsFlag and not paramCmd) {
triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 1, static_cast<uint32_t>(voltage * 10));
voltageOutOfBoundsFlag = true;
}
sif::error << "Power Controller::Voltage is too low: " << voltage << std::endl;
return returnvalue::FAILED;
}
voltageOutOfBoundsFlag = false;
while (lookUpTableOcv[1][idx] > voltage) {
idx--;
}
return returnvalue::OK;
}
ReturnValue_t PowerController::calculateCoulombCounterChargeUpperThreshold() {
uint8_t lookUpTableIdx = LOOK_UP_TABLE_MAX_IDX;
ReturnValue_t result =
lookUpTableOcvIdxFinder(coulombCounterVoltageUpperThreshold, lookUpTableIdx, true);
if (result != returnvalue::OK) {
return result;
}
coulombCounterChargeUpperThreshold =
linearInterpolation(coulombCounterVoltageUpperThreshold, lookUpTableOcv[1][lookUpTableIdx],
lookUpTableOcv[1][lookUpTableIdx + 1], lookUpTableOcv[0][lookUpTableIdx],
lookUpTableOcv[0][lookUpTableIdx + 1]);
return returnvalue::OK;
}

131
mission/controller/PowerController.h Normal file
View File

@ -0,0 +1,131 @@
#ifndef MISSION_CONTROLLER_POWERCONTROLLER_H_
#define MISSION_CONTROLLER_POWERCONTROLLER_H_
#include <eive/objects.h>
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
#include <mission/controller/controllerdefinitions/PowerCtrlDefinitions.h>
#include <mission/power/bpxBattDefs.h>
#include <mission/power/gsDefs.h>
#include <cmath>
class PowerController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
public:
static constexpr dur_millis_t INIT_DELAY = 500;
PowerController(object_id_t objectId, bool enableHkSets);
MessageQueueId_t getCommandQueue() const;
ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
uint16_t startAtIndex) override;
private:
bool enableHkSets = false;
ParameterHelper parameterHelper;
enum class InternalState { STARTUP, INITIAL_DELAY, INIT, READY };
InternalState internalState = InternalState::STARTUP;
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY);
ReturnValue_t initialize() override;
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) override;
LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t* msToReachTheMode) override;
void performControlOperation() override;
void calculateStateOfCharge();
void watchStateOfCharge();
ReturnValue_t calculateOpenCircuitVoltageCharge();
ReturnValue_t calculateCoulombCounterCharge();
ReturnValue_t updateEpsData();
float charge2stateOfCharge(float capacity, bool coulombCounter);
ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx, bool paramCmd);
float linearInterpolation(float x, float x0, float x1, float y0, float y1);
ReturnValue_t calculateCoulombCounterChargeUpperThreshold();
// Parameters
float batteryInternalResistance = 0.06798200367; // [Ohm]
float batteryMaximumCapacity = 2.6 * 2; // [Ah]
float coulombCounterVoltageUpperThreshold = 16.2; // [V]
double maxAllowedTimeDiff = 1.5; // [s]
float payloadOpLimitOn = 0.90; // [%]
float payloadOpLimitLow = 0.75; // [%]
float higherModesLimit = 0.6; // [%]
// OCV Look-up-Table {[Ah],[V]}
static constexpr uint8_t LOOK_UP_TABLE_MAX_IDX = 99;
float lookUpTableOcv[2][100] = {
{0.00000000e+00, 3.16227766e-04, 4.52809661e-04, 6.48382625e-04, 9.28425483e-04,
1.32942162e-03, 1.90361194e-03, 2.72580074e-03, 3.90310099e-03, 5.58888885e-03,
8.00278514e-03, 1.14592671e-02, 1.64086377e-02, 2.34956903e-02, 3.36437110e-02,
4.81747620e-02, 6.89819174e-02, 9.87758887e-02, 1.41438170e-01, 2.02526713e-01,
2.90000000e-01, 3.00000000e-01, 3.62820513e-01, 4.25641026e-01, 4.88461538e-01,
5.51282051e-01, 6.14102564e-01, 6.76923077e-01, 7.39743590e-01, 8.02564103e-01,
8.65384615e-01, 9.28205128e-01, 9.91025641e-01, 1.05384615e+00, 1.11666667e+00,
1.17948718e+00, 1.24230769e+00, 1.30512821e+00, 1.36794872e+00, 1.43076923e+00,
1.49358974e+00, 1.55641026e+00, 1.61923077e+00, 1.68205128e+00, 1.74487179e+00,
1.80769231e+00, 1.87051282e+00, 1.93333333e+00, 1.99615385e+00, 2.05897436e+00,
2.12179487e+00, 2.18461538e+00, 2.24743590e+00, 2.31025641e+00, 2.37307692e+00,
2.43589744e+00, 2.49871795e+00, 2.56153846e+00, 2.62435897e+00, 2.68717949e+00,
2.75000000e+00, 2.81282051e+00, 2.87564103e+00, 2.93846154e+00, 3.00128205e+00,
3.06410256e+00, 3.12692308e+00, 3.18974359e+00, 3.25256410e+00, 3.31538462e+00,
3.37820513e+00, 3.44102564e+00, 3.50384615e+00, 3.56666667e+00, 3.62948718e+00,
3.69230769e+00, 3.75512821e+00, 3.81794872e+00, 3.88076923e+00, 3.94358974e+00,
4.00641026e+00, 4.06923077e+00, 4.13205128e+00, 4.19487179e+00, 4.25769231e+00,
4.32051282e+00, 4.38333333e+00, 4.44615385e+00, 4.50897436e+00, 4.57179487e+00,
4.63461538e+00, 4.69743590e+00, 4.76025641e+00, 4.82307692e+00, 4.88589744e+00,
4.94871795e+00, 5.01153846e+00, 5.07435897e+00, 5.13717949e+00, 5.20000000e+00},
{12.52033533, 12.58720948, 12.61609309, 12.65612591, 12.67105282, 12.69242681, 12.72303245,
12.76685696, 12.80313768, 12.83600741, 12.8830739, 12.94720576, 13.00112629, 13.07833563,
13.17486308, 13.27128842, 13.37713879, 13.49275604, 13.60395193, 13.68708863, 13.75196335,
13.7582376, 13.79298643, 13.82885799, 13.87028849, 13.91585718, 13.96701874, 14.02343574,
14.07665641, 14.12626342, 14.1675095, 14.20582917, 14.23342159, 14.25724476, 14.27264301,
14.28922389, 14.30898535, 14.32750837, 14.34358057, 14.35965277, 14.37698366, 14.3943261,
14.41079196, 14.42679817, 14.44261008, 14.45771025, 14.47281042, 14.48751461, 14.50193089,
14.5164887, 14.53193477, 14.54738084, 14.56341235, 14.58054578, 14.59799552, 14.61632769,
14.63716465, 14.66935073, 14.70511347, 14.74315094, 14.77251031, 14.80005585, 14.8315427,
14.86078285, 14.89444687, 14.93495892, 14.97114013, 15.01055751, 15.0538516, 15.09698825,
15.14850029, 15.18947994, 15.24249483, 15.28521713, 15.335695, 15.37950723, 15.43241224,
15.48082213, 15.53314287, 15.58907248, 15.64030253, 15.68385331, 15.74149122, 15.80051882,
15.84959348, 15.90443241, 15.95743724, 16.01283068, 16.07629253, 16.13470801, 16.1890518,
16.24200781, 16.30521118, 16.37368429, 16.43661267, 16.49604875, 16.56223813, 16.62741412,
16.67249918, 16.74926904}};
// Variables
timeval now;
timeval oldTime;
int16_t iBat = 0; // [mA]
float openCircuitVoltageCharge = 0.0; // [Ah]
float coulombCounterCharge = 0.0; // [Ah]
float coulombCounterChargeUpperThreshold = 0.0; // [Ah]
float oldCoulombCounterVoltageUpperThreshold = 0.0; // [V]
static constexpr float CONVERT_FROM_MILLI = 1e-3;
static constexpr float SECONDS_TO_HOURS = 1. / (60. * 60.);
static constexpr int16_t INVALID_TOTAL_BATTERY_CURRENT = 0;
static constexpr float INVALID_SOC = -1;
bool pwrLvlLowFlag = false;
bool pwrLvlCriticalFlag = false;
bool voltageOutOfBoundsFlag = false;
// HK Datasets for Calculation
BpxBatteryHk bpxBatteryHk = BpxBatteryHk(objects::BPX_BATT_HANDLER);
P60Dock::CoreHkSet p60CoreHk = P60Dock::CoreHkSet(objects::P60DOCK_HANDLER);
// Output Dataset
pwrctrl::CoreHk pwrCtrlCoreHk;
// Dataset for PL Flag
pwrctrl::EnablePl enablePl;
};
#endif /* MISSION_CONTROLLER_POWERCONTROLLER_H_ */

51
mission/controller/controllerdefinitions/PowerCtrlDefinitions.h Normal file
View File

@ -0,0 +1,51 @@
#ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_POWERCTRLDEFINITIONS_H_
#define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_POWERCTRLDEFINITIONS_H_
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
#include <fsfw/datapoollocal/localPoolDefinitions.h>
#include <mission/power/defs.h>
#include <cstdint>
namespace pwrctrl {
enum SetIds : uint32_t { CORE_HK, ENABLE_PL };
enum PoolIds : lp_id_t {
TOTAL_BATTERY_CURRENT,
OPEN_CIRCUIT_VOLTAGE_CHARGE,
COULOMB_COUNTER_CHARGE,
PAYLOAD_FLAG
};
static constexpr uint8_t CORE_HK_ENTRIES = 3;
static constexpr uint8_t ENABLE_PL_ENTRIES = 1;
class CoreHk : public StaticLocalDataSet<CORE_HK_ENTRIES> {
public:
CoreHk(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CORE_HK) {}
lp_var_t<int16_t> totalBatteryCurrent =
lp_var_t<int16_t>(sid.objectId, TOTAL_BATTERY_CURRENT, this);
lp_var_t<float> openCircuitVoltageCharge =
lp_var_t<float>(sid.objectId, OPEN_CIRCUIT_VOLTAGE_CHARGE, this);
lp_var_t<float> coulombCounterCharge =
lp_var_t<float>(sid.objectId, COULOMB_COUNTER_CHARGE, this);
private:
};
class EnablePl : public StaticLocalDataSet<ENABLE_PL_ENTRIES> {
public:
EnablePl(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ENABLE_PL) {}
EnablePl(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, ENABLE_PL)) {}
lp_var_t<uint8_t> plUseAllowed = lp_var_t<uint8_t>(sid.objectId, PAYLOAD_FLAG, this);
private:
};
} // namespace pwrctrl
#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_POWERCTRLDEFINITIONS_H_ */

49
mission/genericFactory.cpp
View File

@ -22,7 +22,9 @@
#include <fsfw/tcdistribution/PusDistributor.h>
#include <fsfw/timemanager/CdsShortTimeStamper.h>
#include <fsfw_hal/host/HostFilesystem.h>
#include <mission/cfdp/CfdpFaultHandler.h>
#include <mission/cfdp/CfdpHandler.h>
#include <mission/cfdp/CfdpUser.h>
#include <mission/controller/ThermalController.h>
#include <mission/genericFactory.h>
#include <mission/persistentTmStoreDefs.h>
@ -44,7 +46,6 @@
#include "devices/gpioIds.h"
#include "eive/definitions.h"
#include "fsfw/pus/Service11TelecommandScheduling.h"
#include "mission/cfdp/Config.h"
#include "mission/system/acs/RwAssembly.h"
#include "mission/system/acs/acsModeTree.h"
#include "mission/system/tcs/tcsModeTree.h"
@ -85,8 +86,6 @@ EntityId REMOTE_CFDP_ID(UnsignedByteField<uint16_t>(config::EIVE_GROUND_CFDP_ENT
RemoteEntityCfg GROUND_REMOTE_CFG(REMOTE_CFDP_ID);
OneRemoteConfigProvider REMOTE_CFG_PROVIDER(GROUND_REMOTE_CFG);
HostFilesystem HOST_FS;
EiveUserHandler USER_HANDLER(HOST_FS);
EiveFaultHandler EIVE_FAULT_HANDLER;
} // namespace cfdp
@ -98,7 +97,8 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan,
StorageManagerIF** ipcStore, StorageManagerIF** tmStore,
PersistentTmStores& stores,
uint32_t eventManagerQueueDepth, bool enableHkSets) {
uint32_t eventManagerQueueDepth, bool enableHkSets,
bool routeToPersistentStores) {
// Framework objects
new EventManager(objects::EVENT_MANAGER, eventManagerQueueDepth);
auto healthTable = new HealthTable(objects::HEALTH_TABLE);
@ -111,44 +111,45 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
StorageManagerIF* tcStore;
{
PoolManager::LocalPoolConfig poolCfg = {{250, 16}, {250, 32}, {250, 64},
{150, 128}, {120, 1024}, {120, 2048}};
{150, 128}, {120, 1200}, {120, 2048}};
tcStore = new PoolManager(objects::TC_STORE, poolCfg);
}
{
PoolManager::LocalPoolConfig poolCfg = {{600, 32}, {400, 64}, {400, 128},
{300, 512}, {250, 1024}, {150, 2048}};
{400, 512}, {800, 1200}, {150, 2048}};
*tmStore = new PoolManager(objects::TM_STORE, poolCfg);
}
{
PoolManager::LocalPoolConfig poolCfg = {{300, 16}, {250, 32}, {150, 64}, {150, 128},
{100, 256}, {50, 512}, {50, 1024}, {10, 2048}};
{100, 256}, {50, 512}, {50, 1200}, {10, 2048}};
*ipcStore = new PoolManager(objects::IPC_STORE, poolCfg);
}
PoolManager::LocalPoolConfig poolCfg = {{300, 32}, {400, 64}, {250, 128},
{150, 512}, {150, 1024}, {150, 2048}};
{150, 512}, {400, 1200}, {150, 2048}};
auto* ramToFileStore = new PoolManager(objects::DOWNLINK_RAM_STORE, poolCfg);
#if OBSW_ADD_TCPIP_SERVERS == 1
#if OBSW_ADD_TMTC_UDP_SERVER == 1
auto udpBridge =
new UdpTmTcBridge(objects::UDP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR, 120);
auto udpBridge = new UdpTmTcBridge(objects::UDP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR,
config::UDP_MSG_QUEUE_DEPTH);
new UdpTcPollingTask(objects::UDP_TMTC_POLLING_TASK, objects::UDP_TMTC_SERVER);
sif::info << "Created UDP server for TMTC commanding with listener port "
<< udpBridge->getUdpPort() << std::endl;
udpBridge->setMaxNumberOfPacketsStored(config::MAX_STORED_CMDS_UDP);
udpBridge->setMaxNumberOfPacketsStored(config::UDP_MAX_STORED_CMDS);
#endif
#if OBSW_ADD_TMTC_TCP_SERVER == 1
auto tcpBridge =
new TcpTmTcBridge(objects::TCP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR, 120);
auto tcpBridge = new TcpTmTcBridge(objects::TCP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR,
config::TCP_MSG_QUEUE_DEPTH);
TcpTmTcServer::TcpConfig cfg(true, true);
auto tcpServer = new TcpTmTcServer(objects::TCP_TMTC_POLLING_TASK, objects::TCP_TMTC_SERVER, cfg);
// TCP is stream based. Use packet ID as start marker when parsing for space packets
tcpServer->setSpacePacketParsingOptions({common::PUS_PACKET_ID, common::CFDP_PACKET_ID});
sif::info << "Created TCP server for TMTC commanding with listener port "
<< tcpServer->getTcpPort() << std::endl;
tcpBridge->setMaxNumberOfPacketsStored(config::MAX_STORED_CMDS_TCP);
tcpBridge->setMaxNumberOfPacketsStored(config::TCP_MAX_STORED_CMDS);
tcpBridge->setNumberOfSentPacketsPerCycle(config::TCP_MAX_NUMBER_TMS_SENT_PER_CYCLE);
#endif /* OBSW_USE_TMTC_TCP_BRIDGE == 0 */
#endif /* OBSW_ADD_TCPIP_BRIDGE == 1 */
@ -223,8 +224,12 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
**ipcStore, config::MAX_CFDP_FUNNEL_QUEUE_DEPTH, sdcMan,
config::CFDP_SEQUENCE_COUNT_FILE,
core::SAVE_CFDP_SEQUENCE_COUNT);
*cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, stores.cfdpStore->getReportReceptionQueue(0),
*ramToFileStore, config::EIVE_CFDP_APID);
std::optional<MessageQueueId_t> fileStoreDest{};
if (routeToPersistentStores) {
fileStoreDest = stores.cfdpStore->getReportReceptionQueue(0);
}
*cfdpFunnel =
new CfdpTmFunnel(cfdpFunnelCfg, fileStoreDest, *ramToFileStore, config::EIVE_CFDP_APID);
#if OBSW_ADD_TCPIP_SERVERS == 1
#if OBSW_ADD_TMTC_UDP_SERVER == 1
@ -274,16 +279,20 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
CfdpDistribCfg distribCfg(objects::CFDP_DISTRIBUTOR, *tcStore, cfdpMsgQueue);
new CfdpDistributor(distribCfg);
auto* msgQueue = QueueFactory::instance()->createMessageQueue(32);
auto* tmtcQueue = QueueFactory::instance()->createMessageQueue(32);
auto* cfdpQueue = QueueFactory::instance()->createMessageQueue(16);
auto eiveUserHandler = new cfdp::EiveUserHandler(HOST_FS, **ipcStore, cfdpQueue->getId());
FsfwHandlerParams params(objects::CFDP_HANDLER, HOST_FS, **cfdpFunnel, *tcStore, **tmStore,
*msgQueue);
**ipcStore, *tmtcQueue, *cfdpQueue);
cfdp::IndicationCfg indicationCfg;
UnsignedByteField<uint16_t> apid(config::EIVE_LOCAL_CFDP_ENTITY_ID);
cfdp::EntityId localId(apid);
GROUND_REMOTE_CFG.defaultChecksum = cfdp::ChecksumType::CRC_32;
CfdpHandlerCfg cfdpCfg(localId, indicationCfg, USER_HANDLER, EIVE_FAULT_HANDLER, PACKET_LIST,
GROUND_REMOTE_CFG.maxFileSegmentLen = config::CFDP_MAX_FILE_SEGMENT_LEN;
auto eiveFaultHandler = new EiveFaultHandler(objects::CFDP_FAULT_HANDLER);
CfdpHandlerCfg cfdpCfg(localId, indicationCfg, *eiveUserHandler, *eiveFaultHandler, PACKET_LIST,
LOST_SEGMENTS, REMOTE_CFG_PROVIDER);
auto* cfdpHandler = new CfdpHandler(params, cfdpCfg);
auto* cfdpHandler = new CfdpHandler(params, cfdpCfg, signals::CFDP_CHANNEL_THROTTLE_SIGNAL);
// All CFDP packets arrive wrapped inside CCSDS space packets
CcsdsDistributorIF::DestInfo info("CFDP Destination", config::EIVE_CFDP_APID,
cfdpHandler->getRequestQueue(), true);

2
mission/genericFactory.h
View File

@ -46,7 +46,7 @@ void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel,
CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan,
StorageManagerIF** ipcStore, StorageManagerIF** tmStore,
PersistentTmStores& stores, uint32_t eventManagerQueueDepth,
bool enableHkSets);
bool enableHkSets, bool routeToPersistentStores);
void createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& pwrSwitcher,
HeaterHandler*& heaterHandler);

25
mission/payload/PayloadPcduHandler.cpp
View File

@ -395,7 +395,6 @@ void PayloadPcduHandler::quickTransitionBackToOff(bool startTransitionToOff, boo
void PayloadPcduHandler::checkAdcValues() {
using namespace plpcdu;
checkJsonFileInit();
adcSet.processed[U_BAT_DIV_6] =
static_cast<float>(adcSet.channels[0]) * VOLTAGE_DIV / MAX122X_BIT * MAX122X_VREF;
adcSet.processed[U_NEG_V_FB] =
@ -529,6 +528,10 @@ void PayloadPcduHandler::checkJsonFileInit() {
params.initialize(sdcMan->getCurrentMountPrefix());
jsonFileInitComplete = true;
}
} else {
if (not sdcMan->isSdCardUsable(std::nullopt)) {
jsonFileInitComplete = false;
}
}
}
@ -564,6 +567,26 @@ bool PayloadPcduHandler::checkCurrent(float val, float upperBound, Event event)
return true;
}
ReturnValue_t PayloadPcduHandler::initialize() {
checkJsonFileInit();
return DeviceHandlerBase::initialize();
}
void PayloadPcduHandler::performOperationHook() { checkJsonFileInit(); }
ReturnValue_t PayloadPcduHandler::checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
uint32_t* msToReachTheMode) {
if (commandedMode != MODE_OFF) {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
if (enablePl.plUseAllowed.isValid() and not enablePl.plUseAllowed.value) {
return NON_OP_STATE_OF_CHARGE;
}
}
}
return DeviceHandlerBase::checkModeCommand(commandedMode, commandedSubmode, msToReachTheMode);
}
ReturnValue_t PayloadPcduHandler::isModeCombinationValid(Mode_t mode, Submode_t submode) {
using namespace plpcdu;
if (mode == MODE_NORMAL) {

7
mission/payload/PayloadPcduHandler.h
View File

@ -4,6 +4,7 @@
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <fsfw/globalfunctions/PeriodicOperationDivider.h>
#include <fsfw/timemanager/Countdown.h>
#include <mission/controller/controllerdefinitions/PowerCtrlDefinitions.h>
#include <mission/payload/payloadPcduDefinitions.h>
#include <mission/system/objects/Stack5VHandler.h>
@ -66,7 +67,9 @@ class PayloadPcduHandler : public DeviceHandlerBase {
SdCardMountedIF* sdcMan, Stack5VHandler& stackHandler, bool periodicPrintout);
void setToGoToNormalModeImmediately(bool enable);
void performOperationHook() override;
void enablePeriodicPrintout(bool enable, uint8_t divider);
ReturnValue_t initialize() override;
#ifdef XIPHOS_Q7S
static ReturnValue_t extConvAsTwoCallback(SpiComIF* comIf, SpiCookie* cookie,
@ -170,6 +173,10 @@ class PayloadPcduHandler : public DeviceHandlerBase {
ReturnValue_t handleDoubleParamUpdate(std::string key, ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues);
LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
pwrctrl::EnablePl enablePl = pwrctrl::EnablePl(objects::POWER_CONTROLLER);
ReturnValue_t checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
uint32_t* msToReachTheMode) override;
};
#endif /* LINUX_DEVICES_PLPCDUHANDLER_H_ */

10
mission/payload/ScexDeviceHandler.cpp
View File

@ -38,9 +38,13 @@ void ScexDeviceHandler::doShutDown() {
setMode(_MODE_POWER_DOWN);
}
ReturnValue_t ScexDeviceHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { return OK; }
ReturnValue_t ScexDeviceHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
return NOTHING_TO_SEND;
}
ReturnValue_t ScexDeviceHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { return OK; }
ReturnValue_t ScexDeviceHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
return NOTHING_TO_SEND;
}
ReturnValue_t ScexDeviceHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
const uint8_t* commandData,
@ -269,7 +273,7 @@ ReturnValue_t ScexDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, cons
// Unknown DeviceCommand
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
if (helper.getPacketCounter() == helper.getTotalPacketCounter()) {
if (helper.getPacketCounter() >= helper.getTotalPacketCounter()) {
reader.finish();
commandActive = false;
if (id != PING) {

4
mission/pollingSeqTables.cpp
View File

@ -133,6 +133,7 @@ ReturnValue_t pst::pstGompaceCan(FixedTimeslotTaskIF *thisSequence) {
thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::ACU_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
if (thisSequence->checkSequence() != returnvalue::OK) {
sif::error << "GomSpace PST initialization failed" << std::endl;
return returnvalue::FAILED;
@ -602,5 +603,8 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::POWER_CONTROLLER, length * config::spiSched::SCHED_BLOCK_10_PERIOD,
0);
return returnvalue::OK;
}

17
mission/power/defs.h
View File

@ -47,6 +47,23 @@ static constexpr Event SWITCHING_Q7S_DENIED = event::makeEvent(SUBSYSTEM_ID, 2,
static constexpr Event FDIR_REACTION_IGNORED = event::makeEvent(SUBSYSTEM_ID, 3, severity::MEDIUM);
//! [EXPORT] : [COMMENT] The dataset read for the inputs of the Power Controller has failed.
static constexpr Event DATASET_READ_FAILED = event::makeEvent(SUBSYSTEM_ID, 4, severity::INFO);
//! [EXPORT] : [COMMENT] The battery voltage read is out of the bounds in which it is supposed to
//! be.
//! P1: 1 too high, 0 too low
//! P2: voltage in V * 10
static constexpr Event VOLTAGE_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 5, severity::HIGH);
//! [EXPORT] : [COMMENT] Time difference for Coulomb Counter was too large.
//! P1: time in s * 10
static constexpr Event TIMEDELTA_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 6, severity::LOW);
//! [EXPORT] : [COMMENT] The State of Charge is below the limit for payload use. Setting Payload to
//! faulty.
static constexpr Event POWER_LEVEL_LOW = event::makeEvent(SUBSYSTEM_ID, 7, severity::HIGH);
//! [EXPORT] : [COMMENT] The State of Charge is below the limit for higher modes. Setting Reaction
//! Wheels to faulty.
static constexpr Event POWER_LEVEL_CRITICAL = event::makeEvent(SUBSYSTEM_ID, 8, severity::HIGH);
enum class States { IDLE, SWITCHING_POWER, CHECKING_POWER, MODE_COMMANDING };
enum class OpCodes { NONE, TO_OFF_DONE, TO_NOT_OFF_DONE, TIMEOUT_OCCURED };

14
mission/scheduling.cpp
View File

@ -4,13 +4,13 @@
#include "fsfw/tasks/PeriodicTaskIF.h"
void scheduling::scheduleTmpTempSensors(PeriodicTaskIF* tmpTask) {
const std::array<object_id_t, 4> tmpIds = {objects::TMP1075_HANDLER_TCS_0,
objects::TMP1075_HANDLER_TCS_1,
objects::TMP1075_HANDLER_PLPCDU_0,
// damaged.
// objects::TMP1075_HANDLER_PLPCDU_1,
objects::TMP1075_HANDLER_IF_BOARD};
void scheduling::scheduleTmpTempSensors(PeriodicTaskIF* tmpTask, bool schedulePlPcdu1) {
std::vector<object_id_t> tmpIds = {objects::TMP1075_HANDLER_TCS_0, objects::TMP1075_HANDLER_TCS_1,
objects::TMP1075_HANDLER_PLPCDU_0,
objects::TMP1075_HANDLER_IF_BOARD};
if (schedulePlPcdu1) {
tmpIds.push_back(objects::TMP1075_HANDLER_PLPCDU_1);
}
for (const auto& tmpId : tmpIds) {
tmpTask->addComponent(tmpId, DeviceHandlerIF::PERFORM_OPERATION);
tmpTask->addComponent(tmpId, DeviceHandlerIF::SEND_WRITE);

2
mission/scheduling.h
View File

@ -4,7 +4,7 @@
class PeriodicTaskIF;
namespace scheduling {
void scheduleTmpTempSensors(PeriodicTaskIF* tmpSensors);
void scheduleTmpTempSensors(PeriodicTaskIF* tmpSensors, bool schedulePlPcdu1);
void scheduleRtdSensors(PeriodicTaskIF* periodicTask);
} // namespace scheduling

8
mission/sysDefs.h
View File

@ -9,7 +9,15 @@
#include <atomic>
#include <cstring>
#include "eive/eventSubsystemIds.h"
namespace signals {
extern std::atomic_bool CFDP_CHANNEL_THROTTLE_SIGNAL;
extern std::atomic_uint16_t I2C_FATAL_ERRORS;
extern std::atomic_uint32_t CFDP_MSG_COUNTER;
} // namespace signals
namespace satsystem {

117
mission/system/EiveSystem.cpp
View File

@ -70,6 +70,9 @@ void EiveSystem::performChildOperation() {
}
pdecRecoveryLogic();
i2cRecoveryLogic();
if (forcePlOffState != ForcePlOffState::NONE) {
forceOffPayload();
}
}
ReturnValue_t EiveSystem::initialize() {
@ -81,6 +84,55 @@ ReturnValue_t EiveSystem::initialize() {
return result;
}
auto* plSs = ObjectManager::instance()->get<HasModesIF>(objects::PL_SUBSYSTEM);
if (plSs == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
plSsQueueId = plSs->getCommandQueue();
auto* plPcdu = ObjectManager::instance()->get<HasHealthIF>(objects::PLPCDU_HANDLER);
if (plPcdu == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
plPcduQueueId = plPcdu->getCommandQueue();
auto* plocMpsoc = ObjectManager::instance()->get<HasHealthIF>(objects::PLOC_MPSOC_HANDLER);
if (plocMpsoc == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
plocMpsocQueueId = plocMpsoc->getCommandQueue();
auto* plocSupervisor =
ObjectManager::instance()->get<HasHealthIF>(objects::PLOC_SUPERVISOR_HANDLER);
if (plocSupervisor == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
plocSupervisorQueueId = plocSupervisor->getCommandQueue();
auto* camera = ObjectManager::instance()->get<HasHealthIF>(objects::CAM_SWITCHER);
if (camera == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
cameraQueueId = camera->getCommandQueue();
auto* scex = ObjectManager::instance()->get<HasHealthIF>(objects::SCEX);
if (scex == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
scexQueueId = scex->getCommandQueue();
auto* radSensor = ObjectManager::instance()->get<HasHealthIF>(objects::RAD_SENSOR);
if (radSensor == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
radSensorQueueId = radSensor->getCommandQueue();
auto* str = ObjectManager::instance()->get<HasHealthIF>(objects::STAR_TRACKER);
if (str == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
}
strQueueId = str->getCommandQueue();
auto* bpxDest = ObjectManager::instance()->get<HasActionsIF>(objects::BPX_BATT_HANDLER);
if (bpxDest == nullptr) {
return ObjectManager::CHILD_INIT_FAILED;
@ -120,6 +172,8 @@ ReturnValue_t EiveSystem::initialize() {
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::OBC_OVERHEATING));
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(tcsCtrl::MGT_OVERHEATING));
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(pdec::INVALID_TC_FRAME));
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_LOW));
manager->subscribeToEvent(eventQueue->getId(), event::getEventId(power::POWER_LEVEL_CRITICAL));
return Subsystem::initialize();
}
@ -151,6 +205,25 @@ void EiveSystem::handleEventMessages() {
commandSelfToSafe();
break;
}
case power::POWER_LEVEL_LOW: {
forcePlOffState = ForcePlOffState::FORCE_ALL_EXCEPT_SUPV_OFF;
break;
}
case power::POWER_LEVEL_CRITICAL: {
// Force payload off in any case. It really should not be on when the power level
// becomes critical, but better be safe than sorry..
forcePlOffState = ForcePlOffState::FORCE_ALL_EXCEPT_SUPV_OFF;
// Also set the STR assembly to faulty, which should cause a fallback to SAFE mode.
CommandMessage msg;
HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);
ReturnValue_t result = MessageQueueSenderIF::sendMessage(
strQueueId, &msg, MessageQueueIF::NO_QUEUE, false);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to STR Assembly failed"
<< std::endl;
}
break;
}
}
break;
default:
@ -336,6 +409,50 @@ void EiveSystem::pdecRecoveryLogic() {
}
}
void EiveSystem::forceOffPayload() {
CommandMessage msg;
ReturnValue_t result;
// set PL to faulty
HealthMessage::setHealthMessage(&msg, HealthMessage::HEALTH_SET, HasHealthIF::FAULTY);
if (forcePlOffState == ForcePlOffState::FORCE_ALL_EXCEPT_SUPV_OFF) {
result = commandQueue->sendMessage(plocMpsocQueueId, &msg);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to PLOC MPSOC failed" << std::endl;
}
result = commandQueue->sendMessage(cameraQueueId, &msg);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to PL CAM failed" << std::endl;
}
result = commandQueue->sendMessage(scexQueueId, &msg);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to SCEX failed" << std::endl;
}
result = commandQueue->sendMessage(radSensorQueueId, &msg);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to RAD SENSOR failed" << std::endl;
}
result = commandQueue->sendMessage(plPcduQueueId, &msg);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to PL PCDU failed" << std::endl;
}
forcePlOffState = ForcePlOffState::WAITING;
supvOffDelay.resetTimer();
}
if (forcePlOffState == ForcePlOffState::WAITING and supvOffDelay.hasTimedOut()) {
forcePlOffState = ForcePlOffState::FORCE_SUPV_OFF;
}
if (forcePlOffState == ForcePlOffState::FORCE_SUPV_OFF) {
result = commandQueue->sendMessage(plocSupervisorQueueId, &msg);
if (result != returnvalue::OK) {
sif::error << "EIVE System: Sending FAULTY command to PLOC SUPERVISOR failed" << std::endl;
}
forcePlOffState = ForcePlOffState::NONE;
}
}
void EiveSystem::commonI2cRecoverySequenceFinish() {
alreadyTriedI2cRecovery = true;
performI2cReboot = false;

18
mission/system/EiveSystem.h
View File

@ -22,6 +22,12 @@ class EiveSystem : public Subsystem, public HasActionsIF {
[[nodiscard]] MessageQueueId_t getCommandQueue() const override;
private:
enum class ForcePlOffState {
NONE,
FORCE_ALL_EXCEPT_SUPV_OFF,
WAITING,
FORCE_SUPV_OFF
} forcePlOffState = ForcePlOffState::NONE;
enum class I2cRebootState {
NONE,
SYSTEM_MODE_BOOT,
@ -37,6 +43,7 @@ class EiveSystem : public Subsystem, public HasActionsIF {
bool alreadyTriedI2cRecovery = false;
uint8_t frameDirtyErrorCounter = 0;
Countdown supvOffDelay = Countdown(3000);
Countdown frameDirtyCheckCd = Countdown(10000);
// If the PDEC reset was already attempted in the last 2 minutes, there is a high chance that
// only a full reboot will fix the issue.
@ -49,6 +56,15 @@ class EiveSystem : public Subsystem, public HasActionsIF {
PowerSwitchIF* powerSwitcher = nullptr;
std::atomic_uint16_t& i2cErrors;
MessageQueueId_t plSsQueueId = MessageQueueIF::NO_QUEUE;
MessageQueueId_t plPcduQueueId = MessageQueueIF::NO_QUEUE;
MessageQueueId_t plocMpsocQueueId = MessageQueueIF::NO_QUEUE;
MessageQueueId_t plocSupervisorQueueId = MessageQueueIF::NO_QUEUE;
MessageQueueId_t cameraQueueId = MessageQueueIF::NO_QUEUE;
MessageQueueId_t scexQueueId = MessageQueueIF::NO_QUEUE;
MessageQueueId_t radSensorQueueId = MessageQueueIF::NO_QUEUE;
MessageQueueId_t strQueueId = MessageQueueIF::NO_QUEUE;
MessageQueueId_t pdecHandlerQueueId = MessageQueueIF::NO_QUEUE;
MessageQueueId_t bpxBattQueueId = MessageQueueIF::NO_QUEUE;
@ -68,6 +84,8 @@ class EiveSystem : public Subsystem, public HasActionsIF {
ReturnValue_t sendFullRebootCommand();
ReturnValue_t sendSelfRebootCommand();
void forceOffPayload();
void pdecRecoveryLogic();
void i2cRecoveryLogic();

16
mission/system/objects/CamSwitcher.cpp
View File

@ -1,6 +1,6 @@
#include "CamSwitcher.h"
CamSwitcher::CamSwitcher(object_id_t objectId, PowerSwitchIF &pwrSwitcher,
CamSwitcher::CamSwitcher(object_id_t objectId, PowerSwitchIF& pwrSwitcher,
power::Switch_t pwrSwitch)
: PowerSwitcherComponent(objectId, &pwrSwitcher, pwrSwitch) {}
void CamSwitcher::performFaultyOperation() {
@ -8,3 +8,17 @@ void CamSwitcher::performFaultyOperation() {
switcher.turnOff();
}
}
ReturnValue_t CamSwitcher::checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
uint32_t* msToReachTheMode) {
if (commandedMode != MODE_OFF) {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
if (enablePl.plUseAllowed.isValid() and not enablePl.plUseAllowed.value) {
return TRANS_NOT_ALLOWED;
}
}
}
return PowerSwitcherComponent::checkModeCommand(commandedMode, commandedSubmode,
msToReachTheMode);
}

8
mission/system/objects/CamSwitcher.h
View File

@ -1,13 +1,19 @@
#ifndef MISSION_SYSTEM_OBJECTS_CAMSWITCHER_H_
#define MISSION_SYSTEM_OBJECTS_CAMSWITCHER_H_
#include <common/config/eive/objects.h>
#include <fsfw/power/PowerSwitcherComponent.h>
#include <mission/controller/controllerdefinitions/PowerCtrlDefinitions.h>
class CamSwitcher : public PowerSwitcherComponent {
public:
CamSwitcher(object_id_t objectId, PowerSwitchIF &pwrSwitcher, power::Switch_t pwrSwitch);
CamSwitcher(object_id_t objectId, PowerSwitchIF& pwrSwitcher, power::Switch_t pwrSwitch);
private:
pwrctrl::EnablePl enablePl = pwrctrl::EnablePl(objects::POWER_CONTROLLER);
ReturnValue_t checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
uint32_t* msToReachTheMode) override;
void performFaultyOperation() override;
};

3
mission/system/power/CMakeLists.txt
View File

@ -1 +1,2 @@
target_sources(${LIB_EIVE_MISSION} PRIVATE GomspacePowerFdir.cpp)
target_sources(${LIB_EIVE_MISSION} PRIVATE epsModeTree.cpp EpsSubsystem.cpp
GomspacePowerFdir.cpp)

27
mission/system/power/EpsSubsystem.cpp Normal file
View File

@ -0,0 +1,27 @@
#include <mission/system/power/EpsSubsystem.h>
#include "fsfw/devicehandlers/DeviceHandlerIF.h"
EpsSubsystem::EpsSubsystem(object_id_t objectId, uint32_t maxNumberOfSequences,
uint32_t maxNumberOfTables)
: Subsystem(objectId, maxNumberOfSequences, maxNumberOfTables) {}
void EpsSubsystem::announceMode(bool recursive) {
const char* modeStr = "UNKNOWN";
switch (mode) {
case (HasModesIF::MODE_OFF): {
modeStr = "OFF";
break;
}
case (HasModesIF::MODE_ON): {
modeStr = "ON";
break;
}
case (DeviceHandlerIF::MODE_NORMAL): {
modeStr = "NORMAL";
break;
}
}
sif::info << "EPS subsystem is now in " << modeStr << " mode" << std::endl;
return Subsystem::announceMode(recursive);
}

13
mission/system/power/EpsSubsystem.h Normal file
View File

@ -0,0 +1,13 @@
#ifndef MISSION_SYSTEM_OBJECTS_EPSSUBSYSTEM_H_
#define MISSION_SYSTEM_OBJECTS_EPSSUBSYSTEM_H_
#include <fsfw/subsystem/Subsystem.h>
class EpsSubsystem : public Subsystem {
public:
EpsSubsystem(object_id_t objectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);
private:
void announceMode(bool recursive) override;
};
#endif /* MISSION_SYSTEM_OBJECTS_EPSSUBSYSTEM_H_ */

104
mission/system/power/epsModeTree.cpp Normal file
View File

@ -0,0 +1,104 @@
#include <mission/system/power/epsModeTree.h>
#include "eive/objects.h"
#include "fsfw/devicehandlers/DeviceHandlerIF.h"
#include "fsfw/subsystem/Subsystem.h"
#include "mission/system/treeUtil.h"
EpsSubsystem satsystem::eps::EPS_SUBSYSTEM(objects::EPS_SUBSYSTEM, 12, 24);
namespace {
// Alias for checker function
const auto check = subsystem::checkInsert;
void buildOffSequence(Subsystem& ss, ModeListEntry& eh);
void buildNormalSequence(Subsystem& ss, ModeListEntry& eh);
} // namespace
static const auto OFF = HasModesIF::MODE_OFF;
static const auto NML = DeviceHandlerIF::MODE_NORMAL;
auto EPS_SEQUENCE_OFF = std::make_pair(OFF, FixedArrayList<ModeListEntry, 3>());
auto EPS_TABLE_OFF_TGT = std::make_pair((OFF << 24) | 1, FixedArrayList<ModeListEntry, 2>());
auto EPS_TABLE_OFF_TRANS_0 = std::make_pair((OFF << 24) | 2, FixedArrayList<ModeListEntry, 2>());
auto EPS_SEQUENCE_NORMAL = std::make_pair(NML, FixedArrayList<ModeListEntry, 3>());
auto EPS_TABLE_NORMAL_TGT = std::make_pair((NML << 24) | 1, FixedArrayList<ModeListEntry, 2>());
auto EPS_TABLE_NORMAL_TRANS_0 = std::make_pair((NML << 24) | 2, FixedArrayList<ModeListEntry, 7>());
Subsystem& satsystem::eps::init() {
ModeListEntry entry;
buildOffSequence(EPS_SUBSYSTEM, entry);
buildNormalSequence(EPS_SUBSYSTEM, entry);
EPS_SUBSYSTEM.setInitialMode(NML);
return EPS_SUBSYSTEM;
}
namespace {
void buildOffSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::eps::buildOffSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
check(table.insert(eh), ctxc);
};
// Insert Helper Sequence
auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
bool checkSuccess) {
eh.setTableId(tableId);
eh.setWaitSeconds(waitSeconds);
eh.setCheckSuccess(checkSuccess);
check(sequence.insert(eh), ctxc);
};
// OFF target table is empty
check(ss.addTable(TableEntry(EPS_TABLE_OFF_TGT.first, &EPS_TABLE_OFF_TGT.second)), ctxc);
// Transition 0
iht(objects::POWER_CONTROLLER, OFF, 0, EPS_TABLE_OFF_TRANS_0.second);
check(ss.addTable(TableEntry(EPS_TABLE_OFF_TRANS_0.first, &EPS_TABLE_OFF_TRANS_0.second)), ctxc);
ihs(EPS_SEQUENCE_OFF.second, EPS_TABLE_OFF_TGT.first, 0, false);
ihs(EPS_SEQUENCE_OFF.second, EPS_TABLE_OFF_TRANS_0.first, 0, false);
check(ss.addSequence(SequenceEntry(EPS_SEQUENCE_OFF.first, &EPS_SEQUENCE_OFF.second,
EPS_SEQUENCE_OFF.first)),
ctxc);
}
void buildNormalSequence(Subsystem& ss, ModeListEntry& eh) {
std::string context = "satsystem::tcs::buildNormalSequence";
auto ctxc = context.c_str();
// Insert Helper Table
auto iht = [&](object_id_t obj, Mode_t mode, Submode_t submode, ArrayList<ModeListEntry>& table) {
eh.setObject(obj);
eh.setMode(mode);
eh.setSubmode(submode);
check(table.insert(eh), ctxc);
};
// Insert Helper Sequence
auto ihs = [&](ArrayList<ModeListEntry>& sequence, Mode_t tableId, uint32_t waitSeconds,
bool checkSuccess) {
eh.setTableId(tableId);
eh.setWaitSeconds(waitSeconds);
eh.setCheckSuccess(checkSuccess);
check(sequence.insert(eh), ctxc);
};
// Normal table target table is empty
check(ss.addTable(TableEntry(EPS_TABLE_NORMAL_TGT.first, &EPS_TABLE_NORMAL_TGT.second)), ctxc);
// Transition 0
iht(objects::POWER_CONTROLLER, NML, 0, EPS_TABLE_NORMAL_TRANS_0.second);
check(ss.addTable(TableEntry(EPS_TABLE_NORMAL_TRANS_0.first, &EPS_TABLE_NORMAL_TRANS_0.second)),
ctxc);
ihs(EPS_SEQUENCE_NORMAL.second, EPS_TABLE_NORMAL_TGT.first, 0, false);
ihs(EPS_SEQUENCE_NORMAL.second, EPS_TABLE_NORMAL_TRANS_0.first, 0, false);
check(ss.addSequence(SequenceEntry(EPS_SEQUENCE_NORMAL.first, &EPS_SEQUENCE_NORMAL.second,
EPS_SEQUENCE_NORMAL.first)),
ctxc);
}
} // namespace

15
mission/system/power/epsModeTree.h Normal file
View File

@ -0,0 +1,15 @@
#ifndef MISSION_SYSTEM_TREE_EPSMODETREE_H_
#define MISSION_SYSTEM_TREE_EPSMODETREE_H_
#include <mission/system/power/EpsSubsystem.h>
namespace satsystem {
namespace eps {
extern EpsSubsystem EPS_SUBSYSTEM;
Subsystem& init();
} // namespace eps
} // namespace satsystem
#endif /* MISSION_SYSTEM_TREE_EPSMODETREE_H_ */

21
mission/system/systemTree.cpp
View File

@ -10,7 +10,9 @@
#include "eive/objects.h"
#include "mission/com/defs.h"
#include "mission/sysDefs.h"
#include "mission/system/acs/acsModeTree.h"
#include "mission/system/power/epsModeTree.h"
#include "mission/system/tcs/tcsModeTree.h"
#include "mission/system/tree/payloadModeTree.h"
#include "treeUtil.h"
@ -40,6 +42,8 @@ void satsystem::init(bool commandPlPcdu1) {
tcsSubsystem.connectModeTreeParent(EIVE_SYSTEM);
auto& comSubsystem = com::init();
comSubsystem.connectModeTreeParent(EIVE_SYSTEM);
auto& epsSubsystem = eps::init();
epsSubsystem.connectModeTreeParent(EIVE_SYSTEM);
ModeListEntry entry;
buildBootSequence(EIVE_SYSTEM, entry);
buildSafeSequence(EIVE_SYSTEM, entry);
@ -51,7 +55,8 @@ void satsystem::init(bool commandPlPcdu1) {
EIVE_SYSTEM.setInitialMode(satsystem::Mode::BOOT, 0);
}
EiveSystem satsystem::EIVE_SYSTEM = EiveSystem(objects::EIVE_SYSTEM, 12, 24, I2C_FATAL_ERRORS);
EiveSystem satsystem::EIVE_SYSTEM =
EiveSystem(objects::EIVE_SYSTEM, 12, 24, signals::I2C_FATAL_ERRORS);
auto EIVE_SEQUENCE_BOOT = std::make_pair(satsystem::Mode::BOOT, FixedArrayList<ModeListEntry, 5>());
auto EIVE_TABLE_BOOT_TGT =
@ -141,9 +146,11 @@ void buildBootSequence(Subsystem& ss, ModeListEntry& eh) {
iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TGT.second);
iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_BOOT_TGT.second);
iht(objects::TCS_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TGT.second);
iht(objects::EPS_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TGT.second);
check(ss.addTable(TableEntry(EIVE_TABLE_BOOT_TGT.first, &EIVE_TABLE_BOOT_TGT.second)), ctxc);
// Build SAFE transition 0.
// Build BOOT transition 0.
iht(objects::EPS_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TRANS_0.second);
iht(objects::TCS_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TRANS_0.second);
iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_BOOT_TRANS_0.second);
iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_BOOT_TRANS_0.second);
@ -151,7 +158,7 @@ void buildBootSequence(Subsystem& ss, ModeListEntry& eh) {
check(ss.addTable(TableEntry(EIVE_TABLE_BOOT_TRANS_0.first, &EIVE_TABLE_BOOT_TRANS_0.second)),
ctxc);
// Build Safe sequence
// Build BOOT sequence
ihs(EIVE_SEQUENCE_BOOT.second, EIVE_TABLE_BOOT_TGT.first, 0, false);
ihs(EIVE_SEQUENCE_BOOT.second, EIVE_TABLE_BOOT_TRANS_0.first, 0, false);
check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_BOOT.first, &EIVE_SEQUENCE_BOOT.second,
@ -187,13 +194,14 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TGT.first, &EIVE_TABLE_SAFE_TGT.second)), ctxc);
// Build SAFE transition 0.
iht(objects::EPS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second);
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second);
iht(objects::PL_SUBSYSTEM, OFF, 0, EIVE_TABLE_SAFE_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::SAFE, 0, EIVE_TABLE_SAFE_TRANS_0.second, true);
check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_0.first, &EIVE_TABLE_SAFE_TRANS_0.second)),
ctxc);
// Build Safe sequence
// Build SAFE sequence
ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TGT.first, 0, false);
ihs(EIVE_SEQUENCE_SAFE.second, EIVE_TABLE_SAFE_TRANS_0.first, 0, false);
check(ss.addSequence(SequenceEntry(EIVE_SEQUENCE_SAFE.first, &EIVE_SEQUENCE_SAFE.second,
@ -224,6 +232,7 @@ void buildIdleSequence(Subsystem& ss, ModeListEntry& eh) {
check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TGT.first, &EIVE_TABLE_IDLE_TGT.second)), ctxc);
// Build IDLE transition 0
iht(objects::EPS_SUBSYSTEM, NML, 0, EIVE_TABLE_IDLE_TRANS_0.second);
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_IDLE_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_IDLE, 0, EIVE_TABLE_IDLE_TRANS_0.second);
check(ss.addTable(TableEntry(EIVE_TABLE_IDLE_TRANS_0.first, &EIVE_TABLE_IDLE_TRANS_0.second)),
@ -261,6 +270,7 @@ void buildPtgNadirSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc);
// Build PTG_NADIR transition 0
iht(objects::EPS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second);
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_NADIR, 0, EIVE_TABLE_PTG_NADIR_TRANS_0.second);
check(ss.addTable(
@ -299,6 +309,7 @@ void buildPtgTargetSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc);
// Build PTG_TARGET transition 0
iht(objects::EPS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second);
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET, 0, EIVE_TABLE_PTG_TARGET_TRANS_0.second);
check(ss.addTable(
@ -338,6 +349,7 @@ void buildPtgTargetGsSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc);
// Build PTG_TARGET_GS transition 0
iht(objects::EPS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second);
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_TARGET_GS, 0,
EIVE_TABLE_PTG_TARGET_GS_TRANS_0.second);
@ -379,6 +391,7 @@ void buildPtgInertialSequence(Subsystem& ss, ModeListEntry& eh) {
ctxc);
// Build PTG_INERTIAL transition 0
iht(objects::EPS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_INERTIAL_TRANS_0.second);
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_PTG_INERTIAL_TRANS_0.second);
iht(objects::ACS_SUBSYSTEM, acs::AcsMode::PTG_INERTIAL, 0,
EIVE_TABLE_PTG_INERTIAL_TRANS_0.second);

37
mission/tmtc/CfdpTmFunnel.cpp
View File

@ -3,8 +3,10 @@
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/tmtcpacket/ccsds/SpacePacketCreator.h"
#include "fsfw/tmtcservices/TmTcMessage.h"
#include "mission/sysDefs.h"
CfdpTmFunnel::CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, MessageQueueId_t fileStoreDest,
CfdpTmFunnel::CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg,
std::optional<MessageQueueId_t> fileStoreDest,
StorageManagerIF& ramToFileStore, uint16_t cfdpInCcsdsApid)
: TmFunnelBase(cfg),
fileStoreDest(fileStoreDest),
@ -25,7 +27,9 @@ ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
saveSequenceCount = false;
}
status = tmQueue->receiveMessage(&currentMessage);
uint32_t handledPackets = 0;
while (status == returnvalue::OK) {
handledPackets++;
status = handlePacket(currentMessage);
if (status != returnvalue::OK) {
sif::warning << "CfdpTmFunnel packet handling failed" << std::endl;
@ -38,6 +42,11 @@ ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
}
status = tmQueue->receiveMessage(&currentMessage);
}
if (handledPackets > 0) {
// Very useful for profiling and debugging
// sif::debug << "CfdpFunnel: Handled " << handledPackets << " packets in one cycle" <<
// std::endl;
}
if (status == MessageQueueIF::EMPTY) {
return returnvalue::OK;
@ -57,7 +66,7 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
auto spacePacketHeader =
SpacePacketCreator(ccsds::PacketType::TM, false, cfdpInCcsdsApid,
ccsds::SequenceFlags::UNSEGMENTED, sourceSequenceCount++, 0);
sourceSequenceCount = sourceSequenceCount & ccsds::LIMIT_SEQUENCE_COUNT;
sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT;
spacePacketHeader.setCcsdsLenFromTotalDataFieldLen(cfdpPacketLen);
uint8_t* newPacketData = nullptr;
store_address_t newStoreId{};
@ -88,14 +97,22 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
msg.setStorageId(newStoreId);
store_address_t origStoreId = newStoreId;
store_address_t storageId;
result = ramToFileStore.addData(&storageId, newPacketData, packetLen);
TmTcMessage fileMsg(storageId);
if (result != returnvalue::OK) {
sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl;
} else {
tmQueue->sendMessage(fileStoreDest, &fileMsg);
if (fileStoreDest.has_value()) {
store_address_t storageId;
result = ramToFileStore.addData(&storageId, newPacketData, packetLen);
TmTcMessage fileMsg(storageId);
if (result == returnvalue::OK) {
tmQueue->sendMessage(fileStoreDest.value(), &fileMsg);
} else if (result == StorageManagerIF::DATA_STORAGE_FULL) {
sif::error << "CfdpTmFunnel::handlePacket: RAM to File Store too full to create data copy"
<< std::endl;
}
}
return demultiplexLivePackets(origStoreId, newPacketData, packetLen);
}
uint32_t CfdpTmFunnel::addLiveDestination(const char* name,
const AcceptsTelemetryIF& downlinkDestination,
uint8_t vcid) {
return TmFunnelBase::addLiveDestination(name, downlinkDestination, vcid);
}

6
mission/tmtc/CfdpTmFunnel.h
View File

@ -13,16 +13,18 @@
class CfdpTmFunnel : public TmFunnelBase {
public:
CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, MessageQueueId_t fileStoreDest,
CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, std::optional<MessageQueueId_t> fileStoreDest,
StorageManagerIF& ramToFileStore, uint16_t cfdpInCcsdsApid);
[[nodiscard]] const char* getName() const override;
uint32_t addLiveDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination,
uint8_t vcid = 0) override;
ReturnValue_t performOperation(uint8_t opCode);
ReturnValue_t initialize() override;
private:
ReturnValue_t handlePacket(TmTcMessage& msg);
MessageQueueId_t fileStoreDest;
std::optional<MessageQueueId_t> fileStoreDest;
StorageManagerIF& ramToFileStore;
uint16_t cfdpInCcsdsApid;
};

35
mission/tmtc/DirectTmSinkIF.h
View File

@ -13,18 +13,45 @@ class DirectTmSinkIF {
static constexpr uint8_t CLASS_ID = CLASS_ID::TM_SINK;
static constexpr ReturnValue_t IS_BUSY = returnvalue::makeCode(CLASS_ID, 0);
static constexpr ReturnValue_t PARTIALLY_WRITTEN = returnvalue::makeCode(CLASS_ID, 1);
static constexpr ReturnValue_t NO_WRITE_ACTIVE = returnvalue::makeCode(CLASS_ID, 2);
/**
* @brief Implements the functionality to write to a TM sink directly
* @brief Implements the functionality to write to a TM sink directly.
*
* The write might not be completed immediately! If PARTIALLY_WRITTEN is returned, the user
* should poll the ready for packet status bit and call @advanceWrite continuously until
* the transfer is completed.
*
* @param data Pointer to buffer holding the data to write
* @param size Number of bytes to write
* @return returnvalue::OK on success, returnvalue::FAILED on failure, IS_BUSY
* if the TM sink is busy.
* @param writtenSize Size written during write call.
* @return returnvalue::OK on full write success, IS_BUSY if a previous write transfer has not
* been completed yet or the PAPB interface is not ready for a packet, PARTIALLY_WRITTEN
* if some bytes were written, but the transfer has not been completed yet.
*/
virtual ReturnValue_t write(const uint8_t* data, size_t size) = 0;
virtual ReturnValue_t write(const uint8_t* data, size_t size, size_t& writtenSize) = 0;
/**
* Advances a active file transfer.
* @param writtenSize
* @return returnvalue::OK if the packet write process is complete, PARTIALLY_WRITTEN if
* some bytes were written but the transfer is not complete yet.
* NO_WRITE_ACTIVE if this is called without a valid previous write call.
*/
virtual ReturnValue_t advanceWrite(size_t& writtenSize) = 0;
/**
* Is busy, so no write operation can not be started and write advancement
* is not possible.
* @return
*/
virtual bool isBusy() const = 0;
/**
* The PAPB interface is currently busy writing a packet and a new packet can not be written yet.
* @return
*/
virtual bool writeActive() const = 0;
};
#endif /* MISSION_TMTC_DIRECTTMSINKIF_H_ */

43
mission/tmtc/PusLiveDemux.cpp
View File

@ -9,8 +9,13 @@ ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore,
store_address_t origStoreId, const uint8_t* tmData,
size_t tmSize) {
ReturnValue_t result = returnvalue::OK;
// sif::debug << "tm size: " << tmSize << " for " << destinations.size() << " destinations" <<
// std::endl;
for (unsigned int idx = 0; idx < destinations.size(); idx++) {
const auto& dest = destinations[idx];
if (dest.isFull) {
continue;
}
if ((destinations.size() > 1) and (idx < (destinations.size() - 1))) {
// Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
// to bother with send order and where the data is deleted.
@ -18,9 +23,20 @@ ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore,
result = tmStore.addData(&storeId, tmData, tmSize);
if (result == returnvalue::OK) {
message.setStorageId(storeId);
} else {
} else if (result == StorageManagerIF::DATA_STORAGE_FULL) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl;
uint8_t fillCounts[16];
uint8_t written = 0;
tmStore.getFillCount(fillCounts, &written);
sif::error << "Fill counts: [";
for (uint8_t fillIdx = 0; fillIdx < written; fillIdx++) {
sif::error << fillCounts[fillIdx];
if (fillIdx < written - 1) {
sif::error << ", ";
}
}
sif::error << "]" << std::endl;
#endif
}
} else {
@ -39,8 +55,25 @@ ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore,
return result;
}
void PusLiveDemux::addDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination,
uint8_t vcid) {
auto queueId = downlinkDestination.getReportReceptionQueue(vcid);
destinations.emplace_back(name, queueId, vcid);
uint32_t PusLiveDemux::addDestination(const char* name,
const AcceptsTelemetryIF& downlinkDestination, uint8_t vcid) {
return addDestinationByRawId(name, downlinkDestination.getReportReceptionQueue(vcid), vcid);
}
void PusLiveDemux::setDestFull(uint32_t listIndex) {
if (destinations.size() > 0 and listIndex <= destinations.size() - 1) {
destinations[listIndex].isFull = true;
}
}
void PusLiveDemux::setDestAvailable(uint32_t listIndex) {
if (destinations.size() > 0 and listIndex <= destinations.size() - 1) {
destinations[listIndex].isFull = false;
}
}
uint32_t PusLiveDemux::addDestinationByRawId(const char* name, MessageQueueId_t downlinkDestination,
uint8_t vcid) {
destinations.emplace_back(name, downlinkDestination, vcid);
return destinations.size() - 1;
}

9
mission/tmtc/PusLiveDemux.h
View File

@ -14,8 +14,12 @@ class PusLiveDemux {
ReturnValue_t demultiplexPackets(StorageManagerIF& tmStore, store_address_t origStoreId,
const uint8_t* tmData, size_t tmSize);
void addDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination,
uint8_t vcid = 0);
uint32_t addDestinationByRawId(const char* name, MessageQueueId_t downlinkDestination,
uint8_t vcid = 0);
uint32_t addDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination,
uint8_t vcid = 0);
void setDestFull(uint32_t listIndex);
void setDestAvailable(uint32_t listIndex);
private:
struct Destination {
@ -24,6 +28,7 @@ class PusLiveDemux {
const char* name;
MessageQueueId_t queueId;
bool isFull = false;
uint8_t virtualChannel = 0;
};

13
mission/tmtc/TmFunnelBase.cpp
View File

@ -30,9 +30,10 @@ MessageQueueId_t TmFunnelBase::getReportReceptionQueue(uint8_t virtualChannel) c
return tmQueue->getId();
}
void TmFunnelBase::addLiveDestination(const char *name,
const AcceptsTelemetryIF &downlinkDestination, uint8_t vcid) {
liveDemux.addDestination(name, downlinkDestination, vcid);
uint32_t TmFunnelBase::addLiveDestination(const char *name,
const AcceptsTelemetryIF &downlinkDestination,
uint8_t vcid) {
return liveDemux.addDestination(name, downlinkDestination, vcid);
}
ReturnValue_t TmFunnelBase::initialize() {
@ -69,3 +70,9 @@ ReturnValue_t TmFunnelBase::saveSequenceCountToFile() {
ofile << sourceSequenceCount << "\n";
return returnvalue::OK;
}
uint32_t TmFunnelBase::addLiveDestinationByRawId(const char *name,
MessageQueueId_t downlinkDestination,
uint8_t vcid) {
return liveDemux.addDestinationByRawId(name, downlinkDestination, vcid);
}

7
mission/tmtc/TmFunnelBase.h
View File

@ -37,8 +37,11 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
};
explicit TmFunnelBase(FunnelCfg cfg);
[[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override;
void addLiveDestination(const char* name, const AcceptsTelemetryIF& downlinkDestination,
uint8_t vcid = 0);
virtual uint32_t addLiveDestinationByRawId(const char* name, MessageQueueId_t downlinkDestination,
uint8_t vcid = 0);
virtual uint32_t addLiveDestination(const char* name,
const AcceptsTelemetryIF& downlinkDestination,
uint8_t vcid = 0);
ReturnValue_t demultiplexLivePackets(store_address_t origStoreId, const uint8_t* tmData,
size_t tmSize);
ReturnValue_t initialize() override;