Compare commits

..

69 Commits

Author SHA1 Message Date
42867ad0cb remove unuseed include 2024-04-30 15:10:40 +02:00
6381d9a83c Merge branch 'develop' of https://egit.irs.uni-stuttgart.de/eive/fsfw into develop 2024-04-29 13:18:54 +02:00
73bf1cc229
add setter function for source seq counter 2024-04-29 13:18:29 +02:00
222f02aefd Merge pull request 'Some more fixes' (#174) from fdir-fix-fresh-dhb into develop
Reviewed-on: #174
Reviewed-by: Marius Eggert <eggertm@irs.uni-stuttgart.de>
2024-04-29 11:45:28 +02:00
e9b4fc9825 Merge branch 'develop' into fdir-fix-fresh-dhb 2024-04-17 20:47:33 +02:00
0660457c92
check for FDIR failures 2024-04-17 12:11:07 +02:00
7c9b9e4cd8
initialize FDIR component properly 2024-04-17 12:08:08 +02:00
3b0ee7ca31
i forgot to push.. 2024-04-17 10:07:09 +02:00
fb2e480705 did i forgot to push? 2024-04-16 13:58:10 +02:00
b8ae646060 this include might be missing 2024-04-16 13:53:35 +02:00
f307a86d9a
improve linux interface 2024-04-11 13:11:51 +02:00
8b21dd276d Merge pull request 'implement relative timeshift' (#173) from time-service-relative-timeshift into develop
Reviewed-on: #173
Reviewed-by: Marius Eggert <eggertm@irs.uni-stuttgart.de>
2024-04-09 13:31:13 +02:00
e00da212cd Merge branch 'develop' into time-service-relative-timeshift 2024-04-09 11:11:59 +02:00
a229748aa4 distinct clock dump events 2024-04-09 10:57:00 +02:00
e6e3753324 Merge pull request 're-work some FDIR logic' (#172) from improve-dev-fdir into develop
Reviewed-on: #172
2024-04-09 10:47:32 +02:00
94bd1ba2ab fix comment blocks 2024-04-08 13:38:14 +02:00
e12a8cfa29 make this change downwards compatible 2024-04-08 13:31:21 +02:00
efbcddc2e5 improvements for time service 2024-04-08 13:26:55 +02:00
31d4b85523 some optimizations 2024-04-08 13:14:20 +02:00
aff6bb673b
make marius the happies man alive 2024-04-08 10:34:28 +02:00
f8e3777c43
more fixes 2024-04-03 16:23:21 +02:00
0e2fa8dc83
lets not forget error handling 2024-04-03 16:19:40 +02:00
0cfe559b93
implement relative timeshift 2024-04-03 16:15:07 +02:00
c5159fb645
optimize docs 2024-04-03 12:52:32 +02:00
203c0bac5c
re-work some FDIR logic 2024-04-03 12:47:11 +02:00
43ea29cb84 Merge pull request 'Function to prevent Quaternion Sign Jump' (#171) from quat-no-sign-jump into develop
Reviewed-on: #171
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
2024-03-18 10:59:19 +01:00
27c8a97d45 added function to prevent quaternion sign jump 2024-03-13 16:59:14 +01:00
47b21caf5f Merge pull request 'Removed duplicate Code' (#170) from cleanup into develop
Reviewed-on: #170
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
2024-03-06 10:54:12 +01:00
2673070204 actually do something with this rtval 2024-03-06 09:41:02 +01:00
518a07d05b removed duplicate code 2024-03-05 14:29:47 +01:00
516357d855 Merge pull request 'Expand Globalfunctions' (#168) from expand-globalfunctions into develop
Reviewed-on: #168
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
2024-02-27 12:50:28 +01:00
ac28b7e00d Merge branch 'develop' into expand-globalfunctions 2024-02-26 13:57:04 +01:00
0d4a862c1a Merge pull request 'Increase configurability of PusServiceBase' (#169) from psb-update into develop
Reviewed-on: #169
Reviewed-by: Marius Eggert <eggertm@irs.uni-stuttgart.de>
2024-02-26 13:56:46 +01:00
b2576d3422 why are we ignoring usec 2024-02-23 11:33:20 +01:00
bccaf4a9ea capital 2024-02-23 11:31:11 +01:00
7d4e77843b frmt 2024-02-23 11:09:59 +01:00
cd2cd61ba5 Increase configurability of PusServiceBase 2024-02-13 15:57:43 +01:00
90f3f8ef1f removed printout 2024-02-12 14:51:54 +01:00
9894935e99 time systems 2024-02-12 14:42:58 +01:00
a3a6c0720c fixes 2024-02-12 14:36:14 +01:00
d5a52eadbb i should have done this ages ago 2024-02-12 14:20:04 +01:00
b5e7179af1 Merge pull request 'Rotation from Quaternions' (#167) from rotation-from-quaternions into develop
Reviewed-on: #167
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
2024-01-29 14:11:08 +01:00
5879eb7f7b rotation from quaternions 2024-01-26 11:06:05 +01:00
e64e8b274d Merge pull request 'check whether map is full on insert' (#166) from insert-full-check into develop
Reviewed-on: #166
Reviewed-by: Marius Eggert <eggertm@irs.uni-stuttgart.de>
2023-12-12 17:53:14 +01:00
48bcce65b1 check whether map is full on insert 2023-12-12 11:24:35 +01:00
7105e199c6 Merge pull request 'SLERP for antiparallel Quaternions' (#165) from slerp-fix-antiparallel-quaternions into develop
Reviewed-on: #165
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
2023-12-06 10:41:02 +01:00
94de483836 small fix 2023-12-06 09:26:22 +01:00
c648229c99 Merge pull request 'SLERP' (#163) from slerp into develop
Reviewed-on: #163
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
2023-12-05 09:25:48 +01:00
5b661551a8 fixes 2023-12-04 17:52:24 +01:00
3978524181 Merge branch 'develop' into slerp 2023-11-29 15:19:09 +01:00
7187f2b5cd Merge pull request 'TLE turns too old after just 7 Days' (#164) from tle-too-old into develop
Reviewed-on: #164
2023-11-29 15:17:39 +01:00
8e367abb47 Merge branch 'develop' into tle-too-old 2023-11-29 15:17:19 +01:00
b28174db24 Merge pull request 'Fresh DHB pre-queue check hook' (#162) from fresh-dhb-pre-queue-hook into develop
Reviewed-on: #162
Reviewed-by: Marius Eggert <eggertm@irs.uni-stuttgart.de>
2023-11-29 14:18:23 +01:00
c906acd659 tle turns too old after just 7 days 2023-11-29 09:48:11 +01:00
8dfc84c0b5 smoll fix 2023-11-27 10:52:47 +01:00
74775bb59d slerp 2023-11-27 10:38:25 +01:00
c02d9e009e docs improvements and fix 2023-11-21 17:40:06 +01:00
0021aa29f5 pre-queue hook 2023-11-21 17:37:02 +01:00
41d67bff63 Merge pull request 'New simpler DHB' (#161) from new-dhb into develop
Reviewed-on: #161
Reviewed-by: Ulrich Mohr <mohr@irs.uni-stuttgart.de>
2023-11-16 11:02:44 +01:00
8d7bb51d44
add missing call to perform HK op 2023-11-15 11:39:04 +01:00
7673d8b396
add paramHelper 2023-11-15 10:03:56 +01:00
91b194d8eb Merge branch 'new-dhb' of https://egit.irs.uni-stuttgart.de/eive/fsfw into new-dhb 2023-11-14 11:48:20 +01:00
698897bfc5 default args 2023-11-14 11:48:17 +01:00
d554062b86
trigger event default arguments 2023-11-13 15:32:42 +01:00
bf7fac071c add mode setters 2023-11-09 17:18:33 +01:00
35be7da353
some more docs 2023-11-09 11:28:59 +01:00
18cc870c8e
add FDIR handling 2023-11-09 11:04:45 +01:00
133ca51d18
visibility changes 2023-11-09 10:45:21 +01:00
0a40391a75
thats the baseline 2023-11-09 10:36:45 +01:00
27 changed files with 805 additions and 78 deletions

View File

@ -28,9 +28,12 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
- add CFDP subsystem ID - add CFDP subsystem ID
https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/742 https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/742
- `PusTmZcWriter` now exposes API to set message counter field. - `PusTmZcWriter` now exposes API to set message counter field.
- Relative timeshift in the PUS time service.
## Changed ## Changed
- The PUS time service now dumps the time before setting a new time and after having set the
time.
- HK generation is now countdown based. - HK generation is now countdown based.
- Bump ETL version to 20.35.14 - Bump ETL version to 20.35.14
https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/748 https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/748
@ -41,6 +44,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
- Assert that `FixedArrayList` is larger than 0 at compile time. - Assert that `FixedArrayList` is larger than 0 at compile time.
https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/740 https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/740
- Health functions are virtual now. - Health functions are virtual now.
- PUS Service Base request queue depth and maximum number of handled packets per cycle is now
configurable.
# [v6.0.0] 2023-02-10 # [v6.0.0] 2023-02-10

View File

@ -72,18 +72,17 @@ void ControllerBase::getMode(Mode_t* mode_, Submode_t* submode_) {
*submode_ = this->submode; *submode_ = this->submode;
} }
void ControllerBase::setToExternalControl() { healthHelper.setHealth(EXTERNAL_CONTROL); }
void ControllerBase::announceMode(bool recursive) { triggerEvent(MODE_INFO, mode, submode); } void ControllerBase::announceMode(bool recursive) { triggerEvent(MODE_INFO, mode, submode); }
void ControllerBase::modeChanged(Mode_t mode_, Submode_t submode_) {}
void ControllerBase::setToExternalControl() { healthHelper.setHealth(EXTERNAL_CONTROL); }
ReturnValue_t ControllerBase::performOperation(uint8_t opCode) { ReturnValue_t ControllerBase::performOperation(uint8_t opCode) {
handleQueue(); handleQueue();
performControlOperation(); performControlOperation();
return returnvalue::OK; return returnvalue::OK;
} }
void ControllerBase::modeChanged(Mode_t mode_, Submode_t submode_) {}
ReturnValue_t ControllerBase::setHealth(HealthState health) { ReturnValue_t ControllerBase::setHealth(HealthState health) {
switch (health) { switch (health) {
case HEALTHY: case HEALTHY:

View File

@ -6,6 +6,8 @@
#include "fsfw/globalfunctions/constants.h" #include "fsfw/globalfunctions/constants.h"
#include "fsfw/globalfunctions/math/MatrixOperations.h" #include "fsfw/globalfunctions/math/MatrixOperations.h"
#include "fsfw/globalfunctions/math/VectorOperations.h" #include "fsfw/globalfunctions/math/VectorOperations.h"
#include "fsfw/globalfunctions/sign.h"
#include "fsfw/serviceinterface.h"
void CoordinateTransformations::positionEcfToEci(const double* ecfPosition, double* eciPosition, void CoordinateTransformations::positionEcfToEci(const double* ecfPosition, double* eciPosition,
timeval* timeUTC) { timeval* timeUTC) {
@ -97,7 +99,14 @@ void CoordinateTransformations::ecfToEci(const double* ecfCoordinates, double* e
double CoordinateTransformations::getJuleanCenturiesTT(timeval timeUTC) { double CoordinateTransformations::getJuleanCenturiesTT(timeval timeUTC) {
timeval timeTT; timeval timeTT;
Clock::convertUTCToTT(timeUTC, &timeTT); ReturnValue_t result = Clock::convertUTCToTT(timeUTC, &timeTT);
if (result != returnvalue::OK) {
// i think it is better to continue here than to abort
timeTT = timeUTC;
sif::error << "CoordinateTransformations::Conversion from UTC to TT failed. Continuing "
"calculations with UTC."
<< std::endl;
}
double jD2000TT; double jD2000TT;
Clock::convertTimevalToJD2000(timeTT, &jD2000TT); Clock::convertTimevalToJD2000(timeTT, &jD2000TT);
@ -207,3 +216,61 @@ void CoordinateTransformations::getTransMatrixECITOECF(timeval timeUTC, double T
MatrixOperations<double>::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3); MatrixOperations<double>::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3);
}; };
void CoordinateTransformations::cartesianFromLatLongAlt(const double lat, const double longi,
const double alt, double* cartesianOutput) {
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
* longitude and altitude
* @param: lat geodetic latitude [rad]
* longi longitude [rad]
* alt altitude [m]
* cartesianOutput Cartesian Coordinates in ECEF (3x1)
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
* Landis Markley and John L. Crassidis*/
double radiusPolar = 6356752.314;
double radiusEqua = 6378137;
double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
};
void CoordinateTransformations::latLongAltFromCartesian(const double* vector, double& latitude,
double& longitude, double& altitude) {
/* @brief: latLongAltFromCartesian() - calculates latitude, longitude and altitude from
* cartesian coordinates in ECEF
* @param: x x-value of position vector [m]
* y y-value of position vector [m]
* z z-value of position vector [m]
* latitude geodetic latitude [rad]
* longitude longitude [rad]
* altitude altitude [m]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f
* Landis Markley and John L. Crassidis*/
// From World Geodetic System the Earth Radii
double a = 6378137.0; // semimajor axis [m]
double b = 6356752.3142; // semiminor axis [m]
// Calculation
double e2 = 1 - pow(b, 2) / pow(a, 2);
double epsilon2 = pow(a, 2) / pow(b, 2) - 1;
double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2));
double p = std::abs(vector[2]) / epsilon2;
double s = pow(rho, 2) / (e2 * epsilon2);
double q = pow(p, 2) - pow(b, 2) + s;
double u = p / sqrt(q);
double v = pow(b, 2) * pow(u, 2) / q;
double P = 27 * v * s / q;
double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.);
double t = (1 + Q + 1 / Q) / 6;
double c = sqrt(pow(u, 2) - 1 + 2 * t);
double w = (c - u) / 2;
double d = sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.));
double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2));
latitude = asin((epsilon2 + 1) * d / N);
altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N;
longitude = atan2(vector[1], vector[0]);
}

View File

@ -23,6 +23,12 @@ class CoordinateTransformations {
static void getEarthRotationMatrix(timeval timeUTC, double matrix[][3]); static void getEarthRotationMatrix(timeval timeUTC, double matrix[][3]);
static void cartesianFromLatLongAlt(const double lat, const double longi, const double alt,
double* cartesianOutput);
static void latLongAltFromCartesian(const double* vector, double& latitude, double& longitude,
double& altitude);
private: private:
CoordinateTransformations(); CoordinateTransformations();
static void ecfToEci(const double* ecfCoordinates, double* eciCoordinates, static void ecfToEci(const double* ecfCoordinates, double* eciCoordinates,

View File

@ -166,9 +166,9 @@ ReturnValue_t Sgp4Propagator::propagate(double* position, double* velocity, time
timeval timeSinceEpoch = time - epoch; timeval timeSinceEpoch = time - epoch;
double minutesSinceEpoch = timeSinceEpoch.tv_sec / 60. + timeSinceEpoch.tv_usec / 60000000.; double minutesSinceEpoch = timeSinceEpoch.tv_sec / 60. + timeSinceEpoch.tv_usec / 60000000.;
double monthsSinceEpoch = minutesSinceEpoch / 60 / 24 / 30; double daysSinceEpoch = minutesSinceEpoch / 60 / 24;
if ((monthsSinceEpoch > 1) || (monthsSinceEpoch < -1)) { if ((daysSinceEpoch > 7) || (daysSinceEpoch < -7)) {
return TLE_TOO_OLD; return TLE_TOO_OLD;
} }

View File

@ -7,4 +7,5 @@ target_sources(
DeviceHandlerFailureIsolation.cpp DeviceHandlerFailureIsolation.cpp
DeviceHandlerMessage.cpp DeviceHandlerMessage.cpp
DeviceTmReportingWrapper.cpp DeviceTmReportingWrapper.cpp
FreshDeviceHandlerBase.cpp
HealthDevice.cpp) HealthDevice.cpp)

View File

@ -26,6 +26,11 @@ ReturnValue_t DeviceHandlerFailureIsolation::eventReceived(EventMessage* event)
if (isFdirInActionOrAreWeFaulty(event)) { if (isFdirInActionOrAreWeFaulty(event)) {
return returnvalue::OK; return returnvalue::OK;
} }
// As mentioned in the function documentation, no FDIR reaction are performed when the device
// is in external control.
if (owner->getHealth() == HasHealthIF::EXTERNAL_CONTROL) {
return returnvalue::OK;
}
ReturnValue_t result = returnvalue::FAILED; ReturnValue_t result = returnvalue::FAILED;
switch (event->getEvent()) { switch (event->getEvent()) {
case HasModesIF::MODE_TRANSITION_FAILED: case HasModesIF::MODE_TRANSITION_FAILED:
@ -186,15 +191,6 @@ void DeviceHandlerFailureIsolation::setFdirState(FDIRState state) {
fdirState = state; fdirState = state;
} }
void DeviceHandlerFailureIsolation::triggerEvent(Event event, uint32_t parameter1,
uint32_t parameter2) {
// Do not throw error events if fdirState != none.
// This will still forward MODE and HEALTH INFO events in any case.
if (fdirState == NONE || event::getSeverity(event) == severity::INFO) {
FailureIsolationBase::triggerEvent(event, parameter1, parameter2);
}
}
bool DeviceHandlerFailureIsolation::isFdirActionInProgress() { return (fdirState != NONE); } bool DeviceHandlerFailureIsolation::isFdirActionInProgress() { return (fdirState != NONE); }
void DeviceHandlerFailureIsolation::startRecovery(Event reason) { void DeviceHandlerFailureIsolation::startRecovery(Event reason) {

View File

@ -17,7 +17,6 @@ class DeviceHandlerFailureIsolation : public FailureIsolationBase {
uint8_t eventQueueDepth = 10); uint8_t eventQueueDepth = 10);
~DeviceHandlerFailureIsolation(); ~DeviceHandlerFailureIsolation();
ReturnValue_t initialize(); ReturnValue_t initialize();
void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0);
bool isFdirActionInProgress(); bool isFdirActionInProgress();
virtual ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, virtual ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId,
ParameterWrapper* parameterWrapper, ParameterWrapper* parameterWrapper,
@ -41,6 +40,19 @@ class DeviceHandlerFailureIsolation : public FailureIsolationBase {
static const uint32_t DEFAULT_MAX_MISSED_REPLY_COUNT = 5; static const uint32_t DEFAULT_MAX_MISSED_REPLY_COUNT = 5;
static const uint32_t DEFAULT_MISSED_REPLY_TIME_MS = 10000; static const uint32_t DEFAULT_MISSED_REPLY_TIME_MS = 10000;
/**
* This is the default implementation of the eventReceived function.
*
* It will perform recoveries or failures on a pre-defined set of events. If the user wants
* to add handling for custom events, this function should be overriden.
*
* It should be noted that the default implementation will not perform FDIR reactions if the
* handler is faulty or in external control by default. If the user commands the device
* manually, this might be related to debugging to testing the device in a low-level way. FDIR
* reactions might get in the way of this process by restarting the device or putting it in
* the faulty state. If the user still requires FDIR handling in the EXTERNAL_CONTROL case,
* this function should be overriden.
*/
virtual ReturnValue_t eventReceived(EventMessage* event); virtual ReturnValue_t eventReceived(EventMessage* event);
virtual void eventConfirmed(EventMessage* event); virtual void eventConfirmed(EventMessage* event);
void wasParentsFault(EventMessage* event); void wasParentsFault(EventMessage* event);

View File

@ -0,0 +1,203 @@
#include "FreshDeviceHandlerBase.h"
#include "fsfw/devicehandlers/DeviceHandlerFailureIsolation.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/subsystem/helper.h"
FreshDeviceHandlerBase::FreshDeviceHandlerBase(DhbConfig config)
: SystemObject(config.objectId),
actionHelper(this, nullptr),
modeHelper(this),
healthHelper(this, getObjectId()),
paramHelper(this),
poolManager(this, nullptr),
fdirInstance(config.fdirInstance),
defaultFdirParent(config.defaultFdirParent) {
auto mqArgs = MqArgs(config.objectId, static_cast<void*>(this));
messageQueue = QueueFactory::instance()->createMessageQueue(
config.msgQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
}
FreshDeviceHandlerBase::~FreshDeviceHandlerBase() {
QueueFactory::instance()->deleteMessageQueue(messageQueue);
if (not hasCustomFdir) {
delete fdirInstance;
}
}
[[nodiscard]] object_id_t FreshDeviceHandlerBase::getObjectId() const {
return SystemObject::getObjectId();
}
ReturnValue_t FreshDeviceHandlerBase::performOperation(uint8_t opCode) {
performDeviceOperationPreQueueHandling(opCode);
handleQueue();
fdirInstance->checkForFailures();
performDeviceOperation(opCode);
poolManager.performHkOperation();
return returnvalue::OK;
}
ReturnValue_t FreshDeviceHandlerBase::performDeviceOperationPreQueueHandling(uint8_t opCode) {
return returnvalue::OK;
}
void FreshDeviceHandlerBase::startTransition(Mode_t mode_, Submode_t submode_) {
triggerEvent(CHANGING_MODE, mode_, submode_);
// Complete mode transition immediately by default.
setMode(mode_, submode_);
}
void FreshDeviceHandlerBase::setMode(Mode_t newMode, Submode_t newSubmode) {
mode = newMode;
submode = newSubmode;
modeHelper.modeChanged(mode, submode);
modeChanged(mode, submode);
announceMode(false);
}
void FreshDeviceHandlerBase::setMode(Mode_t newMode) { setMode(newMode, submode); }
void FreshDeviceHandlerBase::getMode(Mode_t* mode_, Submode_t* submode_) {
*mode_ = this->mode;
*submode_ = this->submode;
}
void FreshDeviceHandlerBase::announceMode(bool recursive) {
triggerEvent(MODE_INFO, mode, submode);
}
void FreshDeviceHandlerBase::modeChanged(Mode_t mode_, Submode_t submode_) {}
[[nodiscard]] MessageQueueId_t FreshDeviceHandlerBase::getCommandQueue() const {
return messageQueue->getId();
}
ReturnValue_t FreshDeviceHandlerBase::handleQueue() {
CommandMessage command;
ReturnValue_t result;
for (result = messageQueue->receiveMessage(&command); result == returnvalue::OK;
result = messageQueue->receiveMessage(&command)) {
result = actionHelper.handleActionMessage(&command);
if (result == returnvalue::OK) {
continue;
}
result = modeHelper.handleModeCommand(&command);
if (result == returnvalue::OK) {
continue;
}
result = healthHelper.handleHealthCommand(&command);
if (result == returnvalue::OK) {
continue;
}
result = paramHelper.handleParameterMessage(&command);
if (result == returnvalue::OK) {
continue;
}
result = poolManager.handleHousekeepingMessage(&command);
if (result == returnvalue::OK) {
continue;
}
result = handleCommandMessage(&command);
if (result == returnvalue::OK) {
continue;
}
command.setToUnknownCommand();
messageQueue->reply(&command);
}
return result;
}
HasHealthIF::HealthState FreshDeviceHandlerBase::getHealth() { return healthHelper.getHealth(); }
const HasHealthIF* FreshDeviceHandlerBase::getOptHealthIF() const { return this; }
const HasModesIF& FreshDeviceHandlerBase::getModeIF() const { return *this; }
ModeTreeChildIF& FreshDeviceHandlerBase::getModeTreeChildIF() { return *this; }
ReturnValue_t FreshDeviceHandlerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper);
}
// Executable Overrides.
void FreshDeviceHandlerBase::setTaskIF(PeriodicTaskIF* task_) { executingTask = task_; }
// Pool Manager overrides.
LocalDataPoolManager* FreshDeviceHandlerBase::getHkManagerHandle() { return &poolManager; }
[[nodiscard]] uint32_t FreshDeviceHandlerBase::getPeriodicOperationFrequency() const {
return this->executingTask->getPeriodMs();
}
ReturnValue_t FreshDeviceHandlerBase::initializeAfterTaskCreation() {
return poolManager.initializeAfterTaskCreation();
}
ReturnValue_t FreshDeviceHandlerBase::setHealth(HasHealthIF::HealthState health) {
// Assembly should handle commanding to OFF.
healthHelper.setHealth(health);
return returnvalue::OK;
}
void FreshDeviceHandlerBase::triggerEvent(Event event, uint32_t parameter1, uint32_t parameter2) {
fdirInstance->triggerEvent(event, parameter1, parameter2);
}
void FreshDeviceHandlerBase::forwardEvent(Event event, uint32_t parameter1,
uint32_t parameter2) const {
fdirInstance->triggerEvent(event, parameter1, parameter2);
}
void FreshDeviceHandlerBase::setToExternalControl() { setHealth(HealthState::EXTERNAL_CONTROL); }
// System Object overrides.
ReturnValue_t FreshDeviceHandlerBase::initialize() {
ReturnValue_t result = modeHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
result = healthHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
result = actionHelper.initialize(messageQueue);
if (result != returnvalue::OK) {
return result;
}
result = paramHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
result = poolManager.initialize(messageQueue);
if (result != returnvalue::OK) {
return result;
}
if (fdirInstance == nullptr) {
hasCustomFdir = false;
fdirInstance = new DeviceHandlerFailureIsolation(getObjectId(), defaultFdirParent);
}
result = fdirInstance->initialize();
if (result != returnvalue::OK) {
return result;
}
return SystemObject::initialize();
}
ReturnValue_t FreshDeviceHandlerBase::getParameter(uint8_t domainId, uint8_t uniqueId,
ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues,
uint16_t startAtIndex) {
ReturnValue_t result =
fdirInstance->getParameter(domainId, uniqueId, parameterWrapper, newValues, startAtIndex);
if (result != INVALID_DOMAIN_ID) {
return result;
}
return INVALID_DOMAIN_ID;
}

View File

@ -0,0 +1,167 @@
#pragma once
#include "fsfw/action.h"
#include "fsfw/datapoollocal/HasLocalDataPoolIF.h"
#include "fsfw/datapoollocal/LocalDataPoolManager.h"
#include "fsfw/devicehandlers/DeviceHandlerIF.h"
#include "fsfw/fdir/FailureIsolationBase.h"
#include "fsfw/health/HasHealthIF.h"
#include "fsfw/health/HealthHelper.h"
#include "fsfw/modes/HasModesIF.h"
#include "fsfw/objectmanager.h"
#include "fsfw/parameters/ParameterHelper.h"
#include "fsfw/parameters/ReceivesParameterMessagesIF.h"
#include "fsfw/retval.h"
#include "fsfw/subsystem/ModeTreeChildIF.h"
#include "fsfw/subsystem/ModeTreeConnectionIF.h"
#include "fsfw/tasks/ExecutableObjectIF.h"
#include "fsfw/tasks/PeriodicTaskIF.h"
struct DhbConfig {
explicit DhbConfig(object_id_t objectId) : objectId(objectId) {}
object_id_t objectId;
FailureIsolationBase* fdirInstance = nullptr;
object_id_t defaultFdirParent = objects::NO_OBJECT;
uint32_t msgQueueDepth = 10;
};
class FreshDeviceHandlerBase : public SystemObject,
public DeviceHandlerIF,
public HasModesIF,
public HasHealthIF,
public ExecutableObjectIF,
public ModeTreeChildIF,
public ModeTreeConnectionIF,
public HasActionsIF,
public ReceivesParameterMessagesIF,
public HasLocalDataPoolIF {
public:
explicit FreshDeviceHandlerBase(DhbConfig config);
~FreshDeviceHandlerBase() override;
/**
* Periodic helper executed function, implemented by child class.
*/
virtual void performDeviceOperation(uint8_t opCode) = 0;
[[nodiscard]] object_id_t getObjectId() const override;
[[nodiscard]] MessageQueueId_t getCommandQueue() const override;
HasHealthIF::HealthState getHealth() override;
// Mode Tree Overrides.
[[nodiscard]] const HasHealthIF* getOptHealthIF() const override;
[[nodiscard]] const HasModesIF& getModeIF() const override;
ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) override;
ModeTreeChildIF& getModeTreeChildIF() override;
[[nodiscard]] uint32_t getPeriodicOperationFrequency() const override;
protected:
// Pool Manager overrides.
LocalDataPoolManager* getHkManagerHandle() override;
ActionHelper actionHelper;
ModeHelper modeHelper;
HealthHelper healthHelper;
ParameterHelper paramHelper;
LocalDataPoolManager poolManager;
bool hasCustomFdir = false;
FailureIsolationBase* fdirInstance;
object_id_t defaultFdirParent;
/**
* Pointer to the task which executes this component,
* is invalid before setTaskIF was called.
*/
PeriodicTaskIF* executingTask = nullptr;
Mode_t mode = HasModesIF::MODE_UNDEFINED;
Submode_t submode = 0;
MessageQueueIF* messageQueue;
/**
* The default queue handler will process all messages for the interfaces implemented
* by this class. If there are special requirements, for example that action commands are
* received on a different queue, the user can override this function for those special
* requirements.
*/
virtual ReturnValue_t handleQueue();
// Mode Helpers.
virtual void modeChanged(Mode_t mode, Submode_t submode);
/**
* The default implementation sets the new mode immediately. If this is not applicable for
* certain modes, the user should provide a custom implementation, which performs rougly
* the same functionality of this function, when all the steps have been taken to reach the
* new mode.
*/
void startTransition(Mode_t mode, Submode_t submode) override;
virtual void setMode(Mode_t newMode, Submode_t newSubmode);
virtual void setMode(Mode_t newMode);
void getMode(Mode_t* mode, Submode_t* submode) override;
void setToExternalControl() override;
void announceMode(bool recursive) override;
// System Object overrides.
ReturnValue_t initialize() override;
/**
* Implemented by child class. Handle all command messages which are
* not health, mode, action or housekeeping messages.
* @param message
* @return
*/
virtual ReturnValue_t handleCommandMessage(CommandMessage* message) = 0;
// HK manager abstract functions.
LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override = 0;
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) override = 0;
// Mode abstract functions
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t* msToReachTheMode) override = 0;
// Health Overrides.
ReturnValue_t setHealth(HealthState health) override;
// Action override. Forward to user.
ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
const uint8_t* data, size_t size) override = 0;
// Executable overrides.
virtual ReturnValue_t performOperation(uint8_t opCode) override;
ReturnValue_t initializeAfterTaskCreation() override;
/**
* This calls the FDIR instance event trigger function.
* @param event
* @param parameter1
* @param parameter2
*/
void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0) override;
/**
* This calls the FDIR instance event forward function.
* @param event
* @param parameter1
* @param parameter2
*/
void forwardEvent(Event event, uint32_t parameter1, uint32_t parameter2) const override;
/**
* This implementation handles the FDIR parameters. The user can override this to handle
* custom parameters.
* @param domainId
* @param uniqueId
* @param parameterWrapper
* @param newValues
* @param startAtIndex
* @return
*/
ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues, uint16_t startAtIndex) override;
virtual ReturnValue_t performDeviceOperationPreQueueHandling(uint8_t opCode);
private:
// Executable Overrides.
void setTaskIF(PeriodicTaskIF* task) override;
};

View File

@ -148,25 +148,16 @@ void FailureIsolationBase::doConfirmFault(EventMessage* event) {
ReturnValue_t FailureIsolationBase::confirmFault(EventMessage* event) { return YOUR_FAULT; } ReturnValue_t FailureIsolationBase::confirmFault(EventMessage* event) { return YOUR_FAULT; }
void FailureIsolationBase::triggerEvent(Event event, uint32_t parameter1, uint32_t parameter2) { void FailureIsolationBase::triggerEvent(Event event, uint32_t parameter1, uint32_t parameter2) {
// With this mechanism, all events are disabled for a certain device. // By default, we trigger all events and also call the handler function to handle FDIR reactions
// That's not so good for visibility. // which might occur due to these events. This makes all events visible. If the handling of
if (isFdirDisabledForSeverity(event::getSeverity(event))) { // FDIR reaction should be disabled, this should be done through dedicated logic inside the
return; // eventReceived function.
}
EventMessage message(event, ownerId, parameter1, parameter2); EventMessage message(event, ownerId, parameter1, parameter2);
EventManagerIF::triggerEvent(&message, eventQueue->getId()); EventManagerIF::triggerEvent(&message, eventQueue->getId());
eventReceived(&message); eventReceived(&message);
} }
bool FailureIsolationBase::isFdirDisabledForSeverity(EventSeverity_t severity) { bool FailureIsolationBase::isFdirDisabledForSeverity(EventSeverity_t severity) { return false; }
if ((owner != NULL) && (severity != severity::INFO)) {
if (owner->getHealth() == HasHealthIF::EXTERNAL_CONTROL) {
// External control disables handling of fault messages.
return true;
}
}
return false;
}
void FailureIsolationBase::throwFdirEvent(Event event, uint32_t parameter1, uint32_t parameter2) { void FailureIsolationBase::throwFdirEvent(Event event, uint32_t parameter1, uint32_t parameter2) {
EventMessage message(event, ownerId, parameter1, parameter2); EventMessage message(event, ownerId, parameter1, parameter2);

View File

@ -44,13 +44,13 @@ class FailureIsolationBase : public ConfirmsFailuresIF, public HasParametersIF {
virtual void wasParentsFault(EventMessage* event); virtual void wasParentsFault(EventMessage* event);
virtual ReturnValue_t confirmFault(EventMessage* event); virtual ReturnValue_t confirmFault(EventMessage* event);
virtual void decrementFaultCounters() = 0; virtual void decrementFaultCounters() = 0;
virtual bool isFdirDisabledForSeverity(EventSeverity_t severity);
ReturnValue_t sendConfirmationRequest(EventMessage* event, ReturnValue_t sendConfirmationRequest(EventMessage* event,
MessageQueueId_t destination = MessageQueueIF::NO_QUEUE); MessageQueueId_t destination = MessageQueueIF::NO_QUEUE);
void throwFdirEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0); void throwFdirEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0);
private: private:
void doConfirmFault(EventMessage* event); void doConfirmFault(EventMessage* event);
bool isFdirDisabledForSeverity(EventSeverity_t severity);
}; };
#endif /* FRAMEWORK_FDIR_FAILUREISOLATIONBASE_H_ */ #endif /* FRAMEWORK_FDIR */

View File

@ -1,9 +1,12 @@
#ifndef MATRIXOPERATIONS_H_ #ifndef MATRIXOPERATIONS_H_
#define MATRIXOPERATIONS_H_ #define MATRIXOPERATIONS_H_
#include <fsfw/retval.h>
#include <stdint.h> #include <stdint.h>
#include <cmath> #include <cmath>
#include <cstring>
#include <utility>
template <typename T1, typename T2 = T1, typename T3 = T2> template <typename T1, typename T2 = T1, typename T3 = T2>
class MatrixOperations { class MatrixOperations {
@ -95,6 +98,139 @@ class MatrixOperations {
} }
} }
} }
static bool isFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
for (uint8_t col = 0; col < cols; col++) {
for (uint8_t row = 0; row < rows; row++) {
if (not std::isfinite(inputMatrix[row * cols + cols])) {
return false;
}
}
}
return true;
}
static void writeSubmatrix(T1 *mainMatrix, T1 *subMatrix, uint8_t subRows, uint8_t subCols,
uint8_t mainRows, uint8_t mainCols, uint8_t startRow,
uint8_t startCol) {
if ((startRow + subRows > mainRows) or (startCol + subCols > mainCols)) {
return;
}
for (uint8_t row = 0; row < subRows; row++) {
for (uint8_t col = 0; col < subCols; col++) {
mainMatrix[(startRow + row) * mainCols + (startCol + col)] = subMatrix[row * subCols + col];
}
}
}
static ReturnValue_t inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
// Stopwatch stopwatch;
T1 matrix[size][size], identity[size][size];
// reformat array to matrix
for (uint8_t row = 0; row < size; row++) {
for (uint8_t col = 0; col < size; col++) {
matrix[row][col] = inputMatrix[row * size + col];
}
}
// init identity matrix
std::memset(identity, 0.0, sizeof(identity));
for (uint8_t diag = 0; diag < size; diag++) {
identity[diag][diag] = 1;
}
// gauss-jordan algo
// sort matrix such as no diag entry shall be 0
for (uint8_t row = 0; row < size; row++) {
if (matrix[row][row] == 0.0) {
bool swaped = false;
uint8_t rowIndex = 0;
while ((rowIndex < size) && !swaped) {
if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
}
swaped = true;
}
rowIndex++;
}
if (!swaped) {
return returnvalue::FAILED; // matrix not invertible
}
}
}
for (int row = 0; row < size; row++) {
if (matrix[row][row] == 0.0) {
uint8_t rowIndex;
if (row == 0) {
rowIndex = size - 1;
} else {
rowIndex = row - 1;
}
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
}
row--;
if (row < 0) {
return returnvalue::FAILED; // Matrix is not invertible
}
}
}
// remove non diag elements in matrix (jordan)
for (int row = 0; row < size; row++) {
for (int rowIndex = 0; rowIndex < size; rowIndex++) {
if (row != rowIndex) {
double ratio = matrix[rowIndex][row] / matrix[row][row];
for (int colIndex = 0; colIndex < size; colIndex++) {
matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
}
}
}
}
// normalize rows in matrix (gauss)
for (int row = 0; row < size; row++) {
for (int col = 0; col < size; col++) {
identity[row][col] = identity[row][col] / matrix[row][row];
}
}
std::memcpy(inverse, identity, sizeof(identity));
return returnvalue::OK; // successful inversion
}
static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
int i, j;
double determinant = 0;
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
{matrix[3], matrix[4], matrix[5]},
{matrix[6], matrix[7], matrix[8]}};
for (i = 0; i < 3; i++) {
determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
}
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
(mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
determinant;
}
}
}
static void skewMatrix(const T1 *vector, T2 *result) {
// Input Dimension [3], Output [3][3]
result[0] = 0;
result[1] = -vector[2];
result[2] = vector[1];
result[3] = vector[2];
result[4] = 0;
result[5] = -vector[0];
result[6] = -vector[1];
result[7] = vector[0];
result[8] = 0;
}
}; };
#endif /* MATRIXOPERATIONS_H_ */ #endif /* MATRIXOPERATIONS_H_ */

