From c3884b5bc1949a2bbdf61c40deef9d0799035152 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 17 May 2021 20:26:25 -0400 Subject: [PATCH] fake_imu (formally fake_gyro) updates for testing gyro filtering - fake_imu now publishes sine sweeps over 10 seconds - accel is also published so that fake IMU can be selected when the only option --- boards/airmind/mindpx-v2/default.cmake | 2 +- boards/av/x-v1/default.cmake | 2 +- boards/beaglebone/blue/default.cmake | 2 +- boards/cuav/nora/default.cmake | 2 +- boards/cuav/nora/test.cmake | 2 +- boards/cuav/x7pro/default.cmake | 2 +- boards/cuav/x7pro/test.cmake | 2 +- boards/cubepilot/cubeorange/default.cmake | 2 +- boards/cubepilot/cubeorange/test.cmake | 2 +- boards/cubepilot/cubeyellow/default.cmake | 2 +- boards/cubepilot/cubeyellow/test.cmake | 2 +- boards/emlid/navio2/default.cmake | 2 +- boards/holybro/durandal-v1/default.cmake | 2 +- boards/holybro/durandal-v1/test.cmake | 2 +- boards/holybro/pix32v5/default.cmake | 2 +- boards/holybro/pix32v5/test.cmake | 2 +- boards/modalai/fc-v1/default.cmake | 2 +- boards/modalai/fc-v1/rtps.cmake | 2 +- boards/modalai/fc-v1/test.cmake | 2 +- boards/mro/ctrl-zero-f7-oem/default.cmake | 2 +- boards/mro/ctrl-zero-f7/default.cmake | 2 +- boards/mro/ctrl-zero-h7-oem/default.cmake | 2 +- boards/mro/ctrl-zero-h7/default.cmake | 2 +- boards/mro/pixracerpro/default.cmake | 2 +- boards/mro/x21-777/default.cmake | 2 +- boards/mro/x21/default.cmake | 2 +- boards/nxp/fmuk66-e/default.cmake | 2 +- boards/nxp/fmuk66-e/rtps.cmake | 2 +- boards/nxp/fmuk66-e/socketcan.cmake | 2 +- boards/nxp/fmuk66-v3/default.cmake | 2 +- boards/nxp/fmuk66-v3/rtps.cmake | 2 +- boards/nxp/fmuk66-v3/socketcan.cmake | 2 +- boards/nxp/fmuk66-v3/test.cmake | 2 +- boards/nxp/fmurt1062-v1/default.cmake | 2 +- boards/px4/fmu-v2/default.cmake | 2 +- boards/px4/fmu-v3/default.cmake | 2 +- boards/px4/fmu-v3/test.cmake | 2 +- boards/px4/fmu-v4/default.cmake | 2 +- boards/px4/fmu-v4/test.cmake | 2 +- boards/px4/fmu-v4pro/default.cmake | 2 +- boards/px4/fmu-v4pro/test.cmake | 2 +- boards/px4/fmu-v5/ctrlalloc.cmake | 2 +- boards/px4/fmu-v5/debug.cmake | 2 +- boards/px4/fmu-v5/default.cmake | 2 +- boards/px4/fmu-v5/optimized.cmake | 2 +- boards/px4/fmu-v5/rtps.cmake | 2 +- boards/px4/fmu-v5/stackcheck.cmake | 2 +- boards/px4/fmu-v5/test.cmake | 2 +- boards/px4/fmu-v5/uavcanv0periph.cmake | 2 +- boards/px4/fmu-v5/uavcanv1.cmake | 2 +- boards/px4/fmu-v5x/base_phy_DP83848C.cmake | 2 +- boards/px4/fmu-v5x/default.cmake | 2 +- boards/px4/fmu-v5x/test.cmake | 2 +- boards/px4/fmu-v6u/default.cmake | 2 +- boards/px4/fmu-v6u/test.cmake | 2 +- boards/px4/fmu-v6x/default.cmake | 2 +- boards/px4/fmu-v6x/test.cmake | 2 +- boards/px4/raspberrypi/default.cmake | 2 +- boards/px4/sitl/ctrlalloc.cmake | 2 +- boards/px4/sitl/default.cmake | 2 +- boards/px4/sitl/nolockstep.cmake | 2 +- boards/px4/sitl/rtps.cmake | 2 +- boards/px4/sitl/test.cmake | 2 +- boards/scumaker/pilotpi/arm64.cmake | 2 +- boards/spracing/h7extreme/default.cmake | 2 +- platforms/posix/cmake/sitl_tests.cmake | 14 +++ .../SITL/init/test/test_imu_filtering | 51 +++++++++++ .../{fake_gyro => fake_imu}/CMakeLists.txt | 9 +- .../FakeGyro.cpp => fake_imu/FakeImu.cpp} | 86 +++++++++++++------ .../FakeGyro.hpp => fake_imu/FakeImu.hpp} | 15 ++-- 70 files changed, 206 insertions(+), 99 deletions(-) create mode 100644 posix-configs/SITL/init/test/test_imu_filtering rename src/examples/{fake_gyro => fake_imu}/CMakeLists.txt (95%) rename src/examples/{fake_gyro/FakeGyro.cpp => fake_imu/FakeImu.cpp} (53%) rename src/examples/{fake_gyro/FakeGyro.hpp => fake_imu/FakeImu.hpp} (87%) diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index c0e41eb833..3ccd60f3e6 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -117,7 +117,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 5a922c19f3..03d5f9ef93 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -114,7 +114,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index 70f22a1fc8..be1f8976a5 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -95,7 +95,7 @@ px4_add_board( EXAMPLES dyn_hello # dynamically loading modules example fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/cuav/nora/default.cmake b/boards/cuav/nora/default.cmake index f7d6066136..9e8ad80133 100644 --- a/boards/cuav/nora/default.cmake +++ b/boards/cuav/nora/default.cmake @@ -122,7 +122,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/cuav/nora/test.cmake b/boards/cuav/nora/test.cmake index f6e63a088a..9c16da950d 100644 --- a/boards/cuav/nora/test.cmake +++ b/boards/cuav/nora/test.cmake @@ -125,7 +125,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/cuav/x7pro/default.cmake b/boards/cuav/x7pro/default.cmake index 421f8aaea6..63ffab2d59 100644 --- a/boards/cuav/x7pro/default.cmake +++ b/boards/cuav/x7pro/default.cmake @@ -123,7 +123,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/cuav/x7pro/test.cmake b/boards/cuav/x7pro/test.cmake index 8ab832f90e..66fecef2bb 100644 --- a/boards/cuav/x7pro/test.cmake +++ b/boards/cuav/x7pro/test.cmake @@ -126,7 +126,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/cubepilot/cubeorange/default.cmake b/boards/cubepilot/cubeorange/default.cmake index 1f49c0135f..51c6a128d9 100644 --- a/boards/cubepilot/cubeorange/default.cmake +++ b/boards/cubepilot/cubeorange/default.cmake @@ -120,7 +120,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/cubepilot/cubeorange/test.cmake b/boards/cubepilot/cubeorange/test.cmake index 48c2171be6..23cad23a4b 100644 --- a/boards/cubepilot/cubeorange/test.cmake +++ b/boards/cubepilot/cubeorange/test.cmake @@ -122,7 +122,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/cubepilot/cubeyellow/default.cmake b/boards/cubepilot/cubeyellow/default.cmake index cf2cc59078..dbd1851c3c 100644 --- a/boards/cubepilot/cubeyellow/default.cmake +++ b/boards/cubepilot/cubeyellow/default.cmake @@ -118,7 +118,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/cubepilot/cubeyellow/test.cmake b/boards/cubepilot/cubeyellow/test.cmake index 36c84be658..8a06a367b4 100644 --- a/boards/cubepilot/cubeyellow/test.cmake +++ b/boards/cubepilot/cubeyellow/test.cmake @@ -120,7 +120,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index b4fac09e05..f8db8a75a5 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -95,7 +95,7 @@ px4_add_board( EXAMPLES dyn_hello # dynamically loading modules example fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index 4f30ca4802..e9666a2b76 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -118,7 +118,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/holybro/durandal-v1/test.cmake b/boards/holybro/durandal-v1/test.cmake index 2837778cc2..6b2ac41dfa 100644 --- a/boards/holybro/durandal-v1/test.cmake +++ b/boards/holybro/durandal-v1/test.cmake @@ -121,7 +121,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/holybro/pix32v5/default.cmake b/boards/holybro/pix32v5/default.cmake index 08b777024b..98c681422b 100644 --- a/boards/holybro/pix32v5/default.cmake +++ b/boards/holybro/pix32v5/default.cmake @@ -122,7 +122,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/holybro/pix32v5/test.cmake b/boards/holybro/pix32v5/test.cmake index c45ec98b38..ab73ec5a43 100644 --- a/boards/holybro/pix32v5/test.cmake +++ b/boards/holybro/pix32v5/test.cmake @@ -125,7 +125,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index 532eb1d402..cfed7048c7 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -115,7 +115,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/modalai/fc-v1/rtps.cmake b/boards/modalai/fc-v1/rtps.cmake index 48b193229e..3e56f4cf6c 100644 --- a/boards/modalai/fc-v1/rtps.cmake +++ b/boards/modalai/fc-v1/rtps.cmake @@ -115,7 +115,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/modalai/fc-v1/test.cmake b/boards/modalai/fc-v1/test.cmake index 0f15eab4cc..3f0e0c0f3b 100644 --- a/boards/modalai/fc-v1/test.cmake +++ b/boards/modalai/fc-v1/test.cmake @@ -118,7 +118,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/mro/ctrl-zero-f7-oem/default.cmake b/boards/mro/ctrl-zero-f7-oem/default.cmake index fda64a3468..3abfbb2f11 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.cmake +++ b/boards/mro/ctrl-zero-f7-oem/default.cmake @@ -119,7 +119,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index f4141e36ff..2e6f081af5 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -119,7 +119,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/mro/ctrl-zero-h7-oem/default.cmake b/boards/mro/ctrl-zero-h7-oem/default.cmake index 81b9cd8fd9..9a5903c138 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.cmake +++ b/boards/mro/ctrl-zero-h7-oem/default.cmake @@ -117,7 +117,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/mro/ctrl-zero-h7/default.cmake b/boards/mro/ctrl-zero-h7/default.cmake index 534e97bbd1..f586b98422 100644 --- a/boards/mro/ctrl-zero-h7/default.cmake +++ b/boards/mro/ctrl-zero-h7/default.cmake @@ -117,7 +117,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/mro/pixracerpro/default.cmake b/boards/mro/pixracerpro/default.cmake index 775f98625a..7c3a07404b 100644 --- a/boards/mro/pixracerpro/default.cmake +++ b/boards/mro/pixracerpro/default.cmake @@ -118,7 +118,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/mro/x21-777/default.cmake b/boards/mro/x21-777/default.cmake index 2ee9d2e32e..4d619071bd 100644 --- a/boards/mro/x21-777/default.cmake +++ b/boards/mro/x21-777/default.cmake @@ -119,7 +119,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/mro/x21/default.cmake b/boards/mro/x21/default.cmake index 065081a609..f776ba56ec 100644 --- a/boards/mro/x21/default.cmake +++ b/boards/mro/x21/default.cmake @@ -120,7 +120,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/nxp/fmuk66-e/default.cmake b/boards/nxp/fmuk66-e/default.cmake index 35066fcab7..53b0a29631 100644 --- a/boards/nxp/fmuk66-e/default.cmake +++ b/boards/nxp/fmuk66-e/default.cmake @@ -115,7 +115,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/nxp/fmuk66-e/rtps.cmake b/boards/nxp/fmuk66-e/rtps.cmake index 4ce17c52e7..325c939082 100644 --- a/boards/nxp/fmuk66-e/rtps.cmake +++ b/boards/nxp/fmuk66-e/rtps.cmake @@ -116,7 +116,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/nxp/fmuk66-e/socketcan.cmake b/boards/nxp/fmuk66-e/socketcan.cmake index 9f73b26897..4b41341924 100644 --- a/boards/nxp/fmuk66-e/socketcan.cmake +++ b/boards/nxp/fmuk66-e/socketcan.cmake @@ -116,7 +116,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 245125ea2f..7a3c307f85 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -115,7 +115,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/nxp/fmuk66-v3/rtps.cmake b/boards/nxp/fmuk66-v3/rtps.cmake index 4745361c4e..934e0615b3 100644 --- a/boards/nxp/fmuk66-v3/rtps.cmake +++ b/boards/nxp/fmuk66-v3/rtps.cmake @@ -116,7 +116,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/nxp/fmuk66-v3/socketcan.cmake b/boards/nxp/fmuk66-v3/socketcan.cmake index 059571b4bc..7e5d131f72 100644 --- a/boards/nxp/fmuk66-v3/socketcan.cmake +++ b/boards/nxp/fmuk66-v3/socketcan.cmake @@ -116,7 +116,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/nxp/fmuk66-v3/test.cmake b/boards/nxp/fmuk66-v3/test.cmake index c60de35f3b..4e997f16d6 100644 --- a/boards/nxp/fmuk66-v3/test.cmake +++ b/boards/nxp/fmuk66-v3/test.cmake @@ -117,7 +117,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index c00e8e7d73..51048b6569 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -117,7 +117,7 @@ px4_add_board( work_queue EXAMPLES #fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index fb909a8b23..d42509f681 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -127,7 +127,7 @@ px4_add_board( #work_queue EXAMPLES #fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index f2099704e0..f72a5b29c6 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -121,7 +121,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v3/test.cmake b/boards/px4/fmu-v3/test.cmake index 874c548bf7..b6db44292d 100644 --- a/boards/px4/fmu-v3/test.cmake +++ b/boards/px4/fmu-v3/test.cmake @@ -124,7 +124,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 3c8cc66c35..e420738e70 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -119,7 +119,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v4/test.cmake b/boards/px4/fmu-v4/test.cmake index b3184b1b2e..f87dfed600 100644 --- a/boards/px4/fmu-v4/test.cmake +++ b/boards/px4/fmu-v4/test.cmake @@ -122,7 +122,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 2f66bccf01..218fad1cbd 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -119,7 +119,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v4pro/test.cmake b/boards/px4/fmu-v4pro/test.cmake index bf5ac8c217..92719ead4d 100644 --- a/boards/px4/fmu-v4pro/test.cmake +++ b/boards/px4/fmu-v4pro/test.cmake @@ -122,7 +122,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake index 763091e6af..859f8d4bab 100644 --- a/boards/px4/fmu-v5/ctrlalloc.cmake +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -123,7 +123,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v5/debug.cmake b/boards/px4/fmu-v5/debug.cmake index 365ba4610c..d81f579a0c 100644 --- a/boards/px4/fmu-v5/debug.cmake +++ b/boards/px4/fmu-v5/debug.cmake @@ -125,7 +125,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 19b9d24cb7..a7993d236e 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -121,7 +121,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v5/optimized.cmake b/boards/px4/fmu-v5/optimized.cmake index d23fdebe9c..10d69ad912 100644 --- a/boards/px4/fmu-v5/optimized.cmake +++ b/boards/px4/fmu-v5/optimized.cmake @@ -129,7 +129,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index b32fc5a789..be6ef4eb90 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -114,7 +114,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index ca4442fbcf..0880c905bd 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -123,7 +123,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v5/test.cmake b/boards/px4/fmu-v5/test.cmake index ad1c8ae33d..cdbdd348a6 100644 --- a/boards/px4/fmu-v5/test.cmake +++ b/boards/px4/fmu-v5/test.cmake @@ -124,7 +124,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/fmu-v5/uavcanv0periph.cmake b/boards/px4/fmu-v5/uavcanv0periph.cmake index bfb12da01c..dbca72ce3c 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.cmake +++ b/boards/px4/fmu-v5/uavcanv0periph.cmake @@ -124,7 +124,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v5/uavcanv1.cmake b/boards/px4/fmu-v5/uavcanv1.cmake index 3c8fcc78ce..a21c434755 100644 --- a/boards/px4/fmu-v5/uavcanv1.cmake +++ b/boards/px4/fmu-v5/uavcanv1.cmake @@ -122,7 +122,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake index 9d3502d9f0..49d71826c8 100644 --- a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake +++ b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake @@ -122,7 +122,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 52fa96f29c..98579eb1f7 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -123,7 +123,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v5x/test.cmake b/boards/px4/fmu-v5x/test.cmake index 458845e495..6360f7be14 100644 --- a/boards/px4/fmu-v5x/test.cmake +++ b/boards/px4/fmu-v5x/test.cmake @@ -125,7 +125,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v6u/default.cmake b/boards/px4/fmu-v6u/default.cmake index b85e78768f..4550ba9256 100644 --- a/boards/px4/fmu-v6u/default.cmake +++ b/boards/px4/fmu-v6u/default.cmake @@ -118,7 +118,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v6u/test.cmake b/boards/px4/fmu-v6u/test.cmake index 6ee6152ede..226e7f25cd 100644 --- a/boards/px4/fmu-v6u/test.cmake +++ b/boards/px4/fmu-v6u/test.cmake @@ -122,7 +122,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - fake_gyro + fake_imu fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v6x/default.cmake b/boards/px4/fmu-v6x/default.cmake index 2fbc1dc9a3..4821936dd9 100644 --- a/boards/px4/fmu-v6x/default.cmake +++ b/boards/px4/fmu-v6x/default.cmake @@ -122,7 +122,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v6x/test.cmake b/boards/px4/fmu-v6x/test.cmake index e9b5c078fb..786ee745e8 100644 --- a/boards/px4/fmu-v6x/test.cmake +++ b/boards/px4/fmu-v6x/test.cmake @@ -126,7 +126,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index cadd33913c..1523199dec 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -92,7 +92,7 @@ px4_add_board( EXAMPLES dyn_hello # dynamically loading modules example fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/sitl/ctrlalloc.cmake b/boards/px4/sitl/ctrlalloc.cmake index f94773b6ee..45571cff0c 100644 --- a/boards/px4/sitl/ctrlalloc.cmake +++ b/boards/px4/sitl/ctrlalloc.cmake @@ -89,7 +89,7 @@ px4_add_board( EXAMPLES dyn_hello # dynamically loading modules example fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 8c2e8f815e..6bf5e1de3b 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -88,7 +88,7 @@ px4_add_board( EXAMPLES dyn_hello # dynamically loading modules example fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/sitl/nolockstep.cmake b/boards/px4/sitl/nolockstep.cmake index e86fbfffac..308920a272 100644 --- a/boards/px4/sitl/nolockstep.cmake +++ b/boards/px4/sitl/nolockstep.cmake @@ -87,7 +87,7 @@ px4_add_board( EXAMPLES dyn_hello # dynamically loading modules example fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 898adca74b..a446b93f2e 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -86,7 +86,7 @@ px4_add_board( EXAMPLES dyn_hello # dynamically loading modules example fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 117d5b9681..e950e9579d 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -86,7 +86,7 @@ px4_add_board( EXAMPLES dyn_hello # dynamically loading modules example fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/scumaker/pilotpi/arm64.cmake b/boards/scumaker/pilotpi/arm64.cmake index 3e9f0f0261..c4e74856ee 100644 --- a/boards/scumaker/pilotpi/arm64.cmake +++ b/boards/scumaker/pilotpi/arm64.cmake @@ -87,7 +87,7 @@ px4_add_board( EXAMPLES dyn_hello # dynamically loading modules example fake_gps - fake_gyro + fake_imu fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/spracing/h7extreme/default.cmake b/boards/spracing/h7extreme/default.cmake index 67c08daade..30ff1d28e4 100644 --- a/boards/spracing/h7extreme/default.cmake +++ b/boards/spracing/h7extreme/default.cmake @@ -105,7 +105,7 @@ px4_add_board( work_queue EXAMPLES fake_gps - #fake_gyro + #fake_imu #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index ddf6f30362..dd9972fcd5 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -96,6 +96,20 @@ set_tests_properties(sitl-mavlink PROPERTIES PASS_REGULAR_EXPRESSION "ALL TESTS sanitizer_fail_test_on_error(sitl-mavlink) +# IMU filtering +add_test(NAME sitl-imu_filtering + COMMAND $ + -s ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_imu_filtering + -t ${PX4_SOURCE_DIR}/test_data + ${PX4_SOURCE_DIR}/ROMFS/px4fmu_test + WORKING_DIRECTORY ${SITL_WORKING_DIR} +) + +set_tests_properties(sitl-imu_filtering PROPERTIES FAIL_REGULAR_EXPRESSION "FAIL") +set_tests_properties(sitl-imu_filtering PROPERTIES PASS_REGULAR_EXPRESSION "ALL TESTS PASSED") +sanitizer_fail_test_on_error(sitl-imu_filtering) + + # # Shutdown test # add_test(NAME sitl-shutdown diff --git a/posix-configs/SITL/init/test/test_imu_filtering b/posix-configs/SITL/init/test/test_imu_filtering new file mode 100644 index 0000000000..492775ae1e --- /dev/null +++ b/posix-configs/SITL/init/test/test_imu_filtering @@ -0,0 +1,51 @@ +#!/bin/sh +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +. px4-alias.sh + +param load +param set CBRK_SUPPLY_CHK 894281 +param set SYS_RESTART_TYPE 0 + +dataman start + +tone_alarm start + +ver all + +param set IMU_GYRO_RATEMAX 2000 + +param set IMU_GYRO_FFT_EN 1 +param set IMU_GYRO_FFT_MIN 1 +param set IMU_GYRO_FFT_MAX 1000 + +# test values +param set IMU_GYRO_CUTOFF 40 +param set IMU_DGYRO_CUTOFF 30 +#param set IMU_GYRO_NF_FREQ 60 +#param set IMU_GYRO_NF_BW 5 + +# log nearly everything +param set SDLOG_PROFILE 859 + +fake_imu start + +sensors start +gyro_fft start + +logger start -f -t -b 10000 -p sensor_gyro +logger on + +echo "Running for 10 seconds" +sleep 10 + +logger off + +sensors status +uorb top -a -1 +listener sensor_gyro +listener sensor_gyro_fifo + +echo "ALL TESTS PASSED" + +shutdown diff --git a/src/examples/fake_gyro/CMakeLists.txt b/src/examples/fake_imu/CMakeLists.txt similarity index 95% rename from src/examples/fake_gyro/CMakeLists.txt rename to src/examples/fake_imu/CMakeLists.txt index add79c0933..2d037d7387 100644 --- a/src/examples/fake_gyro/CMakeLists.txt +++ b/src/examples/fake_imu/CMakeLists.txt @@ -32,14 +32,15 @@ ############################################################################ px4_add_module( - MODULE modules__fake_gyro - MAIN fake_gyro + MODULE modules__fake_imu + MAIN fake_imu COMPILE_FLAGS ${MAX_CUSTOM_OPT_LEVEL} SRCS - FakeGyro.cpp - FakeGyro.hpp + FakeImu.cpp + FakeImu.hpp DEPENDS + drivers_accelerometer drivers_gyroscope px4_work_queue ) diff --git a/src/examples/fake_gyro/FakeGyro.cpp b/src/examples/fake_imu/FakeImu.cpp similarity index 53% rename from src/examples/fake_gyro/FakeGyro.cpp rename to src/examples/fake_imu/FakeImu.cpp index 3de0245521..f25c74457f 100644 --- a/src/examples/fake_gyro/FakeGyro.cpp +++ b/src/examples/fake_imu/FakeImu.cpp @@ -31,25 +31,32 @@ * ****************************************************************************/ -#include "FakeGyro.hpp" +#include "FakeImu.hpp" using namespace time_literals; -FakeGyro::FakeGyro() : +FakeImu::FakeImu() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _px4_gyro(1310988) // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_accel(1310988), // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_gyro(1310988) // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION { + _sensor_interval_us = roundf(1.e6f / _px4_gyro.get_max_rate_hz()); + + PX4_INFO("Rate %.3f, Interval: %d us", (double)_px4_gyro.get_max_rate_hz(), _sensor_interval_us); + + _px4_accel.set_range(2000.f); // don't care + _px4_gyro.set_scale(math::radians(2000.f) / static_cast(INT16_MAX - 1)); // 2000 degrees/second max } -bool FakeGyro::init() +bool FakeImu::init() { - ScheduleOnInterval(SENSOR_RATE, SENSOR_RATE); + ScheduleOnInterval(_sensor_interval_us); return true; } -void FakeGyro::Run() +void FakeImu::Run() { if (should_exit()) { ScheduleClear(); @@ -59,29 +66,60 @@ void FakeGyro::Run() sensor_gyro_fifo_s gyro{}; gyro.timestamp_sample = hrt_absolute_time(); - gyro.samples = GYRO_RATE / (1e6f / SENSOR_RATE); - gyro.dt = 1e6f / GYRO_RATE; // 8 kHz fake gyro; + gyro.samples = roundf(IMU_RATE_HZ / (1e6 / _sensor_interval_us)); + gyro.dt = 1e6 / IMU_RATE_HZ; + + const double dt_s = 1 / IMU_RATE_HZ; - const float dt_s = gyro.dt * 1e-6f; - const float x_freq = 15.f; // 15,0 Hz X frequency - const float y_freq = 63.5f; // 63.5 Hz Y frequency - const float z_freq = 135.f; // 135.0 Hz Z frequency + const double x_f0 = 0.0; // 0 Hz X frequency start + const double x_f1 = 10.0; // 10 Hz X frequency stop + + const double y_f0 = 0.0; // 10 Hz Y frequency start + const double y_f1 = 100.0; // 1000 Hz Y frequency stop + + const double z_f0 = 0.0; // 100 Hz Z frequency start + const double z_f1 = 1000.0; // 1000 Hz Z frequency stop + + // amplitude + static constexpr double A = (INT16_MAX - 1); + + if (_time_start_us == 0) { + _time_start_us = gyro.timestamp_sample; + } + + // 10 second sweep + const double T = 10.0; + + const double timestamp_sample_s = static_cast(gyro.timestamp_sample - _time_start_us) / 1e6; for (int n = 0; n < gyro.samples; n++) { - _time += dt_s; - const float k = 2.f * M_PI_F * _time; + // timestamp_sample corresponds to last sample + const double t = timestamp_sample_s - (gyro.samples - n - 1) * dt_s; + + // linear-frequency chirp, see https://en.wikipedia.org/wiki/Chirp + const double x_F = x_f0 + (x_f1 - x_f0) * t / (2 * T); + const double y_F = y_f0 + (y_f1 - y_f0) * t / (2 * T); + const double z_F = z_f0 + (z_f1 - z_f0) * t / (2 * T); - gyro.x[n] = (INT16_MAX - 1) * sinf(k * x_freq); - gyro.y[n] = (INT16_MAX - 1) / 2 * sinf(k * y_freq); - gyro.z[n] = (INT16_MAX - 1) * cosf(k * z_freq); + gyro.x[n] = roundf(A * sin(2 * M_PI * x_F * t)); + gyro.y[n] = roundf(A * sin(2 * M_PI * y_F * t)); + gyro.z[n] = roundf(A * sin(2 * M_PI * z_F * t)); + + if (n == 0) { + float x_freq = (x_f1 - x_f0) * (t / T) + x_f0; + float y_freq = (y_f1 - y_f0) * (t / T) + y_f0; + float z_freq = (z_f1 - z_f0) * (t / T) + z_f0; + + _px4_accel.update(gyro.timestamp_sample, x_freq, y_freq, z_freq); + } } _px4_gyro.updateFIFO(gyro); } -int FakeGyro::task_spawn(int argc, char *argv[]) +int FakeImu::task_spawn(int argc, char *argv[]) { - FakeGyro *instance = new FakeGyro(); + FakeImu *instance = new FakeImu(); if (instance) { _object.store(instance); @@ -102,12 +140,12 @@ int FakeGyro::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -int FakeGyro::custom_command(int argc, char *argv[]) +int FakeImu::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } -int FakeGyro::print_usage(const char *reason) +int FakeImu::print_usage(const char *reason) { if (reason) { PX4_WARN("%s\n", reason); @@ -119,13 +157,13 @@ int FakeGyro::print_usage(const char *reason) )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("fake_gyro", "driver"); + PRINT_MODULE_USAGE_NAME("fake_imu", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } -extern "C" __EXPORT int fake_gyro_main(int argc, char *argv[]) +extern "C" __EXPORT int fake_imu_main(int argc, char *argv[]) { - return FakeGyro::main(argc, argv); + return FakeImu::main(argc, argv); } diff --git a/src/examples/fake_gyro/FakeGyro.hpp b/src/examples/fake_imu/FakeImu.hpp similarity index 87% rename from src/examples/fake_gyro/FakeGyro.hpp rename to src/examples/fake_imu/FakeImu.hpp index 2b47419c12..f119f85619 100644 --- a/src/examples/fake_gyro/FakeGyro.hpp +++ b/src/examples/fake_imu/FakeImu.hpp @@ -38,16 +38,17 @@ #include #include #include +#include #include #include #include #include -class FakeGyro : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +class FakeImu : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - FakeGyro(); - ~FakeGyro() override = default; + FakeImu(); + ~FakeImu() override = default; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -61,12 +62,14 @@ public: bool init(); private: - static constexpr uint32_t SENSOR_RATE = 1250; - static constexpr float GYRO_RATE = 8000; + static constexpr double IMU_RATE_HZ = 8000; void Run() override; + PX4Accelerometer _px4_accel; PX4Gyroscope _px4_gyro; - float _time{0.f}; + hrt_abstime _time_start_us{0}; + + uint32_t _sensor_interval_us{1250}; };