From 9b12e30c4750d9fa5e1c0f75cfd5d286cab60f22 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 27 Jan 2020 18:18:04 -0500 Subject: [PATCH] accel and gyro calibration cleanup - kill IOCTLs for accel and gyro calibration - move accel/calibration entirely to sensors module --- boards/airmind/mindpx-v2/default.cmake | 1 - boards/atlflight/eagle/default.cmake | 1 - boards/atlflight/excelsior/default.cmake | 1 - boards/av/x-v1/default.cmake | 1 - boards/bitcraze/crazyflie/default.cmake | 1 - boards/holybro/durandal-v1/default.cmake | 1 - boards/holybro/durandal-v1/stackcheck.cmake | 1 - boards/holybro/kakutef7/default.cmake | 1 - boards/intel/aerofc-v1/default.cmake | 1 - boards/intel/aerofc-v1/rtps.cmake | 1 - boards/modalai/fc-v1/default.cmake | 1 - boards/mro/ctrl-zero-f7/default.cmake | 1 - boards/mro/x21-777/default.cmake | 1 - boards/mro/x21/default.cmake | 1 - boards/nxp/fmuk66-v3/default.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/lpe.cmake | 1 - boards/px4/fmu-v2/multicopter.cmake | 1 - boards/px4/fmu-v2/rover.cmake | 1 - boards/px4/fmu-v2/test.cmake | 1 - boards/px4/fmu-v3/default.cmake | 1 - boards/px4/fmu-v3/rtps.cmake | 1 - boards/px4/fmu-v3/stackcheck.cmake | 1 - boards/px4/fmu-v4/default.cmake | 1 - boards/px4/fmu-v4/rtps.cmake | 1 - boards/px4/fmu-v4/stackcheck.cmake | 1 - boards/px4/fmu-v4pro/default.cmake | 1 - boards/px4/fmu-v4pro/rtps.cmake | 1 - boards/px4/fmu-v5/critmonitor.cmake | 1 - boards/px4/fmu-v5/default.cmake | 1 - boards/px4/fmu-v5/fixedwing.cmake | 1 - boards/px4/fmu-v5/irqmonitor.cmake | 1 - boards/px4/fmu-v5/multicopter.cmake | 1 - boards/px4/fmu-v5/rover.cmake | 1 - boards/px4/fmu-v5/rtps.cmake | 1 - boards/px4/fmu-v5/stackcheck.cmake | 1 - boards/px4/fmu-v5x/default.cmake | 1 - boards/px4/sitl/default.cmake | 1 - boards/px4/sitl/rtps.cmake | 1 - boards/px4/sitl/test.cmake | 1 - boards/uvify/core/default.cmake | 1 - .../px4_platform_common/module_params.h | 14 +- .../px4_work_queue/WorkQueueManager.hpp | 2 +- .../distance_sensor/cm8jl65/CM8JL65.cpp | 2 + .../distance_sensor/leddar_one/LeddarOne.cpp | 1 + src/drivers/distance_sensor/sf0x/SF0X.cpp | 1 + src/drivers/distance_sensor/tfmini/TFMINI.cpp | 2 + src/drivers/drv_accel.h | 75 ------ src/drivers/drv_gyro.h | 70 ----- src/drivers/imu/bma180/bma180.cpp | 10 - .../matlab_csv_serial/matlab_csv_serial.c | 2 - .../accelerometer/PX4Accelerometer.cpp | 45 +--- .../accelerometer/PX4Accelerometer.hpp | 11 +- src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 36 +-- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 12 +- .../drivers/rangefinder/PX4Rangefinder.cpp | 25 +- .../drivers/rangefinder/PX4Rangefinder.hpp | 8 +- .../commander/accelerometer_calibration.cpp | 230 ++++------------ src/modules/commander/baro_calibration.cpp | 1 - src/modules/commander/gyro_calibration.cpp | 215 ++++----------- src/modules/commander/mag_calibration.cpp | 30 +-- .../sensors/sensor_corrections/CMakeLists.txt | 2 + .../sensor_corrections/SensorCorrections.cpp | 87 ++++++ .../sensor_corrections/SensorCorrections.hpp | 6 +- src/modules/sensors/voted_sensors_update.cpp | 198 +++----------- src/modules/sensors/voted_sensors_update.h | 26 +- src/systemcmds/config/CMakeLists.txt | 42 --- src/systemcmds/config/config.c | 251 ------------------ 71 files changed, 308 insertions(+), 1140 deletions(-) delete mode 100644 src/drivers/drv_accel.h delete mode 100644 src/drivers/drv_gyro.h delete mode 100644 src/systemcmds/config/CMakeLists.txt delete mode 100644 src/systemcmds/config/config.c diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 695d8c7213b9..c5cad8877906 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -81,7 +81,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config #dmesg dumpfile esc_calib diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index 110504387051..8ad03ea4f81b 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -88,7 +88,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS #bl_update - #config #dumpfile esc_calib #hardfault_log diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 65d5ab461158..feb5a8a8b72e 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -87,7 +87,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS #bl_update - #config #dumpfile esc_calib #hardfault_log diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 87cf58848cc1..77abc616554d 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -81,7 +81,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS #bl_update - config dmesg dumpfile esc_calib diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index 523ff3b75dec..a6a82805dbe3 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -35,7 +35,6 @@ px4_add_board( #temperature_compensation SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index 408ae2558fcf..adcb7155f7e0 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -86,7 +86,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/holybro/durandal-v1/stackcheck.cmake b/boards/holybro/durandal-v1/stackcheck.cmake index 3b1993db69a4..a43f7948e0aa 100644 --- a/boards/holybro/durandal-v1/stackcheck.cmake +++ b/boards/holybro/durandal-v1/stackcheck.cmake @@ -87,7 +87,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake index 8c8aa301daa6..2dfdea402a4b 100644 --- a/boards/holybro/kakutef7/default.cmake +++ b/boards/holybro/kakutef7/default.cmake @@ -49,7 +49,6 @@ px4_add_board( #temperature_compensation SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 2b80bc0f3981..23eb7255f34f 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -61,7 +61,6 @@ px4_add_board( #vtol_att_control SYSTEMCMDS bl_update - config #dmesg dumpfile esc_calib diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index 969499672334..f1fc060b6cf9 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -61,7 +61,6 @@ px4_add_board( #vtol_att_control SYSTEMCMDS bl_update - config #dmesg dumpfile esc_calib diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index 465e152ca15f..7d2135b1b4d1 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -79,7 +79,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 7fc3c4156a50..1f55ffd2762b 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -85,7 +85,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/mro/x21-777/default.cmake b/boards/mro/x21-777/default.cmake index 39d8163ab6de..9eef94c3258a 100644 --- a/boards/mro/x21-777/default.cmake +++ b/boards/mro/x21-777/default.cmake @@ -79,7 +79,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/mro/x21/default.cmake b/boards/mro/x21/default.cmake index d478204d0777..cde7b6408f79 100644 --- a/boards/mro/x21/default.cmake +++ b/boards/mro/x21/default.cmake @@ -82,7 +82,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config #dmesg dumpfile esc_calib diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 10c0abe3f750..d62d4d8a7d97 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -83,7 +83,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config #dmesg dumpfile esc_calib diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index 4641540a877a..2417ed018255 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -85,7 +85,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index 4c7255250105..9eca739c24be 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -70,7 +70,6 @@ px4_add_board( #vtol_att_control SYSTEMCMDS #bl_update - config dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 51bec7ec0215..2f33f7340aff 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -93,7 +93,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - #config #dmesg #dumpfile #esc_calib diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake index eb6a7b9b2a5e..ad2e385c8f58 100644 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -60,7 +60,6 @@ px4_add_board( #vmount SYSTEMCMDS #bl_update - #config #dumpfile #esc_calib hardfault_log diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index 7c6492bda528..e00983939341 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -88,7 +88,6 @@ px4_add_board( SYSTEMCMDS bl_update - #config #dumpfile #esc_calib hardfault_log diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index d97b61914c28..47dcb3d92f54 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -59,7 +59,6 @@ px4_add_board( vmount SYSTEMCMDS #bl_update - #config #dumpfile #esc_calib hardfault_log diff --git a/boards/px4/fmu-v2/rover.cmake b/boards/px4/fmu-v2/rover.cmake index f86769502d45..59f2ed2777e7 100644 --- a/boards/px4/fmu-v2/rover.cmake +++ b/boards/px4/fmu-v2/rover.cmake @@ -55,7 +55,6 @@ px4_add_board( SYSTEMCMDS bl_update - #config #dumpfile #esc_calib hardfault_log diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index 50799a85f0cc..6b6fae442e9c 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -91,7 +91,6 @@ px4_add_board( #vtol_att_control SYSTEMCMDS #bl_update - #config #dmesg #dumpfile #esc_calib diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 2c2e1625c104..e3aacb9e1269 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -91,7 +91,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config #dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 4420644a4bb4..6652ef5c46ff 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -91,7 +91,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config #dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index da095e8e180b..8fbb5bb1aea5 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -88,7 +88,6 @@ px4_add_board( SYSTEMCMDS bl_update - config dumpfile esc_calib hardfault_log diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index a933f5a215ad..f361b90b780d 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -86,7 +86,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config #dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 5b0eaef93d31..a2cdd31f9b73 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -86,7 +86,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config #dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index a264e22ad5e0..f6d1ce65adec 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -86,7 +86,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config #dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 9c2ba3336224..35097ecd52ed 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -88,7 +88,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config #dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 438173542fa0..293716a64a66 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -88,7 +88,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config #dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index 227095a33a18..62c84f3973ff 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -87,7 +87,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 297f0dc5dcab..d8de9e693374 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -90,7 +90,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index 75da10aa68d7..d026a88a2a5e 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -63,7 +63,6 @@ px4_add_board( vmount SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index c51d4495b68e..c3c9bc6b2ca3 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -87,7 +87,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index 7eb6b0a9bda6..51ce34e31b28 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -73,7 +73,6 @@ px4_add_board( vmount SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index f7389fe63c53..1c8624db8215 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -63,7 +63,6 @@ px4_add_board( vmount SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 881d41af4374..d22556c0232e 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -87,7 +87,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 121f05e06177..6de663b4ddfa 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -88,7 +88,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 8a497d45855a..401ce2207596 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -88,7 +88,6 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 5b2b9e8033f3..96d7fe724f5c 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -51,7 +51,6 @@ px4_add_board( uuv_att_control SYSTEMCMDS - #config #dumpfile dyn esc_calib diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index a0b2ee79a3cf..09eb8b06931c 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -51,7 +51,6 @@ px4_add_board( vmount vtol_att_control SYSTEMCMDS - #config #dumpfile dyn esc_calib diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index c2f9e6f3ace7..89c9fcdea11f 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -49,7 +49,6 @@ px4_add_board( vmount vtol_att_control SYSTEMCMDS - #config #dumpfile dyn esc_calib diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake index 2a2fc1c029d1..979709629b3d 100644 --- a/boards/uvify/core/default.cmake +++ b/boards/uvify/core/default.cmake @@ -61,7 +61,6 @@ px4_add_board( vmount SYSTEMCMDS bl_update - config dmesg dumpfile esc_calib diff --git a/platforms/common/include/px4_platform_common/module_params.h b/platforms/common/include/px4_platform_common/module_params.h index d2e518f0cebc..4b4a46977d0e 100644 --- a/platforms/common/include/px4_platform_common/module_params.h +++ b/platforms/common/include/px4_platform_common/module_params.h @@ -52,6 +52,14 @@ class ModuleParams : public ListNode setParent(parent); } + // empty copy and move constructors (don't copy _children) + ModuleParams(const ModuleParams &) {}; + ModuleParams(ModuleParams &&) {}; + + // no assignment + ModuleParams &operator=(const ModuleParams &) = delete; + ModuleParams &operator=(ModuleParams &&) = delete; + /** * @brief Sets the parent module. This is typically not required, * only in cases where the parent cannot be set via constructor. @@ -65,12 +73,6 @@ class ModuleParams : public ListNode virtual ~ModuleParams() = default; - // Disallow copy construction and move assignment. - ModuleParams(const ModuleParams &) = delete; - ModuleParams &operator=(const ModuleParams &) = delete; - ModuleParams(ModuleParams &&) = delete; - ModuleParams &operator=(ModuleParams &&) = delete; - protected: /** * @brief Call this method whenever the module gets a parameter change notification. 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 49e7c9c890af..53492fd6ae6f 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 @@ -48,7 +48,7 @@ struct wq_config_t { namespace wq_configurations { -static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority +static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1800, 0}; // PX4 inner loop highest priority static constexpr wq_config_t SPI0{"wq:SPI0", 2000, -1}; static constexpr wq_config_t SPI1{"wq:SPI1", 2000, -2}; diff --git a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp index 4b591e3f4525..5ede856f7013 100644 --- a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp +++ b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp @@ -33,6 +33,8 @@ #include "CM8JL65.hpp" +#include + static constexpr unsigned char crc_msb_vector[] = { 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, diff --git a/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp b/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp index 6c6368ef0d8a..b44a5a723c9c 100644 --- a/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp +++ b/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp @@ -33,6 +33,7 @@ #include "LeddarOne.hpp" +#include #include #include diff --git a/src/drivers/distance_sensor/sf0x/SF0X.cpp b/src/drivers/distance_sensor/sf0x/SF0X.cpp index c05d129e02cb..4014ca767526 100644 --- a/src/drivers/distance_sensor/sf0x/SF0X.cpp +++ b/src/drivers/distance_sensor/sf0x/SF0X.cpp @@ -33,6 +33,7 @@ #include "SF0X.hpp" +#include #include /* Configuration Constants */ diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.cpp b/src/drivers/distance_sensor/tfmini/TFMINI.cpp index cefb74f2ab53..03e176d09006 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.cpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.cpp @@ -33,6 +33,8 @@ #include "TFMINI.hpp" +#include + TFMINI::TFMINI(const char *port, uint8_t rotation) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), _px4_rangefinder(0 /* TODO: device id */, ORB_PRIO_DEFAULT, rotation) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h deleted file mode 100644 index 00002ad7b223..000000000000 --- a/src/drivers/drv_accel.h +++ /dev/null @@ -1,75 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 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 drv_accel.h - * - * Accelerometer driver interface. - */ - -#ifndef _DRV_ACCEL_H -#define _DRV_ACCEL_H - -#include -#include - -#include "drv_sensor.h" -#include "drv_orb_dev.h" - -#define ACCEL_BASE_DEVICE_PATH "/dev/accel" - -#include - -/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ -struct accel_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; -}; -/* - * ioctl() definitions - * - * Accelerometer drivers also implement the generic sensor driver - * interfaces from drv_sensor.h - */ - -#define _ACCELIOCBASE (0x2100) -#define _ACCELIOC(_n) (_PX4_IOC(_ACCELIOCBASE, _n)) - -/** set the accel scaling constants to the structure pointed to by (arg) */ -#define ACCELIOCSSCALE _ACCELIOC(5) - -#endif /* _DRV_ACCEL_H */ diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h deleted file mode 100644 index 945ee92e97ae..000000000000 --- a/src/drivers/drv_gyro.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 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 drv_gyro.h - * - * Gyroscope driver interface. - */ - -#ifndef _DRV_GYRO_H -#define _DRV_GYRO_H - -#include -#include - -#include "drv_sensor.h" -#include "drv_orb_dev.h" - -#define GYRO_BASE_DEVICE_PATH "/dev/gyro" - -#include - -/** gyro scaling factors; Vout = Vin + Voffset */ -struct gyro_calibration_s { - float x_offset; - float y_offset; - float z_offset; -}; - -/* - * ioctl() definitions - */ - -#define _GYROIOCBASE (0x2300) -#define _GYROIOC(_n) (_PX4_IOC(_GYROIOCBASE, _n)) - -/** set the gyro scaling constants to (arg) */ -#define GYROIOCSSCALE _GYROIOC(4) - -#endif /* _DRV_GYRO_H */ diff --git a/src/drivers/imu/bma180/bma180.cpp b/src/drivers/imu/bma180/bma180.cpp index d802439f12cf..4f292fd9ad55 100644 --- a/src/drivers/imu/bma180/bma180.cpp +++ b/src/drivers/imu/bma180/bma180.cpp @@ -64,7 +64,6 @@ #include #include -#include #include #include @@ -425,15 +424,6 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) } } - case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; - - case ACCELIOCSSCALE: - /* copy scale in */ - memcpy(&_accel_scale, (struct accel_calibration_s *) arg, sizeof(_accel_scale)); - return OK; - default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index c1c14f0d448f..78a7b79a0e0c 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -56,8 +56,6 @@ #include #include #include -#include -#include #include #include #include diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index bf6345c7bb75..87e0b3a06eaf 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -64,7 +64,6 @@ static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit } PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) : - CDev(nullptr), _sensor_pub{ORB_ID(sensor_accel), priority}, _sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority}, _sensor_integrated_pub{ORB_ID(sensor_accel_integrated), priority}, @@ -73,36 +72,6 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro _rotation{rotation}, _rotation_dcm{get_rot_matrix(rotation)} { - _class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); -} - -PX4Accelerometer::~PX4Accelerometer() -{ - if (_class_device_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _class_device_instance); - } -} - -int PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case ACCELIOCSSCALE: { - // Copy offsets and scale factors in - accel_calibration_s cal{}; - memcpy(&cal, (accel_calibration_s *) arg, sizeof(cal)); - - _calibration_offset = Vector3f{cal.x_offset, cal.y_offset, cal.z_offset}; - _calibration_scale = Vector3f{cal.x_scale, cal.y_scale, cal.z_scale}; - } - - return PX4_OK; - - case DEVIOCGDEVICEID: - return _device_id; - - default: - return -ENOTTY; - } } void PX4Accelerometer::set_device_type(uint8_t devtype) @@ -208,17 +177,16 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample) // Apply rotation (before scaling) rotate_3f(_rotation, x, y, z); - // Apply range scale and the calibrating offset/scale - const Vector3f val_calibrated{((Vector3f{x, y, z} * _scale) - _calibration_offset).emult(_calibration_scale)}; + const Vector3f raw{x, y, z}; sensor_accel_s report; report.timestamp_sample = sample.timestamp_sample; report.device_id = _device_id; report.temperature = _temperature; - report.x = val_calibrated(0); - report.y = val_calibrated(1); - report.z = val_calibrated(2); + report.x = raw(0); + report.y = raw(1); + report.z = raw(2); report.timestamp = hrt_absolute_time(); _sensor_pub.publish(report); @@ -365,7 +333,10 @@ void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity) void PX4Accelerometer::print_status() { - PX4_INFO(ACCEL_BASE_DEVICE_PATH " device instance: %d", _class_device_instance); + char device_id_buffer[80] {}; + device::Device::device_id_print_buffer(device_id_buffer, sizeof(device_id_buffer), _device_id); + PX4_INFO("device id: %d (%s)", _device_id, device_id_buffer); + PX4_INFO("rotation: %d", _rotation); PX4_INFO("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1), (double)_calibration_scale(2)); diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index e4d6b06786e2..312bf58b790e 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -33,9 +33,7 @@ #pragma once -#include #include -#include #include #include #include @@ -46,13 +44,11 @@ #include #include -class PX4Accelerometer : public cdev::CDev +class PX4Accelerometer { public: PX4Accelerometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); - ~PX4Accelerometer() override; - - int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; + ~PX4Accelerometer() = default; uint32_t get_device_id() const { return _device_id; } @@ -106,8 +102,6 @@ class PX4Accelerometer : public cdev::CDev matrix::Vector3f _delta_velocity_prev{0.f, 0.f, 0.f}; // delta velocity from the previous IMU measurement float _vibration_metric{0.f}; // high frequency vibration level in the IMU delta velocity data (m/s) - int _class_device_instance{-1}; - uint32_t _device_id{0}; const enum Rotation _rotation; const matrix::Dcmf _rotation_dcm; @@ -132,5 +126,4 @@ class PX4Accelerometer : public cdev::CDev uint8_t _integrator_samples{0}; uint8_t _integrator_fifo_samples{0}; uint8_t _integrator_clipping{0}; - }; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 3bcd4fbeb46f..d8fb64f553ef 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -64,7 +64,6 @@ static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit } PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) : - CDev(nullptr), ModuleParams(nullptr), _sensor_pub{ORB_ID(sensor_gyro), priority}, _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority}, @@ -74,39 +73,9 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r _rotation{rotation}, _rotation_dcm{get_rot_matrix(rotation)} { - _class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); - updateParams(); } -PX4Gyroscope::~PX4Gyroscope() -{ - if (_class_device_instance != -1) { - unregister_class_devname(GYRO_BASE_DEVICE_PATH, _class_device_instance); - } -} - -int PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case GYROIOCSSCALE: { - // Copy offsets and scale factors in - gyro_calibration_s cal{}; - memcpy(&cal, (gyro_calibration_s *) arg, sizeof(cal)); - - _calibration_offset = Vector3f{cal.x_offset, cal.y_offset, cal.z_offset}; - } - - return PX4_OK; - - case DEVIOCGDEVICEID: - return _device_id; - - default: - return -ENOTTY; - } -} - void PX4Gyroscope::set_device_type(uint8_t devtype) { // current DeviceStructure @@ -372,7 +341,10 @@ void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle) void PX4Gyroscope::print_status() { - PX4_INFO(GYRO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance); + char device_id_buffer[80] {}; + device::Device::device_id_print_buffer(device_id_buffer, sizeof(device_id_buffer), _device_id); + PX4_INFO("device id: %d (%s)", _device_id, device_id_buffer); + PX4_INFO("rotation: %d", _rotation); PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1), (double)_calibration_offset(2)); diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 12198f49c3a5..8238bf35df7e 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2020 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 @@ -33,9 +33,7 @@ #pragma once -#include #include -#include #include #include #include @@ -46,13 +44,11 @@ #include #include -class PX4Gyroscope : public cdev::CDev, public ModuleParams +class PX4Gyroscope : public ModuleParams { public: PX4Gyroscope(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); - ~PX4Gyroscope() override; - - int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; + ~PX4Gyroscope() override = default; uint32_t get_device_id() const { return _device_id; } @@ -108,8 +104,6 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams float _vibration_metric{0.f}; // high frequency vibration level in the IMU delta angle data (rad) float _coning_vibration{0.f}; // Level of coning vibration in the IMU delta angles (rad^2) - int _class_device_instance{-1}; - uint32_t _device_id{0}; const enum Rotation _rotation; const matrix::Dcmf _rotation_dcm; diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp index 7bcdf36406de..4055849ec8ba 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp @@ -36,24 +36,13 @@ #include PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t priority, const uint8_t device_orientation) : - CDev(nullptr), _distance_sensor_pub{ORB_ID(distance_sensor), priority} { - _class_device_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - set_device_id(device_id); set_orientation(device_orientation); } -PX4Rangefinder::~PX4Rangefinder() -{ - if (_class_device_instance != -1) { - unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_device_instance); - } -} - -void -PX4Rangefinder::set_device_type(uint8_t device_type) +void PX4Rangefinder::set_device_type(uint8_t device_type) { // TODO: range finders should have device ids @@ -68,14 +57,12 @@ PX4Rangefinder::set_device_type(uint8_t device_type) // _distance_sensor_pub.get().device_id = device_id.devid; } -void -PX4Rangefinder::set_orientation(const uint8_t device_orientation) +void PX4Rangefinder::set_orientation(const uint8_t device_orientation) { _distance_sensor_pub.get().orientation = device_orientation; } -void -PX4Rangefinder::update(const hrt_abstime timestamp, const float distance, const int8_t quality) +void PX4Rangefinder::update(const hrt_abstime timestamp, const float distance, const int8_t quality) { distance_sensor_s &report = _distance_sensor_pub.get(); @@ -93,10 +80,6 @@ PX4Rangefinder::update(const hrt_abstime timestamp, const float distance, const _distance_sensor_pub.update(); } -void -PX4Rangefinder::print_status() +void PX4Rangefinder::print_status() { - PX4_INFO(RANGE_FINDER_BASE_DEVICE_PATH " device instance: %d", _class_device_instance); - - print_message(_distance_sensor_pub.get()); } diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index bbef35238416..efa6c6811451 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -35,20 +35,18 @@ #include #include -#include #include -#include #include #include -class PX4Rangefinder : public cdev::CDev +class PX4Rangefinder { public: PX4Rangefinder(const uint32_t device_id, const uint8_t priority = ORB_PRIO_DEFAULT, const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING); - ~PX4Rangefinder() override; + ~PX4Rangefinder() = default; void print_status(); @@ -72,6 +70,4 @@ class PX4Rangefinder : public cdev::CDev uORB::PublicationMultiData _distance_sensor_pub; - int _class_device_instance{-1}; - }; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 579f45841940..a654bae42ded 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -138,7 +138,6 @@ #include #include #include -#include #include #include #include @@ -146,6 +145,7 @@ #include #include #include +#include #include #include @@ -178,93 +178,20 @@ typedef struct { int do_accel_calibration(orb_advert_t *mavlink_log_pub) { -#if 1 // TODO: replace all IOCTL usage - int fd; -#endif - calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); - struct accel_calibration_s accel_scale; - accel_scale.x_offset = 0.0f; - accel_scale.x_scale = 1.0f; - accel_scale.y_offset = 0.0f; - accel_scale.y_scale = 1.0f; - accel_scale.z_offset = 0.0f; - accel_scale.z_scale = 1.0f; + float x_offset = 0.0f; + float x_scale = 1.0f; + float y_offset = 0.0f; + float y_scale = 1.0f; + float z_offset = 0.0f; + float z_scale = 1.0f; int res = PX4_OK; - char str[30]; - - /* reset all sensors */ - for (unsigned s = 0; s < max_accel_sens; s++) { -#if 1 // TODO: replace all IOCTL usage - sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); - /* reset all offsets to zero and all scales to one */ - fd = px4_open(str, 0); - - if (fd < 0) { - continue; - } - - device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - - res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); - px4_close(fd); - - if (res != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s); - } - -#else - (void)sprintf(str, "CAL_ACC%u_XOFF", s); - res = param_set_no_notification(param_find(str), &accel_scale.x_offset); - - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_YOFF", s); - res = param_set_no_notification(param_find(str), &accel_scale.y_offset); - - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_ZOFF", s); - res = param_set_no_notification(param_find(str), &accel_scale.z_offset); - - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_XSCALE", s); - res = param_set_no_notification(param_find(str), &accel_scale.x_scale); - - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_YSCALE", s); - res = param_set_no_notification(param_find(str), &accel_scale.y_scale); - - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_ZSCALE", s); - res = param_set_no_notification(param_find(str), &accel_scale.z_scale); - - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - param_notify_changes(); -#endif - } - - float accel_offs[max_accel_sens][3]; - float accel_T[max_accel_sens][3][3]; + char str[30] {}; + float accel_offs[max_accel_sens][3] {}; + float accel_T[max_accel_sens][3][3] {}; unsigned active_sensors = 0; /* measure and calculate offsets & scales */ @@ -292,15 +219,12 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) } /* measurements completed successfully, rotate calibration values */ - param_t board_rotation_h = param_find("SENS_BOARD_ROT"); - int32_t board_rotation_int; - param_get(board_rotation_h, &(board_rotation_int)); - enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; - Dcmf board_rotation = get_rot_matrix(board_rotation_id); + int32_t board_rotation_int = 0; + param_get(param_find("SENS_BOARD_ROT"), &board_rotation_int); + const Dcmf board_rotation = get_rot_matrix((enum Rotation)board_rotation_int); + const Dcmf board_rotation_t = board_rotation.transpose(); - Dcmf board_rotation_t = board_rotation.transpose(); - - bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator + bool tc_locked[3] {}; // true when the thermal parameter instance has already been adjusted by the calibrator for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) { @@ -310,30 +234,24 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) Matrix3f accel_T_mat(accel_T[uorb_index]); Matrix3f accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation; - accel_scale.x_offset = accel_offs_rotated(0); - accel_scale.x_scale = accel_T_rotated(0, 0); - accel_scale.y_offset = accel_offs_rotated(1); - accel_scale.y_scale = accel_T_rotated(1, 1); - accel_scale.z_offset = accel_offs_rotated(2); - accel_scale.z_scale = accel_T_rotated(2, 2); + x_offset = accel_offs_rotated(0); + x_scale = accel_T_rotated(0, 0); + y_offset = accel_offs_rotated(1); + y_scale = accel_T_rotated(1, 1); + z_offset = accel_offs_rotated(2); + z_scale = accel_T_rotated(2, 2); bool failed = false; failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); - - PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, - (double)accel_scale.x_offset, - (double)accel_scale.y_offset, - (double)accel_scale.z_offset); - PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, - (double)accel_scale.x_scale, - (double)accel_scale.y_scale, - (double)accel_scale.z_scale); + PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, (double)x_offset, (double)y_offset, + (double)z_offset); + PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, (double)x_scale, (double)y_scale, (double)z_scale); /* check if thermal compensation is enabled */ - int32_t tc_enabled_int; - param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int)); + int32_t tc_enabled_int = 0; + param_get(param_find("TC_A_ENABLE"), &tc_enabled_int); if (tc_enabled_int == 1) { /* Get struct containing sensor thermal compensation data */ @@ -346,23 +264,20 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) tc_locked[sensor_correction.accel_mapping[uorb_index]] = true; /* update the _X0_ terms to include the additional offset */ - int32_t handle; - float val; - for (unsigned axis_index = 0; axis_index < 3; axis_index++) { - val = 0.0f; - (void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index); - handle = param_find(str); + sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index); + param_t handle = param_find(str); + float val = 0.0f; param_get(handle, &val); if (axis_index == 0) { - val += accel_scale.x_offset; + val += x_offset; } else if (axis_index == 1) { - val += accel_scale.y_offset; + val += y_offset; } else if (axis_index == 2) { - val += accel_scale.z_offset; + val += z_offset; } failed |= (PX4_OK != param_set_no_notification(handle, &val)); @@ -370,18 +285,20 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) /* update the _SCL_ terms to include the scale factor */ for (unsigned axis_index = 0; axis_index < 3; axis_index++) { - val = 1.0f; - (void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index); - handle = param_find(str); + + sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index); + param_t handle = param_find(str); + + float val = 1.0f; if (axis_index == 0) { - val = accel_scale.x_scale; + val = x_scale; } else if (axis_index == 1) { - val = accel_scale.y_scale; + val = y_scale; } else if (axis_index == 2) { - val = accel_scale.z_scale; + val = z_scale; } failed |= (PX4_OK != param_set_no_notification(handle, &val)); @@ -391,27 +308,27 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) } // Ensure the calibration values used by the driver are at default settings when we are using thermal calibration data - accel_scale.x_offset = 0.f; - accel_scale.y_offset = 0.f; - accel_scale.z_offset = 0.f; - accel_scale.x_scale = 1.f; - accel_scale.y_scale = 1.f; - accel_scale.z_scale = 1.f; + x_offset = 0.f; + y_offset = 0.f; + z_offset = 0.f; + x_scale = 1.f; + y_scale = 1.f; + z_scale = 1.f; } // save the driver level calibration data (void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index); - failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); + failed |= (PX4_OK != param_set_no_notification(param_find(str), &(x_offset))); (void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index); - failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset))); + failed |= (PX4_OK != param_set_no_notification(param_find(str), &(y_offset))); (void)sprintf(str, "CAL_ACC%u_ZOFF", uorb_index); - failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset))); + failed |= (PX4_OK != param_set_no_notification(param_find(str), &(z_offset))); (void)sprintf(str, "CAL_ACC%u_XSCALE", uorb_index); - failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale))); + failed |= (PX4_OK != param_set_no_notification(param_find(str), &(x_scale))); (void)sprintf(str, "CAL_ACC%u_YSCALE", uorb_index); - failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale))); + failed |= (PX4_OK != param_set_no_notification(param_find(str), &(y_scale))); (void)sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index); - failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale))); + failed |= (PX4_OK != param_set_no_notification(param_find(str), &(z_scale))); (void)sprintf(str, "CAL_ACC%u_ID", uorb_index); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index]))); @@ -419,25 +336,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); return PX4_ERROR; } - -#if 1 // TODO: replace all IOCTL usage - sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, uorb_index); - fd = px4_open(str, 0); - - if (fd < 0) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist"); - res = PX4_ERROR; - - } else { - res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); - px4_close(fd); - } - - if (res != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG); - } - -#endif } if (res == PX4_OK) { @@ -501,7 +399,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub worker_data.subs[i] = -1; } - uint64_t timestamps[max_accel_sens] = {}; + uint64_t timestamps[max_accel_sens] {}; // We should not try to subscribe if the topic doesn't actually exist and can be counted. const unsigned orb_accel_count = orb_group_count(ORB_ID(sensor_accel)); @@ -520,32 +418,12 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub for (unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) { worker_data.subs[cur_accel] = orb_subscribe_multi(ORB_ID(sensor_accel), i); - sensor_accel_s report = {}; + sensor_accel_s report{}; orb_copy(ORB_ID(sensor_accel), worker_data.subs[cur_accel], &report); -#if 1 // TODO: replace all IOCTL usage - - // For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL - // and match it up with the one from the uORB subscription, because the - // instance ordering of uORB and the order of the FDs may not be the same. - - if (report.device_id == (uint32_t)device_id[cur_accel]) { - // Device IDs match, correct ORB instance for this accel - found_cur_accel = true; - // store initial timestamp - used to infer which sensors are active - timestamps[cur_accel] = report.timestamp; - - } else { - orb_unsubscribe(worker_data.subs[cur_accel]); - } - -#else - // For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report. device_id[cur_accel] = report.device_id; found_cur_accel = true; - -#endif } if (!found_cur_accel) { diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index e86f9abd29ec..168a42f7b85f 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 044ba15211c5..7992f29cc7f8 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -53,38 +53,42 @@ #include #include #include +#include #include #include -#include +#include #include #include #include -static const char *sensor_name = "gyro"; - +static constexpr char sensor_name[] {"gyro"}; static constexpr unsigned max_gyros = 3; +struct gyro_calibration_s { + float x_offset; + float y_offset; + float z_offset; +}; + /// Data passed to calibration worker routine typedef struct { - orb_advert_t *mavlink_log_pub; - int32_t device_id[max_gyros]; - int gyro_sensor_sub[max_gyros]; - int sensor_correction_sub; - struct gyro_calibration_s gyro_scale[max_gyros]; - float last_sample_0[3]; + orb_advert_t *mavlink_log_pub{nullptr}; + int32_t device_id[max_gyros] {}; + int gyro_sensor_sub[max_gyros] {-1, -1, -1}; + uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; + gyro_calibration_s gyro_scale[max_gyros] {}; + float last_sample_0[3] {}; } gyro_worker_data_t; -static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) +static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data_t *worker_data) { - gyro_worker_data_t *worker_data = (gyro_worker_data_t *)(data); - unsigned calibration_counter[max_gyros] = { 0 }, slow_count = 0; - const unsigned calibration_count = 250; - sensor_gyro_s gyro_report; - unsigned poll_errcount = 0; + unsigned calibration_counter[max_gyros] {}; + static constexpr unsigned calibration_count = 250; + unsigned poll_errcount = 0; - struct sensor_correction_s sensor_correction {}; /**< sensor thermal corrections */ + sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */ - if (orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction) != 0) { + if (!worker_data->sensor_correction_sub.copy(&sensor_correction)) { for (unsigned i = 0; i < 3; i++) { sensor_correction.gyro_scale_0[i] = 1.0f; sensor_correction.gyro_scale_1[i] = 1.0f; @@ -92,7 +96,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) } } - px4_pollfd_struct_t fds[max_gyros]; + px4_pollfd_struct_t fds[max_gyros] {}; for (unsigned s = 0; s < max_gyros; s++) { fds[s].fd = worker_data->gyro_sensor_sub[s]; @@ -102,18 +106,15 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) memset(&worker_data->last_sample_0, 0, sizeof(worker_data->last_sample_0)); /* use slowest gyro to pace, but count correctly per-gyro for statistics */ + unsigned slow_count = 0; + while (slow_count < calibration_count) { if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) { return calibrate_return_cancelled; } /* check if there are new thermal corrections */ - bool updated; - orb_check(worker_data->sensor_correction_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction); - } + worker_data->sensor_correction_sub.update(&sensor_correction); int poll_ret = px4_poll(&fds[0], max_gyros, 1000); @@ -126,12 +127,13 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) continue; } - bool changed; + bool changed = false; orb_check(worker_data->gyro_sensor_sub[s], &changed); if (changed) { + sensor_gyro_s gyro_report{}; orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report); - float sample[3]; + float sample[3] {}; if (s == 0) { // take a working copy @@ -157,21 +159,19 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) sample[0] = gyro_report.x; sample[1] = gyro_report.y; sample[2] = gyro_report.z; - } worker_data->gyro_scale[s].x_offset += sample[0]; worker_data->gyro_scale[s].y_offset += sample[1]; worker_data->gyro_scale[s].z_offset += sample[2]; - calibration_counter[s]++; + calibration_counter[s]++; } // Maintain the sample count of the slowest sensor if (calibration_counter[s] && calibration_counter[s] < update_count) { update_count = calibration_counter[s]; } - } if (update_count % (calibration_count / 20) == 0) { @@ -209,76 +209,33 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) int do_gyro_calibration(orb_advert_t *mavlink_log_pub) { - int res = PX4_OK; - gyro_worker_data_t worker_data = {}; + int res = PX4_OK; + gyro_worker_data_t worker_data{}; calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); worker_data.mavlink_log_pub = mavlink_log_pub; - gyro_calibration_s gyro_scale_zero{}; int device_prio_max = 0; int32_t device_id_primary = 0; - worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); - for (unsigned s = 0; s < max_gyros; s++) { - char str[30]; // Reset gyro ids to unavailable. worker_data.device_id[s] = 0; + // And set default subscriber values. worker_data.gyro_sensor_sub[s] = -1; + + char str[30] {}; (void)sprintf(str, "CAL_GYRO%u_ID", s); - res = param_set_no_notification(param_find(str), &(worker_data.device_id[s])); + + res = param_set_no_notification(param_find(str), &worker_data.device_id[s]); if (res != PX4_OK) { calibration_log_critical(mavlink_log_pub, "Unable to reset CAL_GYRO%u_ID", s); return PX4_ERROR; } - - // Reset all offsets to 0 - (void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero)); -#if 1 // TODO: replace all IOCTL usage - sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = px4_open(str, 0); - - if (fd >= 0) { - worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); - px4_close(fd); - - if (res != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s); - return PX4_ERROR; - } - } - -#else - (void)sprintf(str, "CAL_GYRO%u_XOFF", s); - res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_offset); - - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "CAL_GYRO%u_YOFF", s); - res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_offset); - - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); - res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_offset); - - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } - - param_notify_changes(); -#endif - } // We should not try to subscribe if the topic doesn't actually exist and can be counted. @@ -291,48 +248,14 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) for (unsigned cur_gyro = 0; cur_gyro < orb_gyro_count && cur_gyro < max_gyros; cur_gyro++) { - // Lock in to correct ORB instance - bool found_cur_gyro = false; - - for (unsigned i = 0; i < orb_gyro_count && !found_cur_gyro; i++) { - worker_data.gyro_sensor_sub[cur_gyro] = orb_subscribe_multi(ORB_ID(sensor_gyro), i); - - sensor_gyro_s report{}; - orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[cur_gyro], &report); - -#if 1 // TODO: replace all IOCTL usage - - // For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL - // and match it up with the one from the uORB subscription, because the - // instance ordering of uORB and the order of the FDs may not be the same. - - if (report.device_id == (uint32_t)worker_data.device_id[cur_gyro]) { - // Device IDs match, correct ORB instance for this gyro - found_cur_gyro = true; - - } else { - orb_unsubscribe(worker_data.gyro_sensor_sub[cur_gyro]); - } - -#else - - // For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report. - worker_data.device_id[cur_gyro] = report.device_id; - found_cur_gyro = true; - -#endif - } - - if (!found_cur_gyro) { - calibration_log_critical(mavlink_log_pub, "Gyro #%u (ID %u) no matching uORB devid", cur_gyro, - worker_data.device_id[cur_gyro]); - res = calibrate_return_error; - break; - } + worker_data.gyro_sensor_sub[cur_gyro] = orb_subscribe_multi(ORB_ID(sensor_gyro), cur_gyro); + sensor_gyro_s report{}; + orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[cur_gyro], &report); + worker_data.device_id[cur_gyro] = report.device_id; if (worker_data.device_id[cur_gyro] != 0) { // Get priority - int32_t prio; + int32_t prio = 0; orb_priority(worker_data.gyro_sensor_sub[cur_gyro], &prio); if (prio > device_prio_max) { @@ -345,7 +268,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) } } - int cancel_sub = calibrate_cancel_subscribe(); + int cancel_sub = calibrate_cancel_subscribe(); unsigned try_count = 0; unsigned max_tries = 20; @@ -407,36 +330,33 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) /* set offset parameters to new values */ bool failed = false; - failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary))); + failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &device_id_primary)); - bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator + bool tc_locked[3] {}; // true when the thermal parameter instance has already been adjusted by the calibrator for (unsigned uorb_index = 0; uorb_index < max_gyros; uorb_index++) { if (worker_data.device_id[uorb_index] != 0) { - char str[30]; - /* check if thermal compensation is enabled */ - int32_t tc_enabled_int; - param_get(param_find("TC_G_ENABLE"), &(tc_enabled_int)); + int32_t tc_enabled_int = 0; + param_get(param_find("TC_G_ENABLE"), &tc_enabled_int); if (tc_enabled_int == 1) { /* Get struct containing sensor thermal compensation data */ - struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */ - memset(&sensor_correction, 0, sizeof(sensor_correction)); - orb_copy(ORB_ID(sensor_correction), worker_data.sensor_correction_sub, &sensor_correction); + sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */ + worker_data.sensor_correction_sub.copy(&sensor_correction); /* don't allow a parameter instance to be calibrated again by another uORB instance */ if (!tc_locked[sensor_correction.gyro_mapping[uorb_index]]) { + tc_locked[sensor_correction.gyro_mapping[uorb_index]] = true; /* update the _X0_ terms to include the additional offset */ - int32_t handle; - float val; - for (unsigned axis_index = 0; axis_index < 3; axis_index++) { - val = 0.0f; - (void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index); - handle = param_find(str); + + char str[30] {}; + sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index); + param_t handle = param_find(str); + float val = 0.0f; param_get(handle, &val); if (axis_index == 0) { @@ -447,13 +367,10 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) } else if (axis_index == 2) { val += worker_data.gyro_scale[uorb_index].z_offset; - } failed |= (PX4_OK != param_set_no_notification(handle, &val)); } - - param_notify_changes(); } // Ensure the calibration values used the driver are at default settings @@ -462,6 +379,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) worker_data.gyro_scale[uorb_index].z_offset = 0.f; } + char str[30] {}; (void)sprintf(str, "CAL_GYRO%u_XOFF", uorb_index); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].x_offset))); (void)sprintf(str, "CAL_GYRO%u_YOFF", uorb_index); @@ -471,25 +389,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) (void)sprintf(str, "CAL_GYRO%u_ID", uorb_index); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.device_id[uorb_index]))); - -#if 1 // TODO: replace all IOCTL usage - /* apply new scaling and offsets */ - (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, uorb_index); - int fd = px4_open(str, 0); - - if (fd < 0) { - failed = true; - continue; - } - - res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[uorb_index]); - px4_close(fd); - - if (res != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG); - } - -#endif } } @@ -499,6 +398,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) } } + param_notify_changes(); + /* if there is a any preflight-check system response, let the barrage of messages through */ px4_usleep(200000); @@ -509,8 +410,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); } - orb_unsubscribe(worker_data.sensor_correction_sub); - /* give this message enough time to propagate */ px4_usleep(600000); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 1a45c10e1937..72a390289d76 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -52,14 +52,13 @@ #include #include #include -#include -#include #include #include +#include #include #include #include -#include +#include static const char *sensor_name = "mag"; static constexpr unsigned max_mags = 4; @@ -369,17 +368,15 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta hrt_abstime detection_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds * 5; hrt_abstime last_gyro = 0; - float gyro_x_integral = 0.0f; - float gyro_y_integral = 0.0f; - float gyro_z_integral = 0.0f; + matrix::Vector3f gyro_integral{0.0f, 0.0f, 0.0f}; const float gyro_int_thresh_rad = 0.5f; - int sub_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + int sub_gyro = orb_subscribe(ORB_ID(vehicle_angular_velocity)); - while (fabsf(gyro_x_integral) < gyro_int_thresh_rad && - fabsf(gyro_y_integral) < gyro_int_thresh_rad && - fabsf(gyro_z_integral) < gyro_int_thresh_rad) { + while (fabsf(gyro_integral(0)) < gyro_int_thresh_rad && + fabsf(gyro_integral(1)) < gyro_int_thresh_rad && + fabsf(gyro_integral(2)) < gyro_int_thresh_rad) { /* abort on request */ if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) { @@ -391,7 +388,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta /* abort with timeout */ if (hrt_absolute_time() > detection_deadline) { result = calibrate_return_error; - warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral); + warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_integral(0), (double)gyro_integral(1), (double)gyro_integral(2)); calibration_log_critical(worker_data->mavlink_log_pub, "Failed: This calibration requires rotation."); break; } @@ -405,17 +402,14 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { - sensor_gyro_s gyro{}; - orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro); + vehicle_angular_velocity_s gyro{}; + orb_copy(ORB_ID(vehicle_angular_velocity), sub_gyro, &gyro); /* ensure we have a valid first timestamp */ if (last_gyro > 0) { - - /* integrate */ + // integrate float delta_t = (gyro.timestamp - last_gyro) / 1e6f; - gyro_x_integral += gyro.x * delta_t; - gyro_y_integral += gyro.y * delta_t; - gyro_z_integral += gyro.z * delta_t; + gyro_integral += matrix::Vector3f{gyro.xyz} * delta_t; } last_gyro = gyro.timestamp; diff --git a/src/modules/sensors/sensor_corrections/CMakeLists.txt b/src/modules/sensors/sensor_corrections/CMakeLists.txt index 932e2029c5bd..560975ebd31e 100644 --- a/src/modules/sensors/sensor_corrections/CMakeLists.txt +++ b/src/modules/sensors/sensor_corrections/CMakeLists.txt @@ -35,3 +35,5 @@ px4_add_library(sensor_corrections SensorCorrections.cpp SensorCorrections.hpp ) + +target_include_directories(sensor_corrections PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/..) diff --git a/src/modules/sensors/sensor_corrections/SensorCorrections.cpp b/src/modules/sensors/sensor_corrections/SensorCorrections.cpp index f5b51d523bcb..75890208b0ec 100644 --- a/src/modules/sensors/sensor_corrections/SensorCorrections.cpp +++ b/src/modules/sensors/sensor_corrections/SensorCorrections.cpp @@ -51,6 +51,7 @@ void SensorCorrections::set_device_id(uint32_t device_id) if (_device_id != device_id) { _device_id = device_id; SensorCorrectionsUpdate(true); + ParametersUpdate(); } } @@ -67,6 +68,67 @@ const char *SensorCorrections::SensorString() const return nullptr; } +int SensorCorrections::FindCalibrationIndex(uint32_t device_id) const +{ + if (device_id == 0) { + return -1; + } + + for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) { + char str[16] {}; + sprintf(str, "CAL_%s%u_ID", SensorString(), i); + + int32_t device_id_val = 0; + + if (param_get(param_find(str), &device_id_val) != OK) { + PX4_ERR("Could not access param %s", str); + continue; + } + + if ((uint32_t)device_id_val == device_id) { + return i; + } + } + + return -1; +} + +Vector3f SensorCorrections::CalibrationOffset(uint8_t calibration_index) const +{ + // offsets (x, y, z) + Vector3f offset{0.f, 0.f, 0.f}; + char str[20] {}; + + sprintf(str, "CAL_%s%u_XOFF", SensorString(), calibration_index); + param_get(param_find(str), &offset(0)); + + sprintf(str, "CAL_%s%u_YOFF", SensorString(), calibration_index); + param_get(param_find(str), &offset(1)); + + sprintf(str, "CAL_%s%u_ZOFF", SensorString(), calibration_index); + param_get(param_find(str), &offset(2)); + + return offset; +} + +Vector3f SensorCorrections::CalibrationScale(uint8_t calibration_index) const +{ + // scale factors (x, y, z) + Vector3f scale{1.f, 1.f, 1.f}; + char str[20] {}; + + sprintf(str, "CAL_%s%u_XSCALE", SensorString(), calibration_index); + param_get(param_find(str), &scale(0)); + + sprintf(str, "CAL_%s%u_YSCALE", SensorString(), calibration_index); + param_get(param_find(str), &scale(1)); + + sprintf(str, "CAL_%s%u_ZSCALE", SensorString(), calibration_index); + param_get(param_find(str), &scale(2)); + + return scale; +} + void SensorCorrections::SensorCorrectionsUpdate(bool force) { // check if the selected sensor has updated @@ -152,10 +214,35 @@ void SensorCorrections::ParametersUpdate() // get transformation matrix from sensor/board to body frame _board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); + + // temperature calibration disabled + if (_corrections_selected_instance < 0) { + int calibration_index = FindCalibrationIndex(_device_id); + + if (calibration_index >= 0) { + _offset = CalibrationOffset(calibration_index); + + // gyroscope doesn't have a scale factor calibration + if (_type != SensorType::Gyroscope) { + _scale = CalibrationScale(calibration_index); + + } else { + _scale = Vector3f{1.f, 1.f, 1.f}; + } + + } else { + _offset.zero(); + _scale = Vector3f{1.f, 1.f, 1.f}; + } + } } void SensorCorrections::PrintStatus() { + if (_corrections_selected_instance >= 0) { + PX4_INFO("%s %d temperature calibration enabled", SensorString(), _device_id); + } + if (_offset.norm() > 0.f) { PX4_INFO("%s %d offset: [%.3f %.3f %.3f]", SensorString(), _device_id, (double)_offset(0), (double)_offset(1), (double)_offset(2)); diff --git a/src/modules/sensors/sensor_corrections/SensorCorrections.hpp b/src/modules/sensors/sensor_corrections/SensorCorrections.hpp index 75881e72f884..e0d5fb33ce48 100644 --- a/src/modules/sensors/sensor_corrections/SensorCorrections.hpp +++ b/src/modules/sensors/sensor_corrections/SensorCorrections.hpp @@ -53,7 +53,6 @@ class SensorCorrections : public ModuleParams Gyroscope, }; - SensorCorrections() = delete; SensorCorrections(ModuleParams *parent, SensorType type); ~SensorCorrections() override = default; @@ -73,6 +72,11 @@ class SensorCorrections : public ModuleParams static constexpr int MAX_SENSOR_COUNT = 3; + int FindCalibrationIndex(uint32_t device_id) const; + + matrix::Vector3f CalibrationOffset(uint8_t calibration_index) const; + matrix::Vector3f CalibrationScale(uint8_t calibration_index) const; + const char *SensorString() const; uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index a53ffef54454..83d5d50e8049 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -51,22 +51,11 @@ using namespace sensors; using namespace matrix; -VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled) - : _parameters(parameters), _hil_enabled(hil_enabled) +VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled) : + ModuleParams(nullptr), + _parameters(parameters), + _hil_enabled(hil_enabled) { - for (unsigned i = 0; i < 3; i++) { - _corrections.gyro_scale_0[i] = 1.0f; - _corrections.accel_scale_0[i] = 1.0f; - _corrections.gyro_scale_1[i] = 1.0f; - _corrections.accel_scale_1[i] = 1.0f; - _corrections.gyro_scale_2[i] = 1.0f; - _corrections.accel_scale_2[i] = 1.0f; - } - - _corrections.baro_scale_0 = 1.0f; - _corrections.baro_scale_1 = 1.0f; - _corrections.baro_scale_2 = 1.0f; - _baro.voter.set_timeout(300000); _mag.voter.set_timeout(300000); _mag.voter.set_equal_value_threshold(1000); @@ -138,19 +127,11 @@ void VotedSensorsUpdate::parametersUpdate() unsigned gyro_count = 0; unsigned gyro_cal_found_count = 0; - for (unsigned driver_index = 0; driver_index < GYRO_COUNT_MAX; driver_index++) { + for (uint8_t driver_index = 0; driver_index < GYRO_COUNT_MAX; driver_index++) { + uORB::SubscriptionData report{ORB_ID(sensor_gyro_integrated), driver_index}; _gyro.enabled[driver_index] = true; - char str[30] {}; - (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, driver_index); - - int fd = px4_open(str, O_RDWR); - - if (fd < 0) { - continue; - } - - uint32_t driver_device_id = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + uint32_t driver_device_id = report.get().device_id; bool config_ok = false; /* run through all stored calibrations that are applied at the driver level*/ @@ -158,8 +139,9 @@ void VotedSensorsUpdate::parametersUpdate() /* initially status is ok per config */ failed = false; + char str[16] {}; (void)sprintf(str, "CAL_GYRO%u_ID", i); - int32_t device_id = 0; + int32_t device_id = -1; failed = failed || (PX4_OK != param_get(param_find(str), &device_id)); (void)sprintf(str, "CAL_GYRO%u_EN", i); @@ -182,29 +164,6 @@ void VotedSensorsUpdate::parametersUpdate() _gyro.priority[driver_index] = 0; } - gyro_calibration_s gscale{}; - - (void)sprintf(str, "CAL_GYRO%u_XOFF", i); - failed = failed || (PX4_OK != param_get(param_find(str), &gscale.x_offset)); - - (void)sprintf(str, "CAL_GYRO%u_YOFF", i); - failed = failed || (PX4_OK != param_get(param_find(str), &gscale.y_offset)); - - (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); - failed = failed || (PX4_OK != param_get(param_find(str), &gscale.z_offset)); - - if (failed) { - PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); - - } else { - /* apply new scaling and offsets */ - config_ok = (px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale) == 0); - - if (!config_ok) { - PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i); - } - } - break; } } @@ -212,41 +171,17 @@ void VotedSensorsUpdate::parametersUpdate() if (config_ok) { gyro_count++; } - - px4_close(fd); - } - - // There are fewer gyros than calibrations - // reset the board calibration and fail the initial load - if (gyro_count < gyro_cal_found_count) { - PX4_ERR("fewer accels than calibrations, resetting all CAL_GYROx_ID"); - - // run through all stored calibrations and reset them - for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) { - char str[30] {}; - (void)sprintf(str, "CAL_GYRO%u_ID", i); - int32_t device_id = 0; - (void)param_set(param_find(str), &device_id); - } } /* run through all accel sensors */ unsigned accel_count = 0; unsigned accel_cal_found_count = 0; - for (unsigned driver_index = 0; driver_index < ACCEL_COUNT_MAX; driver_index++) { + for (uint8_t driver_index = 0; driver_index < ACCEL_COUNT_MAX; driver_index++) { + uORB::SubscriptionData report{ORB_ID(sensor_accel_integrated), driver_index}; _accel.enabled[driver_index] = true; - char str[30] {}; - (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, driver_index); - - int fd = px4_open(str, O_RDWR); - - if (fd < 0) { - continue; - } - - uint32_t driver_device_id = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + uint32_t driver_device_id = report.get().device_id; bool config_ok = false; /* run through all stored calibrations */ @@ -254,8 +189,9 @@ void VotedSensorsUpdate::parametersUpdate() /* initially status is ok per config */ failed = false; + char str[16] {}; (void)sprintf(str, "CAL_ACC%u_ID", i); - int32_t device_id = 0; + int32_t device_id = -1; failed = failed || (PX4_OK != param_get(param_find(str), &device_id)); (void)sprintf(str, "CAL_ACC%u_EN", i); @@ -278,38 +214,6 @@ void VotedSensorsUpdate::parametersUpdate() _accel.priority[driver_index] = 0; } - accel_calibration_s ascale{}; - - (void)sprintf(str, "CAL_ACC%u_XOFF", i); - failed = failed || (PX4_OK != param_get(param_find(str), &ascale.x_offset)); - - (void)sprintf(str, "CAL_ACC%u_YOFF", i); - failed = failed || (PX4_OK != param_get(param_find(str), &ascale.y_offset)); - - (void)sprintf(str, "CAL_ACC%u_ZOFF", i); - failed = failed || (PX4_OK != param_get(param_find(str), &ascale.z_offset)); - - (void)sprintf(str, "CAL_ACC%u_XSCALE", i); - failed = failed || (PX4_OK != param_get(param_find(str), &ascale.x_scale)); - - (void)sprintf(str, "CAL_ACC%u_YSCALE", i); - failed = failed || (PX4_OK != param_get(param_find(str), &ascale.y_scale)); - - (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); - failed = failed || (PX4_OK != param_get(param_find(str), &ascale.z_scale)); - - if (failed) { - PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel", i); - - } else { - /* apply new scaling and offsets */ - config_ok = (px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale) == 0); - - if (!config_ok) { - PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel ", i); - } - } - break; } } @@ -317,22 +221,6 @@ void VotedSensorsUpdate::parametersUpdate() if (config_ok) { accel_count++; } - - px4_close(fd); - } - - // There are fewer accels than calibrations - // reset the board calibration and fail the initial load - if (accel_count < accel_cal_found_count) { - PX4_ERR("fewer accels than calibrations, resetting all CAL_ACCx_ID"); - - // run through all stored calibrations and reset them - for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) { - char str[30] {}; - (void)sprintf(str, "CAL_ACC%u_ID", i); - int32_t device_id = 0; - (void)param_set(param_find(str), &device_id); - } } /* run through all mag sensors @@ -362,7 +250,7 @@ void VotedSensorsUpdate::parametersUpdate() // find the driver handle that matches the topic_device_id int fd = -1; - char str[30] {}; + char str[16] {}; for (unsigned driver_index = 0; driver_index < MAG_COUNT_MAX; ++driver_index) { @@ -489,9 +377,6 @@ void VotedSensorsUpdate::parametersUpdate() void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw) { - float *offsets[] = {_corrections.accel_offset_0, _corrections.accel_offset_1, _corrections.accel_offset_2 }; - float *scales[] = {_corrections.accel_scale_0, _corrections.accel_scale_1, _corrections.accel_scale_2 }; - for (int uorb_index = 0; uorb_index < _accel.subscription_count; uorb_index++) { bool accel_updated; orb_check(_accel.subscription[uorb_index], &accel_updated); @@ -514,26 +399,16 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw) int32_t priority = 0; orb_priority(_accel.subscription[uorb_index], &priority); _accel.priority[uorb_index] = (uint8_t)priority; + _accel_corrections[uorb_index].set_device_id(accel_report.device_id); } _accel_device_id[uorb_index] = accel_report.device_id; - - /* - * Correct the raw sensor data for scale factor errors - * and offsets due to temperature variation. It is assumed that any filtering of input - * data required is performed in the sensor driver, preferably before downsampling. - */ + _last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.dt; // convert the delta velocities to an equivalent acceleration before application of corrections const float dt_inv = 1.e6f / (float)accel_report.dt; - Vector3f accel_data = Vector3f{accel_report.delta_velocity} * dt_inv; - - _last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.dt; - // apply temperature compensation - accel_data(0) = (accel_data(0) - offsets[uorb_index][0]) * scales[uorb_index][0]; // X - accel_data(1) = (accel_data(1) - offsets[uorb_index][1]) * scales[uorb_index][1]; // Y - accel_data(2) = (accel_data(2) - offsets[uorb_index][2]) * scales[uorb_index][2]; // Z + Vector3f accel_data{_accel_corrections[uorb_index].Correct(Vector3f{accel_report.delta_velocity} * dt_inv)}; // rotate corrected measurements from sensor to body frame accel_data = _board_rotation * accel_data; @@ -570,9 +445,6 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw) void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw) { - float *offsets[] = {_corrections.gyro_offset_0, _corrections.gyro_offset_1, _corrections.gyro_offset_2 }; - float *scales[] = {_corrections.gyro_scale_0, _corrections.gyro_scale_1, _corrections.gyro_scale_2 }; - for (int uorb_index = 0; uorb_index < _gyro.subscription_count; uorb_index++) { bool gyro_updated; orb_check(_gyro.subscription[uorb_index], &gyro_updated); @@ -595,26 +467,16 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw) int32_t priority = 0; orb_priority(_gyro.subscription[uorb_index], &priority); _gyro.priority[uorb_index] = (uint8_t)priority; + _gyro_corrections[uorb_index].set_device_id(gyro_report.device_id); } _gyro_device_id[uorb_index] = gyro_report.device_id; - - /* - * Correct the raw sensor data for scale factor errors - * and offsets due to temperature variation. It is assumed that any filtering of input - * data required is performed in the sensor driver, preferably before downsampling. - */ + _last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.dt; // convert the delta angles to an equivalent angular rate before application of corrections const float dt_inv = 1.e6f / (float)gyro_report.dt; - Vector3f gyro_rate = Vector3f{gyro_report.delta_angle} * dt_inv; - - _last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.dt; - // apply temperature compensation - gyro_rate(0) = (gyro_rate(0) - offsets[uorb_index][0]) * scales[uorb_index][0]; // X - gyro_rate(1) = (gyro_rate(1) - offsets[uorb_index][1]) * scales[uorb_index][1]; // Y - gyro_rate(2) = (gyro_rate(2) - offsets[uorb_index][2]) * scales[uorb_index][2]; // Z + Vector3f gyro_rate{_gyro_corrections[uorb_index].Correct(Vector3f{gyro_report.delta_angle} * dt_inv)}; // rotate corrected measurements from sensor to body frame gyro_rate = _board_rotation * gyro_rate; @@ -718,8 +580,6 @@ void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer) void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata) { bool got_update = false; - float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2 }; - float *scales[] = {&_corrections.baro_scale_0, &_corrections.baro_scale_1, &_corrections.baro_scale_2 }; for (int uorb_index = 0; uorb_index < _baro.subscription_count; uorb_index++) { bool baro_updated; @@ -738,7 +598,7 @@ void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata) float corrected_pressure = 100.0f * baro_report.pressure; // apply temperature compensation - corrected_pressure = (corrected_pressure - *offsets[uorb_index]) * *scales[uorb_index]; + corrected_pressure = (corrected_pressure - _baro_offset[uorb_index]) * _baro_scale[uorb_index]; // First publication with data if (_baro.priority[uorb_index] == 0) { @@ -924,7 +784,19 @@ void VotedSensorsUpdate::printStatus() void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata, vehicle_magnetometer_s &magnetometer) { - _corrections_sub.update(&_corrections); + if (_corrections_sub.updated()) { + sensor_correction_s corrections; + + if (_corrections_sub.copy(&corrections)) { + _baro_offset[0] = corrections.baro_offset_0; + _baro_offset[1] = corrections.baro_offset_1; + _baro_offset[2] = corrections.baro_offset_2; + + _baro_scale[0] = corrections.baro_scale_0; + _baro_scale[1] = corrections.baro_scale_1; + _baro_scale[1] = corrections.baro_scale_2; + } + } accelPoll(raw); gyroPoll(raw); diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 7643fc316d8e..525164ad8e45 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -41,22 +41,19 @@ #include "parameters.h" -#include -#include #include -#include #include - #include #include - #include #include - +#include +#include #include #include #include #include +#include #include #include #include @@ -76,7 +73,7 @@ namespace sensors * * Handling of sensor updates with voting */ -class VotedSensorsUpdate +class VotedSensorsUpdate : ModuleParams { public: /** @@ -217,6 +214,18 @@ class VotedSensorsUpdate SensorData _mag {}; SensorData _baro {}; + SensorCorrections _accel_corrections[ACCEL_COUNT_MAX] { + {this, SensorCorrections::SensorType::Accelerometer}, + {this, SensorCorrections::SensorType::Accelerometer}, + {this, SensorCorrections::SensorType::Accelerometer}, + }; + + SensorCorrections _gyro_corrections[GYRO_COUNT_MAX] { + {this, SensorCorrections::SensorType::Gyroscope}, + {this, SensorCorrections::SensorType::Gyroscope}, + {this, SensorCorrections::SensorType::Gyroscope}, + }; + orb_advert_t _mavlink_log_pub{nullptr}; uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */ @@ -224,6 +233,8 @@ class VotedSensorsUpdate /* sensor thermal compensation */ uORB::Subscription _corrections_sub{ORB_ID(sensor_correction)}; + float _baro_scale[BARO_COUNT_MAX] {1.f, 1.f, 1.f}; + float _baro_offset[BARO_COUNT_MAX] {0.f, 0.f, 0.f}; sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ vehicle_air_data_s _last_airdata[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ @@ -248,7 +259,6 @@ class VotedSensorsUpdate uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {}; /**< latest full timestamp */ - sensor_correction_s _corrections {}; /**< struct containing the sensor corrections to be published to the uORB */ sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */ subsystem_info_s _info {}; /**< subsystem info publication */ }; diff --git a/src/systemcmds/config/CMakeLists.txt b/src/systemcmds/config/CMakeLists.txt deleted file mode 100644 index 1865c83ab09f..000000000000 --- a/src/systemcmds/config/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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 systemcmds__config - MAIN config - STACK_MAIN 4096 - COMPILE_FLAGS - SRCS - config.c - DEPENDS - ) - diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c deleted file mode 100644 index 262f6b0dbc16..000000000000 --- a/src/systemcmds/config/config.c +++ /dev/null @@ -1,251 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 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 config.c - * @author Lorenz Meier - * @author Julian Oes - * - * config tool. Takes the device name as the first parameter. - */ - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include - -__EXPORT int config_main(int argc, char *argv[]); - -static int do_gyro(int argc, char *argv[]); -static int do_accel(int argc, char *argv[]); -static int do_mag(int argc, char *argv[]); -static void print_usage(void); - -int -config_main(int argc, char *argv[]) -{ - if (argc >= 3) { - if (!strncmp(argv[2], "/dev/gyro", 9)) { - return do_gyro(argc - 1, argv + 1); - - } else if (!strncmp(argv[2], "/dev/accel", 10)) { - return do_accel(argc - 1, argv + 1); - - } else if (!strncmp(argv[2], "/dev/mag", 8)) { - return do_mag(argc - 1, argv + 1); - } - } - - print_usage(); - return 1; -} -static void -print_usage(void) -{ - PRINT_MODULE_DESCRIPTION("Configure a sensor driver (sampling & publication rate, range, etc.)"); - - PRINT_MODULE_USAGE_NAME("config", "command"); - PRINT_MODULE_USAGE_PARAM_COMMENT("The argument is typically one of /dev/{gyro,accel,mag}i"); - - PRINT_MODULE_USAGE_COMMAND_DESCR("block", "Block sensor topic publication"); - PRINT_MODULE_USAGE_ARG("", "Sensor device file", false); - PRINT_MODULE_USAGE_COMMAND_DESCR("unblock", "Unblock sensor topic publication"); - PRINT_MODULE_USAGE_ARG("", "Sensor device file", false); - - PRINT_MODULE_USAGE_COMMAND_DESCR("sampling", "Set sensor sampling rate"); - PRINT_MODULE_USAGE_ARG(" ", "Sensor device file and sampling rate in Hz", false); - PRINT_MODULE_USAGE_COMMAND_DESCR("rate", "Set sensor publication rate"); - PRINT_MODULE_USAGE_ARG(" ", "Sensor device file and publication rate in Hz", false); - PRINT_MODULE_USAGE_COMMAND_DESCR("range", "Set sensor measurement range"); - PRINT_MODULE_USAGE_ARG(" ", "Sensor device file and range", false); - PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Perform sensor self-test (and print info)"); - PRINT_MODULE_USAGE_ARG("", "Sensor device file", false); -} - -static int -do_gyro(int argc, char *argv[]) -{ - int fd; - - fd = open(argv[1], 0); - - if (fd < 0) { - PX4_ERR("open %s failed (%i)", argv[1], errno); - return 1; - - } else { - - int ret; - - if (argc == 3 && !strcmp(argv[0], "rate")) { - - /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); - - if (ret) { - PX4_ERR("pollrate could not be set"); - return 1; - } - - } else { - print_usage(); - return 1; - } - - int id = ioctl(fd, DEVIOCGDEVICEID, 0); - int32_t calibration_id = 0; - - param_get(param_find("CAL_GYRO0_ID"), &(calibration_id)); - - PX4_INFO("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)", - id, calibration_id); - - close(fd); - } - - return 0; -} - -static int -do_mag(int argc, char *argv[]) -{ - int fd; - - fd = open(argv[1], 0); - - if (fd < 0) { - PX4_ERR("open %s failed (%i)", argv[1], errno); - return 1; - - } else { - - int ret; - - if (argc == 3 && !strcmp(argv[0], "rate")) { - - /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); - - if (ret) { - PX4_ERR("pollrate could not be set"); - return 1; - } - - } else if (argc == 3 && !strcmp(argv[0], "range")) { - - /* set the range to i G */ - ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0)); - - if (ret) { - PX4_ERR("range could not be set"); - return 1; - } - - } else { - print_usage(); - return 1; - } - - int id = ioctl(fd, DEVIOCGDEVICEID, 0); - int32_t calibration_id = 0; - - param_get(param_find("CAL_MAG0_ID"), &(calibration_id)); - - PX4_INFO("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)", - id, calibration_id); - - close(fd); - } - - return 0; -} - -static int -do_accel(int argc, char *argv[]) -{ - int fd; - - fd = open(argv[1], 0); - - if (fd < 0) { - PX4_ERR("open %s failed (%i)", argv[1], errno); - return 1; - - } else { - - int ret; - - if (argc == 3 && !strcmp(argv[0], "rate")) { - - /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); - - if (ret) { - PX4_ERR("pollrate could not be set"); - return 1; - } - - } else { - print_usage(); - return 1; - } - - int id = ioctl(fd, DEVIOCGDEVICEID, 0); - int32_t calibration_id = 0; - - param_get(param_find("CAL_ACC0_ID"), &(calibration_id)); - - PX4_INFO("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)", - id, calibration_id); - - close(fd); - } - - return 0; -}