View File

@ -39,6 +39,48 @@ void QuaternionOperations::inverse(const double* quaternion, double* inverseQuat
VectorOperations<double>::mulScalar(inverseQuaternion, -1, inverseQuaternion, 3); VectorOperations<double>::mulScalar(inverseQuaternion, -1, inverseQuaternion, 3);
} }
void QuaternionOperations::slerp(const double q1[4], const double q2[4], const double weight,
double q[4]) {
double q1s[4] = {0, 0, 0, 0}, q2I[4] = {0, 0, 0, 0}, qD[4] = {0, 0, 0, 0}, left[4] = {0, 0, 0, 0},
right[4] = {0, 0, 0, 0}, angle = 0;
// we need to be able to invert this quaternion
std::memcpy(q1s, q1, 4 * sizeof(double));
// calculate angle between orientations
inverse(q2, q2I);
multiply(q1s, q2I, qD);
angle = std::acos(qD[3]);
if (std::cos(angle) < 0.0) {
// we need to invert one quaternion
VectorOperations<double>::mulScalar(q1s, -1, q1s, 4);
multiply(q1s, q2I, qD);
angle = std::acos(qD[3]);
}
if (std::sin(angle) == 0.0) {
// nothing to calculate here
std::memcpy(q, q1s, 4 * sizeof(double));
return;
}
VectorOperations<double>::mulScalar(q1s, std::sin((1 - weight) * angle) / std::sin(angle), left,
4);
VectorOperations<double>::mulScalar(q2, std::sin(weight * angle) / std::sin(angle), right, 4);
VectorOperations<double>::add(left, right, q, 4);
normalize(q);
}
void QuaternionOperations::preventSignJump(double qNew[4], const double qOld[4]) {
double qDiff[4] = {0, 0, 0, 0}, qSum[4] = {0, 0, 0, 0};
VectorOperations<double>::subtract(qOld, qNew, qDiff, 4);
VectorOperations<double>::add(qOld, qNew, qSum, 4);
if (VectorOperations<double>::norm(qDiff, 4) > VectorOperations<double>::norm(qSum, 4)) {
VectorOperations<double>::mulScalar(qNew, -1, qNew, 4);
}
}
QuaternionOperations::QuaternionOperations() {} QuaternionOperations::QuaternionOperations() {}
void QuaternionOperations::normalize(const double* quaternion, double* unitQuaternion) { void QuaternionOperations::normalize(const double* quaternion, double* unitQuaternion) {
@ -120,3 +162,25 @@ double QuaternionOperations::getAngle(const double* quaternion, bool abs) {
} }
} }
} }
void QuaternionOperations::rotationFromQuaternions(const double qNew[4], const double qOld[4],
const double timeDelta, double rotRate[3]) {
double qOldInv[4] = {0, 0, 0, 0};
double qDelta[4] = {0, 0, 0, 0};
inverse(qOld, qOldInv);
multiply(qNew, qOldInv, qDelta);
if (VectorOperations<double>::norm(qDelta, 4) != 0.0) {
normalize(qDelta);
}
if (VectorOperations<double>::norm(qDelta, 3) == 0.0) {
rotRate[0] = 0.0;
rotRate[1] = 0.0;
rotRate[2] = 0.0;
return;
}
double rotVec[3] = {0, 0, 0};
double angle = getAngle(qDelta);
VectorOperations<double>::normalize(qDelta, rotVec, 3);
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotRate, 3);
}

