starting fsfw

bsp_z7/objects/CMakeLists.txt

@@ -0,0 +1,5 @@
+add_subdirectory(communication)
+
+target_sources(
+  ${TARGET_NAME} PRIVATE
+  ObjectFactory.cpp)

bsp_z7/objects/ObjectFactory.cpp

@@ -0,0 +1,72 @@
+#include "ObjectFactory.h"
+
+#include <fsfw/objectmanager/frameworkObjects.h>
+#include <fsfw/storagemanager/PoolManager.h>
+#include <fsfw/tmtc/TmManager.h>
+#include <fsfw/tmtc/UdpTmTcBridge.h>
+#include <mission/controllers/SteeringController.h>
+#include <mission/devicehandlers/RoboclawHandler.h>
+#include <mission/devicehandlers/ServoHandler.h>
+
+#include "fsfw/events/EventManager.h"
+#include "fsfw/health/HealthTable.h"
+#include "fsfw/internalerror/InternalErrorReporter.h"
+#include "objects/communication/SerialTCPCookie.h"
+#include "objects/communication/ServoCommInterface.h"
+#include "systemObjects.h"
+
+namespace objects {
+enum commonObjects : object_id_t {
+
+  /* 0x63 ('C') for core objects */
+  CCSDS_DISTRIBUTOR = 0x63000000,
+  PUS_DISTRIBUTOR = 0x63000001,
+  TM_FUNNEL = 0x63000002,
+  CFDP_DISTRIBUTOR = 0x63000003,
+  CFDP_HANDLER = 0x63000004,
+  PUS_TM_FUNNEL = 0x63000005,
+  CFDP_TM_FUNNEL = 0x64000006,
+};
+}
+
+namespace ObjectFactory {
+uint16_t listeningPort = 0;
+socklen_t defaultDestinationLen = 0;
+const sockaddr *defaultDestination = nullptr;
+const char *simString = nullptr;
+}  // namespace ObjectFactory
+
+void Factory::setStaticFrameworkObjectIds() {
+  // MonitoringReportContent<float>::timeStamperId = objects::TIME_STAMPER;
+  // MonitoringReportContent<double>::timeStamperId = objects::TIME_STAMPER;
+  // MonitoringReportContent<uint32_t>::timeStamperId = objects::TIME_STAMPER;
+  // MonitoringReportContent<int32_t>::timeStamperId = objects::TIME_STAMPER;
+  // MonitoringReportContent<int16_t>::timeStamperId = objects::TIME_STAMPER;
+  // MonitoringReportContent<uint16_t>::timeStamperId = objects::TIME_STAMPER;
+
+  // PusServiceBase::PUS_DISTRIBUTOR = objects::PUS_DISTRIBUTOR;
+  // PusServiceBase::PACKET_DESTINATION = objects::PUS_TM_FUNNEL;
+
+  // CommandingServiceBase::defaultPacketSource = objects::PUS_DISTRIBUTOR;
+  // CommandingServiceBase::defaultPacketDestination = objects::PUS_TM_FUNNEL;
+
+  // VerificationReporter::DEFAULT_RECEIVER = objects::PUS_SERVICE_1_VERIFICATION;
+}
+
+void ObjectFactory::produce(void *args) {
+  Factory::setStaticFrameworkObjectIds();
+
+  
+
+  {
+    LocalPool::LocalPoolConfig poolCfg = {{100, 16}, {50, 32},   {40, 64},
+                                          {30, 128}, {20, 1024}, {10, 2048}};
+    new PoolManager(objects::IPC_STORE, poolCfg);
+  }
+
+  new EventManager(objects::EVENT_MANAGER, 20);
+  new HealthTable(objects::HEALTH_TABLE);
+  new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER);
+
+  new TmManager(objects::TM_MANAGER);
+}

bsp_z7/objects/ObjectFactory.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include <cstdint>
+#include <sys/socket.h>
+
+namespace ObjectFactory {
+extern uint16_t listeningPort;
+extern socklen_t defaultDestinationLen ;
+extern const sockaddr * defaultDestination ;
+extern const char * simString;
+
+void produce(void* args);
+}  // namespace ObjectFactory

bsp_z7/objects/communication/CMakeLists.txt

@@ -0,0 +1,4 @@
+target_sources(
+  ${TARGET_NAME} PRIVATE
+  ServoCommInterface.cpp
+  SerialTCPCookie.cpp)

bsp_z7/objects/communication/SerialTCPCookie.cpp

@@ -0,0 +1,8 @@
+#include "SerialTCPCookie.h"
+
+SerialTCPCookie::SerialTCPCookie(const char* host, uint16_t port, bool flush, size_t bufferSize)
+    : host(host), port(port), flush(flush), bufferSize(bufferSize), socket(-1) {
+  buffer = new uint8_t[bufferSize];
+}
+
+SerialTCPCookie::~SerialTCPCookie() { delete buffer; }

bsp_z7/objects/communication/SerialTCPCookie.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include <fsfw/devicehandlers/CookieIF.h>
+#include <stddef.h>
+
+class SerialTCPCookie : public CookieIF {
+ public:
+  SerialTCPCookie(const char* host, uint16_t port, bool flush, size_t bufferSize);
+  virtual ~SerialTCPCookie();
+
+  const char* const host;
+  const uint16_t port;
+  const bool flush;
+  uint8_t *buffer;
+  const size_t bufferSize;
+  int socket;
+};

bsp_z7/objects/communication/ServoCommInterface.cpp

@@ -0,0 +1,138 @@
+#include "ServoCommInterface.h"
+
+#include <arpa/inet.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <sys/types.h>  // POSIX.1-2001 does not require the inclusion of <sys/types.h>, and this header file is not required on Linux. However, some historical (BSD) implementations required this header file, and portable applications are probably wise to include it.
+#include <unistd.h>
+#include <utility>
+
+#include "SerialTCPCookie.h"
+
+ServoCommInterface::ServoCommInterface(object_id_t setObjectId) : SystemObject(setObjectId) {}
+
+ReturnValue_t ServoCommInterface::initializeInterface(CookieIF *cookie) {
+  SerialTCPCookie *tcpCookie = dynamic_cast<SerialTCPCookie *>(cookie);
+  if (tcpCookie != nullptr) {
+    return initializeInterface(tcpCookie);
+  }
+  return DeviceCommunicationIF::INVALID_COOKIE_TYPE;
+}
+
+ReturnValue_t ServoCommInterface::sendMessage(CookieIF *cookie, const uint8_t *sendData,
+                                              size_t sendLen) {
+  SerialTCPCookie *tcpCookie = dynamic_cast<SerialTCPCookie *>(cookie);
+  if (tcpCookie != nullptr) {
+    return sendMessage(tcpCookie, sendData, sendLen);
+  }
+  return DeviceCommunicationIF::INVALID_COOKIE_TYPE;
+}
+
+ReturnValue_t ServoCommInterface::sendMessage(SerialTCPCookie *cookie, const uint8_t *sendData,
+                                              size_t sendLen) {
+  if (cookie->socket == -1) {
+    return returnvalue::FAILED;
+  }
+
+  ssize_t result = send(cookie->socket, sendData, sendLen, 0);
+
+  if (result < 0){
+    return returnvalue::FAILED;
+  }
+  
+  if (std::cmp_not_equal(result, sendLen)) {
+    return returnvalue::FAILED;
+  }
+
+  return returnvalue::OK;
+}
+
+ReturnValue_t ServoCommInterface::getSendSuccess(CookieIF *cookie) { return returnvalue::OK; }
+
+ReturnValue_t ServoCommInterface::requestReceiveMessage(CookieIF *cookie, size_t requestLen) {
+  return returnvalue::OK;
+}
+
+ReturnValue_t ServoCommInterface::readReceivedMessage(CookieIF *cookie, uint8_t **buffer,
+                                                      size_t *size) {
+  SerialTCPCookie *tcpCookie = dynamic_cast<SerialTCPCookie *>(cookie);
+  if (tcpCookie != nullptr) {
+    return readReceivedMessage(tcpCookie, buffer, size);
+  }
+  return DeviceCommunicationIF::INVALID_COOKIE_TYPE;
+}
+
+ReturnValue_t ServoCommInterface::readReceivedMessage(SerialTCPCookie *cookie, uint8_t **buffer,
+                                                      size_t *size) {
+  if (cookie->socket == -1) {
+    return returnvalue::FAILED;
+  }
+
+  ssize_t result = recv(cookie->socket, cookie->buffer, cookie->bufferSize, 0);
+
+  if (result <= -1) {
+    *size = 0;
+    if (errno == EAGAIN or errno == EWOULDBLOCK) {
+      //no data
+      return returnvalue::OK;
+    }
+    // TODO wrap errno
+    return returnvalue::FAILED;
+  }
+
+  if (result == 0) {
+    // peer shut down
+    close(cookie->socket);
+    cookie->socket = -1;
+    return returnvalue::FAILED;  // TODO connection closed
+  }
+
+  *size = result;
+  *buffer = cookie->buffer;
+
+  
+  if (std::cmp_greater_equal(result, cookie->bufferSize) and cookie->flush) {
+    // we do not know if there is more data in the socket, so we read until we get no more
+    uint8_t ignore[10];
+    while (recv(cookie->socket, ignore, sizeof(ignore), 0) > 0) {
+    }
+    //ignore any fault here as previous read was ok. next call will catch it
+  }
+
+  return returnvalue::OK;
+}
+
+ReturnValue_t ServoCommInterface::initializeInterface(SerialTCPCookie *cookie) {
+  cookie->socket = socket(AF_INET6, SOCK_STREAM, 0);
+  if (cookie->socket == -1) {
+    return returnvalue::FAILED;
+  }
+
+  sockaddr_in6 serverAddr;
+  serverAddr.sin6_family = AF_INET6;
+  serverAddr.sin6_port = htons(cookie->port);
+
+  int retval =inet_pton(AF_INET6, cookie->host, &serverAddr.sin6_addr);
+  if (retval == 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::error << "IpCommInterface::initializeInterface: Invalid IP!" << std::endl;
+#endif
+    return returnvalue::FAILED;
+  }
+
+  retval = connect(cookie->socket, (struct sockaddr *)&serverAddr, sizeof(serverAddr));
+
+  if (retval == -1) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::error << "IpCommInterface::initializeInterface: connection failed with " << errno
+               << std::endl;
+#endif
+    return returnvalue::FAILED;
+  }
+
+  fcntl(cookie->socket, F_SETFL, O_NONBLOCK);
+
+  return returnvalue::OK;
+}

