fmu-v5调试：进行workq+orb调试，采用hrt_delay，目前正常

apps/utest/test_workq/workq_main.cpp

@@ -1,30 +1,104 @@
+#define LOG_TAG "utest_wq"
+#define LOG_LVL LOG_LVL_INFO
+
 #include <nextpilot.h>
 #include <workq/WorkItemScheduled.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_angular_velocity.h>
 
 using namespace time_literals;
 using namespace nextpilot;
 
+static wq_config_t sim_imu{"wq:sim_imu", 4096 * 10, 0};
 static wq_config_t sim_rate_ctrl{"wq:sim_rate_ctrl", 4096 * 10, 0};
 
 /**
- * @brief
+ * @brief 模拟IMU，用于发布角速率数据
+ *
+ */
+class SimIMU : public ModuleCommand<SimIMU, 6>, public nextpilot::WorkItemScheduled {
+public:
+    SimIMU(int instance) :
+        WorkItemScheduled(MODULE_NAME, sim_rate_ctrl),
+        _instance(instance) {
+    }
+
+    ~SimIMU() {
+    }
+
+    /** @see ModuleCommand */
+    static SimIMU *instantiate(int argc, char *argv[]) {
+        int     inst     = atoi(argv[1]);
+        SimIMU *instance = new SimIMU(inst);
+        return instance;
+    }
+
+    /** @see ModuleCommand */
+    static int custom_command(int argc, char *argv[]) {
+        return 0;
+    }
+
+    /** @see ModuleCommand */
+    static int print_usage(const char *reason = nullptr) {
+        return 0;
+    }
+
+    int init() override {
+        ScheduleOnInterval(5_ms); // 200 Hz
+        return 0;
+    }
+
+    void Run() override {
+        hrt_abstime now_us        = hrt_absolute_time();
+        _vangvel.timestamp        = now_us;
+        _vangvel.timestamp_sample = now_us;
+        for (int axis = 0; axis < 3; axis++) {
+            _vangvel.xyz[axis]            = 1.0f;
+            _vangvel.xyz_derivative[axis] = 0.0f;
+        }
+        _vangvel_pub.publish(_vangvel);
+
+
+        //
+        static int cnt = 0;
+        if ((_instance == 0) && (cnt % 10 == 0)) {
+            rt_tick_t current_tick = rt_tick_get();
+            rt_kprintf("[SimIMU]>>>instance=%d, t=%ld, dt=%ld, cnt=%d\n", _instance, current_tick, current_tick - _last_tick, cnt);
+            _last_tick = current_tick;
+        }
+        cnt++;
+    }
+
+private:
+    int       _instance{};
+    rt_tick_t _last_tick{};
+
+    //
+    struct vehicle_angular_velocity_s _vangvel;
+
+    uORB::Publication<vehicle_angular_velocity_s> _vangvel_pub{ORB_ID(vehicle_angular_velocity)};
+};
+
+/**
+ * @brief 模拟角速率控制，订阅角速率
  *
  * ModuleCommand<SimRateControl, 6>: 最多创建6个实例
  */
