✨ feat(uorb test): 增加了uorb的单元测试

apps/controller/manual_control/ManualControlSelector.cpp

@@ -29,6 +29,9 @@ void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_
     const bool start_using_new_input = !_setpoint.valid;
 
     // Switch to new input if it's valid and we don't already have a valid one
+    bool ret = isInputValid(input, now);
+    rt_kprintf("%d, %d, %d\n", ret, update_existing_input, start_using_new_input); // 1 1 0
+
     if (isInputValid(input, now) && (update_existing_input || start_using_new_input)) {
         _setpoint           = input;
         _setpoint.valid     = true;
@@ -53,7 +56,19 @@ bool ManualControlSelector::isInputValid(const manual_control_setpoint_s &input,
     const bool source_any_matched     = (_rc_in_mode == 2);
     const bool source_first_matched   = (_rc_in_mode == 3) && (input.data_source == _first_valid_source || _first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN);
 
-    return sample_from_the_past && sample_newer_than_timeout && (source_rc_matched || source_mavlink_matched || source_any_matched || source_first_matched);
+    // // 正常情况下返回 true
+    // LOG_I("all ret: %d, %d, %d, %d, %d, %d",
+    //       sample_from_the_past,      // ok
+    //       sample_newer_than_timeout, // error
+    //       source_rc_matched,
+    //       source_mavlink_matched,
+    //       source_any_matched,
+    //       source_first_matched);
+
+    // LOG_I("now: %llu, input.timestamp_sample: %llu, _timeout: %llu", now, input.timestamp_sample, _timeout);
+
+    return (sample_from_the_past && sample_newer_than_timeout
+            && (source_rc_matched || source_mavlink_matched || source_any_matched || source_first_matched));
 }
 
 manual_control_setpoint_s &ManualControlSelector::setpoint() {

apps/estimator/ekf2_fake/ekf2_fake.hpp

@@ -16,7 +16,7 @@ using namespace nextpilot;
 using namespace time_literals;
 using namespace nextpilot::global_params;
 
-// #define USING_COMMANDER_FAKE
+#define USING_COMMANDER_FAKE
 
 class EKF2FAKE {
 public:

apps/utest/test_uorb/Kconfig

@@ -0,0 +1,6 @@
+menuconfig TEST_UORB
+    bool "test uorb"
+    default n
+
+if TEST_UORB
+endif

apps/utest/test_uorb/SConscript

@@ -0,0 +1,18 @@
+import os
+import rtconfig
+from building import *
+
+cwd = GetCurrentDir()
+group = []
+
+src = Glob("*.c") + Glob("*.cpp")
+inc = [cwd]
+
+group += DefineGroup("utest", src, depend=[""], CPPPATH=inc)
+
+for d in os.listdir(cwd):
+    path = os.path.join(cwd, d)
+    if os.path.isfile(os.path.join(path, "SConscript")):
+        group += SConscript(os.path.join(d, "SConscript"))
+
+Return("group")

apps/utest/test_uorb/test_uorb_main.cpp

@@ -0,0 +1,49 @@
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
+ *
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
+
+#define LOG_TAG "uorb_test_main"
+#define LOG_LVL LOG_LVL_INFO
+
+#include "nextpilot.h"
+#include "uorb_pub.hpp"
+#include "uorb_sub.hpp"
+
+char      _ut_pub_stack[1024], _ut_sub_stack[1024];
+rt_thread _ut_pub_handle, _ut_sub_handle;
+
+void ut_pub_entry(void *param) {
+    while (1) {
+        uorb_pub::publish_attitude();
+        uorb_pub::publish_sensor_accel();
+        uorb_pub::publish_sensor_gyro();
+        rt_thread_mdelay(1000);
+    }
+}
+
+void ut_sub_entry(void *param) {
+    while (1) {
+        uorb_sub::subscribe_attitude();
+        uorb_sub::copy_sensor_accel();
+        uorb_sub::subscription_interval_sensor_gyro();
+        rt_thread_mdelay(1000);
+    }
+}
+
+int uorb_test_start() {
+    rt_err_t ret = RT_EOK;
+    ret          = rt_thread_init(&_ut_pub_handle, "uorb_pub", ut_pub_entry, nullptr, &_ut_pub_stack[0], sizeof(_ut_pub_stack), 10, 5);
+    ret          = rt_thread_init(&_ut_sub_handle, "uorb_sub", ut_sub_entry, nullptr, &_ut_sub_stack[0], sizeof(_ut_sub_stack), 10, 5);
+    rt_thread_startup(&_ut_pub_handle);
+    rt_thread_startup(&_ut_sub_handle);
+    LOG_I("uorb test start");
+    return ret;
+}
+
+INIT_APP_EXPORT(uorb_test_start);

apps/utest/test_uorb/uorb_pub.hpp

@@ -0,0 +1,64 @@
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
+ *
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
+
+#pragma once
+
+namespace uorb_pub {
+uORB::PublicationData<vehicle_attitude_s> _vehicle_attitude_pub{ORB_ID(vehicle_attitude)};
+uORB::PublicationData<sensor_accel_s>     _sensor_accel_pub{ORB_ID(sensor_accel)};
+uORB::PublicationData<sensor_gyro_s>      _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+
+rt_inline void publish_attitude() {
+    vehicle_attitude_s att_pub{};
+    att_pub.timestamp        = 123456789;
+    att_pub.timestamp_sample = 123456789;
+    for (int i = 0; i < 4; i++) {
+        att_pub.q[i]             = i;
+        att_pub.delta_q_reset[i] = i;
+    }
+    att_pub.quat_reset_counter = 100;
+    _vehicle_attitude_pub.publish(att_pub);
+};
+
+rt_inline void publish_sensor_accel() {
+    sensor_accel_s sensor_accel{};
+    sensor_accel.timestamp        = 147258369;
+    sensor_accel.timestamp_sample = 147258369;
+    sensor_accel.device_id        = 88;
+    sensor_accel.x                = 100;
+    sensor_accel.y                = 200;
+    sensor_accel.z                = 300;
+    sensor_accel.temperature      = 400;
+    sensor_accel.error_count      = 500;
+    for (int i = 0; i < 3; i++) {
+        sensor_accel.clip_counter[i] = i * 100;
+    }
+    sensor_accel.samples = 123;
+    _sensor_accel_pub.publish(sensor_accel);
+}
+
+rt_inline void publish_sensor_gyro() {
+    sensor_gyro_s sensor_gyro{};
+    sensor_gyro.timestamp        = 147258369;
+    sensor_gyro.timestamp_sample = 147258369;
+    sensor_gyro.device_id        = 88;
+    sensor_gyro.x                = 100;
+    sensor_gyro.y                = 200;
+    sensor_gyro.z                = 300;
+    sensor_gyro.temperature      = 400;
+    sensor_gyro.error_count      = 500;
+    for (int i = 0; i < 3; i++) {
+        sensor_gyro.clip_counter[i] = i * 100;
+    }
+    sensor_gyro.samples = 123;
+    _sensor_gyro_pub.publish(sensor_gyro);
+}
+
+} // namespace uorb_pub

apps/utest/test_uorb/uorb_sub.hpp

@@ -0,0 +1,77 @@
+/*****************************************************************
+ *     _   __             __   ____   _  __        __
+ *    / | / /___   _  __ / /_ / __ \ (_)/ /____   / /_
+ *   /  |/ // _ \ | |/_// __// /_/ // // // __ \ / __/
+ *  / /|  //  __/_>  < / /_ / ____// // // /_/ // /_
+ * /_/ |_/ \___//_/|_| \__//_/    /_//_/ \____/ \__/
+ *
+ * Copyright All Reserved © 2015-2024 NextPilot Development Team
+ ******************************************************************/
+
+#pragma once
+
+using namespace time_literals;
+
+namespace uorb_sub {
+uORB::Subscription         _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+uORB::Subscription         _sensor_accel_sub{ORB_ID(sensor_accel)};
+uORB::SubscriptionInterval _sensor_gyro_sub{ORB_ID(sensor_gyro), 1_s};
+
+rt_inline void subscribe_attitude() {
+    vehicle_attitude_s att_sub{};
+    if (_vehicle_attitude_sub.update(&att_sub)) {
+        RT_ASSERT(att_sub.timestamp != 123456789);
+        RT_ASSERT(att_sub.timestamp_sample != 123456789);
+        for (int i = 0; i < 4; i++) {
+            RT_ASSERT(att_sub.q[i] != i);
+            RT_ASSERT(att_sub.delta_q_reset[i] != i);
+        }
+        RT_ASSERT(att_sub.quat_reset_counter != 100);
+    }
+}
+
+rt_inline void copy_sensor_accel() {
+    sensor_accel_s sensor_accel{};
+    if (_sensor_accel_sub.updated()) {
+        const unsigned last_generation = _sensor_accel_sub.get_last_generation();
+        if (_sensor_accel_sub.copy(&sensor_accel)) {
+            RT_ASSERT(last_generation != last_generation + 1);
+            RT_ASSERT(sensor_accel.timestamp != 147258369);
+            RT_ASSERT(sensor_accel.timestamp_sample != 147258369);
+            RT_ASSERT(sensor_accel.device_id != 88);
+            RT_ASSERT(sensor_accel.x != 100);
+            RT_ASSERT(sensor_accel.y != 200);
+            RT_ASSERT(sensor_accel.z != 300);
+            RT_ASSERT(sensor_accel.temperature != 400);
+            RT_ASSERT(sensor_accel.error_count != 500);
+            for (int i = 0; i < 3; i++) {
+                RT_ASSERT(sensor_accel.clip_counter[i] != i * 100);
+            }
+            RT_ASSERT(sensor_accel.samples != 123);
+        }
+    }
+}
+
+rt_inline void subscription_interval_sensor_gyro() {
+    sensor_gyro_s sensor_gyro{};
+    if (_sensor_gyro_sub.updated()) {
+        const unsigned last_generation = _sensor_gyro_sub.get_last_generation();
+        if (_sensor_gyro_sub.copy(&sensor_gyro)) {
+            RT_ASSERT(last_generation != last_generation + 1);
+            RT_ASSERT(sensor_gyro.timestamp != 147258369);
+            RT_ASSERT(sensor_gyro.timestamp_sample != 147258369);
+            RT_ASSERT(sensor_gyro.device_id != 88);
+            RT_ASSERT(sensor_gyro.x != 100);
+            RT_ASSERT(sensor_gyro.y != 200);
+            RT_ASSERT(sensor_gyro.z != 300);
+            RT_ASSERT(sensor_gyro.temperature != 400);
+            RT_ASSERT(sensor_gyro.error_count != 500);
+            for (int i = 0; i < 3; i++) {
+                RT_ASSERT(sensor_gyro.clip_counter[i] != i * 100);
+            }
+            RT_ASSERT(sensor_gyro.samples != 123);
+        }
+    }
+}
+
+} // namespace uorb_sub

bsps/px4/fmu-v5/config/utest.config

@@ -1438,6 +1438,7 @@ CONFIG_BOARD_CONSTRAINED_FLASH=y
 # NextPilot Test Config
 #
 CONFIG_TEST_CREATE_THREAD=y
+CONFIG_TEST_UORB=y
 # end of NextPilot Test Config
 # end of Nextpilot Firmware Config