Add TC scheduler service
- Written by David Woodward as part of the SOURCE project - Adaptions to make it more generic and compatible to FSFW
This commit is contained in:
parent
0d7d2203d2
commit
bfa77cf810
191
src/fsfw/pus/Service11TelecommandScheduling.h
Normal file
191
src/fsfw/pus/Service11TelecommandScheduling.h
Normal file
@ -0,0 +1,191 @@
|
||||
#ifndef MISSION_PUS_SERVICE11TELECOMMANDSCHEDULING_H_
|
||||
#define MISSION_PUS_SERVICE11TELECOMMANDSCHEDULING_H_
|
||||
|
||||
#include <etl/multimap.h>
|
||||
#include <fsfw/tmtcservices/PusServiceBase.h>
|
||||
#include <fsfw/tmtcservices/TmTcMessage.h>
|
||||
|
||||
#include "fsfw/FSFW.h"
|
||||
|
||||
/**
|
||||
* @brief: PUS-Service 11 - Telecommand scheduling.
|
||||
* @details:
|
||||
* PUS-Service 11 - Telecommand scheduling.
|
||||
* Full documentation: ECSS-E-ST-70-41C, p. 168:
|
||||
* ST[11] time-based scheduling
|
||||
*
|
||||
* This service provides the capability to command pre-loaded
|
||||
* application processes (telecommands) by releasing them at their
|
||||
* due-time.
|
||||
* References to telecommands are stored together with their due-timepoints
|
||||
* and are released at their corresponding due-time.
|
||||
*
|
||||
* Necessary subservice functionalities are implemented.
|
||||
* Those are:
|
||||
* TC[11,4] activity insertion
|
||||
* TC[11,5] activity deletion
|
||||
* TC[11,6] filter-based activity deletion
|
||||
* TC[11,7] activity time-shift
|
||||
* TC[11,8] filter-based activity time-shift
|
||||
*
|
||||
* Groups are not supported.
|
||||
* This service remains always enabled. Sending a disable-request has no effect.
|
||||
*/
|
||||
template <size_t MAX_NUM_TCS>
|
||||
class Service11TelecommandScheduling final : public PusServiceBase {
|
||||
public:
|
||||
// The types of PUS-11 subservices
|
||||
enum Subservice {
|
||||
ENABLE_SCHEDULING = 1,
|
||||
DISABLE_SCHEDULING = 2,
|
||||
RESET_SCHEDULING = 3,
|
||||
INSERT_ACTIVITY = 4,
|
||||
DELETE_ACTIVITY = 5,
|
||||
FILTER_DELETE_ACTIVITY = 6,
|
||||
TIMESHIFT_ACTIVITY = 7,
|
||||
FILTER_TIMESHIFT_ACTIVITY = 8,
|
||||
DETAIL_REPORT = 9,
|
||||
TIMEBASE_SCHEDULE_DETAIL_REPORT = 10,
|
||||
TIMESHIFT_ALL_SCHEDULE_ACTIVITIES = 15
|
||||
};
|
||||
|
||||
// The types of time windows for TC[11,6] and TC[11,8], as defined in ECSS-E-ST-70-41C,
|
||||
// requirement 8.11.3c (p. 507)
|
||||
enum TypeOfTimeWindow : uint32_t {
|
||||
SELECT_ALL = 0,
|
||||
FROM_TIMETAG_TO_TIMETAG = 1,
|
||||
FROM_TIMETAG = 2,
|
||||
TO_TIMETAG = 3
|
||||
};
|
||||
|
||||
Service11TelecommandScheduling(object_id_t objectId, uint16_t apid, uint8_t serviceId,
|
||||
AcceptsTelecommandsIF* tcRecipient,
|
||||
uint16_t releaseTimeMarginSeconds = DEFAULT_RELEASE_TIME_MARGIN,
|
||||
bool debugMode = false);
|
||||
|
||||
~Service11TelecommandScheduling();
|
||||
|
||||
/** PusServiceBase overrides */
|
||||
ReturnValue_t handleRequest(uint8_t subservice) override;
|
||||
ReturnValue_t performService() override;
|
||||
ReturnValue_t initialize() override;
|
||||
|
||||
private:
|
||||
struct TelecommandStruct {
|
||||
uint64_t requestId;
|
||||
uint32_t seconds;
|
||||
store_address_t storeAddr; // uint16
|
||||
};
|
||||
|
||||
static constexpr uint16_t DEFAULT_RELEASE_TIME_MARGIN = 5;
|
||||
|
||||
// minimum release time offset to insert into schedule
|
||||
const uint16_t RELEASE_TIME_MARGIN_SECONDS = 5;
|
||||
|
||||
// the maximum amount of stored TCs is defined here
|
||||
static constexpr uint16_t MAX_STORED_TELECOMMANDS = 500;
|
||||
|
||||
bool debugMode = false;
|
||||
StorageManagerIF* tcStore = nullptr;
|
||||
AcceptsTelecommandsIF* tcRecipient = nullptr;
|
||||
MessageQueueId_t recipientMsgQueueId = 0;
|
||||
|
||||
/**
|
||||
* The telecommand map uses the exectution time as a Unix time stamp as
|
||||
* the key. This is mapped to a generic telecommand struct.
|
||||
*/
|
||||
using TelecommandMap = etl::multimap<uint32_t, TelecommandStruct, MAX_NUM_TCS>;
|
||||
using TcMapIter = typename TelecommandMap::iterator;
|
||||
|
||||
TelecommandMap telecommandMap;
|
||||
|
||||
/**
|
||||
* @brief Logic to be performed on an incoming TC[11,4].
|
||||
* @return RETURN_OK if successful
|
||||
*/
|
||||
ReturnValue_t doInsertActivity(void);
|
||||
|
||||
/**
|
||||
* @brief Logic to be performed on an incoming TC[11,5].
|
||||
* @return RETURN_OK if successful
|
||||
*/
|
||||
ReturnValue_t doDeleteActivity(void);
|
||||
|
||||
/**
|
||||
* @brief Logic to be performed on an incoming TC[11,6].
|
||||
* @return RETURN_OK if successful
|
||||
*/
|
||||
ReturnValue_t doFilterDeleteActivity(void);
|
||||
|
||||
/**
|
||||
* @brief Logic to be performed on an incoming TC[11,7].
|
||||
* @return RETURN_OK if successful
|
||||
*/
|
||||
ReturnValue_t doTimeshiftActivity(void);
|
||||
|
||||
/**
|
||||
* @brief Logic to be performed on an incoming TC[11,8].
|
||||
* @return RETURN_OK if successful
|
||||
*/
|
||||
ReturnValue_t doFilterTimeshiftActivity(void);
|
||||
|
||||
/**
|
||||
* @brief Deserializes a generic type from a payload buffer by using the FSFW
|
||||
* SerializeAdapter Interface.
|
||||
* @param output Output to be deserialized
|
||||
* @param buf Payload buffer (application data)
|
||||
* @param bufsize Remaining size of payload buffer (application data size)
|
||||
* @return RETURN_OK if successful
|
||||
*/
|
||||
template <typename T>
|
||||
ReturnValue_t deserializeViaFsfwInterface(T& output, const uint8_t* buf, size_t bufsize);
|
||||
|
||||
/**
|
||||
* @brief Extracts the Request ID from the Application Data of a TC by utilizing a ctor of the
|
||||
* class TcPacketPus.
|
||||
* NOTE: This only works if the payload data is a TC (e.g. not for TC[11,5] which does not
|
||||
* send a TC as payload)!
|
||||
* @param data The Application data of the TC (get via getApplicationData()).
|
||||
* @return requestId
|
||||
*/
|
||||
uint64_t getRequestIdFromDataTC(const uint8_t* data) const;
|
||||
|
||||
/**
|
||||
* @brief Extracts the Request ID from the Application Data directly, assuming it is packed
|
||||
* as follows (acc. to ECSS): | source ID (uint32) | apid (uint32) | ssc (uint32) |.
|
||||
* @param data Pointer to first byte described data
|
||||
* @param dataSize Remaining size of data NOTE: non-const, this is modified by the function
|
||||
* @param [out] requestId Request ID
|
||||
* @return RETURN_OK if successful
|
||||
*/
|
||||
ReturnValue_t getRequestIdFromData(const uint8_t* data, size_t& dataSize, uint64_t& requestId);
|
||||
|
||||
/**
|
||||
* @brief Builds the Request ID from its three elements.
|
||||
* @param sourceId Source ID
|
||||
* @param apid Application Process ID (APID)
|
||||
* @param ssc Source Sequence Count
|
||||
* @return Request ID
|
||||
*/
|
||||
uint64_t buildRequestId(uint32_t sourceId, uint16_t apid, uint16_t ssc) const;
|
||||
|
||||
/**
|
||||
* @brief Gets the filter range for filter TCs from a data packet
|
||||
* @param data TC data
|
||||
* @param dataSize TC data size
|
||||
* @param [out] itBegin Begin of filter range
|
||||
* @param [out] itEnd End of filter range
|
||||
* @return RETURN_OK if successful
|
||||
*/
|
||||
ReturnValue_t getMapFilterFromData(const uint8_t* data, size_t dataSize, TcMapIter& itBegin,
|
||||
TcMapIter& itEnd);
|
||||
|
||||
/**
|
||||
* @brief Prints content of multimap. Use for simple debugging only.
|
||||
*/
|
||||
void debugPrintMultimapContent(void) const;
|
||||
};
|
||||
|
||||
#include "Service11TelecommandScheduling.tpp"
|
||||
|
||||
#endif /* MISSION_PUS_SERVICE11TELECOMMANDSCHEDULING_H_ */
|
||||
608
src/fsfw/pus/Service11TelecommandScheduling.tpp
Normal file
608
src/fsfw/pus/Service11TelecommandScheduling.tpp
Normal file
@ -0,0 +1,608 @@
|
||||
#pragma once
|
||||
|
||||
#include <fsfw/objectmanager/ObjectManager.h>
|
||||
#include <fsfw/serialize/SerializeAdapter.h>
|
||||
#include <fsfw/tmtcservices/AcceptsTelecommandsIF.h>
|
||||
|
||||
#include <cstddef>
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline Service11TelecommandScheduling<MAX_NUM_TCS>::Service11TelecommandScheduling(
|
||||
object_id_t objectId, uint16_t apid, uint8_t serviceId, AcceptsTelecommandsIF *tcRecipient,
|
||||
uint16_t releaseTimeMarginSeconds, bool debugMode)
|
||||
: PusServiceBase(objectId, apid, serviceId),
|
||||
RELEASE_TIME_MARGIN_SECONDS(releaseTimeMarginSeconds),
|
||||
debugMode(debugMode),
|
||||
tcRecipient(tcRecipient) {}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline Service11TelecommandScheduling<MAX_NUM_TCS>::~Service11TelecommandScheduling() {}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::handleRequest(
|
||||
uint8_t subservice) {
|
||||
if (debugMode) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::info << "PUS11::handleRequest: Handling request " << static_cast<int>(subservice);
|
||||
#else
|
||||
sif::printInfo("PUS11::handleRequest: Handling request %d\n", subservice);
|
||||
#endif
|
||||
}
|
||||
switch (subservice) {
|
||||
case Subservice::INSERT_ACTIVITY:
|
||||
return doInsertActivity();
|
||||
case Subservice::DELETE_ACTIVITY:
|
||||
return doDeleteActivity();
|
||||
case Subservice::FILTER_DELETE_ACTIVITY:
|
||||
return doFilterDeleteActivity();
|
||||
case Subservice::TIMESHIFT_ACTIVITY:
|
||||
return doTimeshiftActivity();
|
||||
case Subservice::FILTER_TIMESHIFT_ACTIVITY:
|
||||
return doFilterTimeshiftActivity();
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::performService() {
|
||||
// DEBUG
|
||||
// DebugPrintMultimapContent();
|
||||
|
||||
// get current time as UNIX timestamp
|
||||
timeval tNow = {};
|
||||
Clock::getClock_timeval(&tNow);
|
||||
|
||||
// NOTE: The iterator is increased in the loop here. Increasing the iterator as for-loop arg
|
||||
// does not work in this case as we are deleting the current element here.
|
||||
for (auto it = telecommandMap.begin(); it != telecommandMap.end();) {
|
||||
if (it->first <= tNow.tv_sec) {
|
||||
// release tc
|
||||
TmTcMessage releaseMsg(it->second.storeAddr);
|
||||
auto sendRet = this->requestQueue->sendMessage(recipientMsgQueueId, &releaseMsg, false);
|
||||
|
||||
if (sendRet != HasReturnvaluesIF::RETURN_OK) {
|
||||
return sendRet;
|
||||
}
|
||||
|
||||
telecommandMap.erase(it++);
|
||||
|
||||
if (debugMode) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::info << "Released TC & erased it from TC map" << std::endl;
|
||||
#else
|
||||
sif::printInfo("Released TC & erased it from TC map\n");
|
||||
#endif
|
||||
}
|
||||
continue;
|
||||
}
|
||||
it++;
|
||||
}
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::initialize() {
|
||||
ReturnValue_t res = PusServiceBase::initialize();
|
||||
if (res != HasReturnvaluesIF::RETURN_OK) {
|
||||
return res;
|
||||
}
|
||||
|
||||
tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
|
||||
if (!tcStore) {
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
|
||||
if (tcRecipient == nullptr) {
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
recipientMsgQueueId = tcRecipient->getRequestQueue();
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivity(void) {
|
||||
// Get de-serialized Timestamp
|
||||
const uint8_t *data = currentPacket.getApplicationData();
|
||||
size_t dataSize = currentPacket.getApplicationDataSize();
|
||||
|
||||
uint32_t timestamp = 0;
|
||||
if (deserializeViaFsfwInterface<uint32_t>(timestamp, data, dataSize) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
data += sizeof(uint32_t); // move ptr past timestamp (for later)
|
||||
dataSize -= sizeof(uint32_t); // and reduce size accordingly
|
||||
|
||||
// Insert possible if sched. time is above margin
|
||||
// (See requirement for Time margin)
|
||||
timeval tNow = {};
|
||||
Clock::getClock_timeval(&tNow);
|
||||
if (timestamp - tNow.tv_sec <= RELEASE_TIME_MARGIN_SECONDS) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "Service11TelecommandScheduling::doInsertActivity: Release time too close to "
|
||||
"current time"
|
||||
<< std::endl;
|
||||
#else
|
||||
sif::printWarning(
|
||||
"Service11TelecommandScheduling::doInsertActivity: Release time too close to current "
|
||||
"time\n");
|
||||
#endif
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
// store currentPacket and receive the store address
|
||||
store_address_t addr;
|
||||
if (tcStore->addData(&addr, data, dataSize) != RETURN_OK ||
|
||||
addr.raw == storeId::INVALID_STORE_ADDRESS) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "Service11TelecommandScheduling::doInsertActivity: Adding data to TC Store failed"
|
||||
<< std::endl;
|
||||
#else
|
||||
sif::printError(
|
||||
"Service11TelecommandScheduling::doInsertActivity: Adding data to TC Store failed\n");
|
||||
#endif
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
// insert into multimap with new store address
|
||||
TelecommandStruct tc;
|
||||
tc.seconds = timestamp;
|
||||
tc.storeAddr = addr;
|
||||
tc.requestId =
|
||||
getRequestIdFromDataTC(data); // TODO: Missing sanity check of the returned request id
|
||||
|
||||
auto it = telecommandMap.insert(std::pair<uint32_t, TelecommandStruct>(timestamp, tc));
|
||||
if (it == telecommandMap.end()) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
if (debugMode) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::info << "PUS11::doInsertActivity: Inserted into Multimap:" << std::endl;
|
||||
#else
|
||||
sif::printInfo("PUS11::doInsertActivity: Inserted into Multimap:\n");
|
||||
#endif
|
||||
debugPrintMultimapContent();
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doDeleteActivity(void) {
|
||||
const uint8_t *data = currentPacket.getApplicationData();
|
||||
size_t dataSize = currentPacket.getApplicationDataSize();
|
||||
|
||||
// Get request ID
|
||||
uint64_t requestId;
|
||||
if (getRequestIdFromData(data, dataSize, requestId) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
// DEBUG
|
||||
if (debugMode) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::info << "PUS11::doDeleteActivity: requestId: " << requestId << std::endl;
|
||||
#else
|
||||
sif::printInfo("PUS11::doDeleteActivity: requestId: %d\n", requestId);
|
||||
#endif
|
||||
}
|
||||
|
||||
TcMapIter tcToDelete; // handle to the TC to be deleted, can be used if counter is valid
|
||||
int tcToDeleteCount = 0; // counter of all found TCs. Should be 1.
|
||||
|
||||
for (auto it = telecommandMap.begin(); it != telecommandMap.end(); it++) {
|
||||
if (it->second.requestId == requestId) {
|
||||
tcToDelete = it;
|
||||
tcToDeleteCount++;
|
||||
}
|
||||
}
|
||||
|
||||
// check if 1 explicit TC is found via request ID
|
||||
if (tcToDeleteCount == 0 || tcToDeleteCount > 1) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "Service11TelecommandScheduling::doDeleteActivity: No or more than 1 TC found. "
|
||||
"Cannot explicitly delete TC"
|
||||
<< std::endl;
|
||||
#else
|
||||
sif::printWarning(
|
||||
"Service11TelecommandScheduling::doDeleteActivity: No or more than 1 TC found. "
|
||||
"Cannot explicitly delete TC");
|
||||
#endif
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
// delete packet from store
|
||||
if (tcStore->deleteData(tcToDelete->second.storeAddr) != RETURN_OK) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "Service11TelecommandScheduling::doDeleteActivity: Could not delete TC from Store"
|
||||
<< std::endl;
|
||||
#else
|
||||
sif::printError(
|
||||
"Service11TelecommandScheduling::doDeleteActivity: Could not delete TC from Store\n");
|
||||
#endif
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
telecommandMap.erase(tcToDelete);
|
||||
if (debugMode) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::info << "PUS11::doDeleteActivity: Deleted TC from map" << std::endl;
|
||||
#else
|
||||
sif::printInfo("PUS11::doDeleteActivity: Deleted TC from map\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doFilterDeleteActivity(void) {
|
||||
const uint8_t *data = currentPacket.getApplicationData();
|
||||
size_t dataSize = currentPacket.getApplicationDataSize();
|
||||
|
||||
TcMapIter itBegin;
|
||||
TcMapIter itEnd;
|
||||
|
||||
// get the filter window as map range via dedicated method
|
||||
if (getMapFilterFromData(data, dataSize, itBegin, itEnd) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
int deletedTCs = 0;
|
||||
for (TcMapIter it = itBegin; it != itEnd; it++) {
|
||||
// delete packet from store
|
||||
if (tcStore->deleteData(it->second.storeAddr) != RETURN_OK) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "Service11TelecommandScheduling::doFilterDeleteActivity: Could not delete TC "
|
||||
"from Store"
|
||||
<< std::endl;
|
||||
#else
|
||||
sif::printError(
|
||||
"Service11TelecommandScheduling::doFilterDeleteActivity: Could not delete TC from "
|
||||
"Store\n");
|
||||
#endif
|
||||
continue;
|
||||
}
|
||||
deletedTCs++;
|
||||
}
|
||||
|
||||
// NOTE: Spec says this function erases all elements including itBegin but not itEnd,
|
||||
// see here: https://www.cplusplus.com/reference/map/multimap/erase/
|
||||
// Therefore we need to increase itEnd by 1. (Note that end() returns the "past-the-end" iterator)
|
||||
if (itEnd != telecommandMap.end()) {
|
||||
itEnd++;
|
||||
}
|
||||
|
||||
telecommandMap.erase(itBegin, itEnd);
|
||||
|
||||
if (debugMode) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::info << "PUS11::doFilterDeleteActivity: Deleted " << deletedTCs << " TCs" << std::endl;
|
||||
#else
|
||||
sif::printInfo("PUS11::doFilterDeleteActivity: Deleted %d TCs\n", deletedTCs);
|
||||
#endif
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doTimeshiftActivity(void) {
|
||||
const uint8_t *data = currentPacket.getApplicationData();
|
||||
size_t dataSize = currentPacket.getApplicationDataSize();
|
||||
|
||||
// Get relative time
|
||||
uint32_t relativeTime = 0;
|
||||
if (deserializeViaFsfwInterface<uint32_t>(relativeTime, data, dataSize) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
if (relativeTime == 0) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
// TODO further check sanity of the relative time?
|
||||
|
||||
data += sizeof(uint32_t);
|
||||
dataSize -= sizeof(uint32_t);
|
||||
|
||||
// Get request ID
|
||||
uint64_t requestId;
|
||||
if (getRequestIdFromData(data, dataSize, requestId) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
if (debugMode) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::info << "PUS11::doTimeshiftActivity: requestId: " << requestId << std::endl;
|
||||
#else
|
||||
sif::printInfo("PUS11::doTimeshiftActivity: requestId: %d\n", requestId);
|
||||
#endif
|
||||
}
|
||||
|
||||
// NOTE: Despite having C++17 ETL multimap has no member function extract :(
|
||||
|
||||
TcMapIter tcToTimeshiftIt;
|
||||
int tcToTimeshiftCount = 0;
|
||||
|
||||
for (auto it = telecommandMap.begin(); it != telecommandMap.end(); it++) {
|
||||
if (it->second.requestId == requestId) {
|
||||
tcToTimeshiftIt = it;
|
||||
tcToTimeshiftCount++;
|
||||
}
|
||||
}
|
||||
|
||||
if (tcToTimeshiftCount == 0 || tcToTimeshiftCount > 1) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "Service11TelecommandScheduling::doTimeshiftActivity: Either 0 or more than 1 "
|
||||
"TCs found. No explicit timeshifting "
|
||||
"possible"
|
||||
<< std::endl;
|
||||
|
||||
#else
|
||||
sif::printWarning(
|
||||
"Service11TelecommandScheduling::doTimeshiftActivity: Either 0 or more than 1 TCs found. "
|
||||
"No explicit timeshifting possible\n");
|
||||
#endif
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
// temporarily hold the item
|
||||
TelecommandStruct tempTc(tcToTimeshiftIt->second);
|
||||
uint32_t tempKey = tcToTimeshiftIt->first + relativeTime;
|
||||
|
||||
// delete old entry from the mm
|
||||
telecommandMap.erase(tcToTimeshiftIt);
|
||||
|
||||
// and then insert it again as new entry
|
||||
telecommandMap.insert(std::make_pair(tempKey, tempTc));
|
||||
|
||||
if (debugMode) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::info << "PUS11::doTimeshiftActivity: Shifted TC" << std::endl;
|
||||
#else
|
||||
sif::printDebug("PUS11::doTimeshiftActivity: Shifted TC\n");
|
||||
#endif
|
||||
debugPrintMultimapContent();
|
||||
}
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doFilterTimeshiftActivity(void) {
|
||||
const uint8_t *data = currentPacket.getApplicationData();
|
||||
uint32_t dataSize = currentPacket.getApplicationDataSize();
|
||||
|
||||
// Get relative time
|
||||
uint32_t relativeTime = 0;
|
||||
if (deserializeViaFsfwInterface<uint32_t>(relativeTime, data, dataSize) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
if (relativeTime == 0) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
data += sizeof(uint32_t);
|
||||
dataSize -= sizeof(uint32_t);
|
||||
|
||||
// Do time window
|
||||
TcMapIter itBegin;
|
||||
TcMapIter itEnd;
|
||||
if (getMapFilterFromData(data, dataSize, itBegin, itEnd) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
int shiftedItemsCount = 0;
|
||||
for (auto it = itBegin; it != itEnd;) {
|
||||
// temporarily hold the item
|
||||
TelecommandStruct tempTc(it->second);
|
||||
uint32_t tempKey = it->first + relativeTime;
|
||||
|
||||
// delete the old entry from the mm
|
||||
telecommandMap.erase(it++);
|
||||
|
||||
// and then insert it again as new entry
|
||||
telecommandMap.insert(std::make_pair(tempKey, tempTc));
|
||||
shiftedItemsCount++;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (debugMode) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::info << "PUS11::doFilterTimeshiftActivity: shiftedItemsCount: " << shiftedItemsCount
|
||||
<< std::endl;
|
||||
#else
|
||||
sif::printInfo("PUS11::doFilterTimeshiftActivity: shiftedItemsCount: %d\n", shiftedItemsCount);
|
||||
#endif
|
||||
debugPrintMultimapContent();
|
||||
}
|
||||
|
||||
if (shiftedItemsCount > 0) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline uint64_t Service11TelecommandScheduling<MAX_NUM_TCS>::getRequestIdFromDataTC(
|
||||
const uint8_t *data) const {
|
||||
TcPacketPus mask(data);
|
||||
|
||||
uint32_t sourceId = mask.getSourceId();
|
||||
uint16_t apid = mask.getAPID();
|
||||
uint16_t sequenceCount = mask.getPacketSequenceCount();
|
||||
|
||||
return buildRequestId(sourceId, apid, sequenceCount);
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::getRequestIdFromData(
|
||||
const uint8_t *data, size_t &dataSize, uint64_t &requestId) {
|
||||
uint32_t srcId = 0;
|
||||
uint16_t apid = 0;
|
||||
uint16_t ssc = 0;
|
||||
|
||||
if (deserializeViaFsfwInterface<uint32_t>(srcId, data, dataSize) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
data += sizeof(uint32_t);
|
||||
dataSize -= sizeof(uint32_t);
|
||||
if (deserializeViaFsfwInterface<uint16_t>(apid, data, dataSize) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
data += sizeof(uint32_t);
|
||||
dataSize -= sizeof(uint32_t);
|
||||
if (deserializeViaFsfwInterface<uint16_t>(ssc, data, dataSize) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
requestId = buildRequestId(srcId, apid, ssc);
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline uint64_t Service11TelecommandScheduling<MAX_NUM_TCS>::buildRequestId(uint32_t sourceId,
|
||||
uint16_t apid,
|
||||
uint16_t ssc) const {
|
||||
uint64_t sourceId64 = static_cast<uint64_t>(sourceId);
|
||||
uint64_t apid64 = static_cast<uint64_t>(apid);
|
||||
uint64_t ssc64 = static_cast<uint64_t>(ssc);
|
||||
|
||||
return (sourceId64 << 32) | (apid64 << 16) | ssc64;
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::getMapFilterFromData(
|
||||
const uint8_t *data, size_t dataSize, TcMapIter &itBegin, TcMapIter &itEnd) {
|
||||
// get filter type first
|
||||
uint32_t typeRaw;
|
||||
if (deserializeViaFsfwInterface(typeRaw, data, dataSize) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
// can be modified as the uint8_t pointer is passed by value (copy of ptr is modified)
|
||||
data += sizeof(uint32_t);
|
||||
dataSize -= sizeof(uint32_t);
|
||||
|
||||
TypeOfTimeWindow type = static_cast<TypeOfTimeWindow>(typeRaw);
|
||||
|
||||
// we now have the type of delete activity - so now we set the range to delete,
|
||||
// according to the type of time window.
|
||||
// NOTE: Blocks are used in this switch-case statement so that timestamps can be
|
||||
// cleanly redefined for each case.
|
||||
switch (type) {
|
||||
case TypeOfTimeWindow::SELECT_ALL: {
|
||||
itBegin = telecommandMap.begin();
|
||||
itEnd = telecommandMap.end();
|
||||
break;
|
||||
}
|
||||
|
||||
case TypeOfTimeWindow::FROM_TIMETAG: {
|
||||
uint32_t fromTimestamp;
|
||||
if (deserializeViaFsfwInterface<uint32_t>(fromTimestamp, data, dataSize) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
itBegin = telecommandMap.begin();
|
||||
while (itBegin->first < fromTimestamp && itBegin != telecommandMap.end()) {
|
||||
itBegin++;
|
||||
}
|
||||
|
||||
itEnd = telecommandMap.end();
|
||||
break;
|
||||
}
|
||||
|
||||
case TypeOfTimeWindow::TO_TIMETAG: {
|
||||
uint32_t toTimestamp;
|
||||
if (deserializeViaFsfwInterface<uint32_t>(toTimestamp, data, dataSize) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
itBegin = telecommandMap.begin();
|
||||
itEnd = telecommandMap.begin();
|
||||
while (itEnd->first <= toTimestamp && itEnd != telecommandMap.end()) {
|
||||
itEnd++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case TypeOfTimeWindow::FROM_TIMETAG_TO_TIMETAG: {
|
||||
uint32_t fromTimestamp;
|
||||
uint32_t toTimestamp;
|
||||
|
||||
if (deserializeViaFsfwInterface<uint32_t>(fromTimestamp, data, dataSize) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
data += sizeof(uint32_t);
|
||||
dataSize -= sizeof(uint32_t);
|
||||
|
||||
if (deserializeViaFsfwInterface<uint32_t>(toTimestamp, data, dataSize) != RETURN_OK) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
itBegin = telecommandMap.begin();
|
||||
itEnd = telecommandMap.begin();
|
||||
|
||||
while (itBegin->first < fromTimestamp && itBegin != telecommandMap.end()) {
|
||||
itBegin++;
|
||||
}
|
||||
while (itEnd->first <= toTimestamp && itEnd != telecommandMap.end()) {
|
||||
itEnd++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
// additional security check, this should never be true
|
||||
if (itBegin->first > itEnd->first) {
|
||||
sif::printError("11::GetMapFilterFromData: itBegin > itEnd\n");
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
// the map range should now be set according to the sent filter.
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
template <typename T>
|
||||
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::deserializeViaFsfwInterface(
|
||||
T &output, const uint8_t *buf, size_t bufsize) {
|
||||
if (buf == nullptr) {
|
||||
#if FSFW_VERBOSE_LEVEL >= 1
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "Service11TelecommandScheduling::deserializeViaFsfwInterface: "
|
||||
"Invalid buffer"
|
||||
<< std::endl;
|
||||
#else
|
||||
sif::printWarning(
|
||||
"Service11TelecommandScheduling::deserializeViaFsfwInterface: "
|
||||
"Invalid buffer\n");
|
||||
#endif
|
||||
#endif
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
return SerializeAdapter::deSerialize<T>(&output, &buf, &bufsize, SerializeIF::Endianness::BIG);
|
||||
}
|
||||
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline void Service11TelecommandScheduling<MAX_NUM_TCS>::debugPrintMultimapContent(void) const {
|
||||
#if FSFW_DISABLE_PRINTOUT == 0
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::debug << "Service11TelecommandScheduling::debugPrintMultimapContent: Multimap Content"
|
||||
<< std::endl;
|
||||
sif::debug << "[" << dit->first << "]: Request ID: " << dit->second.requestId << " | "
|
||||
<< "Store Address: " << dit->second.storeAddr << std::endl;
|
||||
#else
|
||||
sif::printDebug("Service11TelecommandScheduling::debugPrintMultimapContent: Multimap Content\n");
|
||||
for (auto dit = telecommandMap.begin(); dit != telecommandMap.end(); ++dit) {
|
||||
sif::printDebug("[%d]: Request ID: %d | Store Address: %d\n", dit->first,
|
||||
dit->second.requestId, dit->second.storeAddr);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user