Work on FANET

src/lib/CMakeLists.txt

@@ -26,6 +26,7 @@ add_subdirectory(gdloverudp)
 add_subdirectory(dataport)
 add_subdirectory(airconnect)
 add_subdirectory(bluetooth)
+add_subdirectory(fanet)
 
 
 if (PICO_SDK_VERSION_STRING)

src/lib/fanet/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.10.0)
+
+project(fanet VERSION 0.0.0 LANGUAGES CXX)
+set(MSG_PREFIX "${PROJECT_NAME} |")
+
+set(MODULE_SOURCE_FILES
+    ace/fanetace.cpp
+)
+
+set(MODULE_TARGET_LINK
+    utils
+)
+
+set(PICO_EXTRA_LIBS
+    pico_cyw43_arch_lwip_sys_freertos
+)
+
+include(${CMAKE_CURRENT_SOURCE_DIR}/../openace_module.cmake)

src/lib/fanet/README.md

@@ -0,0 +1 @@
+Protocol and specification from : https://github.com/3s1d/fanet-stm32

src/lib/fanet/ace/FreeRTOSConfig.h

@@ -0,0 +1,2 @@
+#pragma once
+#include "../include/FreeRTOSConfig.h"

src/lib/fanet/ace/fanetace.cpp

@@ -0,0 +1,66 @@
+#include <stdio.h>
+
+#include "pico/stdlib.h"
+
+#include "../fanet/fanet_common.hpp"
+#include "../fanet/fanet_packet.hpp"
+#include "../fanet/fanet_builder.hpp"
+
+#include "fanetace.hpp"
+
+#include "ace/messagerouter.hpp"
+#include "ace/coreutils.hpp"
+
+void FanetAce::start()
+{
+    xTaskCreate(FanetAceTask, "FanetAceTask", configMINIMAL_STACK_SIZE, this, tskIDLE_PRIORITY + 2, &taskHandle);
+};
+
+void FanetAce::stop()
+{
+    xTaskNotify(taskHandle, TaskState::EXIT, eSetBits);
+};
+
+OpenAce::PostConstruct FanetAce::postConstruct()
+{
+    return OpenAce::PostConstruct::OK;
+}
+
+void FanetAce::FanetAceTask(void *arg)
+{
+    (void)arg;
+    // FanetAce *FanetAce = static_cast<FanetAce *>(arg);
+    // FanetAce->statistics.queueFullErr = 0;
+    // OpenAce::ADSBString sentence;
+    while (true)
+    {
+        if (uint32_t notifyValue = ulTaskNotifyTake(pdTRUE, portMAX_DELAY))
+        {
+            (void)notifyValue;
+//             if (notifyValue & TaskState::EXIT)
+//             {
+//                 vTaskDelete(nullptr);
+//                 return;
+//             }
+
+//             if (notifyValue & TaskState::NEW)
+//             {
+//                 while (FanetAce->queue.pop(sentence))
+//                 {
+//                     FanetAce->statistics.totalReceived++;
+//                     // Finalise implementation to create the binary message here and send
+// //                    FanetAce->getBus().receive(OpenAce::ADSBMessageBin{sentence});
+//                 }
+//             }
+        }
+    }
+}
+
+void FanetAce::getData(etl::string_stream &stream, const etl::string_view path) const
+{
+    (void)path;
+    stream << "{";
+    stream << "\"totalReceived\":" << statistics.totalReceived;
+    stream << ",\"queueFullErr\":" << statistics.queueFullErr;
+    stream << "}\n";
+}

src/lib/fanet/ace/fanetace.hpp

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <stdint.h>
+
+#include "ace/constants.hpp"
+#include "ace/messagerouter.hpp"
+#include "ace/basemodule.hpp"
+#include "ace/messages.hpp"
+
+#include "etl/map.h"
+#include "etl/message_bus.h"
+#include "etl/queue_spsc_atomic.h"
+
+class FanetAce : public BaseModule, public etl::message_router<FanetAce>
+{
+    static constexpr uint8_t QUEUE_SIZE = 6;
+
+private:
+    friend class message_router;
+    enum TaskState : uint32_t
+    {
+        EXIT = 1 << 0,
+        NEW = 1 << 2,
+    };
+    struct
+    {
+        uint32_t totalReceived = 0;
+        uint32_t queueFullErr = 0;
+    } statistics;
+
+    void on_receive_unknown(const etl::imessage &msg)
+    {
+        (void)msg;
+    }
+
+    static void FanetAceTask(void *arg);
+
+    TaskHandle_t taskHandle;
+    etl::queue_spsc_atomic<OpenAce::ADSBString, QUEUE_SIZE, etl::memory_model::MEMORY_MODEL_SMALL> queue;
+
+public:
+    static constexpr const etl::string_view NAME = "FanetAce";
+
+    FanetAce(etl::imessage_bus &bus, const Configuration &config) : BaseModule(bus, NAME), taskHandle(nullptr)
+    {
+        (void)config;
+    }
+
+    virtual ~FanetAce() = default;
+
+    virtual OpenAce::PostConstruct postConstruct() override;
+
+    virtual void start() override;
+
+    virtual void stop() override;
+
+    virtual void getData(etl::string_stream &stream, const etl::string_view path) const override;
+};