bsp_z7/objects/communication/ServoCommInterface.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <fsfw/devicehandlers/DeviceCommunicationIF.h>
+#include <fsfw/objectmanager/SystemObject.h>
+
+#include "SerialTCPCookie.h"
+
+class ServoCommInterface : public DeviceCommunicationIF, public SystemObject {
+ public:
+  ServoCommInterface(object_id_t setObjectId);
+
+  virtual ~ServoCommInterface() {}
+
+  ReturnValue_t initializeInterface(CookieIF *cookie) override;
+
+  ReturnValue_t sendMessage(CookieIF *cookie, const uint8_t *sendData, size_t sendLen) override;
+
+  ReturnValue_t getSendSuccess(CookieIF *cookie) override;
+
+  ReturnValue_t requestReceiveMessage(CookieIF *cookie, size_t requestLen) override;
+
+  ReturnValue_t readReceivedMessage(CookieIF *cookie, uint8_t **buffer, size_t *size) override;
+
+ private:
+  ReturnValue_t initializeInterface(SerialTCPCookie *cookie);
+  ReturnValue_t sendMessage(SerialTCPCookie *cookie, const uint8_t *sendData, size_t sendLen);
+  ReturnValue_t readReceivedMessage(SerialTCPCookie *cookie, uint8_t **buffer, size_t *size);
+};

bsp_z7/objects/systemObjects.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include <fsfw/introspection/ClasslessEnum.h>
+
+FSFW_CLASSLESS_ENUM(SystemObjects, uint32_t, ((SERVO_FRONT_LEFT, "Servo front left"))
+ ((SERVO_FRONT_RIGHT, "Servo front right"))
+  ((SERVO_BACK_LEFT, "Servo back left"))
+   ((SERVO_BACK_RIGHT,"Servo back right"))
+   ((MOTOR_AXLE_LEFT, "Motors left axle"))
+   ((MOTOR_AXLE_RIGHT, "Motors right axle"))
+   ((MOTOR_AXLE_BACK, "Motors back axle"))
+   ((STEERING_CONTROLLER, "Steering Controller"))
+  ((COMM_IF, 123, "CommIF")))