-class SimRateControl : public ModuleCommand<SimRateControl, 6>, public nextpilot::WorkItemScheduled {
+class SimRateControl : public ModuleCommand<SimRateControl, 6>, public nextpilot::WorkItem {
 public:
-    SimRateControl(int idx) :
-        WorkItemScheduled(MODULE_NAME, sim_rate_ctrl) {
-        _idx = idx;
+    SimRateControl(int instance) :
+        WorkItem(MODULE_NAME, sim_rate_ctrl) {
+        _instance = instance;
     }
 
     ~SimRateControl() {
     }
 
     /** @see ModuleCommand */
     static SimRateControl *instantiate(int argc, char *argv[]) {
-        int             idx      = atoi(argv[1]);
-        SimRateControl *instance = new SimRateControl(idx);
+        int             inst     = atoi(argv[1]);
+        SimRateControl *instance = new SimRateControl(inst);
         return instance;
     }
 
@@ -39,13 +113,21 @@ class SimRateControl : public ModuleCommand<SimRateControl, 6>, public nextpilot
     }
 
     int init() override {
-        ScheduleOnInterval(100_ms); // 8 Hz
+        if (!_vehicle_angular_velocity_sub.register_callback()) {
+            PX4_ERR("callback registration failed");
+            return -1;
+        }
         return 0;
     }
 
     rt_tick_t _last_tick;
 
 private:
+    int _instance;
+    //
+    uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
+
+    /* */
     void Run() override {
         static uint32_t cnt   = 0;
         static double   total = 0.0f;
@@ -54,39 +136,43 @@ class SimRateControl : public ModuleCommand<SimRateControl, 6>, public nextpilot
         // rt_thread_mdelay(1000);
 
         /* 1. 计时等待 */
-        rt_tick_t start = rt_tick_get();
-        while (rt_tick_get() - start <= 5) {
-            total += 1.0;
-        }
+        // rt_tick_t start = rt_tick_get();
+        // while (rt_tick_get() - start <= 5) {
+        //     total += 1.0;
+        // }
 
         /* 2. 执行一些很耗时的操作*/
         // for (uint32_t sleep = 0; sleep < 500000; ++sleep) {
         //     total += 1.0 / sleep;
         // }
 
-        if ((_idx == 0) && (cnt % 10 == 0)) {
+        if ((_instance == 0) && (cnt % 10 == 0)) {
             rt_tick_t current_tick = rt_tick_get();
-            rt_kprintf("======idx=%d, t=%ld, dt=%ld, cnt=%d\n", _idx, current_tick, current_tick - _last_tick, cnt);
-            _last_tick = rt_tick_get();
+            rt_kprintf("[SimRateControl]>>>instance=%d, t=%ld, dt=%ld, cnt=%d\n", _instance, current_tick, current_tick - _last_tick, cnt);
+            _last_tick = current_tick;
         }
         cnt++;
     }
-
-    int _idx;
 };
 
+// 启动
+int sim_imu_start() {
+    int         ret;
+    const char *argv[] = {"sim_imu", "start", "0"};
+    int         argc   = sizeof(argv) / sizeof(argv[0]);
+    ret                = SimIMU::main(argc, (char **)argv);
+    return ret;
+}
+
+// INIT_APP_EXPORT(sim_imu_start);
+
 int sim_rate_ctrl_start() {
     int         ret;
     const char *argv[] = {"sim_rate_control", "start", "0"};
     int         argc   = sizeof(argv) / sizeof(argv[0]);
     ret                = SimRateControl::main(argc, (char **)argv);
 
-
-    // const char *argv1[] = {"sim_rate_control1", "start", "1"};
-    // int         argc1   = sizeof(argv1) / sizeof(argv1[0]);
-    // ret                 = SimRateControl::main(argc1, (char **)argv1);
-
     return ret;
 }
 
-INIT_APP_EXPORT(sim_rate_ctrl_start);
+// INIT_APP_EXPORT(sim_rate_ctrl_start);

apps/utest/test_workq/workq_scheduled.cpp

@@ -0,0 +1,95 @@
+#define LOG_TAG "utest_wq"
+#define LOG_LVL LOG_LVL_INFO
+
+#include <nextpilot.h>
+#include <workq/WorkItemScheduled.hpp>
+
+using namespace time_literals;
+using namespace nextpilot;
+
+static wq_config_t workq_scheduled{"wq:workq_scheduled", 4096 * 10, 0};
+
+/**
+ * @brief
+ *
+ * ModuleCommand<UTestWorkqScheduled, 6>: 最多创建6个实例
+ */
+class UTestWorkqScheduled : public ModuleCommand<UTestWorkqScheduled, 6>, public nextpilot::WorkItemScheduled {
+public:
+    UTestWorkqScheduled(int idx) :
+        WorkItemScheduled(MODULE_NAME, workq_scheduled) {
+        instance = idx;
+    }
+
+    ~UTestWorkqScheduled() {
+    }
+
+    /** @see ModuleCommand */
+    static UTestWorkqScheduled *instantiate(int argc, char *argv[]) {
+        int                  idx      = atoi(argv[1]);
+        UTestWorkqScheduled *instance = new UTestWorkqScheduled(idx);
+        return instance;
+    }
+
+    /** @see ModuleCommand */
+    static int custom_command(int argc, char *argv[]) {
+        return 0;
+    }
+
+    /** @see ModuleCommand */
+    static int print_usage(const char *reason = nullptr) {
+        return 0;
+    }
+
+    int init() override {
+        ScheduleOnInterval(100_ms); // 8 Hz
+        return 0;
+    }
+
+    rt_tick_t _last_tick;
+
+private:
+    void Run() override {
+        static uint32_t cnt   = 0;
+        static double   total = 0.0f;
+        if (should_exit()) {
+        }
+        // rt_thread_mdelay(1000);
+
+        /* 1. 计时等待 */
+        rt_tick_t start = rt_tick_get();
+        // while (rt_tick_get() - start <= 5) {
+        //     total += 1.0;
+        // }
+
+        /* 2. 执行一些很耗时的操作*/
+        // for (uint32_t sleep = 0; sleep < 500000; ++sleep) {
+        //     total += 1.0 / sleep;
+        // }
+
+        if (cnt % 10 == 0) {
+            rt_tick_t current_tick = rt_tick_get();
+            rt_kprintf("[wq_scheduled]>>>instance=%d, t=%ld, dt=%ld, cnt=%d\n", instance, current_tick, current_tick - _last_tick, cnt);
+            _last_tick = current_tick;
+        }
+        cnt++;
+    }
+
+    int instance;
+};
+
+int workq_scheduled_start() {
+    int         ret;
+    const char *argv[] = {"utest_workq_scheduled", "start", "0"};
+    int         argc   = sizeof(argv) / sizeof(argv[0]);
+    ret                = UTestWorkqScheduled::main(argc, (char **)argv);
+
+    // 目前还无法实现两个实例一起运行
+    const char *argv1[] = {"utest_workq_scheduled1", "start", "1"};
+    int         argc1   = sizeof(argv1) / sizeof(argv1[0]);
+    ret                 = UTestWorkqScheduled::main(argc1, (char **)argv1);
+
+    return ret;
+}
+
+INIT_APP_EXPORT(workq_scheduled_start);

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

@@ -608,7 +608,26 @@ CONFIG_ULOG_BACKEND_USING_FILE=y
 #
 # CONFIG_PKG_USING_RT_MEMCPY_CM is not set
 # CONFIG_PKG_USING_RT_KPRINTF_THREADSAFE is not set
-# CONFIG_PKG_USING_RT_VSNPRINTF_FULL is not set
+CONFIG_PKG_USING_RT_VSNPRINTF_FULL=y
+CONFIG_PKG_RT_VSNPRINTF_FULL_PATH="/packages/system/enhanced-kservice/rt_vsnprintf_full"
+CONFIG_PKG_VSNPRINTF_SUPPORT_DECIMAL_SPECIFIERS=y
+CONFIG_PKG_VSNPRINTF_SUPPORT_EXPONENTIAL_SPECIFIERS=y
+CONFIG_PKG_VSNPRINTF_SUPPORT_WRITEBACK_SPECIFIER=y
+CONFIG_PKG_VSNPRINTF_SUPPORT_LONG_LONG=y
+CONFIG_PKG_VSNPRINTF_CHECK_FOR_NUL_IN_FORMAT_SPECIFIER=y
+# CONFIG_PKG_VSNPRINTF_SUPPORT_MSVC_STYLE_INTEGER_SPECIFIERS is not set
+CONFIG_PKG_VSNPRINTF_INTEGER_BUFFER_SIZE=32
+CONFIG_PKG_VSNPRINTF_DECIMAL_BUFFER_SIZE=32
+CONFIG_PKG_VSNPRINTF_DEFAULT_FLOAT_PRECISION=6
+CONFIG_PKG_VSNPRINTF_MAX_INTEGRAL_DIGITS_FOR_DECIMAL=9
+CONFIG_PKG_VSNPRINTF_LOG10_TAYLOR_TERMS=4
+# CONFIG_RT_VSNPRINTF_FULL_REPLACING_SPRINTF is not set
+# CONFIG_RT_VSNPRINTF_FULL_REPLACING_SNPRINTF is not set
+# CONFIG_RT_VSNPRINTF_FULL_REPLACING_PRINTF is not set
+# CONFIG_RT_VSNPRINTF_FULL_REPLACING_VSPRINTF is not set
+# CONFIG_RT_VSNPRINTF_FULL_REPLACING_VSNPRINTF is not set
+CONFIG_PKG_USING_RT_VSNPRINTF_FULL_LATEST_VERSION=y
+CONFIG_PKG_RT_VSNPRINTF_FULL_VER="latest"
 # end of enhanced kernel services
 
 #
@@ -1435,11 +1454,12 @@ CONFIG_BOARD_CONSTRAINED_FLASH=y
 # end of Nextpilot Telemetry Config
 
 #
-# NextPilot Test Config
+# NextPilot UnitTest Config
 #
 CONFIG_TEST_CREATE_THREAD=y
 CONFIG_TEST_UORB=y
-# end of NextPilot Test Config
+CONFIG_TEST_WORK_QUEUE=y
+# end of NextPilot UnitTest Config
 # end of Nextpilot Firmware Config
 
 #
@@ -1461,6 +1481,7 @@ CONFIG_HRT_USING_SYSTICK=y
 # CONFIG_HRT_USING_DWT is not set
 # CONFIG_HRT_USING_TSG is not set
 # CONFIG_HRT_USING_RTC is not set
+# CONFIG_HRT_USING_OTHER is not set
 CONFIG_HRT_USING_OSTIMER=y
 CONFIG_PKG_USING_LIBCRC=y
 # CONFIG_PKG_USING_LOAD_MONITOR is not set

bsps/px4/fmu-v5/rtconfig.h

@@ -269,6 +269,18 @@
 
 /* enhanced kernel services */
 
+#define PKG_USING_RT_VSNPRINTF_FULL
+#define PKG_VSNPRINTF_SUPPORT_DECIMAL_SPECIFIERS
+#define PKG_VSNPRINTF_SUPPORT_EXPONENTIAL_SPECIFIERS
+#define PKG_VSNPRINTF_SUPPORT_WRITEBACK_SPECIFIER
+#define PKG_VSNPRINTF_SUPPORT_LONG_LONG
+#define PKG_VSNPRINTF_CHECK_FOR_NUL_IN_FORMAT_SPECIFIER
+#define PKG_VSNPRINTF_INTEGER_BUFFER_SIZE 32
+#define PKG_VSNPRINTF_DECIMAL_BUFFER_SIZE 32
+#define PKG_VSNPRINTF_DEFAULT_FLOAT_PRECISION 6
+#define PKG_VSNPRINTF_MAX_INTEGRAL_DIGITS_FOR_DECIMAL 9
+#define PKG_VSNPRINTF_LOG10_TAYLOR_TERMS 4
+#define PKG_USING_RT_VSNPRINTF_FULL_LATEST_VERSION
 /* end of enhanced kernel services */
 
 /* acceleration: Assembly language or algorithmic acceleration packages */
@@ -473,11 +485,12 @@
 
 /* end of Nextpilot Telemetry Config */
 
-/* NextPilot Test Config */
+/* NextPilot UnitTest Config */
 
 #define TEST_CREATE_THREAD
 #define TEST_UORB
-/* end of NextPilot Test Config */
+#define TEST_WORK_QUEUE
+/* end of NextPilot UnitTest Config */
 /* end of Nextpilot Firmware Config */
 
 /* Nextpilot Packages Config */