src/lib/fanet/fanet/fanet_builder.hpp

@@ -0,0 +1,139 @@
+#pragma once
+
+#include "fanet_packet.hpp"
+#include "fanet_common.hpp"
+#include "etl/optional.h"
+#include "etl/vector.h"
+
+namespace FANET
+{
+    class PacketBuilder
+    {
+    private:
+        static constexpr size_t MAX_HEADER_SIZE = sizeof(Header) + sizeof(Address) + sizeof(Address) + sizeof(ExtendedHeader) + sizeof(uint32_t);
+
+        Header header{};
+        Address source{};
+
+        etl::optional<Address> destination;
+        etl::optional<ExtendedHeader> extHeader;
+        etl::optional<uint32_t> signature;
+
+    public:
+        PacketBuilder(uint32_t src) : PacketBuilder(Address(src))
+        {
+        }
+        PacketBuilder(const Address &src) : source{src}
+        {
+        }
+
+        PacketBuilder &setAck(AckType ackType)
+        {
+            // Ack may not request an ack, and Ack::NONE does not require an extended header
+            if (ackType == AckType::NONE)
+            {
+                return *this;
+            }
+            if (extHeader)
+            {
+                extHeader->ack = ackType;
+            }
+            else
+            {
+                extHeader = ExtendedHeader{.reserved = 0, .geoForwarded = false, .signature = false, .cast = false, .ack = ackType};
+            }
+            return *this;
+        }
+
+        PacketBuilder &setDestination(const Address &destination_)
+        {
+            destination = destination_;
+            header.extended = true;
+            if (extHeader)
+            {
+                extHeader->cast = true;
+            }
+            else
+            {
+                extHeader = ExtendedHeader{.reserved = 0, .geoForwarded = false, .signature = false, .cast = true, .ack = AckType::NONE};
+            }
+            return *this;
+        }
+        PacketBuilder &setDestination(const uint32_t destination_) {
+            return setDestination(Address(destination_));
+        }
+
+        PacketBuilder &setSignature(uint32_t signature_)
+        {
+            if (extHeader)
+            {
+                extHeader->signature = true;
+            }
+            else
+            {
+                extHeader = ExtendedHeader{.reserved = 0, .geoForwarded = false, .signature = true, .cast = true, .ack = AckType::NONE};
+            }
+            signature = signature_;
+            header.extended = true;
+            return *this;
+        }
+
+        ////////////////////////////////
+
+        size_t serializeCommonFields(etl::ivector<uint8_t> &buffer)
+        {
+            size_t offset = 0;
+
+            buffer.resize(offset + sizeof(Header));
+            etl::mem_copy(reinterpret_cast<uint8_t *>(&header), sizeof(Header), buffer.data() + offset);
+            offset += sizeof(Header);
+
+            buffer.resize(offset + sizeof(Address));
+            etl::mem_copy(reinterpret_cast<uint8_t *>(&source), sizeof(Address), buffer.data() + offset);
+            offset += sizeof(Address);
+
+            if (extHeader)
+            {
+                buffer.resize(offset + sizeof(ExtendedHeader));
+                etl::mem_copy(reinterpret_cast<uint8_t *>(&extHeader), sizeof(ExtendedHeader), buffer.data() + offset);
+                offset += sizeof(ExtendedHeader);
+                if (destination)
+                {
+                    buffer.resize(offset + sizeof(Address));
+                    etl::mem_copy(reinterpret_cast<uint8_t *>(&destination), sizeof(Address), buffer.data() + offset);
+                    offset += sizeof(Address);
+                }
+                if (signature)
+                {
+                    buffer.resize(offset + sizeof(uint32_t));
+                    etl::mem_copy(reinterpret_cast<uint8_t *>(&signature), sizeof(uint32_t), buffer.data() + offset);
+                    offset += sizeof(uint32_t);
+                }
+            }
+            return offset;
+        }
+
+        etl::vector<uint8_t, MAX_HEADER_SIZE> build()
+        {
+            etl::vector<uint8_t, MAX_HEADER_SIZE> buffer;
+            auto offset = serializeCommonFields(buffer);
+            return buffer;
+        }
+
+        template <typename PAYLOAD>
+        etl::vector<uint8_t, MAX_HEADER_SIZE + sizeof(PAYLOAD)> build(const PAYLOAD &payload)
+        {
+            etl::vector<uint8_t, MAX_HEADER_SIZE + sizeof(PAYLOAD)> buffer;
+            header.type = payload.type();
+            auto offset = serializeCommonFields(buffer);
+            if (header.type == MessageType::ACK)
+            {
+                return buffer;
+            }
+            buffer.resize(offset + sizeof(PAYLOAD));
+            etl::mem_copy(const_cast<uint8_t *>(reinterpret_cast<const uint8_t *>(&payload)), sizeof(PAYLOAD), buffer.data() + offset);
+            return buffer;
+        }
+
+    };
+};