Service 200: Add mode announcement support

src/fsfw/devicehandlers/DeviceHandlerBase.cpp

@@ -1604,12 +1604,12 @@ const HasModesIF& DeviceHandlerBase::getModeIF() const { return *this; }
 ModeTreeChildIF& DeviceHandlerBase::getModeTreeChildIF() { return *this; }
 
 ReturnValue_t DeviceHandlerBase::finishAction(bool success, DeviceCommandId_t action,
-		ReturnValue_t result) {
+                                              ReturnValue_t result) {
-	  auto commandIter = deviceCommandMap.find(action);
+  auto commandIter = deviceCommandMap.find(action);
-	  if (commandIter == deviceCommandMap.end()) {
+  if (commandIter == deviceCommandMap.end()) {
-	    return MessageQueueIF::NO_QUEUE;
+    return MessageQueueIF::NO_QUEUE;
-	  }
+  }
-	  commandIter->second.isExecuting = false;
+  commandIter->second.isExecuting = false;
-	  actionHelper.finish(success, commandIter->second.sendReplyTo, action, result);
+  actionHelper.finish(success, commandIter->second.sendReplyTo, action, result);
-	  return returnvalue::OK;
+  return returnvalue::OK;
 }

src/fsfw/globalfunctions/DleParser.cpp

@@ -11,8 +11,7 @@ DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPa
     : decodeRingBuf(decodeRingBuf),
       decoder(decoder),
       encodedBuf(encodedBuf),
