Merge pull request 'Removed duplicate Code' (#170) from cleanup into develop

Reviewed-on: #170
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>

CHANGELOG.md

@@ -41,6 +41,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.
 
 # [v6.0.0] 2023-02-10
 

src/fsfw/coordinates/CoordinateTransformations.cpp

@@ -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]);
+}

src/fsfw/coordinates/CoordinateTransformations.h

@@ -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,

src/fsfw/globalfunctions/math/MatrixOperations.h

@@ -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_ */

src/fsfw/globalfunctions/math/QuaternionOperations.cpp

@@ -153,3 +153,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);
+}

src/fsfw/globalfunctions/math/QuaternionOperations.h

@@ -25,6 +25,9 @@ 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]);
+
   /**
    * returns angle in ]-Pi;Pi] or [0;Pi] if abs == true
    */

src/fsfw/globalfunctions/math/VectorOperations.h

@@ -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();
 };

src/fsfw/pus/Service11TelecommandScheduling.tpp

@@ -150,7 +150,7 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::handleResetCom
 template <size_t MAX_NUM_TCS>
 inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivity(
     const uint8_t *data, size_t size) {
-  if(telecommandMap.full()) {
+  if (telecommandMap.full()) {
     return MAP_IS_FULL;
   }
   uint32_t timestamp = 0;

src/fsfw/tmtcservices/PusServiceBase.cpp

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

src/fsfw/tmtcservices/PusServiceBase.h

@@ -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.