View File

@ -23,6 +23,13 @@ class QuaternionOperations {
static void inverse(const double *quaternion, double *inverseQuaternion); static void inverse(const double *quaternion, double *inverseQuaternion);
static void slerp(const double q1[4], const double q2[4], const double weight, double q[4]);
static void rotationFromQuaternions(const double qNew[4], const double qOld[4],
const double timeDelta, double rotRate[3]);
static void preventSignJump(double qNew[4], const double qOld[4]);
/** /**
* returns angle in ]-Pi;Pi] or [0;Pi] if abs == true * returns angle in ]-Pi;Pi] or [0;Pi] if abs == true
*/ */

View File

@ -99,6 +99,15 @@ class VectorOperations {
static void copy(const T *in, T *out, uint8_t size) { mulScalar(in, 1, out, size); } static void copy(const T *in, T *out, uint8_t size) { mulScalar(in, 1, out, size); }
static bool isFinite(const T *inputVector, uint8_t size) {
for (uint8_t i = 0; i < size; i++) {
if (not std::isfinite(inputVector[i])) {
return false;
}
}
return true;
}
private: private:
VectorOperations(); VectorOperations();
}; };

View File

@ -43,6 +43,7 @@ class Service11TelecommandScheduling final : public PusServiceBase {
static constexpr ReturnValue_t INVALID_RELATIVE_TIME = returnvalue::makeCode(CLASS_ID, 4); static constexpr ReturnValue_t INVALID_RELATIVE_TIME = returnvalue::makeCode(CLASS_ID, 4);
static constexpr ReturnValue_t CONTAINED_TC_TOO_SMALL = returnvalue::makeCode(CLASS_ID, 5); static constexpr ReturnValue_t CONTAINED_TC_TOO_SMALL = returnvalue::makeCode(CLASS_ID, 5);
static constexpr ReturnValue_t CONTAINED_TC_CRC_MISSMATCH = returnvalue::makeCode(CLASS_ID, 6); static constexpr ReturnValue_t CONTAINED_TC_CRC_MISSMATCH = returnvalue::makeCode(CLASS_ID, 6);
static constexpr ReturnValue_t MAP_IS_FULL = returnvalue::makeCode(CLASS_ID, 7);
static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_11; static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_11;

View File

@ -150,6 +150,9 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::handleResetCom
template <size_t MAX_NUM_TCS> template <size_t MAX_NUM_TCS>
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivity( inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivity(
const uint8_t *data, size_t size) { const uint8_t *data, size_t size) {
if (telecommandMap.full()) {
return MAP_IS_FULL;
}
uint32_t timestamp = 0; uint32_t timestamp = 0;
ReturnValue_t result = SerializeAdapter::deSerialize(&timestamp, &data, &size, DEF_END); ReturnValue_t result = SerializeAdapter::deSerialize(&timestamp, &data, &size, DEF_END);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {

View File

@ -2,9 +2,9 @@
#include <cmath> #include <cmath>
#include "fsfw/events/EventManagerIF.h"
#include "fsfw/pus/servicepackets/Service9Packets.h" #include "fsfw/pus/servicepackets/Service9Packets.h"
#include "fsfw/serviceinterface/ServiceInterface.h" #include "fsfw/returnvalues/returnvalue.h"
#include "fsfw/serialize/SerializeAdapter.h"
#include "fsfw/timemanager/CCSDSTime.h" #include "fsfw/timemanager/CCSDSTime.h"
Service9TimeManagement::Service9TimeManagement(PsbParams params) : PusServiceBase(params) { Service9TimeManagement::Service9TimeManagement(PsbParams params) : PusServiceBase(params) {
@ -18,16 +18,53 @@ ReturnValue_t Service9TimeManagement::performService() { return returnvalue::OK;
ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) { ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {
switch (subservice) { switch (subservice) {
case Subservice::SET_TIME: { case Subservice::SET_TIME: {
return setTime(); reportCurrentTime(CLOCK_DUMP_BEFORE_SETTING_TIME);
ReturnValue_t result = setTime();
reportCurrentTime(CLOCK_DUMP_AFTER_SETTING_TIME);
return result;
} }
case Subservice::DUMP_TIME: { case Subservice::DUMP_TIME: {
timeval newTime; reportCurrentTime();
Clock::getClock_timeval(&newTime);
uint32_t subsecondMs =
static_cast<uint32_t>(std::floor(static_cast<double>(newTime.tv_usec) / 1000.0));
triggerEvent(CLOCK_DUMP, newTime.tv_sec, subsecondMs);
return returnvalue::OK; return returnvalue::OK;
} }
case Subservice::RELATIVE_TIMESHIFT: {
timeval currentTime;
ReturnValue_t result = Clock::getClock(&currentTime);
if (result != returnvalue::OK) {
return result;
}
reportTime(CLOCK_DUMP_BEFORE_SETTING_TIME, currentTime);
if (currentPacket.getUserDataLen() != 8) {
return AcceptsTelecommandsIF::ILLEGAL_APPLICATION_DATA;
}
size_t deserLen = 8;
int64_t timeshiftNanos = 0;
result = SerializeAdapter::deSerialize(&timeshiftNanos, currentPacket.getUserData(),
&deserLen, SerializeIF::Endianness::NETWORK);
if (result != returnvalue::OK) {
return result;
}
bool positiveShift = true;
if (timeshiftNanos < 0) {
positiveShift = false;
}
timeval offset{};
offset.tv_sec = std::abs(timeshiftNanos) / NANOS_PER_SECOND;
offset.tv_usec = (std::abs(timeshiftNanos) % NANOS_PER_SECOND) / 1000;
timeval newTime;
if (positiveShift) {
newTime = currentTime + offset;
} else {
newTime = currentTime - offset;
}
result = Clock::setClock(&newTime);
if (result == returnvalue::OK) {
reportTime(CLOCK_DUMP_AFTER_SETTING_TIME, newTime);
}
return result;
}
default: default:
return AcceptsTelecommandsIF::INVALID_SUBSERVICE; return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
} }
@ -43,17 +80,20 @@ ReturnValue_t Service9TimeManagement::setTime() {
return result; return result;
} }
timeval time;
Clock::getClock_timeval(&time);
result = Clock::setClock(&timeToSet); result = Clock::setClock(&timeToSet);
if (result != returnvalue::OK) {
if (result == returnvalue::OK) {
timeval newTime;
Clock::getClock_timeval(&newTime);
triggerEvent(CLOCK_SET, time.tv_sec, newTime.tv_sec);
return returnvalue::OK;
} else {
triggerEvent(CLOCK_SET_FAILURE, result, 0); triggerEvent(CLOCK_SET_FAILURE, result, 0);
return returnvalue::FAILED; return returnvalue::FAILED;
} }
return result;
}
void Service9TimeManagement::reportCurrentTime(Event event) {
timeval currentTime{};
Clock::getClock(&currentTime);
triggerEvent(event, currentTime.tv_sec, currentTime.tv_usec);
}
void Service9TimeManagement::reportTime(Event event, timeval time) {
triggerEvent(event, time.tv_sec, time.tv_usec);
} }

View File

@ -1,18 +1,25 @@
#ifndef FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_ #ifndef FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_
#define FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_ #define FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_
#include "fsfw/returnvalues/returnvalue.h"
#include "fsfw/tmtcservices/PusServiceBase.h" #include "fsfw/tmtcservices/PusServiceBase.h"
class Service9TimeManagement : public PusServiceBase { class Service9TimeManagement : public PusServiceBase {
public: public:
static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_9; static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_9;
//!< Clock has been set. P1: old timeval seconds. P2: new timeval seconds. static constexpr uint32_t NANOS_PER_SECOND = 1'000'000'000;
//!< [EXPORT] : [COMMENT] Clock has been set. P1: old timeval seconds. P2: new timeval seconds.
static constexpr Event CLOCK_SET = MAKE_EVENT(0, severity::INFO); static constexpr Event CLOCK_SET = MAKE_EVENT(0, severity::INFO);
//!< Clock dump event. P1: timeval seconds P2: timeval milliseconds. //!< [EXPORT] : [COMMENT] Clock dump event. P1: timeval seconds P2: timeval milliseconds.
static constexpr Event CLOCK_DUMP = MAKE_EVENT(1, severity::INFO); static constexpr Event CLOCK_DUMP_LEGACY = MAKE_EVENT(1, severity::INFO);
//!< Clock could not be set. P1: Returncode. //!< [EXPORT] : [COMMENT] Clock could not be set. P1: Returncode.
static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(2, severity::LOW); static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(2, severity::LOW);
//!< [EXPORT] : [COMMENT] Clock dump event. P1: timeval seconds P2: timeval microseconds.
static constexpr Event CLOCK_DUMP = MAKE_EVENT(3, severity::INFO);
static constexpr Event CLOCK_DUMP_BEFORE_SETTING_TIME = MAKE_EVENT(4, severity::INFO);
static constexpr Event CLOCK_DUMP_AFTER_SETTING_TIME = MAKE_EVENT(5, severity::INFO);
static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_9; static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_9;
@ -30,12 +37,16 @@ class Service9TimeManagement : public PusServiceBase {
*/ */
ReturnValue_t handleRequest(uint8_t subservice) override; ReturnValue_t handleRequest(uint8_t subservice) override;
void reportCurrentTime(Event eventType = CLOCK_DUMP);
void reportTime(Event event, timeval time);
virtual ReturnValue_t setTime(); virtual ReturnValue_t setTime();
private: private:
enum Subservice { enum Subservice {
SET_TIME = 128, //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format SET_TIME = 128, //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format
DUMP_TIME = 129, DUMP_TIME = 129,
RELATIVE_TIMESHIFT = 130,
}; };
}; };

View File

@ -1,7 +1,6 @@
#include "SpacePacketReader.h" #include "SpacePacketReader.h"
#include "fsfw/serialize/SerializeIF.h" #include "fsfw/serialize/SerializeIF.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
SpacePacketReader::SpacePacketReader(const uint8_t* setAddress, size_t maxSize_) { SpacePacketReader::SpacePacketReader(const uint8_t* setAddress, size_t maxSize_) {
setInternalFields(setAddress, maxSize_); setInternalFields(setAddress, maxSize_);

View File

@ -57,13 +57,6 @@ ReturnValue_t PusTmReader::parseData(bool crcCheck) {
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
return result; return result;
} }
if (crcCheck) {
uint16_t crc16 = CRC::crc16ccitt(spReader.getFullData(), getFullPacketLen());
if (crc16 != 0) {
// Checksum failure
return PusIF::INVALID_CRC_16;
}
}
size_t currentOffset = SpacePacketReader::getHeaderLen(); size_t currentOffset = SpacePacketReader::getHeaderLen();
pointers.secHeaderStart = pointers.spHeaderStart + currentOffset; pointers.secHeaderStart = pointers.spHeaderStart + currentOffset;
currentOffset += PusTmIF::MIN_SEC_HEADER_LEN; currentOffset += PusTmIF::MIN_SEC_HEADER_LEN;
@ -84,6 +77,13 @@ ReturnValue_t PusTmReader::parseData(bool crcCheck) {
} }
currentOffset += sourceDataLen; currentOffset += sourceDataLen;
pointers.crcStart = pointers.spHeaderStart + currentOffset; pointers.crcStart = pointers.spHeaderStart + currentOffset;
if (crcCheck) {
uint16_t crc16 = CRC::crc16ccitt(spReader.getFullData(), getFullPacketLen());
if (crc16 != 0) {
// Checksum failure
return PusIF::INVALID_CRC_16;
}
}
return returnvalue::OK; return returnvalue::OK;
} }
bool PusTmReader::isNull() const { return spReader.isNull() or pointers.secHeaderStart == nullptr; } bool PusTmReader::isNull() const { return spReader.isNull() or pointers.secHeaderStart == nullptr; }

View File

@ -40,7 +40,7 @@ void PusServiceBase::setTaskIF(PeriodicTaskIF* taskHandle_) { this->taskHandle =
void PusServiceBase::handleRequestQueue() { void PusServiceBase::handleRequestQueue() {
TmTcMessage message; TmTcMessage message;
ReturnValue_t result; ReturnValue_t result;
for (uint8_t count = 0; count < PUS_SERVICE_MAX_RECEPTION; count++) { for (uint8_t count = 0; count < psbParams.maxPacketsPerCycle; count++) {
ReturnValue_t status = psbParams.reqQueue->receiveMessage(&message); ReturnValue_t status = psbParams.reqQueue->receiveMessage(&message);
if (status == MessageQueueIF::EMPTY) { if (status == MessageQueueIF::EMPTY) {
break; break;
@ -98,7 +98,7 @@ ReturnValue_t PusServiceBase::initialize() {
} }
if (psbParams.reqQueue == nullptr) { if (psbParams.reqQueue == nullptr) {
ownedQueue = true; ownedQueue = true;
psbParams.reqQueue = QueueFactory::instance()->createMessageQueue(PSB_DEFAULT_QUEUE_DEPTH); psbParams.reqQueue = QueueFactory::instance()->createMessageQueue(psbParams.requestQueueDepth);
} else { } else {
ownedQueue = false; ownedQueue = false;
} }

View File

@ -20,6 +20,14 @@ class StorageManagerIF;
* Configuration parameters for the PUS Service Base * Configuration parameters for the PUS Service Base
*/ */
struct PsbParams { struct PsbParams {
static constexpr uint8_t PSB_DEFAULT_QUEUE_DEPTH = 10;
/**
* This constant sets the maximum number of packets accepted per call.
* Remember that one packet must be completely handled in one
* #handleRequest call.
*/
static constexpr uint8_t MAX_PACKETS_PER_CYCLE = 10;
PsbParams() = default; PsbParams() = default;
PsbParams(uint16_t apid, AcceptsTelemetryIF* tmReceiver) : apid(apid), tmReceiver(tmReceiver) {} PsbParams(uint16_t apid, AcceptsTelemetryIF* tmReceiver) : apid(apid), tmReceiver(tmReceiver) {}
PsbParams(const char* name, uint16_t apid, AcceptsTelemetryIF* tmReceiver) PsbParams(const char* name, uint16_t apid, AcceptsTelemetryIF* tmReceiver)
@ -32,6 +40,9 @@ struct PsbParams {
object_id_t objectId = objects::NO_OBJECT; object_id_t objectId = objects::NO_OBJECT;
uint16_t apid = 0; uint16_t apid = 0;
uint8_t serviceId = 0; uint8_t serviceId = 0;
uint32_t requestQueueDepth = PSB_DEFAULT_QUEUE_DEPTH;
uint32_t maxPacketsPerCycle = MAX_PACKETS_PER_CYCLE;
/** /**
* The default destination ID for generated telemetry. If this is not set, @initialize of PSB * The default destination ID for generated telemetry. If this is not set, @initialize of PSB
* will attempt to find a suitable object with the object ID @PusServiceBase::packetDestination * will attempt to find a suitable object with the object ID @PusServiceBase::packetDestination
@ -100,14 +111,6 @@ class PusServiceBase : public ExecutableObjectIF,
friend void Factory::setStaticFrameworkObjectIds(); friend void Factory::setStaticFrameworkObjectIds();
public: public:
/**
* This constant sets the maximum number of packets accepted per call.
* Remember that one packet must be completely handled in one
* #handleRequest call.
*/
static constexpr uint8_t PUS_SERVICE_MAX_RECEPTION = 10;
static constexpr uint8_t PSB_DEFAULT_QUEUE_DEPTH = 10;
/** /**
* @brief The passed values are set, but inter-object initialization is * @brief The passed values are set, but inter-object initialization is
* done in the initialize method. * done in the initialize method.

View File

@ -1,7 +1,7 @@
#ifndef FSFW_TMTCSERVICES_SOURCESEQUENCECOUNTER_H_ #ifndef FSFW_TMTCSERVICES_SOURCESEQUENCECOUNTER_H_
#define FSFW_TMTCSERVICES_SOURCESEQUENCECOUNTER_H_ #define FSFW_TMTCSERVICES_SOURCESEQUENCECOUNTER_H_
#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h" #include "fsfw/tmtcpacket/ccsds/defs.h"
class SourceSequenceCounter { class SourceSequenceCounter {
private: private:
@ -9,6 +9,7 @@ class SourceSequenceCounter {
public: public:
SourceSequenceCounter(uint16_t initialSequenceCount = 0) : sequenceCount(initialSequenceCount) {} SourceSequenceCounter(uint16_t initialSequenceCount = 0) : sequenceCount(initialSequenceCount) {}
void increment() { sequenceCount = (sequenceCount + 1) % (ccsds::LIMIT_SEQUENCE_COUNT); } void increment() { sequenceCount = (sequenceCount + 1) % (ccsds::LIMIT_SEQUENCE_COUNT); }
void decrement() { sequenceCount = (sequenceCount - 1) % (ccsds::LIMIT_SEQUENCE_COUNT); } void decrement() { sequenceCount = (sequenceCount - 1) % (ccsds::LIMIT_SEQUENCE_COUNT); }
uint16_t get() const { return this->sequenceCount; } uint16_t get() const { return this->sequenceCount; }
@ -25,6 +26,7 @@ class SourceSequenceCounter {
sequenceCount = newCount; sequenceCount = newCount;
return *this; return *this;
} }
void set(uint16_t sequenceCount) { this->sequenceCount = sequenceCount; }
operator uint16_t() { return this->get(); } operator uint16_t() { return this->get(); }
}; };

View File

@ -1,6 +1,8 @@
#include <fsfw_hal/linux/serial/helper.h> #include <fsfw_hal/linux/serial/helper.h>
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include <termios.h>
#include "FSFWConfig.h"
#include "fsfw/serviceinterface.h" #include "fsfw/serviceinterface.h"
void serial::setMode(struct termios& options, UartModes mode) { void serial::setMode(struct termios& options, UartModes mode) {
@ -108,7 +110,7 @@ void serial::setBaudrate(struct termios& options, UartBaudRate baud) {
#endif // ! __APPLE__ #endif // ! __APPLE__
default: default:
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl; sif::warning << "serial::configureBaudrate: Baudrate not supported" << std::endl;
#endif #endif
break; break;
} }
@ -153,15 +155,17 @@ int serial::readCountersAndErrors(int serialPort, serial_icounter_struct& icount
} }
void serial::setStopbits(struct termios& options, StopBits bits) { void serial::setStopbits(struct termios& options, StopBits bits) {
// Regular case: One stop bit.
options.c_cflag &= ~CSTOPB;
if (bits == StopBits::TWO_STOP_BITS) { if (bits == StopBits::TWO_STOP_BITS) {
// Use two stop bits // Use two stop bits
options.c_cflag |= CSTOPB; options.c_cflag |= CSTOPB;
} else {
// Clear stop field, only one stop bit used in communication
options.c_cflag &= ~CSTOPB;
} }
} }
void serial::flushRxBuf(int fd) { tcflush(fd, TCIFLUSH); } void serial::flushRxBuf(int fd) { tcflush(fd, TCIFLUSH); }
void serial::flushTxBuf(int fd) { tcflush(fd, TCOFLUSH); }
void serial::flushTxRxBuf(int fd) { tcflush(fd, TCIOFLUSH); } void serial::flushTxRxBuf(int fd) { tcflush(fd, TCIOFLUSH); }

View File

@ -65,6 +65,7 @@ void setParity(struct termios& options, Parity parity);
void ignoreCtrlLines(struct termios& options); void ignoreCtrlLines(struct termios& options);
void flushRxBuf(int fd); void flushRxBuf(int fd);
void flushTxBuf(int fd);
void flushTxRxBuf(int fd); void flushTxRxBuf(int fd);
int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter); int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter);