From 2257c3767e678b4fd8524517b478c4b1487c6f9c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 6 Mar 2021 13:14:57 -0500 Subject: [PATCH] simple gyro auto calibration module --- .ci/Jenkinsfile-hardware | 2 + ROMFS/px4fmu_common/init.d/rcS | 5 + boards/airmind/mindpx-v2/default.cmake | 1 + boards/atlflight/eagle/default.cmake | 1 + boards/atlflight/eagle/qurt.cmake | 1 + boards/atlflight/excelsior/default.cmake | 1 + boards/atlflight/excelsior/qurt.cmake | 1 + boards/av/x-v1/default.cmake | 1 + boards/beaglebone/blue/default.cmake | 1 + boards/cuav/nora/default.cmake | 1 + boards/cuav/x7pro/default.cmake | 1 + boards/cubepilot/cubeorange/console.cmake | 1 + boards/cubepilot/cubeorange/default.cmake | 1 + boards/cubepilot/cubeyellow/console.cmake | 1 + boards/cubepilot/cubeyellow/default.cmake | 1 + boards/emlid/navio2/default.cmake | 1 + boards/holybro/durandal-v1/default.cmake | 1 + boards/holybro/kakutef7/default.cmake | 1 + boards/holybro/pix32v5/default.cmake | 1 + boards/modalai/fc-v1/default.cmake | 1 + boards/mro/ctrl-zero-f7-oem/default.cmake | 1 + boards/mro/ctrl-zero-f7/default.cmake | 1 + boards/mro/ctrl-zero-h7-oem/default.cmake | 1 + boards/mro/ctrl-zero-h7/default.cmake | 1 + boards/mro/pixracerpro/default.cmake | 1 + boards/mro/x21-777/default.cmake | 1 + boards/mro/x21/default.cmake | 1 + boards/nxp/fmuk66-e/default.cmake | 1 + boards/nxp/fmuk66-e/socketcan.cmake | 1 + boards/nxp/fmuk66-v3/default.cmake | 1 + boards/nxp/fmuk66-v3/rtps.cmake | 1 + boards/nxp/fmuk66-v3/socketcan.cmake | 1 + boards/nxp/fmurt1062-v1/default.cmake | 1 + boards/omnibus/f4sd/default.cmake | 1 + boards/px4/fmu-v2/default.cmake | 1 + boards/px4/fmu-v2/fixedwing.cmake | 1 + boards/px4/fmu-v2/multicopter.cmake | 1 + boards/px4/fmu-v3/default.cmake | 1 + boards/px4/fmu-v4/default.cmake | 1 + boards/px4/fmu-v4/uavcanv1.cmake | 1 + boards/px4/fmu-v4pro/default.cmake | 1 + boards/px4/fmu-v5/ctrlalloc.cmake | 1 + boards/px4/fmu-v5/debug.cmake | 1 + boards/px4/fmu-v5/default.cmake | 1 + boards/px4/fmu-v5/fixedwing.cmake | 1 + boards/px4/fmu-v5/optimized.cmake | 1 + boards/px4/fmu-v5/rtps.cmake | 1 + boards/px4/fmu-v5/stackcheck.cmake | 1 + boards/px4/fmu-v5/uavcanv0periph.cmake | 2 +- boards/px4/fmu-v5/uavcanv1.cmake | 2 +- boards/px4/fmu-v5x/base_phy_DP83848C.cmake | 1 + boards/px4/fmu-v5x/default.cmake | 1 + boards/px4/fmu-v6u/default.cmake | 1 + boards/px4/fmu-v6x/default.cmake | 1 + boards/px4/raspberrypi/default.cmake | 1 + boards/px4/sitl/ctrlalloc.cmake | 1 + boards/px4/sitl/default.cmake | 1 + boards/px4/sitl/nolockstep.cmake | 1 + boards/px4/sitl/rtps.cmake | 1 + boards/px4/sitl/test.cmake | 1 + boards/scumaker/pilotpi/arm64.cmake | 1 + boards/scumaker/pilotpi/default.cmake | 1 + .../px4_work_queue/WorkQueueManager.hpp | 2 +- src/lib/mathlib/math/WelfordMean.hpp | 87 +++++ src/lib/sensor_calibration/Gyroscope.hpp | 1 + src/modules/commander/gyro_calibration.cpp | 39 +-- src/modules/gyro_calibration/CMakeLists.txt | 43 +++ .../gyro_calibration/GyroCalibration.cpp | 328 ++++++++++++++++++ .../gyro_calibration/GyroCalibration.hpp | 108 ++++++ src/modules/gyro_calibration/parameters.c | 41 +++ 70 files changed, 682 insertions(+), 36 deletions(-) create mode 100644 src/lib/mathlib/math/WelfordMean.hpp create mode 100644 src/modules/gyro_calibration/CMakeLists.txt create mode 100644 src/modules/gyro_calibration/GyroCalibration.cpp create mode 100644 src/modules/gyro_calibration/GyroCalibration.hpp create mode 100644 src/modules/gyro_calibration/parameters.c diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index 520857465f..e37c83d5a3 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -1009,6 +1009,7 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dmesg"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gps status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gyro_calibration status; param show CAL_GYRO*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener adc_report"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener battery_status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload"' @@ -1071,6 +1072,7 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ver all"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gyro_calibration status; param show CAL_GYRO*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ekf2 status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "free"' // stop logger diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6fc79fde28..c0edb039e8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -516,6 +516,11 @@ else gyro_fft start fi + if param compare -s IMU_GYRO_CAL_EN 1 + then + gyro_calibration start + fi + # # Optional board supplied extras: rc.board_extras # diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index b976951b2f..d137033bf5 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -61,6 +61,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index 96826faa31..41295d615d 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -67,6 +67,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/atlflight/eagle/qurt.cmake b/boards/atlflight/eagle/qurt.cmake index ad59706374..1c9a1d3c61 100644 --- a/boards/atlflight/eagle/qurt.cmake +++ b/boards/atlflight/eagle/qurt.cmake @@ -58,6 +58,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 878c4750c7..2f7ef433b9 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -66,6 +66,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/atlflight/excelsior/qurt.cmake b/boards/atlflight/excelsior/qurt.cmake index eec151448e..668b69414b 100644 --- a/boards/atlflight/excelsior/qurt.cmake +++ b/boards/atlflight/excelsior/qurt.cmake @@ -58,6 +58,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 4c3f93dbf8..5784fb27c3 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -61,6 +61,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index f2a9128e3a..89b84a72be 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -43,6 +43,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/cuav/nora/default.cmake b/boards/cuav/nora/default.cmake index c73950d652..67f6915686 100644 --- a/boards/cuav/nora/default.cmake +++ b/boards/cuav/nora/default.cmake @@ -65,6 +65,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/cuav/x7pro/default.cmake b/boards/cuav/x7pro/default.cmake index 8bd747f92c..a2d03d6a63 100644 --- a/boards/cuav/x7pro/default.cmake +++ b/boards/cuav/x7pro/default.cmake @@ -68,6 +68,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/cubepilot/cubeorange/console.cmake b/boards/cubepilot/cubeorange/console.cmake index 8fa042e377..c3d1fbc929 100644 --- a/boards/cubepilot/cubeorange/console.cmake +++ b/boards/cubepilot/cubeorange/console.cmake @@ -68,6 +68,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration #gyro_fft land_detector landing_target_estimator diff --git a/boards/cubepilot/cubeorange/default.cmake b/boards/cubepilot/cubeorange/default.cmake index 6db55e059f..74f9ced4ca 100644 --- a/boards/cubepilot/cubeorange/default.cmake +++ b/boards/cubepilot/cubeorange/default.cmake @@ -66,6 +66,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/cubepilot/cubeyellow/console.cmake b/boards/cubepilot/cubeyellow/console.cmake index 5b66f20b56..40df4832dc 100644 --- a/boards/cubepilot/cubeyellow/console.cmake +++ b/boards/cubepilot/cubeyellow/console.cmake @@ -67,6 +67,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/cubepilot/cubeyellow/default.cmake b/boards/cubepilot/cubeyellow/default.cmake index ba148d09f6..7bf1560009 100644 --- a/boards/cubepilot/cubeyellow/default.cmake +++ b/boards/cubepilot/cubeyellow/default.cmake @@ -67,6 +67,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index 682b64d0ed..67fdfc6c63 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -44,6 +44,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index 5fc4b999f9..ed7f606ced 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -66,6 +66,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake index ad2670eda3..10e58d0ea8 100644 --- a/boards/holybro/kakutef7/default.cmake +++ b/boards/holybro/kakutef7/default.cmake @@ -38,6 +38,7 @@ px4_add_board( #ekf2 events flight_mode_manager + gyro_calibration #gyro_fft land_detector load_mon diff --git a/boards/holybro/pix32v5/default.cmake b/boards/holybro/pix32v5/default.cmake index 9a893b9be0..55211533f5 100644 --- a/boards/holybro/pix32v5/default.cmake +++ b/boards/holybro/pix32v5/default.cmake @@ -70,6 +70,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index f95d07c493..e9a77faacc 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -60,6 +60,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/mro/ctrl-zero-f7-oem/default.cmake b/boards/mro/ctrl-zero-f7-oem/default.cmake index 559593c587..46b9fa127e 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.cmake +++ b/boards/mro/ctrl-zero-f7-oem/default.cmake @@ -64,6 +64,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index a118d5ba1c..aacacf3d4c 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -64,6 +64,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/mro/ctrl-zero-h7-oem/default.cmake b/boards/mro/ctrl-zero-h7-oem/default.cmake index a569dac5c2..9abc119b95 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.cmake +++ b/boards/mro/ctrl-zero-h7-oem/default.cmake @@ -66,6 +66,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/mro/ctrl-zero-h7/default.cmake b/boards/mro/ctrl-zero-h7/default.cmake index 19858431b7..5e66d828e6 100644 --- a/boards/mro/ctrl-zero-h7/default.cmake +++ b/boards/mro/ctrl-zero-h7/default.cmake @@ -66,6 +66,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/mro/pixracerpro/default.cmake b/boards/mro/pixracerpro/default.cmake index 77b0614446..539d662908 100644 --- a/boards/mro/pixracerpro/default.cmake +++ b/boards/mro/pixracerpro/default.cmake @@ -66,6 +66,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/mro/x21-777/default.cmake b/boards/mro/x21-777/default.cmake index fad6a148d9..e6fe7ddd76 100644 --- a/boards/mro/x21-777/default.cmake +++ b/boards/mro/x21-777/default.cmake @@ -65,6 +65,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/mro/x21/default.cmake b/boards/mro/x21/default.cmake index 2384034f1a..f71033ae15 100644 --- a/boards/mro/x21/default.cmake +++ b/boards/mro/x21/default.cmake @@ -62,6 +62,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/nxp/fmuk66-e/default.cmake b/boards/nxp/fmuk66-e/default.cmake index 9049949246..8958865896 100644 --- a/boards/nxp/fmuk66-e/default.cmake +++ b/boards/nxp/fmuk66-e/default.cmake @@ -62,6 +62,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/nxp/fmuk66-e/socketcan.cmake b/boards/nxp/fmuk66-e/socketcan.cmake index c86fe0973b..db8c2ee603 100644 --- a/boards/nxp/fmuk66-e/socketcan.cmake +++ b/boards/nxp/fmuk66-e/socketcan.cmake @@ -61,6 +61,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 31d6556a5b..7152c88995 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -62,6 +62,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/nxp/fmuk66-v3/rtps.cmake b/boards/nxp/fmuk66-v3/rtps.cmake index d794c6affc..796491a1f9 100644 --- a/boards/nxp/fmuk66-v3/rtps.cmake +++ b/boards/nxp/fmuk66-v3/rtps.cmake @@ -61,6 +61,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/nxp/fmuk66-v3/socketcan.cmake b/boards/nxp/fmuk66-v3/socketcan.cmake index 64c85e572b..1ba8d64f44 100644 --- a/boards/nxp/fmuk66-v3/socketcan.cmake +++ b/boards/nxp/fmuk66-v3/socketcan.cmake @@ -61,6 +61,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index bd3ed616bd..c91fa23414 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -56,6 +56,7 @@ px4_add_board( ekf2 events flight_mode_manager + #gyro_calibration #gyro_fft land_detector landing_target_estimator diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index 46c7d70f33..b7238b3bc3 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -50,6 +50,7 @@ px4_add_board( flight_mode_manager #fw_att_control #fw_pos_control_l1 + gyro_calibration #gyro_fft land_detector #landing_target_estimator diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 207b06d22a..35f58aec60 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -76,6 +76,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration #gyro_fft land_detector #landing_target_estimator diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake index 10033bf0d7..e46b4510fc 100644 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -52,6 +52,7 @@ px4_add_board( #events fw_att_control fw_pos_control_l1 + gyro_calibration #gyro_fft land_detector load_mon diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index c113138e62..f6b8011e20 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -43,6 +43,7 @@ px4_add_board( dataman ekf2 flight_mode_manager + gyro_calibration #gyro_fft #events land_detector diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 963ab40edf..f807164106 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -74,6 +74,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 1c4704fd5f..b9817c5efb 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -70,6 +70,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/fmu-v4/uavcanv1.cmake b/boards/px4/fmu-v4/uavcanv1.cmake index 6aa991ae72..fa5a15dc17 100644 --- a/boards/px4/fmu-v4/uavcanv1.cmake +++ b/boards/px4/fmu-v4/uavcanv1.cmake @@ -71,6 +71,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 0fea0c5f52..6a4556f6d8 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -67,6 +67,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake index e99ed02f11..7f57b8721e 100644 --- a/boards/px4/fmu-v5/ctrlalloc.cmake +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -73,6 +73,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/fmu-v5/debug.cmake b/boards/px4/fmu-v5/debug.cmake index acb9ae4f93..f889b9720f 100644 --- a/boards/px4/fmu-v5/debug.cmake +++ b/boards/px4/fmu-v5/debug.cmake @@ -71,6 +71,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration #gyro_fft land_detector #landing_target_estimator diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 56de57d909..ccce0896ca 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -71,6 +71,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index 5feaebc58d..df632bdce5 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -53,6 +53,7 @@ px4_add_board( events fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector load_mon diff --git a/boards/px4/fmu-v5/optimized.cmake b/boards/px4/fmu-v5/optimized.cmake index 732c4ed702..4286ea10eb 100644 --- a/boards/px4/fmu-v5/optimized.cmake +++ b/boards/px4/fmu-v5/optimized.cmake @@ -64,6 +64,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration #gyro_fft land_detector #landing_target_estimator diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index b8bf45151b..f8dd672222 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -68,6 +68,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index f13b43579c..65d6bb0270 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -71,6 +71,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration #gyro_fft land_detector #landing_target_estimator diff --git a/boards/px4/fmu-v5/uavcanv0periph.cmake b/boards/px4/fmu-v5/uavcanv0periph.cmake index 52918281ce..8c8d4b6d43 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.cmake +++ b/boards/px4/fmu-v5/uavcanv0periph.cmake @@ -73,6 +73,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration #gyro_fft land_detector landing_target_estimator @@ -129,7 +130,6 @@ px4_add_board( #fake_gyro #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #gyro_fft #hello #hwtest # Hardware test #matlab_csv_serial diff --git a/boards/px4/fmu-v5/uavcanv1.cmake b/boards/px4/fmu-v5/uavcanv1.cmake index c57f3a425d..2eb1848ede 100644 --- a/boards/px4/fmu-v5/uavcanv1.cmake +++ b/boards/px4/fmu-v5/uavcanv1.cmake @@ -72,6 +72,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator @@ -129,7 +130,6 @@ px4_add_board( #fake_gyro #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #gyro_fft #hello #hwtest # Hardware test #matlab_csv_serial diff --git a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake index 1564c07aef..98deb02c12 100644 --- a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake +++ b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake @@ -67,6 +67,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 44d09fb55b..39e4fd584b 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -71,6 +71,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/fmu-v6u/default.cmake b/boards/px4/fmu-v6u/default.cmake index f1b05c2ebf..6d4cf31073 100644 --- a/boards/px4/fmu-v6u/default.cmake +++ b/boards/px4/fmu-v6u/default.cmake @@ -65,6 +65,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/fmu-v6x/default.cmake b/boards/px4/fmu-v6x/default.cmake index 63b7878fd0..a461c2b763 100644 --- a/boards/px4/fmu-v6x/default.cmake +++ b/boards/px4/fmu-v6x/default.cmake @@ -69,6 +69,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index 8c5732d1af..26a1a76ece 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -42,6 +42,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/sitl/ctrlalloc.cmake b/boards/px4/sitl/ctrlalloc.cmake index 8cd3a1ec6c..876ad06dcb 100644 --- a/boards/px4/sitl/ctrlalloc.cmake +++ b/boards/px4/sitl/ctrlalloc.cmake @@ -37,6 +37,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index fc2da63c8a..8100eb5b1c 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -36,6 +36,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/sitl/nolockstep.cmake b/boards/px4/sitl/nolockstep.cmake index 7706555e33..83e40ed7b6 100644 --- a/boards/px4/sitl/nolockstep.cmake +++ b/boards/px4/sitl/nolockstep.cmake @@ -35,6 +35,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 2c06936941..63eb8e398b 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -35,6 +35,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 6712689527..a0e8f1870b 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -35,6 +35,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/scumaker/pilotpi/arm64.cmake b/boards/scumaker/pilotpi/arm64.cmake index e9bc0fb146..e19df827f4 100644 --- a/boards/scumaker/pilotpi/arm64.cmake +++ b/boards/scumaker/pilotpi/arm64.cmake @@ -44,6 +44,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/boards/scumaker/pilotpi/default.cmake b/boards/scumaker/pilotpi/default.cmake index e1114a32cd..de0486cf20 100644 --- a/boards/scumaker/pilotpi/default.cmake +++ b/boards/scumaker/pilotpi/default.cmake @@ -44,6 +44,7 @@ px4_add_board( flight_mode_manager fw_att_control fw_pos_control_l1 + gyro_calibration gyro_fft land_detector landing_target_estimator diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index d1570a5fc7..127d393d96 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -88,7 +88,7 @@ static constexpr wq_config_t UART7{"wq:UART7", 1400, -28}; static constexpr wq_config_t UART8{"wq:UART8", 1400, -29}; static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -30}; -static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50}; +static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50}; static constexpr wq_config_t test1{"wq:test1", 2000, 0}; static constexpr wq_config_t test2{"wq:test2", 2000, 0}; diff --git a/src/lib/mathlib/math/WelfordMean.hpp b/src/lib/mathlib/math/WelfordMean.hpp new file mode 100644 index 0000000000..75bb59bce9 --- /dev/null +++ b/src/lib/mathlib/math/WelfordMean.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file WelfordMean.hpp + * + * Welford's online algorithm for computing mean and variance. + */ + +#pragma once + +namespace math +{ + +template +class WelfordMean +{ +public: + // For a new value, compute the new count, new mean, the new M2. + void update(const T &new_value) + { + if (_count == 0) { + _mean = new_value; + } + + _count++; + + // mean accumulates the mean of the entire dataset + const T delta{new_value - _mean}; + _mean += delta / _count; + + // M2 aggregates the squared distance from the mean + // count aggregates the number of samples seen so far + _M2 += delta.emult(new_value - _mean); + } + + bool valid() const { return _count > 2; } + unsigned count() const { return _count; } + + void reset() + { + _count = 0; + _mean = {}; + _M2 = {}; + } + + // Retrieve the mean, variance and sample variance + T mean() const { return _mean; } + T variance() const { return _M2 / _count; } + T sample_variance() const { return _M2 / (_count - 1); } +private: + T _mean{}; + T _M2{}; + unsigned _count{0}; +}; + +} // namespace math diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index 2d6e6d010d..163960377f 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -73,6 +73,7 @@ public: const int32_t &priority() const { return _priority; } const matrix::Dcmf &rotation() const { return _rotation; } const Rotation &rotation_enum() const { return _rotation_enum; } + const matrix::Vector3f &thermal_offset() const { return _thermal_offset; } // apply offsets and scale // rotate corrected measurements from sensor to body frame diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 7368646a62..b7fc2f83c7 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -57,7 +57,6 @@ #include #include #include -#include #include static constexpr char sensor_name[] {"gyro"}; @@ -83,9 +82,6 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data) static constexpr unsigned CALIBRATION_COUNT = 250; unsigned poll_errcount = 0; - uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; - sensor_correction_s sensor_correction{}; - uORB::SubscriptionBlocking gyro_sub[MAX_GYROS] { {ORB_ID(sensor_gyro), 0, 0}, {ORB_ID(sensor_gyro), 0, 1}, @@ -115,39 +111,16 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data) sensor_gyro_s gyro_report; while (gyro_sub[gyro_index].update(&gyro_report)) { + // fetch optional thermal offset corrections in sensor frame + const Vector3f &thermal_offset{worker_data.calibrations[gyro_index].thermal_offset()}; - // fetch optional thermal offset corrections in sensor/board frame - Vector3f offset{0, 0, 0}; - sensor_correction_sub.update(&sensor_correction); - - if (sensor_correction.timestamp > 0 && gyro_report.device_id != 0) { - for (uint8_t correction_index = 0; correction_index < MAX_GYROS; correction_index++) { - if (sensor_correction.gyro_device_ids[correction_index] == gyro_report.device_id) { - switch (correction_index) { - case 0: - offset = Vector3f{sensor_correction.gyro_offset_0}; - break; - case 1: - offset = Vector3f{sensor_correction.gyro_offset_1}; - break; - case 2: - offset = Vector3f{sensor_correction.gyro_offset_2}; - break; - case 3: - offset = Vector3f{sensor_correction.gyro_offset_3}; - break; - } - } - } - } - - worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - offset; + worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - thermal_offset; calibration_counter[gyro_index]++; if (gyro_index == 0) { - worker_data.filter[0].insert(gyro_report.x - offset(0)); - worker_data.filter[1].insert(gyro_report.y - offset(1)); - worker_data.filter[2].insert(gyro_report.z - offset(2)); + worker_data.filter[0].insert(gyro_report.x - thermal_offset(0)); + worker_data.filter[1].insert(gyro_report.y - thermal_offset(1)); + worker_data.filter[2].insert(gyro_report.z - thermal_offset(2)); } } diff --git a/src/modules/gyro_calibration/CMakeLists.txt b/src/modules/gyro_calibration/CMakeLists.txt new file mode 100644 index 0000000000..5148eab2ce --- /dev/null +++ b/src/modules/gyro_calibration/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__gyro_calibration + MAIN gyro_calibration + COMPILE_FLAGS + SRCS + GyroCalibration.cpp + GyroCalibration.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/modules/gyro_calibration/GyroCalibration.cpp b/src/modules/gyro_calibration/GyroCalibration.cpp new file mode 100644 index 0000000000..7694322248 --- /dev/null +++ b/src/modules/gyro_calibration/GyroCalibration.cpp @@ -0,0 +1,328 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "GyroCalibration.hpp" + +#include + +using namespace time_literals; +using matrix::Vector3f; + +GyroCalibration::GyroCalibration() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ +} + +GyroCalibration::~GyroCalibration() +{ + perf_free(_loop_interval_perf); + perf_free(_calibration_updated_perf); +} + +bool GyroCalibration::init() +{ + ScheduleOnInterval(INTERVAL_US); + return true; +} + +void GyroCalibration::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_count(_loop_interval_perf); + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + if (armed != _armed) { + if (!_armed && armed) { + // disarmed -> armed: run at minimal rate until disarmed + ScheduleOnInterval(10_s); + + } else if (_armed && !armed) { + // armed -> disarmed: start running again + ScheduleOnInterval(INTERVAL_US); + } + + _armed = armed; + Reset(); + } + } + } + + if (_armed) { + // do nothing if armed + return; + } + + if (_vehicle_status_flags_sub.updated()) { + vehicle_status_flags_s vehicle_status_flags; + + if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) { + if (_system_calibrating != vehicle_status_flags.condition_calibration_enabled) { + Reset(); + _system_calibrating = vehicle_status_flags.condition_calibration_enabled; + } + } + } + + if (_system_calibrating) { + // do nothing if system is calibrating + return; + } + + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + + if (_parameter_update_sub.copy(¶m_update)) { + // minimize updates immediately following parameter changes + _last_calibration_update = param_update.timestamp; + } + + for (auto &cal : _gyro_calibration) { + cal.ParametersUpdate(); + } + } + + + // collect raw data from all available gyroscopes (sensor_gyro) + for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { + sensor_gyro_s sensor_gyro; + + while (_sensor_gyro_subs[gyro].update(&sensor_gyro)) { + if (PX4_ISFINITE(sensor_gyro.temperature)) { + if ((fabsf(_temperature[gyro] - sensor_gyro.temperature) > 1.f) || !PX4_ISFINITE(_temperature[gyro])) { + PX4_DEBUG("gyro %d temperature change, resetting all %.6f -> %.6f", gyro, (double)_temperature[gyro], + (double)sensor_gyro.temperature); + + _temperature[gyro] = sensor_gyro.temperature; + + // reset all on any temperature change + Reset(); + } + + } else { + _temperature[gyro] = NAN; + } + + if (_gyro_calibration[gyro].device_id() == sensor_gyro.device_id) { + const Vector3f val{Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z} - _gyro_calibration[gyro].thermal_offset()}; + _gyro_mean[gyro].update(val); + + } else { + // setting device id, reset all + _gyro_calibration[gyro].set_device_id(sensor_gyro.device_id); + Reset(); + } + } + } + + + // check all accelerometers for possible movement + for (int accel = 0; accel < _sensor_accel_subs.size(); accel++) { + sensor_accel_s sensor_accel; + + if (_sensor_accel_subs[accel].update(&sensor_accel)) { + const Vector3f acceleration{sensor_accel.x, sensor_accel.y, sensor_accel.z}; + + if ((acceleration - _acceleration[accel]).longerThan(0.5f)) { + // reset all on any change + PX4_DEBUG("accel %d changed, resetting all %.5f", accel, (double)(acceleration - _acceleration[accel]).length()); + + _acceleration[accel] = acceleration; + Reset(); + return; + + } else if (acceleration.longerThan(CONSTANTS_ONE_G * 1.3f)) { + Reset(); + return; + } + } + } + + + // check if sufficient data has been gathered to update calibration + bool sufficient_samples = false; + + for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { + if (_gyro_calibration[gyro].device_id() != 0) { + // periodically check variance + if ((_gyro_mean[gyro].count() % 100 == 0)) { + PX4_DEBUG("gyro %d (%d) variance, [%.9f, %.9f, %.9f] %.9f", gyro, _gyro_calibration[gyro].device_id(), + (double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2), + (double)_gyro_mean[gyro].variance().length()); + + if (_gyro_mean[gyro].variance().longerThan(0.001f)) { + // reset all + Reset(); + return; + } + } + + if (_gyro_mean[gyro].count() > 3000) { + sufficient_samples = true; + + } else { + sufficient_samples = false; + return; + } + } + } + + + // update calibrations for all available gyros + if (sufficient_samples && (hrt_elapsed_time(&_last_calibration_update) > 10_s)) { + bool calibration_updated = false; + + for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { + if (_gyro_calibration[gyro].device_id() != 0) { + _gyro_calibration[gyro].set_calibration_index(gyro); + + const Vector3f old_offset{_gyro_calibration[gyro].offset()}; + + if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean())) { + calibration_updated = true; + + PX4_INFO("gyro %d (%d) updating calibration, [%.4f, %.4f, %.4f] -> [%.4f, %.4f, %.4f] %.1f°C", + gyro, _gyro_calibration[gyro].device_id(), + (double)old_offset(0), (double)old_offset(1), (double)old_offset(2), + (double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2), + (double)_temperature[gyro]); + + perf_count(_calibration_updated_perf); + } + } + } + + // save all calibrations + if (calibration_updated) { + bool param_save = false; + + for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { + if (_gyro_calibration[gyro].device_id() != 0) { + if (_gyro_calibration[gyro].ParametersSave()) { + param_save = true; + } + } + } + + if (param_save) { + param_notify_changes(); + _last_calibration_update = hrt_absolute_time(); + } + } + + Reset(); + } +} + +int GyroCalibration::task_spawn(int argc, char *argv[]) +{ + GyroCalibration *instance = new GyroCalibration(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int GyroCalibration::print_status() +{ + for (int gyro = 0; gyro < _sensor_gyro_subs.size(); gyro++) { + if (_gyro_calibration[gyro].device_id() != 0) { + PX4_INFO_RAW("gyro %d (%d), [%.5f, %.5f, %.5f] var: [%.9f, %.9f, %.9f] %.1f°C (count %d)\n", + gyro, _gyro_calibration[gyro].device_id(), + (double)_gyro_mean[gyro].mean()(0), (double)_gyro_mean[gyro].mean()(1), (double)_gyro_mean[gyro].mean()(2), + (double)_gyro_mean[gyro].variance()(0), (double)_gyro_mean[gyro].variance()(1), (double)_gyro_mean[gyro].variance()(2), + (double)_temperature[gyro], _gyro_mean[gyro].count()); + } + } + + perf_print_counter(_loop_interval_perf); + perf_print_counter(_calibration_updated_perf); + return 0; +} + +int GyroCalibration::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int GyroCalibration::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Simple online gyroscope calibration. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("gyro_calibration", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int gyro_calibration_main(int argc, char *argv[]) +{ + return GyroCalibration::main(argc, argv); +} diff --git a/src/modules/gyro_calibration/GyroCalibration.hpp b/src/modules/gyro_calibration/GyroCalibration.hpp new file mode 100644 index 0000000000..1b2b925b5e --- /dev/null +++ b/src/modules/gyro_calibration/GyroCalibration.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class GyroCalibration : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + GyroCalibration(); + ~GyroCalibration() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + + int print_status() override; + +private: + static constexpr hrt_abstime INTERVAL_US = 20000_us; + static constexpr int MAX_SENSORS = 4; + + void Run() override; + + void Reset() + { + for (auto &m : _gyro_mean) { + m.reset(); + } + } + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_status_sub{ORB_ID::vehicle_status}; + uORB::Subscription _vehicle_status_flags_sub{ORB_ID::vehicle_status_flags}; + + uORB::SubscriptionMultiArray _sensor_accel_subs{ORB_ID::sensor_accel}; + uORB::SubscriptionMultiArray _sensor_gyro_subs{ORB_ID::sensor_gyro}; + + calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {}; + math::WelfordMean _gyro_mean[MAX_SENSORS] {}; + float _temperature[MAX_SENSORS] {}; + + matrix::Vector3f _acceleration[MAX_SENSORS] {}; + + hrt_abstime _last_calibration_update{0}; + + bool _armed{false}; + bool _system_calibrating{false}; + + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + perf_counter_t _calibration_updated_perf{perf_alloc(PC_COUNT, MODULE_NAME": calibration updated")}; +}; diff --git a/src/modules/gyro_calibration/parameters.c b/src/modules/gyro_calibration/parameters.c new file mode 100644 index 0000000000..dee9df9b74 --- /dev/null +++ b/src/modules/gyro_calibration/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* IMU gyro auto calibration enable. +* +* @boolean +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_INT32(IMU_GYRO_CAL_EN, 1);