From d0d1c0c9fea450cbfe7eafe2517b920058896d0a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 19 Jan 2020 17:45:38 -0500 Subject: [PATCH] create temperature_compensation module --- ROMFS/px4fmu_common/init.d/rc.thermal_cal | 31 +- boards/aerotenna/ocpoc/default.cmake | 1 + boards/airmind/mindpx-v2/default.cmake | 1 + boards/atlflight/eagle/default.cmake | 1 + boards/atlflight/eagle/qurt.cmake | 1 + boards/atlflight/excelsior/qurt.cmake | 1 + boards/auav/x21/default.cmake | 1 + boards/av/x-v1/default.cmake | 1 + boards/beaglebone/blue/default.cmake | 1 + boards/bitcraze/crazyflie/default.cmake | 1 + boards/emlid/navio2/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/nxp/fmuk66-v3/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 | 2 + 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 | 22 +- boards/px4/fmu-v5/default.cmake | 1 + boards/px4/fmu-v5/fixedwing.cmake | 1 + boards/px4/fmu-v5/irqmonitor.cmake | 22 +- 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 | 2 + boards/px4/raspberrypi/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 + msg/sensor_accel_integrated.msg | 2 - msg/sensor_correction.msg | 4 - msg/sensor_gyro_integrated.msg | 2 - platforms/common/px4_work_queue/WorkItem.cpp | 7 +- .../accelerometer/PX4Accelerometer.cpp | 2 - src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 2 - src/modules/events/CMakeLists.txt | 5 - src/modules/events/rc_loss_alarm.cpp | 27 +- src/modules/events/rc_loss_alarm.h | 10 +- src/modules/events/send_event.cpp | 191 ++------ src/modules/events/send_event.h | 25 +- src/modules/events/status_display.cpp | 15 +- src/modules/events/status_display.h | 17 +- src/modules/events/subscriber_handler.cpp | 102 ----- src/modules/events/subscriber_handler.h | 57 --- src/modules/sensors/CMakeLists.txt | 2 - .../VehicleAcceleration.cpp | 65 +-- .../VehicleAcceleration.hpp | 2 +- .../VehicleAngularVelocity.cpp | 65 +-- src/modules/sensors/voted_sensors_update.cpp | 115 +---- src/modules/sensors/voted_sensors_update.h | 15 +- .../temperature_compensation/CMakeLists.txt | 46 ++ .../TemperatureCompensation.cpp} | 71 ++- .../TemperatureCompensation.h} | 78 ++-- .../TemperatureCompensationModule.cpp | 421 ++++++++++++++++++ .../TemperatureCompensationModule.h | 139 ++++++ .../temp_comp_params_accel.c | 0 .../temp_comp_params_baro.c | 0 .../temp_comp_params_gyro.c | 0 .../temperature_calibration/accel.cpp | 3 +- .../temperature_calibration/accel.h | 0 .../temperature_calibration/baro.cpp | 11 +- .../temperature_calibration/baro.h | 0 .../temperature_calibration/common.h | 6 +- .../temperature_calibration/gyro.cpp | 10 +- .../temperature_calibration/gyro.h | 0 .../temperature_calibration/polyfit.hpp | 10 +- .../temperature_calibration/task.cpp | 63 ++- .../temperature_calibration.h | 1 - 87 files changed, 976 insertions(+), 737 deletions(-) delete mode 100644 src/modules/events/subscriber_handler.cpp delete mode 100644 src/modules/events/subscriber_handler.h create mode 100644 src/modules/temperature_compensation/CMakeLists.txt rename src/modules/{sensors/temperature_compensation.cpp => temperature_compensation/TemperatureCompensation.cpp} (91%) rename src/modules/{sensors/temperature_compensation.h => temperature_compensation/TemperatureCompensation.h} (81%) create mode 100644 src/modules/temperature_compensation/TemperatureCompensationModule.cpp create mode 100644 src/modules/temperature_compensation/TemperatureCompensationModule.h rename src/modules/{sensors => temperature_compensation}/temp_comp_params_accel.c (100%) rename src/modules/{sensors => temperature_compensation}/temp_comp_params_baro.c (100%) rename src/modules/{sensors => temperature_compensation}/temp_comp_params_gyro.c (100%) rename src/modules/{events => temperature_compensation}/temperature_calibration/accel.cpp (99%) rename src/modules/{events => temperature_compensation}/temperature_calibration/accel.h (100%) rename src/modules/{events => temperature_compensation}/temperature_calibration/baro.cpp (97%) rename src/modules/{events => temperature_compensation}/temperature_calibration/baro.h (100%) rename src/modules/{events => temperature_compensation}/temperature_calibration/common.h (98%) rename src/modules/{events => temperature_compensation}/temperature_calibration/gyro.cpp (97%) rename src/modules/{events => temperature_compensation}/temperature_calibration/gyro.h (100%) rename src/modules/{events => temperature_compensation}/temperature_calibration/polyfit.hpp (97%) rename src/modules/{events => temperature_compensation}/temperature_calibration/task.cpp (87%) rename src/modules/{events => temperature_compensation}/temperature_calibration/temperature_calibration.h (99%) diff --git a/ROMFS/px4fmu_common/init.d/rc.thermal_cal b/ROMFS/px4fmu_common/init.d/rc.thermal_cal index 6ab8e2db3004..beaf26a2a2d4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.thermal_cal +++ b/ROMFS/px4fmu_common/init.d/rc.thermal_cal @@ -5,12 +5,37 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -set TEMP_CALIB_ARGS "" +set TEMP_COMP_START "" + +if param compare TC_A_ENABLE 1 +then + set TEMP_COMP_START "true" +fi + +if param compare TC_B_ENABLE 1 +then + set TEMP_COMP_START "true" +fi + +if param compare TC_G_ENABLE 1 +then + set TEMP_COMP_START "true" +fi + +if [ "x$TEMP_COMP_START" != "x" ] +then + temperature_compensation start +fi + +unset TEMP_COMP_START + # # Determine if a thermal calibration should be started. # +set TEMP_CALIB_ARGS "" + if param compare SYS_CAL_ACCEL 1 then set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -a" @@ -34,7 +59,7 @@ fi # if [ "x$TEMP_CALIB_ARGS" != "x" ] then - send_event temperature_calibration ${TEMP_CALIB_ARGS} + temperature_compensation calibrate ${TEMP_CALIB_ARGS} fi -unset TEMP_CALIB_ARGS \ No newline at end of file +unset TEMP_CALIB_ARGS diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index a076265e5abd..9cccabb51239 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -59,6 +59,7 @@ px4_add_board( rc_update rover_pos_control sensors + temperature_compensation sih #simulator vmount diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 4a6ca480f77d..695d8c7213b9 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -76,6 +76,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index 3870f2a65654..f317dd71639a 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -83,6 +83,7 @@ px4_add_board( rover_pos_control sensors #sih + temperature_compensation simulator vmount vtol_att_control diff --git a/boards/atlflight/eagle/qurt.cmake b/boards/atlflight/eagle/qurt.cmake index 921e360d3c72..141e5a5825ec 100644 --- a/boards/atlflight/eagle/qurt.cmake +++ b/boards/atlflight/eagle/qurt.cmake @@ -68,6 +68,7 @@ px4_add_board( rc_update rover_pos_control sensors + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/atlflight/excelsior/qurt.cmake b/boards/atlflight/excelsior/qurt.cmake index 420cd7d73d4d..539a32a37fd3 100644 --- a/boards/atlflight/excelsior/qurt.cmake +++ b/boards/atlflight/excelsior/qurt.cmake @@ -68,6 +68,7 @@ px4_add_board( rc_update rover_pos_control sensors + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/auav/x21/default.cmake b/boards/auav/x21/default.cmake index 910d0f394724..9bd21f1e8ce8 100644 --- a/boards/auav/x21/default.cmake +++ b/boards/auav/x21/default.cmake @@ -76,6 +76,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index b48da09c4af2..87cf58848cc1 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -76,6 +76,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index 0b444c9746d1..af78070f1fbf 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -53,6 +53,7 @@ px4_add_board( sensors sih #simulator + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index 28db936667f9..523ff3b75dec 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -32,6 +32,7 @@ px4_add_board( navigator rc_update sensors + #temperature_compensation SYSTEMCMDS bl_update config diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index 32358546fccd..b7b051440d8c 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -55,6 +55,7 @@ px4_add_board( sensors sih #simulator + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index c9056e40755d..408ae2558fcf 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -81,6 +81,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/holybro/durandal-v1/stackcheck.cmake b/boards/holybro/durandal-v1/stackcheck.cmake index 39ee8f48edf1..3b1993db69a4 100644 --- a/boards/holybro/durandal-v1/stackcheck.cmake +++ b/boards/holybro/durandal-v1/stackcheck.cmake @@ -82,6 +82,7 @@ px4_add_board( #rover_pos_control sensors #sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake index a02b4ea631b7..8c8aa301daa6 100644 --- a/boards/holybro/kakutef7/default.cmake +++ b/boards/holybro/kakutef7/default.cmake @@ -46,6 +46,7 @@ px4_add_board( navigator rc_update sensors + #temperature_compensation SYSTEMCMDS bl_update config diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 7e375f0b7e0f..2b80bc0f3981 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -56,6 +56,7 @@ px4_add_board( #rover_pos_control sensors #sih + #temperature_compensation vmount #vtol_att_control SYSTEMCMDS diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index 0780b2bfe9f1..969499672334 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -56,6 +56,7 @@ px4_add_board( #rover_pos_control sensors #sih + #temperature_compensation vmount #vtol_att_control SYSTEMCMDS diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index dc2bc006c0f2..465e152ca15f 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -74,6 +74,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 361dba7da11c..7fc3c4156a50 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -80,6 +80,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 9d4c307d928b..10c0abe3f750 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -78,6 +78,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index caee09e50bb8..e60c55b23d21 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -64,6 +64,7 @@ px4_add_board( #rover_pos_control sensors #sih + #temperature_compensation #vmount #vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 7821fdd89452..2e239d7bc144 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -87,6 +87,7 @@ px4_add_board( #rover_pos_control sensors #sih + #temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake index 4acd86c21040..8bdce98f4d18 100644 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -56,6 +56,7 @@ px4_add_board( navigator rc_update sensors + temperature_compensation vmount SYSTEMCMDS #bl_update diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index 00c99f961b56..7c6492bda528 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -81,6 +81,7 @@ px4_add_board( battery_status rc_update sensors + temperature_compensation vmount #vtol_att_control #airspeed_selector diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index 0bb8e283ee17..d97b61914c28 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -54,6 +54,8 @@ px4_add_board( navigator rc_update sensors + sih + #temperature_compensation vmount SYSTEMCMDS #bl_update diff --git a/boards/px4/fmu-v2/rover.cmake b/boards/px4/fmu-v2/rover.cmake index bf0d85627438..f86769502d45 100644 --- a/boards/px4/fmu-v2/rover.cmake +++ b/boards/px4/fmu-v2/rover.cmake @@ -50,6 +50,7 @@ px4_add_board( battery_status rc_update sensors + temperature_compensation vmount SYSTEMCMDS diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index 3f98365cc41f..50799a85f0cc 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -86,6 +86,7 @@ px4_add_board( #rover_pos_control sensors #sih + #temperature_compensation #vmount #vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 2563230fdc03..2c2e1625c104 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -86,6 +86,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index c66529573baf..4420644a4bb4 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -86,6 +86,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index b5c0730e338e..da095e8e180b 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -81,6 +81,7 @@ px4_add_board( rc_update sensors sih + temperature_compensation vmount vtol_att_control airspeed_selector diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 7c523e2f1512..c84e429e9fe2 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -79,6 +79,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 3c4185725eee..d2d47ca15caf 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -77,6 +77,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 5d52be2eee74..ec75f861dbd8 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -77,6 +77,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index d889b7ad6565..1229673a1341 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -81,6 +81,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index fedb273a0bfb..36400102683b 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -78,6 +78,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index d5586251fd44..227095a33a18 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -10,13 +10,11 @@ px4_add_board( IO px4_io-v2_default TESTING UAVCAN_INTERFACES 2 - SERIAL_PORTS GPS1:/dev/ttyS0 TEL1:/dev/ttyS1 TEL2:/dev/ttyS2 TEL4:/dev/ttyS3 - DRIVERS adc barometer # all available barometer drivers @@ -25,14 +23,15 @@ px4_add_board( camera_trigger differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers + dshot gps #heater + #imu # all available imu drivers imu/adis16448 + imu/adis16477 imu/adis16497 - #imu # all available imu drivers imu/bmi055 imu/mpu6000 - imu/mpu9250 irlock lights/blinkm lights/rgbled @@ -41,6 +40,7 @@ px4_add_board( magnetometer # all available magnetometer drivers mkblctrl optical_flow # all available optical flow drivers + #osd pca9685 power_monitor/ina226 #protocol_splitter @@ -56,9 +56,10 @@ px4_add_board( test_ppm tone_alarm uavcan - MODULES + airspeed_selector attitude_estimator_q + battery_status camera_feedback commander dataman @@ -66,7 +67,6 @@ px4_add_board( events fw_att_control fw_pos_control_l1 - rover_pos_control land_detector landing_target_estimator load_mon @@ -74,17 +74,17 @@ px4_add_board( logger mavlink mc_att_control - mc_rate_control mc_pos_control + mc_rate_control + #micrortps_bridge navigator - battery_status rc_update + rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control - airspeed_selector - SYSTEMCMDS bl_update config @@ -113,7 +113,6 @@ px4_add_board( usb_connected ver work_queue - EXAMPLES bottle_drop # OBC challenge fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control @@ -124,5 +123,4 @@ px4_add_board( px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app uuv_example_app - ) diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index a7ae3e9513da..6110cbe98df5 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -82,6 +82,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index ed2da4aead7e..75da10aa68d7 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -59,6 +59,7 @@ px4_add_board( navigator rc_update sensors + temperature_compensation vmount SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index c283d679c24b..c51d4495b68e 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -10,13 +10,11 @@ px4_add_board( IO px4_io-v2_default TESTING UAVCAN_INTERFACES 2 - SERIAL_PORTS GPS1:/dev/ttyS0 TEL1:/dev/ttyS1 TEL2:/dev/ttyS2 TEL4:/dev/ttyS3 - DRIVERS adc barometer # all available barometer drivers @@ -25,14 +23,15 @@ px4_add_board( camera_trigger differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers + dshot gps #heater + #imu # all available imu drivers imu/adis16448 + imu/adis16477 imu/adis16497 - #imu # all available imu drivers imu/bmi055 imu/mpu6000 - imu/mpu9250 irlock lights/blinkm lights/rgbled @@ -41,6 +40,7 @@ px4_add_board( magnetometer # all available magnetometer drivers mkblctrl optical_flow # all available optical flow drivers + #osd pca9685 power_monitor/ina226 #protocol_splitter @@ -56,9 +56,10 @@ px4_add_board( test_ppm tone_alarm uavcan - MODULES + airspeed_selector attitude_estimator_q + battery_status camera_feedback commander dataman @@ -66,7 +67,6 @@ px4_add_board( events fw_att_control fw_pos_control_l1 - rover_pos_control land_detector landing_target_estimator load_mon @@ -74,17 +74,17 @@ px4_add_board( logger mavlink mc_att_control - mc_rate_control mc_pos_control + mc_rate_control + #micrortps_bridge navigator - battery_status rc_update + rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control - airspeed_selector - SYSTEMCMDS bl_update config @@ -113,7 +113,6 @@ px4_add_board( usb_connected ver work_queue - EXAMPLES bottle_drop # OBC challenge fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control @@ -124,5 +123,4 @@ px4_add_board( px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html rover_steering_control # Rover example app uuv_example_app - ) diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index f537a57a61f3..7eb6b0a9bda6 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -69,6 +69,7 @@ px4_add_board( rc_update sensors sih + temperature_compensation vmount SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 25514919c360..f7389fe63c53 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -59,6 +59,7 @@ px4_add_board( rc_update rover_pos_control sensors + temperature_compensation vmount SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 81ecf32dce44..881d41af4374 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -82,6 +82,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index c0533f67f225..121f05e06177 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -83,6 +83,7 @@ px4_add_board( rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index ad6603c25357..6a760b95ac86 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -76,11 +76,13 @@ px4_add_board( mc_att_control mc_pos_control mc_rate_control + #micrortps_bridge navigator rc_update rover_pos_control sensors sih + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index 4fdb1166798c..8b0c6c4b3f0f 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -51,6 +51,7 @@ px4_add_board( rc_update rover_pos_control sensors + temperature_compensation sih #simulator vmount diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index c7eafcfd5cef..396492ca937a 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -45,6 +45,7 @@ px4_add_board( sensors #sih simulator + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 52b85ecb88b8..a0b2ee79a3cf 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -47,6 +47,7 @@ px4_add_board( sensors #sih simulator + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 36de8c309cbd..c2f9e6f3ace7 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -45,6 +45,7 @@ px4_add_board( sensors #sih simulator + temperature_compensation vmount vtol_att_control SYSTEMCMDS diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake index aac6a27ffaa9..2a2fc1c029d1 100644 --- a/boards/uvify/core/default.cmake +++ b/boards/uvify/core/default.cmake @@ -57,6 +57,7 @@ px4_add_board( rc_update sensors sih + temperature_compensation vmount SYSTEMCMDS bl_update diff --git a/msg/sensor_accel_integrated.msg b/msg/sensor_accel_integrated.msg index ea9a92b64adc..bacea620a377 100644 --- a/msg/sensor_accel_integrated.msg +++ b/msg/sensor_accel_integrated.msg @@ -3,8 +3,6 @@ uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles -float32 temperature # temperature in degrees celsius - uint64 error_count float32[3] delta_velocity # delta velocity in the NED board axis in m/s over the integration time frame (dt) diff --git a/msg/sensor_correction.msg b/msg/sensor_correction.msg index 30295adbee1d..a61caf15e6ee 100644 --- a/msg/sensor_correction.msg +++ b/msg/sensor_correction.msg @@ -46,10 +46,6 @@ float32 baro_scale_1 # barometric pressure scale factors in the sensor frame float32 baro_offset_2 # barometric pressure offsets in the sensor frame in m/s/s float32 baro_scale_2 # barometric pressure scale factors in the sensor frame -uint8 selected_gyro_instance # gyro uORB topic instance for the voted sensor -uint8 selected_accel_instance # accelerometer uORB topic instance for the voted sensor -uint8 selected_baro_instance # barometric pressure uORB topic instance for the voted sensor - # Mapping from uORB index to parameter index for each sensor type. For example accel_mapping[1] contains the # compensation parameter system index for the sensor_accel uORB index 1 data. uint8[3] gyro_mapping diff --git a/msg/sensor_gyro_integrated.msg b/msg/sensor_gyro_integrated.msg index f090ab1689c5..18fe115ebb1b 100644 --- a/msg/sensor_gyro_integrated.msg +++ b/msg/sensor_gyro_integrated.msg @@ -3,8 +3,6 @@ uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles -float32 temperature # temperature in degrees celsius - uint64 error_count float32[3] delta_angle # delta angle in the NED board axis in rad/s over the integration time frame (dt) diff --git a/platforms/common/px4_work_queue/WorkItem.cpp b/platforms/common/px4_work_queue/WorkItem.cpp index 716a1585d6d1..3d8a5e440611 100644 --- a/platforms/common/px4_work_queue/WorkItem.cpp +++ b/platforms/common/px4_work_queue/WorkItem.cpp @@ -100,7 +100,7 @@ WorkItem::average_rate() const { const float rate = _run_count / elapsed_time(); - if (PX4_ISFINITE(rate)) { + if ((_run_count > 0) && PX4_ISFINITE(rate)) { return rate; } @@ -110,9 +110,10 @@ WorkItem::average_rate() const float WorkItem::average_interval() const { - const float interval = 1000000.0f / average_rate(); + const float rate = average_rate(); + const float interval = 1000000.0f / rate; - if (PX4_ISFINITE(interval)) { + if ((rate > 0.0f) && PX4_ISFINITE(interval)) { return interval; } diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 756c4854af55..ec3246565e4c 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -168,7 +168,6 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl report.timestamp_sample = _integrator_timestamp_sample; report.error_count = _error_count; report.device_id = _device_id; - report.temperature = _temperature; delta_velocity.copyTo(report.delta_velocity); report.dt = integral_dt; report.samples = _integrator_samples; @@ -301,7 +300,6 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample) report.timestamp_sample = _integrator_timestamp_sample; report.error_count = _error_count; report.device_id = _device_id; - report.temperature = _temperature; delta_velocity.copyTo(report.delta_velocity); report.dt = integrator_dt_us; report.samples = _integrator_fifo_samples; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index c00f2451c92d..b52f7b089574 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -170,7 +170,6 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float report.timestamp_sample = _integrator_timestamp_sample; report.error_count = _error_count; report.device_id = _device_id; - report.temperature = _temperature; delta_angle.copyTo(report.delta_angle); report.dt = integral_dt; report.samples = _integrator_samples; @@ -317,7 +316,6 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample) report.timestamp_sample = _integrator_timestamp_sample; report.error_count = _error_count; report.device_id = _device_id; - report.temperature = _temperature; delta_angle.copyTo(report.delta_angle); report.dt = integrator_dt_us; report.samples = _integrator_fifo_samples; diff --git a/src/modules/events/CMakeLists.txt b/src/modules/events/CMakeLists.txt index a05f52919b0e..a886cd5f4f16 100644 --- a/src/modules/events/CMakeLists.txt +++ b/src/modules/events/CMakeLists.txt @@ -40,11 +40,6 @@ px4_add_module( send_event.cpp set_leds.cpp status_display.cpp - subscriber_handler.cpp - temperature_calibration/accel.cpp - temperature_calibration/baro.cpp - temperature_calibration/gyro.cpp - temperature_calibration/task.cpp DEPENDS modules__uORB ) diff --git a/src/modules/events/rc_loss_alarm.cpp b/src/modules/events/rc_loss_alarm.cpp index 8df8de41fc86..cd522f28507b 100644 --- a/src/modules/events/rc_loss_alarm.cpp +++ b/src/modules/events/rc_loss_alarm.cpp @@ -52,40 +52,27 @@ namespace events namespace rc_loss { -RC_Loss_Alarm::RC_Loss_Alarm(const events::SubscriberHandler &subscriber_handler) - : _subscriber_handler(subscriber_handler) -{ -} - -bool RC_Loss_Alarm::check_for_updates() -{ - if (_subscriber_handler.vehicle_status_updated()) { - orb_copy(ORB_ID(vehicle_status), _subscriber_handler.get_vehicle_status_sub(), &_vehicle_status); - return true; - } - - return false; -} - void RC_Loss_Alarm::process() { - if (!check_for_updates()) { + vehicle_status_s status{}; + + if (!_vehicle_status_sub.update(&status)) { return; } if (!_was_armed && - _vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { _was_armed = true; // Once true, impossible to go back to false } - if (!_had_rc && !_vehicle_status.rc_signal_lost) { + if (!_had_rc && !status.rc_signal_lost) { _had_rc = true; } - if (_was_armed && _had_rc && _vehicle_status.rc_signal_lost && - _vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + if (_was_armed && _had_rc && status.rc_signal_lost && + status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { play_tune(); _alarm_playing = true; diff --git a/src/modules/events/rc_loss_alarm.h b/src/modules/events/rc_loss_alarm.h index 0000b9033dfd..29397d8b2a91 100644 --- a/src/modules/events/rc_loss_alarm.h +++ b/src/modules/events/rc_loss_alarm.h @@ -39,9 +39,7 @@ #pragma once -#include "subscriber_handler.h" - -#include +#include #include namespace events @@ -53,7 +51,7 @@ class RC_Loss_Alarm { public: - RC_Loss_Alarm(const events::SubscriberHandler &subscriber_handler); + RC_Loss_Alarm() = default; /** regularily called to handle state updates */ void process(); @@ -71,12 +69,12 @@ class RC_Loss_Alarm /** Publish tune control to interrupt any sound */ void stop_tune(); - struct vehicle_status_s _vehicle_status = {}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _was_armed = false; bool _had_rc = false; // Don't trigger alarm for systems without RC bool _alarm_playing = false; orb_advert_t _tune_control_pub = nullptr; - const events::SubscriberHandler &_subscriber_handler; }; } /* namespace rc_loss */ diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp index ae92b5cbf4f7..88833d4591b0 100644 --- a/src/modules/events/send_event.cpp +++ b/src/modules/events/send_event.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "send_event.h" -#include "temperature_calibration/temperature_calibration.h" #include @@ -40,96 +39,66 @@ #include #include +using namespace time_literals; + namespace events { -struct work_s SendEvent::_work = {}; - // Run it at 30 Hz. -const unsigned SEND_EVENT_INTERVAL_US = 33000; - +static constexpr uint32_t SEND_EVENT_INTERVAL_US{1_s / 30}; int SendEvent::task_spawn(int argc, char *argv[]) { - int ret = work_queue(LPWORK, &_work, (worker_t)&SendEvent::initialize_trampoline, nullptr, 0); - - if (ret < 0) { - return ret; - } - - ret = wait_until_running(); + SendEvent *send_event = new SendEvent(); - if (ret < 0) { - return ret; + if (!send_event) { + PX4_ERR("alloc failed"); + return PX4_ERROR; } + _object.store(send_event); _task_id = task_id_is_work_queue; + send_event->start(); + return 0; } -SendEvent::SendEvent() : ModuleParams(nullptr) +SendEvent::SendEvent() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { if (_param_ev_tsk_stat_dis.get()) { - _status_display = new status::StatusDisplay(_subscriber_handler); + _status_display = new status::StatusDisplay(); } if (_param_ev_tsk_rc_loss.get()) { - _rc_loss_alarm = new rc_loss::RC_Loss_Alarm(_subscriber_handler); + _rc_loss_alarm = new rc_loss::RC_Loss_Alarm(); } } SendEvent::~SendEvent() { + ScheduleClear(); + delete _status_display; delete _rc_loss_alarm; } int SendEvent::start() { - if (is_running()) { - return 0; - } - - // Subscribe to the topics. - _subscriber_handler.subscribe(); - - // Kick off the cycling. We can call it directly because we're already in the work queue context. - cycle(); + ScheduleOnInterval(SEND_EVENT_INTERVAL_US, 10000); - return 0; + return PX4_OK; } -void SendEvent::initialize_trampoline(void *arg) -{ - SendEvent *send_event = new SendEvent(); - - if (!send_event) { - PX4_ERR("alloc failed"); - return; - } - - send_event->start(); - _object.store(send_event); -} - -void SendEvent::cycle_trampoline(void *arg) -{ - SendEvent *obj = static_cast(arg); - - obj->cycle(); -} - -void SendEvent::cycle() +void SendEvent::Run() { if (should_exit()) { - _subscriber_handler.unsubscribe(); exit_and_cleanup(); return; } - _subscriber_handler.check_for_updates(); - process_commands(); if (_status_display != nullptr) { @@ -139,52 +108,12 @@ void SendEvent::cycle() if (_rc_loss_alarm != nullptr) { _rc_loss_alarm->process(); } - - work_queue(LPWORK, &_work, (worker_t)&SendEvent::cycle_trampoline, this, - USEC2TICK(SEND_EVENT_INTERVAL_US)); } void SendEvent::process_commands() { - if (!_subscriber_handler.vehicle_command_updated()) { - return; - } - - struct vehicle_command_s cmd; - - orb_copy(ORB_ID(vehicle_command), _subscriber_handler.get_vehicle_command_sub(), &cmd); - - bool got_temperature_calibration_command = false, accel = false, baro = false, gyro = false; - - switch (cmd.command) { - case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: - if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { - gyro = true; - got_temperature_calibration_command = true; - } - - if ((int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { - accel = true; - got_temperature_calibration_command = true; - } - - if ((int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { - baro = true; - got_temperature_calibration_command = true; - } - - if (got_temperature_calibration_command) { - if (run_temperature_calibration(accel, baro, gyro) == 0) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - - } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); - } - } - - break; - } - + // TODO: do something with vehicle commands + // TODO: what is this modules purpose? } void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result) @@ -201,68 +130,6 @@ void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result) command_ack_pub.publish(command_ack); } -int SendEvent::custom_command(int argc, char *argv[]) -{ - if (!strcmp(argv[0], "temperature_calibration")) { - - if (!is_running()) { - PX4_ERR("background task not running"); - return -1; - } - - bool gyro_calib = false, accel_calib = false, baro_calib = false; - bool calib_all = true; - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "abg", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'a': - accel_calib = true; - calib_all = false; - break; - - case 'b': - baro_calib = true; - calib_all = false; - break; - - case 'g': - gyro_calib = true; - calib_all = false; - break; - - default: - print_usage("unrecognized flag"); - return 1; - } - } - - vehicle_command_s vcmd{}; - vcmd.timestamp = hrt_absolute_time(); - vcmd.param1 = (float)((gyro_calib - || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN); - vcmd.param2 = NAN; - vcmd.param3 = NAN; - vcmd.param4 = NAN; - vcmd.param5 = ((accel_calib - || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : (double)NAN); - vcmd.param6 = (double)NAN; - vcmd.param7 = (float)((baro_calib - || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN); - vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION; - - uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; - vcmd_pub.publish(vcmd); - - } else { - print_usage("unrecognized command"); - } - - return 0; -} - int SendEvent::print_usage(const char *reason) { if (reason) { @@ -273,22 +140,26 @@ int SendEvent::print_usage(const char *reason) R"DESCR_STR( ### Description Background process running periodically on the LP work queue to perform housekeeping tasks. -It is currently only responsible for temperature calibration and tone alarm on RC Loss. +It is currently only responsible for tone alarm on RC Loss. The tasks can be started via CLI or uORB topics (vehicle_command from MAVLink, etc.). )DESCR_STR"); PRINT_MODULE_USAGE_NAME("send_event", "system"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task"); - PRINT_MODULE_USAGE_COMMAND_DESCR("temperature_calibration", "Run temperature calibration process"); - PRINT_MODULE_USAGE_PARAM_FLAG('g', "calibrate the gyro", true); - PRINT_MODULE_USAGE_PARAM_FLAG('a', "calibrate the accel", true); - PRINT_MODULE_USAGE_PARAM_FLAG('b', "calibrate the baro (if none of these is given, all will be calibrated)", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } +int SendEvent::custom_command(int argc, char *argv[]) +{ + // TODO: what is my purpose? + print_usage("unrecognized command"); + + return 0; +} + int send_event_main(int argc, char *argv[]) { return SendEvent::main(argc, argv); diff --git a/src/modules/events/send_event.h b/src/modules/events/send_event.h index 741fc5d99917..dcff99775b44 100644 --- a/src/modules/events/send_event.h +++ b/src/modules/events/send_event.h @@ -33,11 +33,10 @@ #pragma once -#include "subscriber_handler.h" #include "status_display.h" #include "rc_loss_alarm.h" -#include +#include #include #include #include @@ -50,7 +49,7 @@ namespace events extern "C" __EXPORT int send_event_main(int argc, char *argv[]); /** @class SendEvent The SendEvent class manages the RC loss audible alarm, LED status display, and thermal calibration. */ -class SendEvent : public ModuleBase, public ModuleParams +class SendEvent : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -93,22 +92,10 @@ class SendEvent : public ModuleBase, public ModuleParams */ void answer_command(const vehicle_command_s &cmd, unsigned result); - /** - * @brief Process cycle trampoline for the work queue. - * @param arg Pointer to the task startup arguments. - */ - static void cycle_trampoline(void *arg); - /** * @brief Calls process_commands() and schedules the next cycle. */ - void cycle(); - - /** - * @brief Trampoline for initialisation. - * @param arg Pointer to the task startup arguments. - */ - static void initialize_trampoline(void *arg); + void Run() override; /** * @brief Checks for new commands and processes them. @@ -121,11 +108,7 @@ class SendEvent : public ModuleBase, public ModuleParams */ int start(); - /** @struct _work The work queue struct. */ - static struct work_s _work; - - /** @var _subscriber_handler The uORB subscriber handler. */ - SubscriberHandler _subscriber_handler; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /** @var _status_display Pointer to the status display object. */ status::StatusDisplay *_status_display = nullptr; diff --git a/src/modules/events/status_display.cpp b/src/modules/events/status_display.cpp index 118e342efc6a..b4fcc6b17dd7 100644 --- a/src/modules/events/status_display.cpp +++ b/src/modules/events/status_display.cpp @@ -48,8 +48,7 @@ namespace events namespace status { -StatusDisplay::StatusDisplay(const events::SubscriberHandler &subscriber_handler) - : _subscriber_handler(subscriber_handler) +StatusDisplay::StatusDisplay() { // set the base color _led_control.priority = 0; @@ -66,23 +65,19 @@ bool StatusDisplay::check_for_updates() { bool got_updates = false; - if (_subscriber_handler.battery_status_updated()) { - orb_copy(ORB_ID(battery_status), _subscriber_handler.get_battery_status_sub(), &_battery_status); + if (_battery_status_sub.update()) { got_updates = true; } - if (_subscriber_handler.cpuload_updated()) { - orb_copy(ORB_ID(cpuload), _subscriber_handler.get_cpuload_sub(), &_cpu_load); + if (_cpu_load_sub.update()) { got_updates = true; } - if (_subscriber_handler.vehicle_status_flags_updated()) { - orb_copy(ORB_ID(vehicle_status_flags), _subscriber_handler.get_vehicle_status_flags_sub(), &_vehicle_status_flags); + if (_vehicle_status_flags_sub.update()) { got_updates = true; } - if (_subscriber_handler.vehicle_status_updated()) { - orb_copy(ORB_ID(vehicle_status), _subscriber_handler.get_vehicle_status_sub(), &_vehicle_status); + if (_vehicle_status_sub.update()) { got_updates = true; } diff --git a/src/modules/events/status_display.h b/src/modules/events/status_display.h index 8a6d35eb2e8d..1324d13ceceb 100644 --- a/src/modules/events/status_display.h +++ b/src/modules/events/status_display.h @@ -40,14 +40,14 @@ #pragma once -#include "subscriber_handler.h" - #include #include +#include #include #include #include +#include #include #include @@ -60,7 +60,7 @@ class StatusDisplay { public: - StatusDisplay(const events::SubscriberHandler &subscriber_handler); + StatusDisplay(); /** regularily called to handle state updates */ void process(); @@ -81,11 +81,11 @@ class StatusDisplay void publish(); // TODO: review if there is a better variant that allocates this in the memory - struct battery_status_s _battery_status = {}; - struct cpuload_s _cpu_load = {}; - struct vehicle_status_s _vehicle_status = {}; - struct vehicle_status_flags_s _vehicle_status_flags = {}; - struct vehicle_attitude_s _vehicle_attitude = {}; + uORB::SubscriptionData _battery_status_sub{ORB_ID(battery_status)}; + uORB::SubscriptionData _cpu_load_sub{ORB_ID(cpuload)}; + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::SubscriptionData _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; + uORB::SubscriptionData _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; struct led_control_s _led_control = {}; @@ -99,7 +99,6 @@ class StatusDisplay uORB::PublicationQueued _led_control_pub{ORB_ID(led_control)}; - const events::SubscriberHandler &_subscriber_handler; }; } /* namespace status */ diff --git a/src/modules/events/subscriber_handler.cpp b/src/modules/events/subscriber_handler.cpp deleted file mode 100644 index 97a61f5106b5..000000000000 --- a/src/modules/events/subscriber_handler.cpp +++ /dev/null @@ -1,102 +0,0 @@ - -#include "subscriber_handler.h" - -#include - -namespace events -{ - -void SubscriberHandler::subscribe() -{ - if (_battery_status_sub < 0) { - _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); - } - - if (_cpuload_sub < 0) { - _cpuload_sub = orb_subscribe(ORB_ID(cpuload)); - } - - if (_vehicle_command_sub < 0) { - _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); - } - - if (_vehicle_status_sub < 0) { - _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - } - - if (_vehicle_status_flags_sub < 0) { - _vehicle_status_flags_sub = orb_subscribe(ORB_ID(vehicle_status_flags)); - } - -} - -void SubscriberHandler::unsubscribe() -{ - if (_battery_status_sub >= 0) { - orb_unsubscribe(_battery_status_sub); - _battery_status_sub = -1; - } - - if (_cpuload_sub >= 0) { - orb_unsubscribe(_cpuload_sub); - _cpuload_sub = -1; - } - - if (_vehicle_command_sub >= 0) { - orb_unsubscribe(_vehicle_command_sub); - _vehicle_command_sub = -1; - } - - if (_vehicle_status_sub >= 0) { - orb_unsubscribe(_vehicle_status_sub); - _vehicle_status_sub = -1; - } - - if (_vehicle_status_flags_sub >= 0) { - orb_unsubscribe(_vehicle_status_flags_sub); - _vehicle_status_flags_sub = -1; - } - -} - -void SubscriberHandler::check_for_updates() -{ - bool updated; - _update_bitfield = 0; - orb_check(_vehicle_command_sub, &updated); - - if (updated) { - _update_bitfield |= (uint32_t)StatusMask::VehicleCommand; - } - - updated = false; - orb_check(_vehicle_status_sub, &updated); - - if (updated) { - _update_bitfield |= (uint32_t)StatusMask::VehicleStatus; - } - - updated = false; - orb_check(_vehicle_status_flags_sub, &updated); - - if (updated) { - _update_bitfield |= (uint32_t)StatusMask::VehicleStatusFlags; - } - - updated = false; - orb_check(_battery_status_sub, &updated); - - if (updated) { - _update_bitfield |= (uint32_t)StatusMask::BatteryStatus; - } - - updated = false; - orb_check(_cpuload_sub, &updated); - - if (updated) { - _update_bitfield |= (uint32_t)StatusMask::CpuLoad; - } - -} - -} /* namespace events */ diff --git a/src/modules/events/subscriber_handler.h b/src/modules/events/subscriber_handler.h deleted file mode 100644 index 7f024efdd51e..000000000000 --- a/src/modules/events/subscriber_handler.h +++ /dev/null @@ -1,57 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace events -{ - -/** - * @class SubscriberHandler - * Contains a list of uORB subscriptions and maintains their update state. - */ -class SubscriberHandler -{ -public: - void subscribe(); - void unsubscribe(); - void check_for_updates(); - - int get_battery_status_sub() const { return _battery_status_sub; } - int get_cpuload_sub() const { return _cpuload_sub; } - int get_vehicle_command_sub() const { return _vehicle_command_sub; } - int get_vehicle_status_sub() const { return _vehicle_status_sub; } - int get_vehicle_status_flags_sub() const { return _vehicle_status_flags_sub; } - - /* update checking methods */ - bool battery_status_updated() const { return _update_bitfield & (uint32_t)StatusMask::BatteryStatus; } - bool cpuload_updated() const { return _update_bitfield & (uint32_t)StatusMask::CpuLoad; } - bool vehicle_command_updated() const { return _update_bitfield & (uint32_t)StatusMask::VehicleCommand; } - bool vehicle_status_updated() const { return _update_bitfield & (uint32_t)StatusMask::VehicleStatus; } - bool vehicle_status_flags_updated() const { return _update_bitfield & (uint32_t)StatusMask::VehicleStatusFlags; } - - -private: - enum class StatusMask : uint32_t { - VehicleCommand = (0x01 << 0), - VehicleStatus = (0x01 << 1), - VehicleStatusFlags = (0x01 << 2), - BatteryStatus = (0x01 << 3), - CpuLoad = (0x01 << 4), - }; - - int _battery_status_sub = -1; - int _cpuload_sub = -1; - int _vehicle_command_sub = -1; - int _vehicle_status_sub = -1; - int _vehicle_status_flags_sub = -1; - - uint32_t _update_bitfield = 0; -}; - -} /* events */ diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 177f7e38e04b..cdc948bc38b2 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -42,8 +42,6 @@ px4_add_module( voted_sensors_update.cpp sensors.cpp parameters.cpp - temperature_compensation.cpp - DEPENDS airspeed conversion diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 66f9be659ec8..33313f4c3b1d 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -37,6 +37,7 @@ using namespace matrix; using namespace time_literals; +using math::radians; VehicleAcceleration::VehicleAcceleration() : ModuleParams(nullptr), @@ -53,11 +54,15 @@ bool VehicleAcceleration::Start() { // force initial updates ParametersUpdate(true); - SensorBiasUpdate(true); - SensorCorrectionsUpdate(true); - // needed to change the active sensor if the primary stops updating - return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true); + // sensor_selection needed to change the active sensor if the primary stops updating + if (!_sensor_selection_sub.registerCallback()) { + PX4_ERR("sensor_selection callback registration failed"); + return false; + } + + ScheduleNow(); + return true; } void VehicleAcceleration::Stop() @@ -97,7 +102,7 @@ void VehicleAcceleration::SensorCorrectionsUpdate(bool force) _sensor_correction_sub.copy(&corrections); // selected sensor has changed, find updated index - if ((_corrections_selected_instance != corrections.selected_accel_instance) || force) { + if ((_corrections_selected_instance < 0) || force) { _corrections_selected_instance = -1; // find sensor_corrections index @@ -122,8 +127,8 @@ void VehicleAcceleration::SensorCorrectionsUpdate(bool force) _scale = Vector3f{corrections.accel_scale_2}; break; default: - _offset = Vector3f{0.0f, 0.0f, 0.0f}; - _scale = Vector3f{1.0f, 1.0f, 1.0f}; + _offset = Vector3f{0.f, 0.f, 0.f}; + _scale = Vector3f{1.f, 1.f, 1.f}; } } } @@ -154,8 +159,11 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force) // clear bias and corrections _bias.zero(); - _offset = Vector3f{0.0f, 0.0f, 0.0f}; - _scale = Vector3f{1.0f, 1.0f, 1.0f}; + _offset = Vector3f{0.f, 0.f, 0.f}; + _scale = Vector3f{1.f, 1.f, 1.f}; + + // force corrections reselection + _corrections_selected_instance = -1; return true; } @@ -182,13 +190,13 @@ void VehicleAcceleration::ParametersUpdate(bool force) updateParams(); // get transformation matrix from sensor/board to body frame - const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); + const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); // fine tune the rotation const Dcmf board_rotation_offset(Eulerf( - math::radians(_param_sens_board_x_off.get()), - math::radians(_param_sens_board_y_off.get()), - math::radians(_param_sens_board_z_off.get()))); + radians(_param_sens_board_x_off.get()), + radians(_param_sens_board_y_off.get()), + radians(_param_sens_board_z_off.get()))); _board_rotation = board_rotation_offset * board_rotation; } @@ -204,28 +212,29 @@ void VehicleAcceleration::Run() if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) { sensor_accel_s sensor_data; - _sensor_sub[_selected_sensor_sub_index].copy(&sensor_data); - // get the sensor data and correct for thermal errors (apply offsets and scale) - const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z}; + if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) { + // get the sensor data and correct for thermal errors (apply offsets and scale) + const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z}; - // apply offsets and scale - Vector3f accel{(val - _offset).emult(_scale)}; + // apply offsets and scale + Vector3f accel{(val - _offset).emult(_scale)}; - // rotate corrected measurements from sensor to body frame - accel = _board_rotation * accel; + // rotate corrected measurements from sensor to body frame + accel = _board_rotation * accel; - // correct for in-run bias errors - accel -= _bias; + // correct for in-run bias errors + accel -= _bias; - // publish - vehicle_acceleration_s out; + // publish + vehicle_acceleration_s out; - out.timestamp_sample = sensor_data.timestamp_sample; - accel.copyTo(out.xyz); - out.timestamp = hrt_absolute_time(); + out.timestamp_sample = sensor_data.timestamp_sample; + accel.copyTo(out.xyz); + out.timestamp = hrt_absolute_time(); - _vehicle_acceleration_pub.publish(out); + _vehicle_acceleration_pub.publish(out); + } } } diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 9479910cf2b5..f92eb5825449 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -96,9 +96,9 @@ class VehicleAcceleration : public ModuleParams, public px4::WorkItem matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + matrix::Vector3f _bias{0.f, 0.f, 0.f}; matrix::Vector3f _offset{0.f, 0.f, 0.f}; matrix::Vector3f _scale{1.f, 1.f, 1.f}; - matrix::Vector3f _bias{0.f, 0.f, 0.f}; uint32_t _selected_sensor_device_id{0}; uint8_t _selected_sensor_sub_index{0}; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 0b8ead9a62a7..e3ed84589832 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -37,6 +37,7 @@ using namespace matrix; using namespace time_literals; +using math::radians; VehicleAngularVelocity::VehicleAngularVelocity() : ModuleParams(nullptr), @@ -53,11 +54,15 @@ bool VehicleAngularVelocity::Start() { // force initial updates ParametersUpdate(true); - SensorBiasUpdate(true); - SensorCorrectionsUpdate(true); - // needed to change the active sensor if the primary stops updating - return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true); + // sensor_selection needed to change the active sensor if the primary stops updating + if (!_sensor_selection_sub.registerCallback()) { + PX4_ERR("sensor_selection callback registration failed"); + return false; + } + + ScheduleNow(); + return true; } void VehicleAngularVelocity::Stop() @@ -97,7 +102,7 @@ void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force) _sensor_correction_sub.copy(&corrections); // selected sensor has changed, find updated index - if ((_corrections_selected_instance != corrections.selected_gyro_instance) || force) { + if ((_corrections_selected_instance < 0) || force) { _corrections_selected_instance = -1; // find sensor_corrections index @@ -122,8 +127,8 @@ void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force) _scale = Vector3f{corrections.gyro_scale_2}; break; default: - _offset = Vector3f{0.0f, 0.0f, 0.0f}; - _scale = Vector3f{1.0f, 1.0f, 1.0f}; + _offset = Vector3f{0.f, 0.f, 0.f}; + _scale = Vector3f{1.f, 1.f, 1.f}; } } } @@ -154,8 +159,11 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) // clear bias and corrections _bias.zero(); - _offset = Vector3f{0.0f, 0.0f, 0.0f}; - _scale = Vector3f{1.0f, 1.0f, 1.0f}; + _offset = Vector3f{0.f, 0.f, 0.f}; + _scale = Vector3f{1.f, 1.f, 1.f}; + + // force corrections reselection + _corrections_selected_instance = -1; return true; } @@ -182,13 +190,13 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) updateParams(); // get transformation matrix from sensor/board to body frame - const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); + const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); // fine tune the rotation const Dcmf board_rotation_offset(Eulerf( - math::radians(_param_sens_board_x_off.get()), - math::radians(_param_sens_board_y_off.get()), - math::radians(_param_sens_board_z_off.get()))); + radians(_param_sens_board_x_off.get()), + radians(_param_sens_board_y_off.get()), + radians(_param_sens_board_z_off.get()))); _board_rotation = board_rotation_offset * board_rotation; } @@ -204,28 +212,29 @@ void VehicleAngularVelocity::Run() if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) { sensor_gyro_s sensor_data; - _sensor_sub[_selected_sensor_sub_index].copy(&sensor_data); - // get the sensor data and correct for thermal errors (apply offsets and scale) - const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z}; + if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) { + // get the sensor data and correct for thermal errors (apply offsets and scale) + const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z}; - // apply offsets and scale - Vector3f rates{(val - _offset).emult(_scale)}; + // apply offsets and scale + Vector3f rates{(val - _offset).emult(_scale)}; - // rotate corrected measurements from sensor to body frame - rates = _board_rotation * rates; + // rotate corrected measurements from sensor to body frame + rates = _board_rotation * rates; - // correct for in-run bias errors - rates -= _bias; + // correct for in-run bias errors + rates -= _bias; - // publish - vehicle_angular_velocity_s out; + // publish + vehicle_angular_velocity_s out; - out.timestamp_sample = sensor_data.timestamp_sample; - rates.copyTo(out.xyz); - out.timestamp = hrt_absolute_time(); + out.timestamp_sample = sensor_data.timestamp_sample; + rates.copyTo(out.xyz); + out.timestamp = hrt_absolute_time(); - _vehicle_angular_velocity_pub.publish(out); + _vehicle_angular_velocity_pub.publish(out); + } } } diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 6a7fdfc8840e..a53ffef54454 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -84,7 +84,6 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw) initializeSensors(); - _corrections_changed = true; //make sure to initially publish the corrections topic _selection_changed = true; return 0; @@ -132,76 +131,6 @@ void VotedSensorsUpdate::parametersUpdate() _mag_rotation[topic_instance] = _board_rotation; } - /* Load & apply the sensor calibrations. - * IMPORTANT: we assume all sensor drivers are running and published sensor data at this point - */ - - /* temperature compensation */ - _temperature_compensation.parameters_update(_hil_enabled); - - /* gyro */ - for (uint8_t topic_instance = 0; topic_instance < _gyro.subscription_count; ++topic_instance) { - - uORB::SubscriptionData report{ORB_ID(sensor_gyro_integrated), topic_instance}; - - int temp = _temperature_compensation.set_sensor_id_gyro(report.get().device_id, topic_instance); - - if (temp < 0) { - PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "gyro", report.get().device_id, - topic_instance); - - _corrections.gyro_mapping[topic_instance] = 0; - _corrections.gyro_device_ids[topic_instance] = 0; - - } else { - _corrections.gyro_mapping[topic_instance] = temp; - _corrections.gyro_device_ids[topic_instance] = report.get().device_id; - } - } - - - /* accel */ - for (uint8_t topic_instance = 0; topic_instance < _accel.subscription_count; ++topic_instance) { - - uORB::SubscriptionData report{ORB_ID(sensor_accel_integrated), topic_instance}; - - int temp = _temperature_compensation.set_sensor_id_accel(report.get().device_id, topic_instance); - - if (temp < 0) { - PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "accel", report.get().device_id, - topic_instance); - - _corrections.accel_mapping[topic_instance] = 0; - _corrections.accel_device_ids[topic_instance] = 0; - - } else { - _corrections.accel_mapping[topic_instance] = temp; - _corrections.accel_device_ids[topic_instance] = report.get().device_id; - } - } - - - /* baro */ - for (uint8_t topic_instance = 0; topic_instance < _baro.subscription_count; ++topic_instance) { - - uORB::SubscriptionData report{ORB_ID(sensor_baro), topic_instance}; - - int temp = _temperature_compensation.set_sensor_id_baro(report.get().device_id, topic_instance); - - if (temp < 0) { - PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "baro", report.get().device_id, - topic_instance); - - _corrections.baro_mapping[topic_instance] = 0; - _corrections.baro_device_ids[topic_instance] = 0; - - } else { - _corrections.baro_mapping[topic_instance] = temp; - _corrections.baro_device_ids[topic_instance] = report.get().device_id; - } - } - - /* set offset parameters to new values */ bool failed = false; @@ -601,11 +530,10 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw) _last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.dt; - // handle temperature compensation - if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature, - offsets[uorb_index], scales[uorb_index]) == 2) { - _corrections_changed = true; - } + // 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 // rotate corrected measurements from sensor to body frame accel_data = _board_rotation * accel_data; @@ -631,8 +559,6 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw) if (best_index != _accel.last_best_vote) { _accel.last_best_vote = (uint8_t)best_index; - _corrections.selected_accel_instance = (uint8_t)best_index; - _corrections_changed = true; } if (_selection.accel_device_id != _accel_device_id[best_index]) { @@ -685,11 +611,10 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw) _last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.dt; - // handle temperature compensation - if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature, - offsets[uorb_index], scales[uorb_index]) == 2) { - _corrections_changed = true; - } + // 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 // rotate corrected measurements from sensor to body frame gyro_rate = _board_rotation * gyro_rate; @@ -716,8 +641,6 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw) if (_gyro.last_best_vote != best_index) { _gyro.last_best_vote = (uint8_t)best_index; - _corrections.selected_gyro_instance = (uint8_t)best_index; - _corrections_changed = true; } if (_selection.gyro_device_id != _gyro_device_id[best_index]) { @@ -814,11 +737,8 @@ void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata) // Convert from millibar to Pa float corrected_pressure = 100.0f * baro_report.pressure; - // handle temperature compensation - if (_temperature_compensation.apply_corrections_baro(uorb_index, corrected_pressure, baro_report.temperature, - offsets[uorb_index], scales[uorb_index]) == 2) { - _corrections_changed = true; - } + // apply temperature compensation + corrected_pressure = (corrected_pressure - *offsets[uorb_index]) * *scales[uorb_index]; // First publication with data if (_baro.priority[uorb_index] == 0) { @@ -850,8 +770,6 @@ void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata) if (_baro.last_best_vote != best_index) { _baro.last_best_vote = (uint8_t)best_index; - _corrections.selected_baro_instance = (uint8_t)best_index; - _corrections_changed = true; } if (_selection.baro_device_id != _baro_device_id[best_index]) { @@ -1001,27 +919,18 @@ void VotedSensorsUpdate::printStatus() _mag.voter.print(); PX4_INFO("baro status:"); _baro.voter.print(); - - _temperature_compensation.print_status(); } void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata, vehicle_magnetometer_s &magnetometer) { + _corrections_sub.update(&_corrections); + accelPoll(raw); gyroPoll(raw); magPoll(magnetometer); baroPoll(airdata); - // publish sensor corrections if necessary - if (_corrections_changed) { - _corrections.timestamp = hrt_absolute_time(); - - _sensor_correction_pub.publish(_corrections); - - _corrections_changed = false; - } - // publish sensor selection if changed if (_selection_changed) { _selection.timestamp = hrt_absolute_time(); diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 87c591905f17..7643fc316d8e 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -48,12 +48,14 @@ #include #include +#include #include #include #include #include +#include #include #include #include @@ -64,7 +66,6 @@ #include #include -#include "temperature_compensation.h" #include "common.h" namespace sensors @@ -218,8 +219,11 @@ class VotedSensorsUpdate orb_advert_t _mavlink_log_pub{nullptr}; - uORB::Publication _sensor_correction_pub{ORB_ID(sensor_correction)}; /**< handle to the sensor correction uORB topic */ - uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */ + uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */ + uORB::PublicationQueued _info_pub{ORB_ID(subsystem_info)}; /* subsystem info publication */ + + /* sensor thermal compensation */ + uORB::Subscription _corrections_sub{ORB_ID(sensor_correction)}; 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 */ @@ -231,7 +235,6 @@ class VotedSensorsUpdate const Parameters &_parameters; const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */ - bool _corrections_changed{false}; bool _selection_changed{false}; /**< true when a sensor selection has changed and not been published */ float _accel_diff[3][2] {}; /**< filtered accel differences between IMU units (m/s/s) */ @@ -248,10 +251,6 @@ class VotedSensorsUpdate 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 */ - - TemperatureCompensation _temperature_compensation{}; /**< sensor thermal compensation */ - - uORB::PublicationQueued _info_pub{ORB_ID(subsystem_info)}; /* subsystem info publication */ }; } /* namespace sensors */ diff --git a/src/modules/temperature_compensation/CMakeLists.txt b/src/modules/temperature_compensation/CMakeLists.txt new file mode 100644 index 000000000000..50e6a181e059 --- /dev/null +++ b/src/modules/temperature_compensation/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__temperature_compensation + MAIN temperature_compensation + SRCS + TemperatureCompensationModule.cpp + TemperatureCompensation.cpp + temperature_calibration/accel.cpp + temperature_calibration/baro.cpp + temperature_calibration/gyro.cpp + temperature_calibration/task.cpp + DEPENDS + mathlib + ) diff --git a/src/modules/sensors/temperature_compensation.cpp b/src/modules/temperature_compensation/TemperatureCompensation.cpp similarity index 91% rename from src/modules/sensors/temperature_compensation.cpp rename to src/modules/temperature_compensation/TemperatureCompensation.cpp index b9865dfc607c..1cc305405485 100644 --- a/src/modules/sensors/temperature_compensation.cpp +++ b/src/modules/temperature_compensation/TemperatureCompensation.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -39,17 +39,17 @@ * @author Paul Riseborough */ -#include "temperature_compensation.h" +#include "TemperatureCompensation.h" #include #include #include -namespace sensors +namespace temperature_compensation { int TemperatureCompensation::initialize_parameter_handles(ParameterHandles ¶meter_handles) { - char nbuf[16]; + char nbuf[16] {}; int ret = PX4_ERROR; /* rate gyro calibration parameters */ @@ -151,24 +151,17 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles ¶ return PX4_OK; } -int TemperatureCompensation::parameters_update(bool hil_enabled) +int TemperatureCompensation::parameters_update() { - int ret = 0; - ParameterHandles parameter_handles; - ret = initialize_parameter_handles(parameter_handles); + int ret = initialize_parameter_handles(parameter_handles); if (ret != 0) { return ret; } /* rate gyro calibration parameters */ - if (!hil_enabled) { - param_get(parameter_handles.gyro_tc_enable, &_parameters.gyro_tc_enable); - - } else { - _parameters.gyro_tc_enable = 0; - } + param_get(parameter_handles.gyro_tc_enable, &_parameters.gyro_tc_enable); if (_parameters.gyro_tc_enable == 1) { for (unsigned j = 0; j < GYRO_COUNT_MAX; j++) { @@ -201,12 +194,7 @@ int TemperatureCompensation::parameters_update(bool hil_enabled) } /* accelerometer calibration parameters */ - if (!hil_enabled) { - param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable); - - } else { - _parameters.accel_tc_enable = 0; - } + param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable); if (_parameters.accel_tc_enable == 1) { for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) { @@ -239,12 +227,7 @@ int TemperatureCompensation::parameters_update(bool hil_enabled) } /* barometer calibration parameters */ - if (!hil_enabled) { - param_get(parameter_handles.baro_tc_enable, &_parameters.baro_tc_enable); - - } else { - _parameters.baro_tc_enable = 0; - } + param_get(parameter_handles.baro_tc_enable, &_parameters.baro_tc_enable); if (_parameters.baro_tc_enable == 1) { for (unsigned j = 0; j < BARO_COUNT_MAX; j++) { @@ -348,7 +331,6 @@ bool TemperatureCompensation::calc_thermal_offsets_3D(const SensorCalData3D &coe } return ret; - } int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_instance) @@ -392,27 +374,30 @@ int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instanc return -1; } -int TemperatureCompensation::apply_corrections_gyro(int topic_instance, matrix::Vector3f &sensor_data, - float temperature, float *offsets, float *scales) +int TemperatureCompensation::update_scales_and_offsets_gyro(int topic_instance, float temperature, float *offsets, + float *scales) { + // Check if temperature compensation is enabled if (_parameters.gyro_tc_enable != 1) { return 0; } + // Map device ID to uORB topic instance uint8_t mapping = _gyro_data.device_mapping[topic_instance]; if (mapping == 255) { return -1; } + // Calculate and update the offsets calc_thermal_offsets_3D(_parameters.gyro_cal_data[mapping], temperature, offsets); - // get the sensor scale factors and correct the data + // Update the scales for (unsigned axis_index = 0; axis_index < 3; axis_index++) { scales[axis_index] = _parameters.gyro_cal_data[mapping].scale[axis_index]; - sensor_data(axis_index) = (sensor_data(axis_index) - offsets[axis_index]) * scales[axis_index]; } + // Check if temperature delta is large enough to warrant a new publication if (fabsf(temperature - _gyro_data.last_temperature[topic_instance]) > 1.0f) { _gyro_data.last_temperature[topic_instance] = temperature; return 2; @@ -421,27 +406,30 @@ int TemperatureCompensation::apply_corrections_gyro(int topic_instance, matrix:: return 1; } -int TemperatureCompensation::apply_corrections_accel(int topic_instance, matrix::Vector3f &sensor_data, - float temperature, float *offsets, float *scales) +int TemperatureCompensation::update_scales_and_offsets_accel(int topic_instance, float temperature, float *offsets, + float *scales) { + // Check if temperature compensation is enabled if (_parameters.accel_tc_enable != 1) { return 0; } + // Map device ID to uORB topic instance uint8_t mapping = _accel_data.device_mapping[topic_instance]; if (mapping == 255) { return -1; } + // Calculate and update the offsets calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets); - // get the sensor scale factors and correct the data + // Update the scales for (unsigned axis_index = 0; axis_index < 3; axis_index++) { scales[axis_index] = _parameters.accel_cal_data[mapping].scale[axis_index]; - sensor_data(axis_index) = (sensor_data(axis_index) - offsets[axis_index]) * scales[axis_index]; } + // Check if temperature delta is large enough to warrant a new publication if (fabsf(temperature - _accel_data.last_temperature[topic_instance]) > 1.0f) { _accel_data.last_temperature[topic_instance] = temperature; return 2; @@ -450,25 +438,28 @@ int TemperatureCompensation::apply_corrections_accel(int topic_instance, matrix: return 1; } -int TemperatureCompensation::apply_corrections_baro(int topic_instance, float &sensor_data, float temperature, - float *offsets, float *scales) +int TemperatureCompensation::update_scales_and_offsets_baro(int topic_instance, float temperature, float *offsets, + float *scales) { + // Check if temperature compensation is enabled if (_parameters.baro_tc_enable != 1) { return 0; } + // Map device ID to uORB topic instance uint8_t mapping = _baro_data.device_mapping[topic_instance]; if (mapping == 255) { return -1; } + // Calculate and update the offsets calc_thermal_offsets_1D(_parameters.baro_cal_data[mapping], temperature, *offsets); - // get the sensor scale factors and correct the data + // Update the scales *scales = _parameters.baro_cal_data[mapping].scale; - sensor_data = (sensor_data - *offsets) * *scales; + // Check if temperature delta is large enough to warrant a new publication if (fabsf(temperature - _baro_data.last_temperature[topic_instance]) > 1.0f) { _baro_data.last_temperature[topic_instance] = temperature; return 2; @@ -517,4 +508,4 @@ void TemperatureCompensation::print_status() } } -} +} // namespace temperature_compensation diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/temperature_compensation/TemperatureCompensation.h similarity index 81% rename from src/modules/sensors/temperature_compensation.h rename to src/modules/temperature_compensation/TemperatureCompensation.h index 69ab49afeca9..c5ee39a7a32c 100644 --- a/src/modules/sensors/temperature_compensation.h +++ b/src/modules/temperature_compensation/TemperatureCompensation.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -46,18 +46,19 @@ #include #include -#include "common.h" - - -namespace sensors +namespace temperature_compensation { -static_assert(GYRO_COUNT_MAX == 3, - "GYRO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)"); +static constexpr uint8_t GYRO_COUNT_MAX = 3; +static constexpr uint8_t ACCEL_COUNT_MAX = 3; +static constexpr uint8_t BARO_COUNT_MAX = 3; + +static_assert(GYRO_COUNT_MAX == 3, "GYRO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)"); static_assert(ACCEL_COUNT_MAX == 3, "ACCEL_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)"); -static_assert(BARO_COUNT_MAX == 3, - "BARO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)"); +static_assert(BARO_COUNT_MAX == 3, "BARO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)"); + +static constexpr uint8_t SENSOR_COUNT_MAX = 3; /** ** class TemperatureCompensation @@ -68,7 +69,7 @@ class TemperatureCompensation public: /** (re)load the parameters. Make sure to call this on startup as well */ - int parameters_update(bool hil_enabled = false); + int parameters_update(); /** supply information which device_id matches a specific uORB topic_instance * (needed if a system has multiple sensors of the same type) @@ -77,7 +78,6 @@ class TemperatureCompensation int set_sensor_id_accel(uint32_t device_id, int topic_instance); int set_sensor_id_baro(uint32_t device_id, int topic_instance); - /** * Apply Thermal corrections to gyro (& other) sensor data. * @param topic_instance uORB topic instance @@ -90,13 +90,9 @@ class TemperatureCompensation * 1: corrections applied but no changes to offsets & scales, * 2: corrections applied and offsets & scales updated */ - int apply_corrections_gyro(int topic_instance, matrix::Vector3f &sensor_data, float temperature, float *offsets, - float *scales); - - int apply_corrections_accel(int topic_instance, matrix::Vector3f &sensor_data, float temperature, float *offsets, - float *scales); - - int apply_corrections_baro(int topic_instance, float &sensor_data, float temperature, float *offsets, float *scales); + int update_scales_and_offsets_gyro(int topic_instance, float temperature, float *offsets, float *scales); + int update_scales_and_offsets_accel(int topic_instance, float temperature, float *offsets, float *scales); + int update_scales_and_offsets_baro(int topic_instance, float temperature, float *offsets, float *scales); /** output current configuration status to console */ void print_status(); @@ -187,22 +183,26 @@ class TemperatureCompensation // create a struct containing all thermal calibration parameters struct Parameters { - int32_t gyro_tc_enable; - SensorCalData3D gyro_cal_data[GYRO_COUNT_MAX]; - int32_t accel_tc_enable; - SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX]; - int32_t baro_tc_enable; - SensorCalData1D baro_cal_data[BARO_COUNT_MAX]; + int32_t gyro_tc_enable{0}; + SensorCalData3D gyro_cal_data[GYRO_COUNT_MAX] {}; + + int32_t accel_tc_enable{0}; + SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX] {}; + + int32_t baro_tc_enable{0}; + SensorCalData1D baro_cal_data[BARO_COUNT_MAX] {}; }; // create a struct containing the handles required to access all calibration parameters struct ParameterHandles { - param_t gyro_tc_enable; - SensorCalHandles3D gyro_cal_handles[GYRO_COUNT_MAX]; - param_t accel_tc_enable; - SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX]; - param_t baro_tc_enable; - SensorCalHandles1D baro_cal_handles[BARO_COUNT_MAX]; + param_t gyro_tc_enable{PARAM_INVALID}; + SensorCalHandles3D gyro_cal_handles[GYRO_COUNT_MAX] {}; + + param_t accel_tc_enable{PARAM_INVALID}; + SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX] {}; + + param_t baro_tc_enable{PARAM_INVALID}; + SensorCalHandles1D baro_cal_handles[BARO_COUNT_MAX] {}; }; @@ -256,22 +256,30 @@ class TemperatureCompensation struct PerSensorData { + PerSensorData() { - for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { device_mapping[i] = 255; last_temperature[i] = -100.0f; } + for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { + device_mapping[i] = 255; + last_temperature[i] = -100.0f; + } } + void reset_temperature() { - for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { last_temperature[i] = -100.0f; } + for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { + last_temperature[i] = -100.0f; + } } - uint8_t device_mapping[SENSOR_COUNT_MAX]; /// map a topic instance to the parameters index - float last_temperature[SENSOR_COUNT_MAX]; + + uint8_t device_mapping[SENSOR_COUNT_MAX] {}; /// map a topic instance to the parameters index + float last_temperature[SENSOR_COUNT_MAX] {}; }; + PerSensorData _gyro_data; PerSensorData _accel_data; PerSensorData _baro_data; - template static inline int set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data, const T *sensor_cal_data, uint8_t sensor_count_max); diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp new file mode 100644 index 000000000000..0269cfb6681b --- /dev/null +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp @@ -0,0 +1,421 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "TemperatureCompensationModule.h" + +#include "temperature_calibration/temperature_calibration.h" + +#include +#include + +#include + +using namespace temperature_compensation; +using namespace time_literals; + +TemperatureCompensationModule::TemperatureCompensationModule() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default), + _loop_perf(perf_alloc(PC_ELAPSED, "temperature_compensation")) +{ + // Initialize the publication variables + 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; +} + +TemperatureCompensationModule::~TemperatureCompensationModule() +{ + perf_free(_loop_perf); +} + +void TemperatureCompensationModule::parameters_update() +{ + _temperature_compensation.parameters_update(); + + // Gyro + for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) { + sensor_gyro_s report; + + if (_gyro_subs[uorb_index].copy(&report)) { + int temp = _temperature_compensation.set_sensor_id_gyro(report.device_id, uorb_index); + + if (temp < 0) { + PX4_ERR("%s init: failed to find device ID %u for instance %i", "gyro", report.device_id, uorb_index); + _corrections.gyro_mapping[uorb_index] = 0; + _corrections.gyro_device_ids[uorb_index] = 0; + + } else { + _corrections.gyro_mapping[uorb_index] = temp; + _corrections.gyro_device_ids[uorb_index] = report.device_id; + } + } + } + + // Accel + for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) { + sensor_accel_s report; + + if (_accel_subs[uorb_index].copy(&report)) { + int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, uorb_index); + + if (temp < 0) { + PX4_ERR("%s init: failed to find device ID %u for instance %i", "accel", report.device_id, uorb_index); + + _corrections.accel_mapping[uorb_index] = 0; + _corrections.accel_device_ids[uorb_index] = 0; + + } else { + _corrections.accel_mapping[uorb_index] = temp; + _corrections.accel_device_ids[uorb_index] = report.device_id; + } + } + } + + // Baro + for (uint8_t uorb_index = 0; uorb_index < BARO_COUNT_MAX; uorb_index++) { + sensor_baro_s report; + + if (_baro_subs[uorb_index].copy(&report)) { + int temp = _temperature_compensation.set_sensor_id_baro(report.device_id, uorb_index); + + if (temp < 0) { + PX4_ERR("%s init: failed to find device ID %u for instance %i", "baro", report.device_id, uorb_index); + _corrections.baro_mapping[uorb_index] = 0; + _corrections.baro_device_ids[uorb_index] = 0; + + } else { + _corrections.baro_mapping[uorb_index] = temp; + _corrections.baro_device_ids[uorb_index] = temp; + + } + } + } +} + +void TemperatureCompensationModule::accelPoll() +{ + 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 each accel instance + for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) { + sensor_accel_s report; + + // Grab temperature from report + if (_accel_subs[uorb_index].update(&report)) { + + // Update the scales and offsets and mark for publication if they've changed + if (_temperature_compensation.update_scales_and_offsets_accel(uorb_index, report.temperature, offsets[uorb_index], + scales[uorb_index]) == 2) { + + _corrections.accel_device_ids[uorb_index] = report.device_id; + _corrections_changed = true; + } + } + } +} + +void TemperatureCompensationModule::gyroPoll() +{ + 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 each gyro instance + for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) { + sensor_gyro_s report; + + // Grab temperature from report + if (_gyro_subs[uorb_index].update(&report)) { + + // Update the scales and offsets and mark for publication if they've changed + if (_temperature_compensation.update_scales_and_offsets_gyro(uorb_index, report.temperature, offsets[uorb_index], + scales[uorb_index]) == 2) { + + _corrections.gyro_device_ids[uorb_index] = report.device_id; + _corrections_changed = true; + } + } + } +} + +void TemperatureCompensationModule::baroPoll() +{ + 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 each baro instance + for (uint8_t uorb_index = 0; uorb_index < BARO_COUNT_MAX; uorb_index++) { + sensor_baro_s report; + + // Grab temperature from report + if (_baro_subs[uorb_index].update(&report)) { + + // Update the scales and offsets and mark for publication if they've changed + if (_temperature_compensation.update_scales_and_offsets_baro(uorb_index, report.temperature, + offsets[uorb_index], scales[uorb_index]) == 2) { + + _corrections.baro_device_ids[uorb_index] = report.device_id; + _corrections_changed = true; + } + } + } +} + +void TemperatureCompensationModule::Run() +{ + perf_begin(_loop_perf); + + // Check if user has requested to run the calibration routine + if (_vehicle_command_sub.updated()) { + vehicle_command_s cmd; + + if (_vehicle_command_sub.copy(&cmd)) { + if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION) { + bool got_temperature_calibration_command = false; + bool accel = false; + bool baro = false; + bool gyro = false; + + if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { + gyro = true; + got_temperature_calibration_command = true; + } + + if ((int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { + accel = true; + got_temperature_calibration_command = true; + } + + if ((int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) { + baro = true; + got_temperature_calibration_command = true; + } + + if (got_temperature_calibration_command) { + int ret = run_temperature_calibration(accel, baro, gyro); + + // publish ACK + vehicle_command_ack_s command_ack{}; + + if (ret == 0) { + command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED; + } + + command_ack.timestamp = hrt_absolute_time(); + command_ack.command = cmd.command; + command_ack.target_system = cmd.source_system; + command_ack.target_component = cmd.source_component; + + uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(command_ack); + } + } + } + } + + // Check if any parameter has changed + if (_params_sub.updated()) { + // Read from param to clear updated flag + parameter_update_s update; + _params_sub.copy(&update); + + parameters_update(); + } + + accelPoll(); + gyroPoll(); + baroPoll(); + + // publish sensor corrections if necessary + if (_corrections_changed) { + _corrections.timestamp = hrt_absolute_time(); + + _sensor_correction_pub.publish(_corrections); + + _corrections_changed = false; + } + + perf_end(_loop_perf); +} + +int TemperatureCompensationModule::task_spawn(int argc, char *argv[]) +{ + TemperatureCompensationModule *instance = new TemperatureCompensationModule(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +bool TemperatureCompensationModule::init() +{ + ScheduleOnInterval(1_s); + return true; +} + +int TemperatureCompensationModule::custom_command(int argc, char *argv[]) +{ + if (!strcmp(argv[0], "calibrate")) { + + bool accel_calib = false; + bool baro_calib = false; + bool gyro_calib = false; + bool calib_all = true; + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "abg", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'a': + accel_calib = true; + calib_all = false; + break; + + case 'b': + baro_calib = true; + calib_all = false; + break; + + case 'g': + gyro_calib = true; + calib_all = false; + break; + + default: + print_usage("unrecognized flag"); + + return PX4_ERROR; + } + } + + if (!is_running()) { + PX4_WARN("background task not running"); + + if (task_spawn(0, nullptr) != PX4_OK) { + return PX4_ERROR; + } + } + + vehicle_command_s vcmd{}; + vcmd.timestamp = hrt_absolute_time(); + vcmd.param1 = (float)((gyro_calib + || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN); + vcmd.param2 = NAN; + vcmd.param3 = NAN; + vcmd.param4 = NAN; + vcmd.param5 = ((accel_calib + || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : (double)NAN); + vcmd.param6 = (double)NAN; + vcmd.param7 = (float)((baro_calib + || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN); + vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION; + + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + vcmd_pub.publish(vcmd); + + return PX4_OK; + + } else { + print_usage("unrecognized command"); + + return PX4_ERROR; + } +} + +int TemperatureCompensationModule::print_status() +{ + _temperature_compensation.print_status(); + + return PX4_OK; +} + +int TemperatureCompensationModule::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The temperature compensation module allows all of the gyro(s), accel(s), and baro(s) in the system to be temperature +compensated. The module monitors the data coming from the sensors and updates the associated sensor_thermal_cal topic +whenever a change in temperature is detected. The module can also be configured to perform the coeffecient calculation +routine at next boot, which allows the thermal calibration coeffecients to be calculated while the vehicle undergoes +a temperature cycle. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("temperature_compensation", "system"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the module, which monitors the sensors and updates the sensor_thermal_cal topic"); + PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run temperature calibration process"); + PRINT_MODULE_USAGE_PARAM_FLAG('g', "calibrate the gyro", true); + PRINT_MODULE_USAGE_PARAM_FLAG('a', "calibrate the accel", true); + PRINT_MODULE_USAGE_PARAM_FLAG('b', "calibrate the baro (if none of these is given, all will be calibrated)", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int temperature_compensation_main(int argc, char *argv[]) +{ + return TemperatureCompensationModule::main(argc, argv); +} diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.h b/src/modules/temperature_compensation/TemperatureCompensationModule.h new file mode 100644 index 000000000000..b8ee47f61a2e --- /dev/null +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.h @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "TemperatureCompensation.h" + +namespace temperature_compensation +{ + +class TemperatureCompensationModule : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + TemperatureCompensationModule(); + ~TemperatureCompensationModule() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static TemperatureCompensationModule *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void Run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; + + /** + * Initializes scheduling on work queue. + */ + bool init(); + +private: + + void accelPoll(); + void gyroPoll(); + void baroPoll(); + + /** + * call this whenever parameters got updated. Make sure to have initialize_sensors() called at least + * once before calling this. + */ + void parameters_update(); + + uORB::Subscription _accel_subs[ACCEL_COUNT_MAX] { + {ORB_ID(sensor_accel), 0}, + {ORB_ID(sensor_accel), 1}, + {ORB_ID(sensor_accel), 2} + }; + + uORB::Subscription _gyro_subs[GYRO_COUNT_MAX] { + {ORB_ID(sensor_gyro), 0}, + {ORB_ID(sensor_gyro), 1}, + {ORB_ID(sensor_gyro), 2} + }; + + uORB::Subscription _baro_subs[BARO_COUNT_MAX] { + {ORB_ID(sensor_baro), 0}, + {ORB_ID(sensor_baro), 1}, + {ORB_ID(sensor_baro), 2} + }; + + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + + perf_counter_t _loop_perf; /**< loop performance counter */ + + orb_advert_t _mavlink_log_pub{nullptr}; + + /* sensor thermal compensation */ + TemperatureCompensation _temperature_compensation; + + sensor_correction_s _corrections{}; /**< struct containing the sensor corrections to be published to the uORB*/ + uORB::Publication _sensor_correction_pub{ORB_ID(sensor_correction)}; + + bool _corrections_changed{true}; +}; + +} // namespace temperature_compensation diff --git a/src/modules/sensors/temp_comp_params_accel.c b/src/modules/temperature_compensation/temp_comp_params_accel.c similarity index 100% rename from src/modules/sensors/temp_comp_params_accel.c rename to src/modules/temperature_compensation/temp_comp_params_accel.c diff --git a/src/modules/sensors/temp_comp_params_baro.c b/src/modules/temperature_compensation/temp_comp_params_baro.c similarity index 100% rename from src/modules/sensors/temp_comp_params_baro.c rename to src/modules/temperature_compensation/temp_comp_params_baro.c diff --git a/src/modules/sensors/temp_comp_params_gyro.c b/src/modules/temperature_compensation/temp_comp_params_gyro.c similarity index 100% rename from src/modules/sensors/temp_comp_params_gyro.c rename to src/modules/temperature_compensation/temp_comp_params_gyro.c diff --git a/src/modules/events/temperature_calibration/accel.cpp b/src/modules/temperature_compensation/temperature_calibration/accel.cpp similarity index 99% rename from src/modules/events/temperature_calibration/accel.cpp rename to src/modules/temperature_compensation/temperature_calibration/accel.cpp index b20afd0c4516..a0464591eab2 100644 --- a/src/modules/events/temperature_calibration/accel.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/accel.cpp @@ -49,8 +49,7 @@ TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_r float max_start_temperature) : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature) { - - //init subscriptions + // init subscriptions _num_sensor_instances = orb_group_count(ORB_ID(sensor_accel)); if (_num_sensor_instances > SENSOR_COUNT_MAX) { diff --git a/src/modules/events/temperature_calibration/accel.h b/src/modules/temperature_compensation/temperature_calibration/accel.h similarity index 100% rename from src/modules/events/temperature_calibration/accel.h rename to src/modules/temperature_compensation/temperature_calibration/accel.h diff --git a/src/modules/events/temperature_calibration/baro.cpp b/src/modules/temperature_compensation/temperature_calibration/baro.cpp similarity index 97% rename from src/modules/events/temperature_calibration/baro.cpp rename to src/modules/temperature_compensation/temperature_calibration/baro.cpp index 85d9fb20e4e3..7927cd26ad4b 100644 --- a/src/modules/events/temperature_calibration/baro.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/baro.cpp @@ -49,8 +49,7 @@ TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_ris float max_start_temperature) : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature) { - - //init subscriptions + // init subscriptions _num_sensor_instances = orb_group_count(ORB_ID(sensor_baro)); if (_num_sensor_instances > SENSOR_COUNT_MAX) { @@ -71,7 +70,7 @@ TemperatureCalibrationBaro::~TemperatureCalibrationBaro() void TemperatureCalibrationBaro::reset_calibration() { - //nothing to do + // nothing to do } int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int sensor_sub) @@ -133,7 +132,7 @@ int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int return 1; } - //TODO: Detect when temperature has stopped rising for more than TBD seconds + // TODO: Detect when temperature has stopped rising for more than TBD seconds if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) { data.hot_soaked = true; } @@ -144,7 +143,7 @@ int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int (double)(data.high_temp - data.low_temp)); } - //update linear fit matrices + // update linear fit matrices double relative_temperature = (double)data.sensor_sample_filt[1] - (double)data.ref_temp; data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]); @@ -173,7 +172,7 @@ int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int return 0; } - double res[POLYFIT_ORDER + 1] = {}; + double res[POLYFIT_ORDER + 1] {}; data.P[0].fit(res); res[POLYFIT_ORDER] = 0.0; // normalise the correction to be zero at the reference temperature by setting the X^0 coefficient to zero diff --git a/src/modules/events/temperature_calibration/baro.h b/src/modules/temperature_compensation/temperature_calibration/baro.h similarity index 100% rename from src/modules/events/temperature_calibration/baro.h rename to src/modules/temperature_compensation/temperature_calibration/baro.h diff --git a/src/modules/events/temperature_calibration/common.h b/src/modules/temperature_compensation/temperature_calibration/common.h similarity index 98% rename from src/modules/events/temperature_calibration/common.h rename to src/modules/temperature_compensation/temperature_calibration/common.h index 5b2f9060cab7..27ec331c9486 100644 --- a/src/modules/events/temperature_calibration/common.h +++ b/src/modules/temperature_compensation/temperature_calibration/common.h @@ -62,7 +62,7 @@ class TemperatureCalibrationBase : _min_temperature_rise(min_temperature_rise), _min_start_temperature(min_start_temperature), _max_start_temperature(max_start_temperature) {} - virtual ~TemperatureCalibrationBase() {} + virtual ~TemperatureCalibrationBase() = default; /** * check & update new sensor data. @@ -97,11 +97,9 @@ class TemperatureCalibrationBase float _max_start_temperature; ///< maximum temperature above which the process does not start and an error is declared }; - - int TemperatureCalibrationBase::set_parameter(const char *format_str, unsigned index, const void *value) { - char param_str[30]; + char param_str[30] {}; (void)sprintf(param_str, format_str, index); int result = param_set_no_notification(param_find(param_str), value); diff --git a/src/modules/events/temperature_calibration/gyro.cpp b/src/modules/temperature_compensation/temperature_calibration/gyro.cpp similarity index 97% rename from src/modules/events/temperature_calibration/gyro.cpp rename to src/modules/temperature_compensation/temperature_calibration/gyro.cpp index 1f34b88c0b42..27a7f92587e0 100644 --- a/src/modules/events/temperature_calibration/gyro.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/gyro.cpp @@ -53,7 +53,6 @@ TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_ris } _num_sensor_instances = num_gyros; - } void TemperatureCalibrationGyro::reset_calibration() @@ -79,7 +78,7 @@ int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int return finished ? 0 : 1; } - sensor_gyro_s gyro_data; + sensor_gyro_s gyro_data{}; orb_copy(ORB_ID(sensor_gyro), sensor_sub, &gyro_data); if (finished) { @@ -128,7 +127,7 @@ int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int return 1; } - //TODO: Detect when temperature has stopped rising for more than TBD seconds + // TODO: Detect when temperature has stopped rising for more than TBD seconds if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) { data.hot_soaked = true; } @@ -171,7 +170,7 @@ int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int return 0; } - double res[3][4] = {}; + double res[3][4] {}; data.P[0].fit(res[0]); PX4_INFO("Result Gyro %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1], (double)res[0][2], @@ -186,7 +185,7 @@ int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int (double)res[2][3]); data.tempcal_complete = true; - char str[30]; + char str[30] {}; float param = 0.0f; int result = PX4_OK; @@ -207,5 +206,6 @@ int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int set_parameter("TC_G%d_TMAX", sensor_index, &data.high_temp); set_parameter("TC_G%d_TMIN", sensor_index, &data.low_temp); set_parameter("TC_G%d_TREF", sensor_index, &data.ref_temp); + return 0; } diff --git a/src/modules/events/temperature_calibration/gyro.h b/src/modules/temperature_compensation/temperature_calibration/gyro.h similarity index 100% rename from src/modules/events/temperature_calibration/gyro.h rename to src/modules/temperature_compensation/temperature_calibration/gyro.h diff --git a/src/modules/events/temperature_calibration/polyfit.hpp b/src/modules/temperature_compensation/temperature_calibration/polyfit.hpp similarity index 97% rename from src/modules/events/temperature_calibration/polyfit.hpp rename to src/modules/temperature_compensation/temperature_calibration/polyfit.hpp index 44fbb7d9316b..70cc28a5a27c 100644 --- a/src/modules/events/temperature_calibration/polyfit.hpp +++ b/src/modules/temperature_compensation/temperature_calibration/polyfit.hpp @@ -93,15 +93,9 @@ Author: Siddharth Bharat Purohit #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include + #include + #include #define DEBUG 0 diff --git a/src/modules/events/temperature_calibration/task.cpp b/src/modules/temperature_compensation/temperature_calibration/task.cpp similarity index 87% rename from src/modules/events/temperature_calibration/task.cpp rename to src/modules/temperature_compensation/temperature_calibration/task.cpp index 548e754c1b9e..2bcfc0151f0e 100644 --- a/src/modules/events/temperature_calibration/task.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/task.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -60,21 +61,14 @@ class TemperatureCalibration; namespace temperature_calibration { -TemperatureCalibration *instance = nullptr; +px4::atomic instance{nullptr}; } - class TemperatureCalibration { public: - /** - * Constructor - */ - TemperatureCalibration(bool accel, bool baro, bool gyro); - /** - * Destructor - */ + TemperatureCalibration(bool accel, bool baro, bool gyro) : _accel(accel), _baro(baro), _gyro(gyro) {} ~TemperatureCalibration() = default; /** @@ -98,21 +92,16 @@ class TemperatureCalibration bool _force_task_exit = false; int _control_task = -1; // task handle for task - bool _accel; ///< enable accel calibration? - bool _baro; ///< enable baro calibration? - bool _gyro; ///< enable gyro calibration? + const bool _accel; ///< enable accel calibration? + const bool _baro; ///< enable baro calibration? + const bool _gyro; ///< enable gyro calibration? }; -TemperatureCalibration::TemperatureCalibration(bool accel, bool baro, bool gyro) - : _accel(accel), _baro(baro), _gyro(gyro) -{ -} - void TemperatureCalibration::task_main() { // subscribe to all gyro instances - int gyro_sub[SENSOR_COUNT_MAX] = {}; - px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {}; + int gyro_sub[SENSOR_COUNT_MAX] {-1, -1, -1}; + px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] {}; unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro)); if (num_gyro > SENSOR_COUNT_MAX) { @@ -136,8 +125,8 @@ void TemperatureCalibration::task_main() param_get(param_find("SYS_CAL_TMAX"), &max_start_temp); //init calibrators - TemperatureCalibrationBase *calibrators[3]; - bool error_reported[3] = {}; + TemperatureCalibrationBase *calibrators[3] {}; + bool error_reported[3] {}; int num_calibrators = 0; if (_accel) { @@ -187,7 +176,7 @@ void TemperatureCalibration::task_main() hrt_abstime next_progress_output = hrt_absolute_time() + 1e6; // control LED's: blink, then turn solid according to progress - led_control_s led_control = {}; + led_control_s led_control{}; led_control.led_mask = 0xff; led_control.mode = led_control_s::MODE_BLINK_NORMAL; led_control.priority = led_control_s::MAX_PRIORITY; @@ -316,20 +305,19 @@ void TemperatureCalibration::task_main() orb_unsubscribe(gyro_sub[i]); } - delete temperature_calibration::instance; - temperature_calibration::instance = nullptr; + delete temperature_calibration::instance.load(); + temperature_calibration::instance.store(nullptr); PX4_INFO("Exiting temperature calibration task"); } int TemperatureCalibration::do_temperature_calibration(int argc, char *argv[]) { - temperature_calibration::instance->task_main(); + temperature_calibration::instance.load()->task_main(); return 0; } int TemperatureCalibration::start() { - _control_task = px4_task_spawn_cmd("temperature_calib", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, @@ -338,8 +326,8 @@ int TemperatureCalibration::start() nullptr); if (_control_task < 0) { - delete temperature_calibration::instance; - temperature_calibration::instance = nullptr; + delete temperature_calibration::instance.load(); + temperature_calibration::instance.store(nullptr); PX4_ERR("start failed"); return -errno; } @@ -355,13 +343,20 @@ void TemperatureCalibration::publish_led_control(led_control_s &led_control) int run_temperature_calibration(bool accel, bool baro, bool gyro) { - PX4_INFO("Starting temperature calibration task (accel=%i, baro=%i, gyro=%i)", (int)accel, (int)baro, (int)gyro); - temperature_calibration::instance = new TemperatureCalibration(accel, baro, gyro); + if (temperature_calibration::instance.load() == nullptr) { + PX4_INFO("Starting temperature calibration task (accel=%i, baro=%i, gyro=%i)", (int)accel, (int)baro, (int)gyro); + temperature_calibration::instance.store(new TemperatureCalibration(accel, baro, gyro)); - if (temperature_calibration::instance == nullptr) { - PX4_ERR("alloc failed"); - return 1; + if (temperature_calibration::instance.load() == nullptr) { + PX4_ERR("alloc failed"); + return 1; + } + + return temperature_calibration::instance.load()->start(); + + } else { + PX4_WARN("temperature calibration task already running"); } - return temperature_calibration::instance->start(); + return PX4_ERROR; } diff --git a/src/modules/events/temperature_calibration/temperature_calibration.h b/src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h similarity index 99% rename from src/modules/events/temperature_calibration/temperature_calibration.h rename to src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h index 4d6dac61e729..d5a7bd257771 100644 --- a/src/modules/events/temperature_calibration/temperature_calibration.h +++ b/src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h @@ -37,4 +37,3 @@ /** start temperature calibration in a new task for one or multiple sensors * @return 0 on success, <0 error otherwise */ int run_temperature_calibration(bool accel, bool baro, bool gyro); -