初步增加uorb的框架

components/utilities/uORB/Kconfig

@@ -0,0 +1,14 @@
+menuconfig RT_USING_UORB
+	bool "enable uorb pkgs"
+	default n
+
+if RT_USING_UORB
+
+	config ORB_MULTI_MAX_INSTANCES
+		int "max number of multi topic instance"
+		default 4
+
+	config ORB_MAX_SUBSCRIBES
+		int "max number of topic subscribe"
+		default 10
+endif

components/utilities/uORB/SConscript

@@ -0,0 +1,10 @@
+from building import *
+
+cwd  = GetCurrentDir()
+src  = Glob('*.c')
+path = [cwd]
+
+
+group = DefineGroup('Utilities', src, depend = ['RT_USING_UORB'], CPPPATH = path)
+
+Return('group')

components/utilities/uORB/uORB.c

@@ -0,0 +1,177 @@
+/*
+ * Copyright (c) 2006-2024, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2024-09-12     latercomer   the first version
+ */
+
+#define LOG_TAG "uorb"
+#define LOG_LVL LOG_LVL_INFO
+
+#include "uORB.h"
+#include <rtdbg.h>
+
+static rt_list_t _node_list;
+
+// round up to nearest power of two
+// Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128
+// Note: When the input value > 128, the output is always 128
+static inline rt_uint8_t
+round_pow_of_two_8(rt_uint8_t n)
+{
+    if (n == 0)
+    {
+        return 1;
+    }
+
+    // Avoid is already a power of 2
+    rt_uint8_t value = n - 1;
+
+    // Fill 1
+    value |= value >> 1U;
+    value |= value >> 2U;
+    value |= value >> 4U;
+
+    // Unable to round-up, take the value of round-down
+    if (value == RT_UINT8_MAX)
+    {
+        value >>= 1U;
+    }
+
+    return value + 1;
+}
+
+
+struct rt_uorb_node *uorb_node_create(const struct orb_metadata *meta, const rt_uint8_t instance, rt_uint8_t queue_size)
+{
+    RT_ASSERT(meta != RT_NULL);
+
+    struct rt_uorb_node *node = (struct rt_uorb_node *)rt_calloc(sizeof(struct rt_uorb_node), 1);
+
+    node->meta             = meta;
+    node->instance         = instance;
+    node->queue_size       = queue_size;
+    node->generation       = 0;
+    node->advertised       = 0;
+    node->subscriber_count = 0;
+    node->data_valid       = 0;
+    node->data             = RT_NULL;
+
+    char *name = rt_calloc(RT_NAME_MAX, 1);
+    rt_snprintf(name, RT_NAME_MAX, "%s%d", meta->o_name, instance);
+
+    rt_list_insert_after(_node_list.prev, &node->list);
+
+    // 注册设备
+    rt_uorb_register(node, name, 0, RT_NULL);
+
+    return node;
+}
+
+rt_err_t uorb_node_delete(struct rt_uorb_node *node)
+{
+    return 0;
+}
+
+struct rt_uorb_node *uorb_node_find(const struct orb_metadata *meta, int instance)
+{
+    // 遍历_node_list
+}
+
+int uorb_node_read(struct rt_uorb_node *node, void *data, int *generation)
+{
+    RT_ASSERT(node != RT_NULL);
+    RT_ASSERT(data != RT_NULL);
+
+    if (!node->data)
+    {
+        return 0;
+    }
+
+    if (node->queue_size == 1)
+    {
+        rt_memcpy(data, node->data, node->meta->o_size);
+        if (generation)
+        {
+            generation = node->generation;
+        }
+    }
+    else
+    {
+        // TODO:
+    }
+
+    return node->meta->o_size;
+}
+
+
+int uorb_node_write(struct rt_uorb_node *node, void *data)
+{
+    RT_ASSERT(node != RT_NULL);
+    RT_ASSERT(data != RT_NULL);
+
+    if (!node->data)
+    {
+        const size_t size = node->meta->o_size * node->queue_size;
+        node->data        = rt_calloc(size, 1);
+    }
+
+    if (!node->data)
+    {
+        return -1;
+    }
+
+    rt_memcpy(node->data, (node->meta->o_size * node->generation % node->queue_size), data, node->meta->o_size);
+
+    // callbacks
+    // for ()
+    // {
+
+    // }
+
+    node->data_valid = 1;
+    node->generation++;
+
+    return node->meta->o_size;
+}
+
+orb_subscribe_t orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
+{
+    struct rt_uorb_subscribe *sub = rt_calloc(sizeof(struct rt_uorb_subscribe), 1);
+
+    sub->meta       = meta;
+    sub->instance   = instance;
+    sub->generation = 0;
+    sub->node       = uorb_node_find(meta, instance);
+
+    return sub;
+}
+
+int orb_check(orb_subscribe_t sub, rt_bool_t *updated)
+{
+}
+
+int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
+{
+}
+
+int orb_unsubscribe(int handle)
+{
+}
+
+orb_advertise_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
+                                          unsigned int queue_size)
+{
+}
+
+
+int orb_publish(const struct orb_metadata *meta, orb_advertise_t handle, const void *data)
+{
+}
+
+int orb_unadvertise(orb_advertise_t handle)
+{
+}

