starting fsfw

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