Merge remote-tracking branch 'eive/fdir-fix-fresh-dhb' into develop
This commit is contained in:
commit
9a1e82de2a
|
|
@ -29,9 +29,12 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
|
|||
- add CFDP subsystem ID
|
||||
https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/742
|
||||
- `PusTmZcWriter` now exposes API to set message counter field.
|
||||
- Relative timeshift in the PUS time service.
|
||||
|
||||
## 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.
|
||||
- Bump ETL version to 20.35.14
|
||||
https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/748
|
||||
|
|
@ -42,6 +45,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
|
|||
- Assert that `FixedArrayList` is larger than 0 at compile time.
|
||||
https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/740
|
||||
- Health functions are virtual now.
|
||||
- PUS Service Base request queue depth and maximum number of handled packets per cycle is now
|
||||
configurable.
|
||||
|
||||
## Added
|
||||
|
||||
|
|
|
|||
|
|
@ -6,6 +6,8 @@
|
|||
#include "fsfw/globalfunctions/constants.h"
|
||||
#include "fsfw/globalfunctions/math/MatrixOperations.h"
|
||||
#include "fsfw/globalfunctions/math/VectorOperations.h"
|
||||
#include "fsfw/globalfunctions/sign.h"
|
||||
#include "fsfw/serviceinterface.h"
|
||||
|
||||
void CoordinateTransformations::positionEcfToEci(const double* ecfPosition, double* eciPosition,
|
||||
timeval* timeUTC) {
|
||||
|
|
@ -97,7 +99,14 @@ void CoordinateTransformations::ecfToEci(const double* ecfCoordinates, double* e
|
|||
|
||||
double CoordinateTransformations::getJuleanCenturiesTT(timeval timeUTC) {
|
||||
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;
|
||||
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);
|
||||
};
|
||||
|
||||
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]);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,6 +23,12 @@ class CoordinateTransformations {
|
|||
|
||||
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:
|
||||
CoordinateTransformations();
|
||||
static void ecfToEci(const double* ecfCoordinates, double* eciCoordinates,
|
||||
|
|
|
|||
|
|
@ -26,6 +26,11 @@ ReturnValue_t DeviceHandlerFailureIsolation::eventReceived(EventMessage* event)
|
|||
if (isFdirInActionOrAreWeFaulty(event)) {
|
||||
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;
|
||||
switch (event->getEvent()) {
|
||||
case HasModesIF::MODE_TRANSITION_FAILED:
|
||||
|
|
@ -186,15 +191,6 @@ void DeviceHandlerFailureIsolation::setFdirState(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); }
|
||||
|
||||
void DeviceHandlerFailureIsolation::startRecovery(Event reason) {
|
||||
|
|
|
|||
|
|
@ -17,7 +17,6 @@ class DeviceHandlerFailureIsolation : public FailureIsolationBase {
|
|||
uint8_t eventQueueDepth = 10);
|
||||
~DeviceHandlerFailureIsolation();
|
||||
ReturnValue_t initialize();
|
||||
void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0);
|
||||
bool isFdirActionInProgress();
|
||||
virtual ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId,
|
||||
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_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 void eventConfirmed(EventMessage* event);
|
||||
void wasParentsFault(EventMessage* event);
|
||||
|
|
|
|||
|
|
@ -31,6 +31,7 @@ FreshDeviceHandlerBase::~FreshDeviceHandlerBase() {
|
|||
ReturnValue_t FreshDeviceHandlerBase::performOperation(uint8_t opCode) {
|
||||
performDeviceOperationPreQueueHandling(opCode);
|
||||
handleQueue();
|
||||
fdirInstance->checkForFailures();
|
||||
performDeviceOperation(opCode);
|
||||
poolManager.performHkOperation();
|
||||
return returnvalue::OK;
|
||||
|
|
|
|||
|
|
@ -129,7 +129,7 @@ class FreshDeviceHandlerBase : public SystemObject,
|
|||
ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
|
||||
const uint8_t* data, size_t size) override = 0;
|
||||
// Executable overrides.
|
||||
ReturnValue_t performOperation(uint8_t opCode) override;
|
||||
virtual ReturnValue_t performOperation(uint8_t opCode) override;
|
||||
ReturnValue_t initializeAfterTaskCreation() override;
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -148,25 +148,16 @@ void FailureIsolationBase::doConfirmFault(EventMessage* event) {
|
|||
ReturnValue_t FailureIsolationBase::confirmFault(EventMessage* event) { return YOUR_FAULT; }
|
||||
|
||||
void FailureIsolationBase::triggerEvent(Event event, uint32_t parameter1, uint32_t parameter2) {
|
||||
// With this mechanism, all events are disabled for a certain device.
|
||||
// That's not so good for visibility.
|
||||
if (isFdirDisabledForSeverity(event::getSeverity(event))) {
|
||||
return;
|
||||
}
|
||||
// By default, we trigger all events and also call the handler function to handle FDIR reactions
|
||||
// which might occur due to these events. This makes all events visible. If the handling of
|
||||
// FDIR reaction should be disabled, this should be done through dedicated logic inside the
|
||||
// eventReceived function.
|
||||
EventMessage message(event, ownerId, parameter1, parameter2);
|
||||
EventManagerIF::triggerEvent(&message, eventQueue->getId());
|
||||
eventReceived(&message);
|
||||
}
|
||||
|
||||
bool FailureIsolationBase::isFdirDisabledForSeverity(EventSeverity_t severity) {
|
||||
if ((owner != NULL) && (severity != severity::INFO)) {
|
||||
if (owner->getHealth() == HasHealthIF::EXTERNAL_CONTROL) {
|
||||
// External control disables handling of fault messages.
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
bool FailureIsolationBase::isFdirDisabledForSeverity(EventSeverity_t severity) { return false; }
|
||||
|
||||
void FailureIsolationBase::throwFdirEvent(Event event, uint32_t parameter1, uint32_t parameter2) {
|
||||
EventMessage message(event, ownerId, parameter1, parameter2);
|
||||
|
|
|
|||
|
|
@ -44,13 +44,13 @@ class FailureIsolationBase : public ConfirmsFailuresIF, public HasParametersIF {
|
|||
virtual void wasParentsFault(EventMessage* event);
|
||||
virtual ReturnValue_t confirmFault(EventMessage* event);
|
||||
virtual void decrementFaultCounters() = 0;
|
||||
virtual bool isFdirDisabledForSeverity(EventSeverity_t severity);
|
||||
ReturnValue_t sendConfirmationRequest(EventMessage* event,
|
||||
MessageQueueId_t destination = MessageQueueIF::NO_QUEUE);
|
||||
void throwFdirEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0);
|
||||
|
||||
private:
|
||||
void doConfirmFault(EventMessage* event);
|
||||
bool isFdirDisabledForSeverity(EventSeverity_t severity);
|
||||
};
|
||||
|
||||
#endif /* FRAMEWORK_FDIR_FAILUREISOLATIONBASE_H_ */
|
||||
#endif /* FRAMEWORK_FDIR */
|
||||
|
|
|
|||
|
|
@ -1,9 +1,12 @@
|
|||
#ifndef MATRIXOPERATIONS_H_
|
||||
#define MATRIXOPERATIONS_H_
|
||||
|
||||
#include <fsfw/retval.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <utility>
|
||||
|
||||
template <typename T1, typename T2 = T1, typename T3 = T2>
|
||||
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_ */
|
||||
|
|
|
|||
|
|
@ -72,6 +72,15 @@ void QuaternionOperations::slerp(const double q1[4], const double q2[4], const d
|
|||
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() {}
|
||||
|
||||
void QuaternionOperations::normalize(const double* quaternion, double* unitQuaternion) {
|
||||
|
|
@ -153,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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,6 +25,11 @@ class QuaternionOperations {
|
|||
|
||||
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
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -99,6 +99,15 @@ class VectorOperations {
|
|||
|
||||
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:
|
||||
VectorOperations();
|
||||
};
|
||||
|
|
|
|||
|
|
@ -2,9 +2,9 @@
|
|||
|
||||
#include <cmath>
|
||||
|
||||
#include "fsfw/events/EventManagerIF.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"
|
||||
|
||||
Service9TimeManagement::Service9TimeManagement(PsbParams params) : PusServiceBase(params) {
|
||||
|
|
@ -18,16 +18,53 @@ ReturnValue_t Service9TimeManagement::performService() { return returnvalue::OK;
|
|||
ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {
|
||||
switch (subservice) {
|
||||
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: {
|
||||
timeval newTime;
|
||||
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);
|
||||
reportCurrentTime();
|
||||
return returnvalue::OK;
|
||||
}
|
||||
case Subservice::RELATIVE_TIMESHIFT: {
|
||||
timeval currentTime;
|
||||
ReturnValue_t result = Clock::getClock(¤tTime);
|
||||
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(×hiftNanos, 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:
|
||||
return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
|
||||
}
|
||||
|
|
@ -43,17 +80,20 @@ ReturnValue_t Service9TimeManagement::setTime() {
|
|||
return result;
|
||||
}
|
||||
|
||||
timeval time;
|
||||
Clock::getClock_timeval(&time);
|
||||
result = Clock::setClock(&timeToSet);
|
||||
|
||||
if (result == returnvalue::OK) {
|
||||
timeval newTime;
|
||||
Clock::getClock_timeval(&newTime);
|
||||
triggerEvent(CLOCK_SET, time.tv_sec, newTime.tv_sec);
|
||||
return returnvalue::OK;
|
||||
} else {
|
||||
if (result != returnvalue::OK) {
|
||||
triggerEvent(CLOCK_SET_FAILURE, result, 0);
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
void Service9TimeManagement::reportCurrentTime(Event event) {
|
||||
timeval currentTime{};
|
||||
Clock::getClock(¤tTime);
|
||||
triggerEvent(event, currentTime.tv_sec, currentTime.tv_usec);
|
||||
}
|
||||
|
||||
void Service9TimeManagement::reportTime(Event event, timeval time) {
|
||||
triggerEvent(event, time.tv_sec, time.tv_usec);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,18 +1,25 @@
|
|||
#ifndef FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_
|
||||
#define FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_
|
||||
|
||||
#include "fsfw/returnvalues/returnvalue.h"
|
||||
#include "fsfw/tmtcservices/PusServiceBase.h"
|
||||
|
||||
class Service9TimeManagement : public PusServiceBase {
|
||||
public:
|
||||
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);
|
||||
//!< Clock dump event. P1: timeval seconds P2: timeval milliseconds.
|
||||
static constexpr Event CLOCK_DUMP = MAKE_EVENT(1, severity::INFO);
|
||||
//!< Clock could not be set. P1: Returncode.
|
||||
//!< [EXPORT] : [COMMENT] Clock dump event. P1: timeval seconds P2: timeval milliseconds.
|
||||
static constexpr Event CLOCK_DUMP_LEGACY = MAKE_EVENT(1, severity::INFO);
|
||||
//!< [EXPORT] : [COMMENT] Clock could not be set. P1: Returncode.
|
||||
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;
|
||||
|
||||
|
|
@ -30,12 +37,16 @@ class Service9TimeManagement : public PusServiceBase {
|
|||
*/
|
||||
ReturnValue_t handleRequest(uint8_t subservice) override;
|
||||
|
||||
void reportCurrentTime(Event eventType = CLOCK_DUMP);
|
||||
void reportTime(Event event, timeval time);
|
||||
|
||||
virtual ReturnValue_t setTime();
|
||||
|
||||
private:
|
||||
enum Subservice {
|
||||
SET_TIME = 128, //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format
|
||||
DUMP_TIME = 129,
|
||||
RELATIVE_TIMESHIFT = 130,
|
||||
};
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@ void PusServiceBase::setTaskIF(PeriodicTaskIF* taskHandle_) { this->taskHandle =
|
|||
void PusServiceBase::handleRequestQueue() {
|
||||
TmTcMessage message;
|
||||
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);
|
||||
if (status == MessageQueueIF::EMPTY) {
|
||||
break;
|
||||
|
|
@ -98,7 +98,7 @@ ReturnValue_t PusServiceBase::initialize() {
|
|||
}
|
||||
if (psbParams.reqQueue == nullptr) {
|
||||
ownedQueue = true;
|
||||
psbParams.reqQueue = QueueFactory::instance()->createMessageQueue(PSB_DEFAULT_QUEUE_DEPTH);
|
||||
psbParams.reqQueue = QueueFactory::instance()->createMessageQueue(psbParams.requestQueueDepth);
|
||||
} else {
|
||||
ownedQueue = false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -20,6 +20,14 @@ class StorageManagerIF;
|
|||
* Configuration parameters for the PUS Service Base
|
||||
*/
|
||||
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(uint16_t apid, AcceptsTelemetryIF* tmReceiver) : apid(apid), tmReceiver(tmReceiver) {}
|
||||
PsbParams(const char* name, uint16_t apid, AcceptsTelemetryIF* tmReceiver)
|
||||
|
|
@ -32,6 +40,9 @@ struct PsbParams {
|
|||
object_id_t objectId = objects::NO_OBJECT;
|
||||
uint16_t apid = 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
|
||||
* 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();
|
||||
|
||||
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
|
||||
* done in the initialize method.
|
||||
|
|
|
|||
|
|
@ -1,6 +1,8 @@
|
|||
#include <fsfw_hal/linux/serial/helper.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include "FSFWConfig.h"
|
||||
#include "fsfw/serviceinterface.h"
|
||||
|
||||
void serial::setMode(struct termios& options, UartModes mode) {
|
||||
|
|
@ -108,7 +110,7 @@ void serial::setBaudrate(struct termios& options, UartBaudRate baud) {
|
|||
#endif // ! __APPLE__
|
||||
default:
|
||||
#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
|
||||
break;
|
||||
}
|
||||
|
|
@ -153,15 +155,17 @@ int serial::readCountersAndErrors(int serialPort, serial_icounter_struct& icount
|
|||
}
|
||||
|
||||
void serial::setStopbits(struct termios& options, StopBits bits) {
|
||||
// Regular case: One stop bit.
|
||||
options.c_cflag &= ~CSTOPB;
|
||||
if (bits == StopBits::TWO_STOP_BITS) {
|
||||
// Use two stop bits
|
||||
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::flushTxBuf(int fd) { tcflush(fd, TCOFLUSH); }
|
||||
|
||||
void serial::flushTxRxBuf(int fd) { tcflush(fd, TCIOFLUSH); }
|
||||
|
||||
|
|
|
|||
|
|
@ -65,6 +65,7 @@ void setParity(struct termios& options, Parity parity);
|
|||
void ignoreCtrlLines(struct termios& options);
|
||||
|
||||
void flushRxBuf(int fd);
|
||||
void flushTxBuf(int fd);
|
||||
void flushTxRxBuf(int fd);
|
||||
|
||||
int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter);
|
||||
|
|
|
|||
Loading…
Reference in New Issue