custom CSP router

bsp_q7s/CMakeLists.txt

@@ -12,7 +12,6 @@ target_sources(${OBSW_NAME} PUBLIC main.cpp obsw.cpp)
 add_subdirectory(boardtest)
 
 add_subdirectory(boardconfig)
-add_subdirectory(comIF)
 add_subdirectory(core)
 
 if(EIVE_Q7S_EM)

bsp_q7s/comIF/CMakeLists.txt

@@ -1 +0,0 @@
-target_sources(${OBSW_NAME} PRIVATE)

bsp_q7s/core/ObjectFactory.cpp

@@ -179,7 +179,7 @@ void ObjectFactory::createCommunicationInterfaces(LinuxLibgpioIF** gpioComIF,
   *gpioComIF = new LinuxLibgpioIF(objects::GPIO_IF);
 
   /* Communication interfaces */
-  new CspComIF(objects::CSP_COM_IF);
+  new CspComIF(objects::CSP_COM_IF, "CSP_ROUTER", 60);
   *i2cComIF = new I2cComIF(objects::I2C_COM_IF);
   *uartComIF = new SerialComIF(objects::UART_COM_IF);
   *spiMainComIF = new SpiComIF(objects::SPI_MAIN_COM_IF, q7s::SPI_DEFAULT_DEV, **gpioComIF);

linux/csp/CspComIF.cpp

@@ -14,7 +14,10 @@
 
 using namespace GOMSPACE;
 
-CspComIF::CspComIF(object_id_t objectId) : SystemObject(objectId) {}
+CspComIF::CspComIF(object_id_t objectId, const char* routeTaskName, uint32_t routerRealTimePriority)
+    : SystemObject(objectId),
+      routerRealTimePriority(routerRealTimePriority),
+      routerTaskName(routeTaskName) {}
 
 CspComIF::~CspComIF() {}
 
@@ -57,10 +60,8 @@ ReturnValue_t CspComIF::initializeInterface(CookieIF* cookie) {
     }
 
     /* Start the route task */
-    unsigned int task_stack_size = 500;
-    unsigned int priority = 0;
-    result = csp_route_start_task(task_stack_size, priority);
-    if (result != CSP_ERR_NONE) {
+    result = startRouterTask();
+    if (result != returnvalue::OK) {
       sif::error << "Failed to start csp route task" << std::endl;
       return returnvalue::FAILED;
     }
@@ -343,3 +344,49 @@ void CspComIF::initiatePingRequest(uint8_t cspAddress, uint16_t querySize) {
   memcpy(replyBuffer, &replyTime, sizeof(replyTime));
   iter->second.replyLen = sizeof(replyTime);
 }
+
+ReturnValue_t CspComIF::startRouterTask() {
+  pthread_attr_t attr;
+  int res = pthread_attr_init(&attr);
+  if (res) {
+    return returnvalue::FAILED;
+  }
+
+  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
+
+  // Set scheduling policy to SCHED_RR
+  res = pthread_attr_setschedpolicy(&attr, SCHED_RR);
+  if (res) {
+    pthread_attr_destroy(&attr);
+    return returnvalue::FAILED;
+  }
+
+  struct sched_param sched_param;
+  sched_param.sched_priority = routerRealTimePriority;
+  res = pthread_attr_setschedparam(&attr, &sched_param);
+  if (res) {
+    pthread_attr_destroy(&attr);
+    return returnvalue::FAILED;
+  }
+
+  res = pthread_setname_np(pthread_self(), routerTaskName);
+  if (res) {
+    pthread_attr_destroy(&attr);
+    return returnvalue::FAILED;
+  }
+
+  res = pthread_create(&routerTaskHandle, &attr, routerWorkWrapper, NULL);
+  pthread_attr_destroy(&attr);
+  if (res) {
+    return returnvalue::FAILED;
+  }
+  return returnvalue::OK;
+}
+
+void* CspComIF::routerWorkWrapper(void* args) {
+  /* Here there be routing */
+  while (1) {
+    csp_route_work(FIFO_TIMEOUT);
+  }
+  return nullptr;
+}

linux/csp/CspComIF.h

@@ -2,9 +2,11 @@
 #define LINUX_CSP_CSPCOMIF_H_
 
 #include <csp/csp.h>
+#include <csp/csp_autoconfig.h>
 #include <fsfw/devicehandlers/DeviceCommunicationIF.h>
 #include <fsfw/objectmanager/SystemObject.h>
 #include <fsfw/returnvalues/returnvalue.h>
+#include <pthread.h>
 
 #include <unordered_map>
 #include <vector>
@@ -17,7 +19,7 @@
  */
 class CspComIF : public DeviceCommunicationIF, public SystemObject {
  public:
-  CspComIF(object_id_t objectId);
+  CspComIF(object_id_t objectId, const char *routeTaskName, uint32_t routerRealTimePriority);
   virtual ~CspComIF();
 
   ReturnValue_t initializeInterface(CookieIF *cookie) override;
@@ -27,6 +29,13 @@ class CspComIF : public DeviceCommunicationIF, public SystemObject {
   ReturnValue_t readReceivedMessage(CookieIF *cookie, uint8_t **readData, size_t *readLen) override;
 
  private:
+#ifdef CSP_USE_RDP
+  //! If RDP is enabled, the router needs to awake some times to check timeouts
+  static constexpr uint32_t FIFO_TIMEOUT = 100;
+#else
+  //! If no RDP, the router can sleep untill data arrives
+  static constexpr uint32_t FIFO_TIMEOUT = CSP_MAX_DELAY;
+#endif
   /**
    * @brief	This function initiates the CSP transfer.
    *
@@ -56,6 +65,10 @@ class CspComIF : public DeviceCommunicationIF, public SystemObject {
   /* This is the CSP address of the OBC. */
   node_t cspOwnAddress = 1;
 
+  pthread_t routerTaskHandle{};
+  uint32_t routerRealTimePriority = 0;
+  const char *routerTaskName;
+
   /* Interface struct for csp protocol stack */
   csp_iface_t csp_if;
   char canInterface[5] = "can0";
@@ -72,6 +85,9 @@ class CspComIF : public DeviceCommunicationIF, public SystemObject {
    * @brief	This function initiates the ping request.
    */
   void initiatePingRequest(uint8_t cspAddress, uint16_t querySize);
+
+  ReturnValue_t startRouterTask();
+  static void *routerWorkWrapper(void *args);
 };
 
 #endif /* LINUX_CSP_CSPCOMIF_H_ */

linux/devices/startracker/StrComHandler.cpp

@@ -155,9 +155,7 @@ ReturnValue_t StrComHandler::startImageDownload(std::string path) {
   return returnvalue::OK;
 }
 
-void StrComHandler::stopProcess() {
-  terminate = true;
-}
+void StrComHandler::stopProcess() { terminate = true; }
 
 void StrComHandler::setDownloadImageName(std::string filename) {
   downloadImage.filename = filename;