i forgot to push..

Reviewed-on: #173
Reviewed-by: Marius Eggert <eggertm@irs.uni-stuttgart.de>

CHANGELOG.md

@@ -28,9 +28,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
@@ -41,6 +44,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

@@ -7,6 +7,7 @@
 #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) {
@@ -98,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);
 

src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp

@@ -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) {

src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.h

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

src/fsfw/fdir/FailureIsolationBase.cpp

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

src/fsfw/fdir/FailureIsolationBase.h

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

src/fsfw/globalfunctions/math/QuaternionOperations.cpp

@@ -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) {

src/fsfw/globalfunctions/math/QuaternionOperations.h

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

src/fsfw/globalfunctions/timeSystems.h

@@ -1,20 +0,0 @@
-#ifndef FSFW_SRC_FSFW_GLOBALFUNCTIONS_TIMESYSTEMS_H_
-#define FSFW_SRC_FSFW_GLOBALFUNCTIONS_TIMESYSTEMS_H_
-
-#ifdef PLATFORM_WIN
-#include <winsock2.h>
-#else
-#include <sys/time.h>
-#endif
-
-class TimeSystems {
- public:
-  virtual ~TimeSystems() {}
-
-  static double convertUnixToJD2000(timeval time) {
-    // time = {{s},{us}}
-    return (time.tv_sec / 86400.0) + 2440587.5 - 2451545;
-  }
-};
-
-#endif /* FSFW_SRC_FSFW_GLOBALFUNCTIONS_TIMESYSTEMS_H_ */

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/pus/Service9TimeManagement.cpp

@@ -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(&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:
       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(&currentTime);
+  triggerEvent(event, currentTime.tv_sec, currentTime.tv_usec);
+}
+
+void Service9TimeManagement::reportTime(Event event, timeval time) {
+  triggerEvent(event, time.tv_sec, time.tv_usec);
 }

src/fsfw/pus/Service9TimeManagement.h

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

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.

src/fsfw_hal/linux/serial/helper.cpp

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

src/fsfw_hal/linux/serial/helper.h

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