-      decodedBuf(decodedBuf) {
+      decodedBuf(decodedBuf) {}
-}
 
 ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) {
   if (data == nullptr or len == 0) {
@@ -24,7 +23,7 @@ ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) {
 ReturnValue_t DleParser::parseRingBuf(size_t& readSize) {
   ctx.setType(DleParser::ContextType::NONE);
   size_t availableData = decodeRingBuf.getAvailableReadData();
-  if(availableData == 0) {
+  if (availableData == 0) {
     return NO_PACKET_FOUND;
   }
   if (availableData > encodedBuf.second) {
@@ -106,7 +105,7 @@ void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* arg
 }
 
 void DleParser::defaultErrorHandler() {
-  if(ctx.getType() != DleParser::ContextType::ERROR) {
+  if (ctx.getType() != DleParser::ContextType::ERROR) {
     errorPrinter("No error");
     return;
   }
@@ -126,7 +125,8 @@ void DleParser::defaultErrorHandler() {
     case (ErrorTypes::ENCODED_BUF_TOO_SMALL):
     case (ErrorTypes::DECODING_BUF_TOO_SMALL): {
       char opt[64];
-      snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.decodedPacket.second);
+      snprintf(opt, sizeof(opt), ": Too small for packet with length %zu",
+               ctx.decodedPacket.second);
       if (ctx.error.first == ErrorTypes::ENCODED_BUF_TOO_SMALL) {
         errorPrinter("Encoded buf too small", opt);
       } else {
@@ -168,10 +168,6 @@ ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) {
   return decodeRingBuf.deleteData(bytesRead);
 }
 
-const DleParser::Context& DleParser::getContext() {
+const DleParser::Context& DleParser::getContext() { return ctx; }
-  return ctx;
-}
 
-void DleParser::reset() {
+void DleParser::reset() { decodeRingBuf.clear(); }
-  decodeRingBuf.clear();
-}

src/fsfw/modes/ModeMessage.cpp

@@ -24,3 +24,13 @@ void ModeMessage::setCantReachMode(CommandMessage* message, ReturnValue_t reason
   message->setParameter(reason);
   message->setParameter2(0);
 }
+
+void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive) {
+  Command_t cmd;
+  if (recursive) {
+    cmd = CMD_MODE_ANNOUNCE_RECURSIVELY;
+  } else {
+    cmd = CMD_MODE_ANNOUNCE;
+  }
+  message.setCommand(cmd);
+}

src/fsfw/modes/ModeMessage.h

@@ -1,43 +1,42 @@
 #ifndef FSFW_MODES_MODEMESSAGE_H_
 #define FSFW_MODES_MODEMESSAGE_H_
 
-#include "../ipc/CommandMessage.h"
+#include "fsfw/ipc/CommandMessage.h"
 
 typedef uint32_t Mode_t;
 typedef uint8_t Submode_t;
 
 class ModeMessage {
- private:
-  ModeMessage();
-
  public:
   static const uint8_t MESSAGE_ID = messagetypes::MODE_COMMAND;
-  static const Command_t CMD_MODE_COMMAND =
+  //!> Command to set the specified Mode, replies are: REPLY_MODE_REPLY,
-      MAKE_COMMAND_ID(0x01);  //!> Command to set the specified Mode, replies are: REPLY_MODE_REPLY,
+  //! REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any replies,
-                              //! REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any replies,
+  //! as this will break the subsystem mode machine!!
-                              //! as this will break the subsystem mode machine!!
+  static const Command_t CMD_MODE_COMMAND = MAKE_COMMAND_ID(0x01);
-  static const Command_t CMD_MODE_COMMAND_FORCED = MAKE_COMMAND_ID(
+  //!> Command to set the specified Mode, regardless of external control flag, replies
-      0xF1);  //!> Command to set the specified Mode, regardless of external control flag, replies
+  //! are: REPLY_MODE_REPLY, REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any
-              //! are: REPLY_MODE_REPLY, REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any
+  //! replies, as this will break the subsystem mode machine!!
-              //! replies, as this will break the subsystem mode machine!!
+  static const Command_t CMD_MODE_COMMAND_FORCED = MAKE_COMMAND_ID(0xF1);
-  static const Command_t REPLY_MODE_REPLY =
+  //!> Reply to a CMD_MODE_COMMAND or CMD_MODE_READ
-      MAKE_COMMAND_ID(0x02);  //!> Reply to a CMD_MODE_COMMAND or CMD_MODE_READ
+  static const Command_t REPLY_MODE_REPLY = MAKE_COMMAND_ID(0x02);
-  static const Command_t REPLY_MODE_INFO =
+  //!> Unrequested info about the current mode (used for composites to
-      MAKE_COMMAND_ID(0x03);  //!> Unrequested info about the current mode (used for composites to
+  //! inform their container of a changed mode)
-                              //! inform their container of a changed mode)
+  static const Command_t REPLY_MODE_INFO = MAKE_COMMAND_ID(0x03);
-  static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID(
+  //!> Reply in case a mode command can't be executed. Par1: returnCode, Par2: 0
-      0x04);  //!> Reply in case a mode command can't be executed. Par1: returnCode, Par2: 0
+  static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID(0x04);
-  static const Command_t REPLY_WRONG_MODE_REPLY =
+  //!> Reply to a CMD_MODE_COMMAND, indicating that a mode was commanded
-      MAKE_COMMAND_ID(0x05);  //!> Reply to a CMD_MODE_COMMAND, indicating that a mode was commanded
+  //! and a transition started but was aborted; the parameters contain
-                              //! and a transition started but was aborted; the parameters contain
+  //! the mode that was reached
-                              //! the mode that was reached
+  static const Command_t REPLY_WRONG_MODE_REPLY = MAKE_COMMAND_ID(0x05);
-  static const Command_t CMD_MODE_READ = MAKE_COMMAND_ID(
+  //!> Command to read the current mode and reply with a REPLY_MODE_REPLY
-      0x06);  //!> Command to read the current mode and reply with a REPLY_MODE_REPLY
+  static const Command_t CMD_MODE_READ = MAKE_COMMAND_ID(0x06);
-  static const Command_t CMD_MODE_ANNOUNCE = MAKE_COMMAND_ID(
+  //!> Command to trigger an ModeInfo Event. This command does NOT have a reply.
-      0x07);  //!> Command to trigger an ModeInfo Event. This command does NOT have a reply.
+  static const Command_t CMD_MODE_ANNOUNCE = MAKE_COMMAND_ID(0x07);
-  static const Command_t CMD_MODE_ANNOUNCE_RECURSIVELY =
+  //!> Command to trigger an ModeInfo Event and to send this command to
-      MAKE_COMMAND_ID(0x08);  //!> Command to trigger an ModeInfo Event and to send this command to
+  //! every child. This command does NOT have a reply.
-                              //! every child. This command does NOT have a reply.
+  static const Command_t CMD_MODE_ANNOUNCE_RECURSIVELY = MAKE_COMMAND_ID(0x08);
+
+  ModeMessage() = delete;
 
   static Mode_t getMode(const CommandMessage* message);
   static Submode_t getSubmode(const CommandMessage* message);
@@ -45,6 +44,7 @@ class ModeMessage {
 
   static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode,
                              Submode_t submode);
+  static void setModeAnnounceMessage(CommandMessage& message, bool recursive);
   static void setCantReachMode(CommandMessage* message, ReturnValue_t reason);
   static void clear(CommandMessage* message);
 };

src/fsfw/pus/CService200ModeCommanding.cpp

@@ -54,15 +54,26 @@ ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
 ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message, uint8_t subservice,
                                                         const uint8_t *tcData, size_t tcDataLen,
                                                         uint32_t *state, object_id_t objectId) {
-  ModePacket modeCommandPacket;
+  ReturnValue_t result = returnvalue::OK;
-  ReturnValue_t result =
+  if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE or
-      modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
+      subservice == Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY) {
-  if (result != returnvalue::OK) {
+    bool recursive = true;
-    return result;
+    if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) {
+      recursive = false;
+    }
+    ModeMessage::setModeAnnounceMessage(*message, recursive);
+  } else {
+    ModePacket modeCommandPacket;
+    ReturnValue_t result =
+        modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
+    if (result != returnvalue::OK) {
+      return result;
+    }
+
+    ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
+                                modeCommandPacket.getSubmode());
   }
 
-  ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
-                              modeCommandPacket.getSubmode());
   return result;
 }
 

src/fsfw/pus/servicepackets/Service200Packets.h

@@ -1,9 +1,9 @@
 #ifndef FSFW_PUS_SERVICEPACKETS_SERVICE200PACKETS_H_
 #define FSFW_PUS_SERVICEPACKETS_SERVICE200PACKETS_H_
 
-#include "../../modes/ModeMessage.h"
+#include "fsfw/modes/ModeMessage.h"
-#include "../../serialize/SerialLinkedListAdapter.h"
+#include "fsfw/serialize/SerialLinkedListAdapter.h"
-#include "../../serialize/SerializeIF.h"
+#include "fsfw/serialize/SerializeIF.h"
 
 /**
  * @brief Subservice 1, 2, 3, 4, 5

src/fsfw_hal/linux/uart/UartComIF.cpp

@@ -1,4 +1,5 @@
 #include "UartComIF.h"
+
 #include <errno.h>
 #include <fcntl.h>
 #include <termios.h>
@@ -458,5 +459,3 @@ ReturnValue_t UartComIF::flushUartTxAndRxBuf(CookieIF* cookie) {
   }
   return returnvalue::FAILED;
 }
-
-

src/fsfw_hal/linux/uart/UartComIF.h

@@ -1,15 +1,14 @@
 #ifndef BSP_Q7S_COMIF_UARTCOMIF_H_
 #define BSP_Q7S_COMIF_UARTCOMIF_H_
 
-#include "UartCookie.h"
-#include "helper.h"
-
 #include <fsfw/devicehandlers/DeviceCommunicationIF.h>
 #include <fsfw/objectmanager/SystemObject.h>
 
 #include <unordered_map>
 #include <vector>
 
+#include "UartCookie.h"
+#include "helper.h"
 
 /**
  * @brief 	This is the communication interface to access serial ports on linux based operating

src/fsfw_hal/linux/uart/UartCookie.h

@@ -1,14 +1,12 @@
 #ifndef SAM9G20_COMIF_COOKIES_UART_COOKIE_H_
 #define SAM9G20_COMIF_COOKIES_UART_COOKIE_H_
 
-#include "helper.h"
-
 #include <fsfw/devicehandlers/CookieIF.h>
 #include <fsfw/objectmanager/SystemObjectIF.h>
 
 #include <string>
 
-
+#include "helper.h"
 
 /**
  * @brief   Cookie for the UartComIF. There are many options available to configure the UART driver.

src/fsfw_hal/linux/uart/helper.cpp

@@ -1,8 +1,9 @@
 #include "helper.h"
-#include "fsfw/serviceinterface.h"
 
 #include <sys/ioctl.h>
 
+#include "fsfw/serviceinterface.h"
+
 void uart::setMode(struct termios& options, UartModes mode) {
   if (mode == UartModes::NON_CANONICAL) {
     /* Disable canonical mode */
@@ -145,6 +146,5 @@ void uart::setBaudrate(struct termios& options, UartBaudRate baud) {
 }
 
 int uart::readCountersAndErrors(int serialPort, serial_icounter_struct& icounter) {
-   return ioctl(serialPort, TIOCGICOUNT, &icounter);
+  return ioctl(serialPort, TIOCGICOUNT, &icounter);
 }
-

src/fsfw_hal/linux/uart/helper.h

@@ -1,8 +1,8 @@
 #ifndef FSFW_HAL_LINUX_UART_HELPER_H_
 #define FSFW_HAL_LINUX_UART_HELPER_H_
 
-#include <termios.h>
 #include <linux/serial.h>
+#include <termios.h>
 
 enum class Parity { NONE, EVEN, ODD };
 
@@ -56,7 +56,6 @@ void setBaudrate(struct termios& options, UartBaudRate baud);
 
 int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter);
 
-}
+}  // namespace uart
-
 
 #endif /* FSFW_HAL_LINUX_UART_HELPER_H_ */