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>

src/fsfw/coordinates/Sgp4Propagator.cpp

@@ -166,9 +166,9 @@ ReturnValue_t Sgp4Propagator::propagate(double* position, double* velocity, time
   timeval timeSinceEpoch = time - epoch;
   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;
   }
 

src/fsfw/devicehandlers/FreshDeviceHandlerBase.cpp

@@ -28,12 +28,17 @@ FreshDeviceHandlerBase::~FreshDeviceHandlerBase() {
 }
 
 ReturnValue_t FreshDeviceHandlerBase::performOperation(uint8_t opCode) {
+  performDeviceOperationPreQueueHandling(opCode);
   handleQueue();
   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.

src/fsfw/devicehandlers/FreshDeviceHandlerBase.h

@@ -82,6 +82,12 @@ class FreshDeviceHandlerBase : public SystemObject,
 
   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.
@@ -153,6 +159,8 @@ class FreshDeviceHandlerBase : public SystemObject,
   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;

src/fsfw/globalfunctions/math/QuaternionOperations.cpp

@@ -39,6 +39,39 @@ void QuaternionOperations::inverse(const double* quaternion, double* inverseQuat
   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);
+}
+
 QuaternionOperations::QuaternionOperations() {}
 
 void QuaternionOperations::normalize(const double* quaternion, double* unitQuaternion) {

src/fsfw/globalfunctions/math/QuaternionOperations.h

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

src/fsfw/pus/Service11TelecommandScheduling.h

@@ -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 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 MAP_IS_FULL = returnvalue::makeCode(CLASS_ID, 7);
 
   static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_11;
 

src/fsfw/pus/Service11TelecommandScheduling.tpp

@@ -150,6 +150,9 @@ 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()) {
+    return MAP_IS_FULL;
+  }
   uint32_t timestamp = 0;
   ReturnValue_t result = SerializeAdapter::deSerialize(&timestamp, &data, &size, DEF_END);
   if (result != returnvalue::OK) {