components/utilities/uORB/uORB.h

@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2006-2024, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2024-09-12     latercomer   the first version
+ */
+
+
+#ifndef _UORB_H_
+#define _UORB_H_
+
+
+#include <rtthread.h>
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif // __cplusplus
+
+
+/**
+ * Object metadata.
+ */
+struct orb_metadata
+{
+    const char    *o_name;            /**< unique object name */
+    const uint16_t o_size;            /**< object size */
+    const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */
+    const char    *o_fields;          /**< semicolon separated list of fields (with type) */
+    uint8_t        o_id;              /**< ORB_ID enum */
+};
+
+typedef const struct orb_metadata *orb_id_t;
+
+
+/**
+ * Generates a pointer to the uORB metadata structure for
+ * a given topic.
+ *
+ * The topic must have been declared previously in scope
+ * with ORB_DECLARE().
+ *
+ * @param _name		The name of the topic.
+ */
+#define ORB_ID(_name) &__orb_##_name
+
+/**
+ * Declare (prototype) the uORB metadata for a topic (used by code generators).
+ *
+ * @param _name		The name of the topic.
+ */
+#if defined(__cplusplus)
+#define ORB_DECLARE(_name) extern "C" const struct orb_metadata __orb_##_name
+#else
+#define ORB_DECLARE(_name) extern const struct orb_metadata __orb_##_name
+#endif //__cplusplus
+
+
+/**
+ * Define (instantiate) the uORB metadata for a topic.
+ *
+ * The uORB metadata is used to help ensure that updates and
+ * copies are accessing the right data.
+ *
+ * Note that there must be no more than one instance of this macro
+ * for each topic.
+ *
+ * @param _name		The name of the topic.
+ * @param _struct	The structure the topic provides.
+ * @param _size_no_padding	Struct size w/o padding at the end
+ * @param _fields	All fields in a semicolon separated list e.g: "float[3] position;bool armed"
+ * @param _orb_id_enum	ORB ID enum e.g.: ORB_ID::vehicle_status
+ */
+#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields, _orb_id_enum) \
+    const struct orb_metadata __orb_##_name = {                             \
+        #_name,                                                             \
+        sizeof(_struct),                                                    \
+        _size_no_padding,                                                   \
+        _fields,                                                            \
+        _orb_id_enum,                                                       \
+    };                                                                      \
+    struct hack
+
+
+/**
+ * ORB topic advertiser handle.
+ *
+ * Advertiser handles are global; once obtained they can be shared freely
+ * and do not need to be closed or released.
+ *
+ * This permits publication from interrupt context and other contexts where
+ * a file-descriptor-based handle would not otherwise be in scope for the
+ * publisher.
+ */
+typedef void *orb_advertise_t;
+typedef void *orb_subscribe_t;
+
+
+struct rt_uorb_node
+{
+    rt_list_t                  list;
+    const struct orb_metadata *meta;
+    rt_uint8_t                 instance;
+    rt_uint8_t                 queue_size;
+    rt_uint32_t                generation;
+    // rt_list_t          callbacks;
+    rt_bool_t   advertised;
+    rt_uint8_t  subscriber_count;
+    rt_bool_t   data_valid;
+    rt_uint8_t *data;
+};
+
+struct rt_uorb_subscribe
+{
+    const struct orb_metadata *meta;
+    rt_uint8_t                 instance;
+    rt_tick_t                  update_interval;
+
+    struct rt_uorb_device *node;
+    rt_uint32_t            generation;
+    rt_tick_t              last_update;
+    rt_bool_t              callback_registered;
+};
+
+
+#ifdef __cplusplus
+}
+#endif // __cplusplus
+
+
+#endif // _UORB_H_