fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fake_imu (formally fake_gyro) updates for testing gyro filtering

Browse Source 
- fake_imu now publishes sine sweeps over 10 seconds
 - accel is also published so that fake IMU can be selected when the only option
This commit is contained in:
Daniel Agar 2021-05-17 20:26:25 -04:00 committed by GitHub
parent 39c90c8fc9
commit c3884b5bc1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
70 changed files with 206 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/airmind/mindpx-v2/default.cmake
View File

@ -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

2
boards/av/x-v1/default.cmake
View File

@ -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

2
boards/beaglebone/blue/default.cmake
View File

@ -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

2
boards/cuav/nora/default.cmake
View File

@ -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

2
boards/cuav/nora/test.cmake
View File

@ -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

2
boards/cuav/x7pro/default.cmake
View File

@ -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

2
boards/cuav/x7pro/test.cmake
View File

@ -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

2
boards/cubepilot/cubeorange/default.cmake
View File

@ -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

2
boards/cubepilot/cubeorange/test.cmake
View File

@ -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

2
boards/cubepilot/cubeyellow/default.cmake
View File

@ -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

2
boards/cubepilot/cubeyellow/test.cmake
View File

@ -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

2
boards/emlid/navio2/default.cmake
View File

@ -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

2
boards/holybro/durandal-v1/default.cmake
View File

@ -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

2
boards/holybro/durandal-v1/test.cmake
View File

@ -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

2
boards/holybro/pix32v5/default.cmake
View File

@ -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

2
boards/holybro/pix32v5/test.cmake
View File

@ -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

2
boards/modalai/fc-v1/default.cmake
View File

@ -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

2
boards/modalai/fc-v1/rtps.cmake
View File

@ -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

2
boards/modalai/fc-v1/test.cmake
View File

@ -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

2
boards/mro/ctrl-zero-f7-oem/default.cmake
View File

@ -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

2
boards/mro/ctrl-zero-f7/default.cmake
View File

@ -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

2
boards/mro/ctrl-zero-h7-oem/default.cmake
View File

@ -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

2
boards/mro/ctrl-zero-h7/default.cmake
View File

@ -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

2
boards/mro/pixracerpro/default.cmake
View File

@ -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

2
boards/mro/x21-777/default.cmake
View File

@ -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

2
boards/mro/x21/default.cmake
View File

@ -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

2
boards/nxp/fmuk66-e/default.cmake
View File

@ -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

2
boards/nxp/fmuk66-e/rtps.cmake
View File

@ -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

2
boards/nxp/fmuk66-e/socketcan.cmake
View File

@ -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

2
boards/nxp/fmuk66-v3/default.cmake
View File

@ -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

2
boards/nxp/fmuk66-v3/rtps.cmake
View File

@ -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

2
boards/nxp/fmuk66-v3/socketcan.cmake
View File

@ -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

2
boards/nxp/fmuk66-v3/test.cmake
View File

@ -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

2
boards/nxp/fmurt1062-v1/default.cmake
View File

@ -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

2
boards/px4/fmu-v2/default.cmake
View File

@ -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

2
boards/px4/fmu-v3/default.cmake
View File

@ -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

2
boards/px4/fmu-v3/test.cmake
View File

@ -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

2
boards/px4/fmu-v4/default.cmake
View File

@ -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

2
boards/px4/fmu-v4/test.cmake
View File

@ -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

2
boards/px4/fmu-v4pro/default.cmake
View File

@ -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

2
boards/px4/fmu-v4pro/test.cmake
View File

@ -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

2
boards/px4/fmu-v5/ctrlalloc.cmake
View File

@ -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

2
boards/px4/fmu-v5/debug.cmake
View File

@ -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

2
boards/px4/fmu-v5/default.cmake
View File

@ -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

2
boards/px4/fmu-v5/optimized.cmake
View File

@ -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

2
boards/px4/fmu-v5/rtps.cmake
View File

@ -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

2
boards/px4/fmu-v5/stackcheck.cmake
View File

@ -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

2
boards/px4/fmu-v5/test.cmake
View File

@ -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

2
boards/px4/fmu-v5/uavcanv0periph.cmake
View File

@ -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

2
boards/px4/fmu-v5/uavcanv1.cmake
View File

@ -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

2
boards/px4/fmu-v5x/base_phy_DP83848C.cmake
View File

@ -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

2
boards/px4/fmu-v5x/default.cmake
View File

@ -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

2
boards/px4/fmu-v5x/test.cmake
View File

@ -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

2
boards/px4/fmu-v6u/default.cmake
View File

@ -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

2
boards/px4/fmu-v6u/test.cmake
View File

@ -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

2
boards/px4/fmu-v6x/default.cmake
View File

@ -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

2
boards/px4/fmu-v6x/test.cmake
View File

@ -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

2
boards/px4/raspberrypi/default.cmake
View File

@ -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

2
boards/px4/sitl/ctrlalloc.cmake
View File

@ -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

2
boards/px4/sitl/default.cmake
View File

@ -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

2
boards/px4/sitl/nolockstep.cmake
View File

@ -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

2
boards/px4/sitl/rtps.cmake
View File

@ -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

2
boards/px4/sitl/test.cmake
View File

@ -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

2
boards/scumaker/pilotpi/arm64.cmake
View File

@ -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

2
boards/spracing/h7extreme/default.cmake
View File

@ -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

14
platforms/posix/cmake/sitl_tests.cmake
View File

@ -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 $<TARGET_FILE:px4>
-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

51
posix-configs/SITL/init/test/test_imu_filtering Normal file
View File

@ -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

9
src/examples/fake_gyro/CMakeLists.txt → src/examples/fake_imu/CMakeLists.txt
View File

@ -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
)

86
src/examples/fake_gyro/FakeGyro.cpp → src/examples/fake_imu/FakeImu.cpp
View File

@ -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<float>(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 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 dt_s = 1 / IMU_RATE_HZ;
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<double>(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;
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);
// 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] = 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);
}

15
src/examples/fake_gyro/FakeGyro.hpp → src/examples/fake_imu/FakeImu.hpp
View File

@ -38,16 +38,17 @@
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_gyro_fifo.h>
class FakeGyro : public ModuleBase<FakeGyro>, public ModuleParams, public px4::ScheduledWorkItem
class FakeImu : public ModuleBase<FakeImu>, 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};
};