Merge pull request 'SLERP' (#163) from slerp into develop

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

src/fsfw/controller/ControllerBase.cpp

@@ -72,18 +72,17 @@ void ControllerBase::getMode(Mode_t* mode_, Submode_t* submode_) {
   *submode_ = this->submode;
 }
 
-void ControllerBase::setToExternalControl() { healthHelper.setHealth(EXTERNAL_CONTROL); }
-
 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) {
   handleQueue();
   performControlOperation();
   return returnvalue::OK;
 }
-
-void ControllerBase::modeChanged(Mode_t mode_, Submode_t submode_) {}
-
 ReturnValue_t ControllerBase::setHealth(HealthState health) {
   switch (health) {
     case HEALTHY:

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/CMakeLists.txt

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

src/fsfw/devicehandlers/FreshDeviceHandlerBase.cpp

@@ -0,0 +1,201 @@
+#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),
+      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();
+  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;
+}

src/fsfw/devicehandlers/FreshDeviceHandlerBase.h

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

src/fsfw/globalfunctions/math/QuaternionOperations.cpp

@@ -39,6 +39,37 @@ 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::sin(angle) == 0.0) {
+    // nothing to calculate here
+    std::memcpy(q, q1s, 4 * sizeof(double));
+    return;
+  } else if (std::cos(angle) < 0.0) {
+    // we need to invert one quaternione
+    VectorOperations<double>::mulScalar(q1s, -1, q1s, 4);
+    multiply(q1s, q2I, qD);
+    angle = std::acos(qD[3]);
+  }
+
+  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/tmtcpacket/pus/tm/PusTmReader.cpp

@@ -57,13 +57,6 @@ ReturnValue_t PusTmReader::parseData(bool crcCheck) {
   if (result != returnvalue::OK) {
     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();
   pointers.secHeaderStart = pointers.spHeaderStart + currentOffset;
   currentOffset += PusTmIF::MIN_SEC_HEADER_LEN;
@@ -84,6 +77,13 @@ ReturnValue_t PusTmReader::parseData(bool crcCheck) {
   }
   currentOffset += sourceDataLen;
   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;
 }
 bool PusTmReader::isNull() const { return spReader.isNull() or pointers.secHeaderStart == nullptr; }