accel and gyro calibration refactor and cleanup

- remove all remaining IOCTLs for accel and gyro and handle all calibration entirely in sensors module with parameters
 - sensor_accel and sensor_gyro are now always raw sensor data
 - calibration procedures no longer need to first clear existing values before starting
 - temperature calibration (TC) remove all scale (SCL) parameters
    - gyro and baro scale are completely unused
    - regular accel calibration scale can be used (CAL_ACC*_xSCALE) instead of TC scale

.ci/Jenkinsfile-hardware

@@ -759,6 +759,8 @@ void statusFTDI() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "adc test"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander check"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate gyro; sleep 2; param show CAL_GYRO*"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate level; sleep 2; param show SENS_BOARD*"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "df"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"'
@@ -820,6 +822,8 @@ void statusSEGGER() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "adc test"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander check"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander status"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate gyro; sleep 2; param show CAL_GYRO*"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate level; sleep 2; param show SENS_BOARD*"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dataman status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "df -h"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dmesg"'

Tools/process_sensor_caldata.py

@@ -122,10 +122,7 @@ gyro_0_params = {
 'TC_G0_X0_2':0.0,
 'TC_G0_X1_2':0.0,
 'TC_G0_X2_2':0.0,
-'TC_G0_X3_2':0.0,
-'TC_G0_SCL_0':1.0,
-'TC_G0_SCL_1':1.0,
-'TC_G0_SCL_2':1.0
+'TC_G0_X3_2':0.0
 }
 
 # curve fit the data for gyro 0 corrections
@@ -174,7 +171,7 @@ if num_gyros >= 1:
     plt.subplot(3,1,1)
     plt.plot(sensor_gyro_0['temperature'],sensor_gyro_0['x'],'b')
     plt.plot(temp_resample,gyro_0_x_resample,'r')
-    plt.title('Gyro 0 Bias vs Temperature')
+    plt.title('Gyro 0 ({}) Bias vs Temperature'.format(gyro_0_params['TC_G0_ID']))
     plt.ylabel('X bias (rad/s)')
     plt.xlabel('temperature (degC)')
     plt.grid()
@@ -218,10 +215,7 @@ gyro_1_params = {
 'TC_G1_X0_2':0.0,
 'TC_G1_X1_2':0.0,
 'TC_G1_X2_2':0.0,
-'TC_G1_X3_2':0.0,
-'TC_G1_SCL_0':1.0,
-'TC_G1_SCL_1':1.0,
-'TC_G1_SCL_2':1.0
+'TC_G1_X3_2':0.0
 }
 
 # curve fit the data for gyro 1 corrections
@@ -270,7 +264,7 @@ if num_gyros >= 2:
     plt.subplot(3,1,1)
     plt.plot(sensor_gyro_1['temperature'],sensor_gyro_1['x'],'b')
     plt.plot(temp_resample,gyro_1_x_resample,'r')
-    plt.title('Gyro 1 Bias vs Temperature')
+    plt.title('Gyro 1 ({}) Bias vs Temperature'.format(gyro_1_params['TC_G1_ID']))
     plt.ylabel('X bias (rad/s)')
     plt.xlabel('temperature (degC)')
     plt.grid()
@@ -314,10 +308,7 @@ gyro_2_params = {
 'TC_G2_X0_2':0.0,
 'TC_G2_X1_2':0.0,
 'TC_G2_X2_2':0.0,
-'TC_G2_X3_2':0.0,
-'TC_G2_SCL_0':1.0,
-'TC_G2_SCL_1':1.0,
-'TC_G2_SCL_2':1.0
+'TC_G2_X3_2':0.0
 }
 
 # curve fit the data for gyro 2 corrections
@@ -366,7 +357,7 @@ if num_gyros >= 3:
     plt.subplot(3,1,1)
     plt.plot(sensor_gyro_2['temperature'],sensor_gyro_2['x'],'b')
     plt.plot(temp_resample,gyro_2_x_resample,'r')
-    plt.title('Gyro 2 Bias vs Temperature')
+    plt.title('Gyro 2 ({}) Bias vs Temperature'.format(gyro_2_params['TC_G2_ID']))
     plt.ylabel('X bias (rad/s)')
     plt.xlabel('temperature (degC)')
     plt.grid()
@@ -410,10 +401,7 @@ accel_0_params = {
 'TC_A0_X0_2':0.0,
 'TC_A0_X1_2':0.0,
 'TC_A0_X2_2':0.0,
-'TC_A0_X3_2':0.0,
-'TC_A0_SCL_0':1.0,
-'TC_A0_SCL_1':1.0,
-'TC_A0_SCL_2':1.0
+'TC_A0_X3_2':0.0
 }
 
 # curve fit the data for accel 0 corrections
@@ -465,7 +453,7 @@ if num_accels >= 1:
     plt.subplot(3,1,1)
     plt.plot(sensor_accel_0['temperature'],correction_x,'b')
     plt.plot(temp_resample,correction_x_resample,'r')
-    plt.title('Accel 0 Bias vs Temperature')
+    plt.title('Accel 0 ({}) Bias vs Temperature'.format(accel_0_params['TC_A0_ID']))
     plt.ylabel('X bias (m/s/s)')
     plt.xlabel('temperature (degC)')
     plt.grid()
@@ -509,10 +497,7 @@ accel_1_params = {
 'TC_A1_X0_2':0.0,
 'TC_A1_X1_2':0.0,
 'TC_A1_X2_2':0.0,
-'TC_A1_X3_2':0.0,
-'TC_A1_SCL_0':1.0,
-'TC_A1_SCL_1':1.0,
-'TC_A1_SCL_2':1.0
+'TC_A1_X3_2':0.0
 }
 
 # curve fit the data for accel 1 corrections
@@ -564,7 +549,7 @@ if num_accels >= 2:
     plt.subplot(3,1,1)
     plt.plot(sensor_accel_1['temperature'],correction_x,'b')
     plt.plot(temp_resample,correction_x_resample,'r')
-    plt.title('Accel 1 Bias vs Temperature')
+    plt.title('Accel 1 ({}) Bias vs Temperature'.format(accel_1_params['TC_A1_ID']))
     plt.ylabel('X bias (m/s/s)')
     plt.xlabel('temperature (degC)')
     plt.grid()
@@ -608,10 +593,7 @@ accel_2_params = {
 'TC_A2_X0_2':0.0,
 'TC_A2_X1_2':0.0,
 'TC_A2_X2_2':0.0,
-'TC_A2_X3_2':0.0,
-'TC_A2_SCL_0':1.0,
-'TC_A2_SCL_1':1.0,
-'TC_A2_SCL_2':1.0
+'TC_A2_X3_2':0.0
 }
 
 # curve fit the data for accel 2 corrections
@@ -663,7 +645,7 @@ if num_accels >= 3:
     plt.subplot(3,1,1)
     plt.plot(sensor_accel_2['temperature'],correction_x,'b')
     plt.plot(temp_resample,correction_x_resample,'r')
-    plt.title('Accel 2 Bias vs Temperature')
+    plt.title('Accel 2 ({}) Bias vs Temperature'.format(accel_2_params['TC_A2_ID']))
     plt.ylabel('X bias (m/s/s)')
     plt.xlabel('temperature (degC)')
     plt.grid()
@@ -701,8 +683,7 @@ baro_0_params = {
 'TC_B0_X2':0.0,
 'TC_B0_X3':0.0,
 'TC_B0_X4':0.0,
-'TC_B0_X5':0.0,
-'TC_B0_SCL':1.0,
+'TC_B0_X5':0.0
 }
 
 # curve fit the data for baro 0 corrections
@@ -734,7 +715,7 @@ plt.figure(7,figsize=(20,13))
 # draw plots
 plt.plot(sensor_baro_0['temperature'],100*sensor_baro_0['pressure']-100*median_pressure,'b')
 plt.plot(temp_resample,baro_0_x_resample,'r')
-plt.title('Baro 0 Bias vs Temperature')
+plt.title('Baro 0 ({}) Bias vs Temperature'.format(baro_0_params['TC_B0_ID']))
 plt.ylabel('Z bias (Pa)')
 plt.xlabel('temperature (degC)')
 plt.grid()
@@ -753,7 +734,6 @@ baro_1_params = {
 'TC_B1_X3':0.0,
 'TC_B1_X4':0.0,
 'TC_B1_X5':0.0,
-'TC_B1_SCL':1.0,
 }
 
 if num_baros >= 2:
@@ -787,7 +767,7 @@ if num_baros >= 2:
     # draw plots
     plt.plot(sensor_baro_1['temperature'],100*sensor_baro_1['pressure']-100*median_pressure,'b')
     plt.plot(temp_resample,baro_1_x_resample,'r')
-    plt.title('Baro 1 Bias vs Temperature')
+    plt.title('Baro 1 ({}) Bias vs Temperature'.format(baro_1_params['TC_B1_ID']))
     plt.ylabel('Z bias (Pa)')
     plt.xlabel('temperature (degC)')
     plt.grid()
@@ -840,7 +820,7 @@ if num_baros >= 3:
     # draw plots
     plt.plot(sensor_baro_2['temperature'],100*sensor_baro_2['pressure']-100*median_pressure,'b')
     plt.plot(temp_resample,baro_2_x_resample,'r')
-    plt.title('Baro 2 Bias vs Temperature')
+    plt.title('Baro 2 ({}) Bias vs Temperature'.format(baro_2_params['TC_B2_ID']))
     plt.ylabel('Z bias (Pa)')
     plt.xlabel('temperature (degC)')
     plt.grid()

msg/sensor_accel_fifo.msg

@@ -11,3 +11,5 @@ uint8 samples             # number of valid samples
 int16[32] x               # acceleration in the NED X board axis in m/s/s
 int16[32] y               # acceleration in the NED Y board axis in m/s/s
 int16[32] z               # acceleration in the NED Z board axis in m/s/s
+
+uint8 rotation            # Direction the sensor faces (see Rotation enum)

msg/sensor_correction.msg

@@ -6,52 +6,21 @@ uint64 timestamp		# time since system start (microseconds)
 
 # Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
-# corrections for uORB instance 0
-float32[3] gyro_offset_0	# gyro XYZ offsets in the sensor frame in rad/s
-float32[3] gyro_scale_0		# gyro XYZ scale factors in the sensor frame
-
-# corrections for uORB instance 1
-float32[3] gyro_offset_1	# gyro XYZ offsets in the sensor frame in rad/s
-float32[3] gyro_scale_1		# gyro XYZ scale factors in the sensor frame
-
-# corrections for uORB instance 2
-float32[3] gyro_offset_2	# gyro XYZ offsets in the sensor frame in rad/s
-float32[3] gyro_scale_2		# gyro XYZ scale factors in the sensor frame
+uint32[3] gyro_device_ids
+float32[3] gyro_offset_0	# gyro 0 XYZ offsets in the sensor frame in rad/s
+float32[3] gyro_offset_1	# gyro 1 XYZ offsets in the sensor frame in rad/s
+float32[3] gyro_offset_2	# gyro 2 XYZ offsets in the sensor frame in rad/s
 
 # Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
-# corrections for uORB instance 0
-float32[3] accel_offset_0	# accelerometer XYZ offsets in the sensor frame in m/s/s
-float32[3] accel_scale_0	# accelerometer XYZ scale factors in the sensor frame
-
-# corrections for uORB instance 1
-float32[3] accel_offset_1	# accelerometer XYZ offsets in the sensor frame in m/s/s
-float32[3] accel_scale_1	# accelerometer XYZ scale factors in the sensor frame
-
-# corrections for uORB instance 2
-float32[3] accel_offset_2	# accelerometer XYZ offsets in the sensor frame in m/s/s
-float32[3] accel_scale_2	# accelerometer XYZ scale factors in the sensor frame
+uint32[3] accel_device_ids
+float32[3] accel_offset_0	# accelerometer 0 XYZ offsets in the sensor frame in m/s/s
+float32[3] accel_offset_1	# accelerometer 1 XYZ offsets in the sensor frame in m/s/s
+float32[3] accel_offset_2	# accelerometer 2 XYZ offsets in the sensor frame in m/s/s
 
 # Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
-# corrections for uORB instance 0
-float32 baro_offset_0		# barometric pressure offsets in the sensor frame in m/s/s
-float32 baro_scale_0		# barometric pressure scale factors in the sensor frame
-
-# corrections for uORB instance 1
-float32 baro_offset_1		# barometric pressure offsets in the sensor frame in m/s/s
-float32 baro_scale_1		# barometric pressure scale factors in the sensor frame
-
-# corrections for uORB instance 2
-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
-
-# 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
-uint8[3] accel_mapping
-uint8[3] baro_mapping
-
-uint32[3] gyro_device_ids
-uint32[3] accel_device_ids
 uint32[3] baro_device_ids
+float32 baro_offset_0		# barometric pressure 0 offsets in the sensor frame in m/s/s
+float32 baro_offset_1		# barometric pressure 1 offsets in the sensor frame in m/s/s
+float32 baro_offset_2		# barometric pressure 2 offsets in the sensor frame in m/s/s

msg/sensor_gyro_fifo.msg

@@ -11,3 +11,5 @@ uint8 samples             # number of valid samples
 int16[32] x               # angular velocity in the NED X board axis in rad/s
 int16[32] y               # angular velocity in the NED Y board axis in rad/s
 int16[32] z               # angular velocity in the NED Z board axis in rad/s
+
+uint8 rotation            # Direction the sensor faces (see Rotation enum)

src/drivers/barometer/bmp280/BMP280.cpp

@@ -191,6 +191,4 @@ BMP280::print_status()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_measure_perf);
 	perf_print_counter(_comms_errors);
-
-	_px4_baro.print_status();
 }

src/drivers/barometer/bmp388/bmp388.cpp

@@ -105,8 +105,6 @@ BMP388::print_status()
 	perf_print_counter(_measure_perf);
 	perf_print_counter(_comms_errors);
 	printf("measurement interval:  %u us \n", _measure_interval);
-
-	_px4_baro.print_status();
 }
 
 void

src/drivers/barometer/dps310/DPS310.cpp

@@ -284,8 +284,6 @@ DPS310::print_status()
 	I2CSPIDriverBase::print_status();
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-
-	_px4_barometer.print_status();
 }
 
 } // namespace dps310

src/drivers/barometer/lps22hb/LPS22HB.cpp

@@ -184,5 +184,4 @@ void LPS22HB::print_status()
 	I2CSPIDriverBase::print_status();
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-	_px4_baro.print_status();
 }

src/drivers/barometer/lps25h/LPS25H.cpp

@@ -199,6 +199,4 @@ void LPS25H::print_status()
 	I2CSPIDriverBase::print_status();
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-
-	_px4_barometer.print_status();
 }

src/drivers/barometer/mpl3115a2/MPL3115A2.cpp

@@ -284,6 +284,4 @@ void MPL3115A2::print_status()
 	I2CSPIDriverBase::print_status();
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-
-	_px4_barometer.print_status();
 }

src/drivers/barometer/ms5611/ms5611.cpp

@@ -340,8 +340,6 @@ void MS5611::print_status()
 	perf_print_counter(_comms_errors);
 
 	printf("device:         %s\n", _device_type == MS5611_DEVICE ? "ms5611" : "ms5607");
-
-	_px4_barometer.print_status();
 }
 
 namespace ms5611

src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp

@@ -327,8 +327,6 @@ CM8JL65::print_info()
 {
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-
-	_px4_rangefinder.print_status();
 }
 
 void

src/drivers/distance_sensor/leddar_one/LeddarOne.cpp

@@ -267,8 +267,6 @@ LeddarOne::print_info()
 {
 	perf_print_counter(_comms_error);
 	perf_print_counter(_sample_perf);
-
-	_px4_rangefinder.print_status();
 }
 
 void

src/drivers/distance_sensor/sf0x/SF0X.cpp

@@ -297,6 +297,4 @@ void SF0X::print_info()
 {
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-
-	_px4_rangefinder.print_status();
 }

src/drivers/distance_sensor/sf1xx/sf1xx.cpp

@@ -278,7 +278,6 @@ void SF1XX::print_status()
 	I2CSPIDriverBase::print_status();
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-	_px4_rangefinder.print_status();
 }
 
 void SF1XX::print_usage()

src/drivers/distance_sensor/srf02/SRF02.cpp

@@ -147,6 +147,4 @@ void SRF02::print_status()
 	I2CSPIDriverBase::print_status();
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-
-	_px4_rangefinder.print_status();
 }

src/drivers/distance_sensor/teraranger/TERARANGER.cpp

@@ -268,6 +268,4 @@ void TERARANGER::print_status()
 	I2CSPIDriverBase::print_status();
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-
-	_px4_rangefinder.print_status();
 }

src/drivers/distance_sensor/tfmini/TFMINI.cpp

@@ -264,6 +264,4 @@ TFMINI::print_info()
 	printf("Using port '%s'\n", _port);
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-
-	_px4_rangefinder.print_status();
 }

src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp

@@ -221,6 +221,4 @@ void AerotennaULanding::print_info()
 {
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
-
-	_px4_rangefinder.print_status();
 }

src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp

@@ -177,7 +177,6 @@ void VL53L0X::print_status()
 	I2CSPIDriverBase::print_status();
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_sample_perf);
-	_px4_rangefinder.print_status();
 }
 
 int VL53L0X::probe()

src/drivers/drv_accel.h

@@ -1,75 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file drv_accel.h
- *
- * Accelerometer driver interface.
- */
-
-#ifndef _DRV_ACCEL_H
-#define _DRV_ACCEL_H
-
-#include <stdint.h>
-#include <sys/ioctl.h>
-
-#include "drv_sensor.h"
-#include "drv_orb_dev.h"
-
-#define ACCEL_BASE_DEVICE_PATH	"/dev/accel"
-
-#include <uORB/topics/sensor_accel.h>
-
-/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
-struct accel_calibration_s {
-	float	x_offset;
-	float	x_scale;
-	float	y_offset;
-	float	y_scale;
-	float	z_offset;
-	float	z_scale;
-};
-/*
- * ioctl() definitions
- *
- * Accelerometer drivers also implement the generic sensor driver
- * interfaces from drv_sensor.h
- */
-
-#define _ACCELIOCBASE		(0x2100)
-#define _ACCELIOC(_n)		(_PX4_IOC(_ACCELIOCBASE, _n))
-
-/** set the accel scaling constants to the structure pointed to by (arg) */
-#define ACCELIOCSSCALE		_ACCELIOC(5)
-
-#endif /* _DRV_ACCEL_H */

src/drivers/drv_gyro.h

@@ -1,70 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file drv_gyro.h
- *
- * Gyroscope driver interface.
- */
-
-#ifndef _DRV_GYRO_H
-#define _DRV_GYRO_H
-
-#include <stdint.h>
-#include <sys/ioctl.h>
-
-#include "drv_sensor.h"
-#include "drv_orb_dev.h"
-
-#define GYRO_BASE_DEVICE_PATH	"/dev/gyro"
-
-#include <uORB/topics/sensor_gyro.h>
-
-/** gyro scaling factors; Vout = Vin + Voffset */
-struct gyro_calibration_s {
-	float	x_offset;
-	float	y_offset;
-	float	z_offset;
-};
-
-/*
- * ioctl() definitions
- */
-
-#define _GYROIOCBASE		(0x2300)
-#define _GYROIOC(_n)		(_PX4_IOC(_GYROIOCBASE, _n))
-
-/** set the gyro scaling constants to (arg) */
-#define GYROIOCSSCALE		_GYROIOC(4)
-
-#endif /* _DRV_GYRO_H */

src/drivers/drv_mixer.h

@@ -85,4 +85,4 @@
  * - save/serialise for saving tuned mixers.
  */
 
-#endif /* _DRV_ACCEL_H */
+#endif /* _DRV_MIXER_H */

src/drivers/imu/adis16448/ADIS16448.cpp

@@ -308,9 +308,6 @@ ADIS16448::print_status()
 	perf_print_counter(_perf_bad_transfer);
 	perf_print_counter(_perf_crc_bad);
 
-	_px4_accel.print_status();
-	_px4_baro.print_status();
-	_px4_gyro.print_status();
 	_px4_mag.print_status();
 }
 

src/drivers/imu/adis16477/ADIS16477.cpp

@@ -385,6 +385,4 @@ ADIS16477::print_status()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_bad_transfers);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }

src/drivers/imu/adis16497/ADIS16497.cpp

@@ -505,6 +505,4 @@ ADIS16497::print_status()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_bad_transfers);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }

src/drivers/imu/bma180/bma180.cpp

@@ -431,7 +431,6 @@ void BMA180::print_status()
 {
 	I2CSPIDriverBase::print_status();
 	perf_print_counter(_sample_perf);
-	_px4_accel.print_status();
 }
 
 void BMA180::print_usage()

src/drivers/imu/bmi160/bmi160.cpp

@@ -599,6 +599,4 @@ void BMI160::print_status()
 	perf_print_counter(_good_transfers);
 	perf_print_counter(_reset_retries);
 	perf_print_counter(_duplicates);
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }

src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp

@@ -82,8 +82,6 @@ void BMI055_Accelerometer::print_status()
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
-
-	_px4_accel.print_status();
 }
 
 int BMI055_Accelerometer::probe()

src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp

@@ -82,8 +82,6 @@ void BMI055_Gyroscope::print_status()
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
-
-	_px4_gyro.print_status();
 }
 
 int BMI055_Gyroscope::probe()

src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp

@@ -82,8 +82,6 @@ void BMI088_Accelerometer::print_status()
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
-
-	_px4_accel.print_status();
 }
 
 int BMI088_Accelerometer::probe()

src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp

@@ -82,8 +82,6 @@ void BMI088_Gyroscope::print_status()
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
-
-	_px4_gyro.print_status();
 }
 
 int BMI088_Gyroscope::probe()

src/drivers/imu/fxas21002c/FXAS21002C.cpp

@@ -418,7 +418,6 @@ void FXAS21002C::print_status()
 		}
 	}
 
-	_px4_gyro.print_status();
 }
 
 void

src/drivers/imu/icm20948/icm20948.cpp

@@ -828,7 +828,5 @@ ICM20948::print_status()
 	perf_print_counter(_good_transfers);
 	perf_print_counter(_duplicates);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 	_mag.print_status();
 }

src/drivers/imu/invensense/icm20602/ICM20602.cpp

@@ -102,8 +102,6 @@ void ICM20602::print_status()
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }
 
 int ICM20602::probe()

src/drivers/imu/invensense/icm20608g/ICM20608G.cpp

@@ -107,8 +107,6 @@ void ICM20608G::print_status()
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }
 
 int ICM20608G::probe()

src/drivers/imu/invensense/icm20649/ICM20649.cpp

@@ -102,8 +102,6 @@ void ICM20649::print_status()
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }
 
 int ICM20649::probe()

src/drivers/imu/invensense/icm20689/ICM20689.cpp

@@ -106,9 +106,6 @@ void ICM20689::print_status()
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
-
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }
 
 int ICM20689::probe()

src/drivers/imu/invensense/icm20948/ICM20948.cpp

@@ -124,8 +124,6 @@ void ICM20948::print_status()
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 
 	if (_slave_ak09916_magnetometer) {
 		_slave_ak09916_magnetometer->PrintInfo();

src/drivers/imu/invensense/icm40609d/ICM40609D.cpp

@@ -101,9 +101,6 @@ void ICM40609D::print_status()
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
-
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }
 
 int ICM40609D::probe()

src/drivers/imu/invensense/icm42688p/ICM42688P.cpp

@@ -102,8 +102,6 @@ void ICM42688P::print_status()
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }
 
 int ICM42688P::probe()

src/drivers/imu/invensense/mpu6000/MPU6000.cpp

@@ -107,8 +107,6 @@ void MPU6000::print_status()
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }
 
 int MPU6000::probe()

src/drivers/imu/invensense/mpu6500/MPU6500.cpp

@@ -102,8 +102,6 @@ void MPU6500::print_status()
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }
 
 int MPU6500::probe()

src/drivers/imu/invensense/mpu9250/MPU9250.cpp

@@ -125,8 +125,6 @@ void MPU9250::print_status()
 	perf_print_counter(_fifo_reset_perf);
 	perf_print_counter(_drdy_interval_perf);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 
 	if (_slave_ak8963_magnetometer) {
 		_slave_ak8963_magnetometer->PrintInfo();

src/drivers/imu/l3gd20/L3GD20.cpp

@@ -414,7 +414,6 @@ L3GD20::print_status()
 		}
 	}
 
-	_px4_gyro.print_status();
 }
 
 void

src/drivers/imu/mpu6000/MPU6000.cpp

@@ -883,8 +883,6 @@ MPU6000::print_status()
 	perf_print_counter(_reset_retries);
 	perf_print_counter(_duplicates);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }
 
 void

src/drivers/imu/mpu9250/mpu9250.cpp

@@ -670,7 +670,5 @@ MPU9250::print_status()
 	perf_print_counter(_bad_registers);
 	perf_print_counter(_duplicates);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 	_mag.print_status();
 }

src/drivers/imu/st/ism330dlc/ISM330DLC.cpp

@@ -339,6 +339,4 @@ void ISM330DLC::print_status()
 	perf_print_counter(_drdy_count_perf);
 	perf_print_counter(_drdy_interval_perf);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }

src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp

@@ -309,6 +309,4 @@ void LSM9DS1::print_status()
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
 
-	_px4_accel.print_status();
-	_px4_gyro.print_status();
 }

src/examples/matlab_csv_serial/matlab_csv_serial.c

@@ -56,8 +56,6 @@
 #include <limits.h>
 #include <math.h>
 #include <uORB/uORB.h>
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
 #include <poll.h>

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -39,7 +39,7 @@
 using namespace time_literals;
 using matrix::Vector3f;
 
-static inline int32_t sum(const int16_t samples[16], uint8_t len)
+static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
 {
 	int32_t sum = 0;
 
@@ -50,7 +50,7 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len)
 	return sum;
 }
 
-static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
+static constexpr uint8_t clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
 {
 	unsigned clip_count = 0;
 
@@ -64,15 +64,13 @@ static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit
 }
 
 PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
-	CDev(nullptr),
 	ModuleParams(nullptr),
 	_sensor_pub{ORB_ID(sensor_accel), priority},
 	_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
 	_device_id{device_id},
 	_rotation{rotation}
 {
-	// register class and advertise immediately to keep instance numbering in sync
-	_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
+	// advertise immediately to keep instance numbering in sync
 	_sensor_pub.advertise();
 
 	updateParams();
@@ -80,36 +78,10 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum R
 
 PX4Accelerometer::~PX4Accelerometer()
 {
-	if (_class_device_instance != -1) {
-		unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _class_device_instance);
-	}
-
 	_sensor_pub.unadvertise();
 	_sensor_fifo_pub.unadvertise();
 }
 
-int PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-	case ACCELIOCSSCALE: {
-			// Copy offsets and scale factors in
-			accel_calibration_s cal{};
-			memcpy(&cal, (accel_calibration_s *) arg, sizeof(cal));
-
-			_calibration_offset = Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
-			_calibration_scale = Vector3f{cal.x_scale, cal.y_scale, cal.z_scale};
-		}
-
-		return PX4_OK;
-
-	case DEVIOCGDEVICEID:
-		return _device_id;
-
-	default:
-		return -ENOTTY;
-	}
-}
-
 void PX4Accelerometer::set_device_type(uint8_t devtype)
 {
 	// current DeviceStructure
@@ -123,39 +95,16 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
 	_device_id = device_id.devid;
 }
 
-void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
+void PX4Accelerometer::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
 {
-	// Apply rotation (before scaling)
-	rotate_3f(_rotation, x, y, z);
-
-	const Vector3f raw{x, y, z};
-
 	// clipping
-	float clip_count_x = (fabsf(raw(0)) > _clip_limit);
-	float clip_count_y = (fabsf(raw(1)) > _clip_limit);
-	float clip_count_z = (fabsf(raw(2)) > _clip_limit);
-
-	rotate_3f(_rotation, clip_count_x, clip_count_y, clip_count_z);
-
-	// Apply range scale and the calibrating offset/scale
-	const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
+	uint8_t clip_count[3];
+	clip_count[0] = (fabsf(x) >= _clip_limit);
+	clip_count[1] = (fabsf(y) >= _clip_limit);
+	clip_count[2] = (fabsf(z) >= _clip_limit);
 
 	// publish
-	sensor_accel_s report;
-
-	report.timestamp_sample = timestamp_sample;
-	report.device_id = _device_id;
-	report.temperature = _temperature;
-	report.error_count = _error_count;
-	report.x = val_calibrated(0);
-	report.y = val_calibrated(1);
-	report.z = val_calibrated(2);
-	report.clip_counter[0] = fabsf(roundf(clip_count_x));
-	report.clip_counter[1] = fabsf(roundf(clip_count_y));
-	report.clip_counter[2] = fabsf(roundf(clip_count_z));
-	report.timestamp = hrt_absolute_time();
-
-	_sensor_pub.publish(report);
+	Publish(timestamp_sample, x, y, z, clip_count);
 }
 
 void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
@@ -165,7 +114,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 
 	{
 		// trapezoidal integration (equally spaced, scaled by dt later)
-		Vector3f integral{
+		const Vector3f integral{
 			(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
 			(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
 			(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
@@ -176,43 +125,20 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 		_last_sample[2] = sample.z[N - 1];
 
 		// clipping
-		float clip_count_x = clipping(sample.x, _clip_limit, N);
-		float clip_count_y = clipping(sample.y, _clip_limit, N);
-		float clip_count_z = clipping(sample.z, _clip_limit, N);
+		uint8_t clip_count[3] {
+			clipping(sample.x, _clip_limit, N),
+			clipping(sample.y, _clip_limit, N),
+			clipping(sample.z, _clip_limit, N),
+		};
 
-		rotate_3f(_rotation, clip_count_x, clip_count_y, clip_count_z);
-
-
-		// Apply rotation (before scaling)
-		rotate_3f(_rotation, integral(0), integral(1), integral(2));
-
-		// average
 		const float x = integral(0) / (float)N;
 		const float y = integral(1) / (float)N;
 		const float z = integral(2) / (float)N;
 
-		// Apply range scale and the calibration offset/scale
-		const Vector3f val_calibrated{((Vector3f{x, y, z} * _scale) - _calibration_offset).emult(_calibration_scale)};
-
 		// publish
-		sensor_accel_s report;
-
-		report.timestamp_sample = sample.timestamp_sample;
-		report.device_id = _device_id;
-		report.temperature = _temperature;
-		report.error_count = _error_count;
-		report.x = val_calibrated(0);
-		report.y = val_calibrated(1);
-		report.z = val_calibrated(2);
-		report.clip_counter[0] = fabsf(roundf(clip_count_x));
-		report.clip_counter[1] = fabsf(roundf(clip_count_y));
-		report.clip_counter[2] = fabsf(roundf(clip_count_z));
-		report.timestamp = hrt_absolute_time();
-
-		_sensor_pub.publish(report);
+		Publish(sample.timestamp_sample, x, y, z, clip_count);
 	}
 
-
 	// publish fifo
 	sensor_accel_fifo_s fifo{};
 
@@ -221,6 +147,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 	fifo.dt = dt;
 	fifo.scale = _scale;
 	fifo.samples = N;
+	fifo.rotation = _rotation;
 
 	memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N);
 	memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N);
@@ -230,20 +157,35 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 	_sensor_fifo_pub.publish(fifo);
 }
 
+void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3])
+{
+	// Apply rotation (before scaling)
+	rotate_3f(_rotation, x, y, z);
+
+	float clipping_x = clip_count[0];
+	float clipping_y = clip_count[1];
+	float clipping_z = clip_count[2];
+	rotate_3f(_rotation, clipping_x, clipping_y, clipping_z);
+
+	sensor_accel_s report;
+
+	report.timestamp_sample = timestamp_sample;
+	report.device_id = _device_id;
+	report.temperature = _temperature;
+	report.error_count = _error_count;
+	report.x = x * _scale;
+	report.y = y * _scale;
+	report.z = z * _scale;
+	report.clip_counter[0] = fabsf(roundf(clipping_x));
+	report.clip_counter[1] = fabsf(roundf(clipping_y));
+	report.clip_counter[2] = fabsf(roundf(clipping_z));
+	report.timestamp = hrt_absolute_time();
+
+	_sensor_pub.publish(report);
+}
+
 void PX4Accelerometer::UpdateClipLimit()
 {
 	// 99.9% of potential max
 	_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
 }
-
-void PX4Accelerometer::print_status()
-{
-#if !defined(CONSTRAINED_FLASH)
-	PX4_INFO(ACCEL_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
-
-	PX4_INFO("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1),
-		 (double)_calibration_scale(2));
-	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
-		 (double)_calibration_offset(2));
-#endif // !CONSTRAINED_FLASH
-}

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -33,25 +33,20 @@
 
 #pragma once
 
-#include <drivers/drv_accel.h>
 #include <drivers/drv_hrt.h>
-#include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
 #include <lib/ecl/geo/geo.h>
 #include <px4_platform_common/module_params.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_accel_fifo.h>
 
-class PX4Accelerometer : public cdev::CDev, public ModuleParams
+class PX4Accelerometer : public ModuleParams
 {
 public:
 	PX4Accelerometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Accelerometer() override;
 
-	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
-
 	uint32_t get_device_id() const { return _device_id; }
 
 	float get_max_rate_hz() const { return _param_imu_gyro_rate_max.get(); }
@@ -64,9 +59,7 @@ public:
 	void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
 	void set_temperature(float temperature) { _temperature = temperature; }
 
-	void update(hrt_abstime timestamp_sample, float x, float y, float z);
-
-	void print_status();
+	void update(const hrt_abstime &timestamp_sample, float x, float y, float z);
 
 	struct FIFOSample {
 		hrt_abstime timestamp_sample;
@@ -84,15 +77,11 @@ public:
 	void updateFIFO(const FIFOSample &sample);
 
 private:
+	void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3]);
 	void UpdateClipLimit();
 
-	uORB::PublicationQueuedMulti<sensor_accel_s>      _sensor_pub;
-	uORB::PublicationMulti<sensor_accel_fifo_s>       _sensor_fifo_pub;
-
-	matrix::Vector3f	_calibration_scale{1.f, 1.f, 1.f};
-	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};
-
-	int			_class_device_instance{-1};
+	uORB::PublicationQueuedMulti<sensor_accel_s> _sensor_pub;
+	uORB::PublicationMulti<sensor_accel_fifo_s>  _sensor_fifo_pub;
 
 	uint32_t		_device_id{0};
 	const enum Rotation	_rotation;

src/lib/drivers/barometer/PX4Barometer.cpp

@@ -79,11 +79,3 @@ PX4Barometer::update(hrt_abstime timestamp, float pressure)
 
 	_sensor_baro_pub.update();
 }
-
-void
-PX4Barometer::print_status()
-{
-	PX4_INFO(BARO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
-
-	print_message(_sensor_baro_pub.get());
-}

src/lib/drivers/barometer/PX4Barometer.hpp

@@ -59,8 +59,6 @@ public:
 
 	int get_class_instance() { return _class_device_instance; };
 
-	void print_status();
-
 private:
 
 	uORB::PublicationMultiData<sensor_baro_s>	_sensor_baro_pub;

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -39,7 +39,7 @@
 using namespace time_literals;
 using matrix::Vector3f;
 
-static inline int32_t sum(const int16_t samples[16], uint8_t len)
+static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
 {
 	int32_t sum = 0;
 
@@ -51,15 +51,13 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len)
 }
 
 PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
-	CDev(nullptr),
 	ModuleParams(nullptr),
 	_sensor_pub{ORB_ID(sensor_gyro), priority},
 	_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority},
 	_device_id{device_id},
 	_rotation{rotation}
 {
-	// register class and advertise immediately to keep instance numbering in sync
-	_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
+	// advertise immediately to keep instance numbering in sync
 	_sensor_pub.advertise();
 
 	updateParams();
@@ -67,35 +65,10 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation
 
 PX4Gyroscope::~PX4Gyroscope()
 {
-	if (_class_device_instance != -1) {
-		unregister_class_devname(GYRO_BASE_DEVICE_PATH, _class_device_instance);
-	}
-
 	_sensor_pub.unadvertise();
 	_sensor_fifo_pub.unadvertise();
 }
 
-int PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-	case GYROIOCSSCALE: {
-			// Copy offsets and scale factors in
-			gyro_calibration_s cal{};
-			memcpy(&cal, (gyro_calibration_s *) arg, sizeof(cal));
-
-			_calibration_offset = Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
-		}
-
-		return PX4_OK;
-
-	case DEVIOCGDEVICEID:
-		return _device_id;
-
-	default:
-		return -ENOTTY;
-	}
-}
-
 void PX4Gyroscope::set_device_type(uint8_t devtype)
 {
 	// current DeviceStructure
@@ -109,29 +82,10 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
 	_device_id = device_id.devid;
 }
 
-void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float z)
+void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
 {
-	// Apply rotation (before scaling)
-	rotate_3f(_rotation, x, y, z);
-
-	const Vector3f raw{x, y, z};
-
-	// Apply range scale and the calibrating offset/scale
-	const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)};
-
 	// publish
-	sensor_gyro_s report;
-
-	report.timestamp_sample = timestamp_sample;
-	report.device_id = _device_id;
-	report.temperature = _temperature;
-	report.error_count = _error_count;
-	report.x = val_calibrated(0);
-	report.y = val_calibrated(1);
-	report.z = val_calibrated(2);
-	report.timestamp = hrt_absolute_time();
-
-	_sensor_pub.publish(report);
+	Publish(timestamp_sample, x, y, z);
 }
 
 void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
@@ -141,7 +95,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 
 	{
 		// trapezoidal integration (equally spaced, scaled by dt later)
-		Vector3f integral{
+		const Vector3f integral{
 			(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
 			(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
 			(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
@@ -151,33 +105,14 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 		_last_sample[1] = sample.y[N - 1];
 		_last_sample[2] = sample.z[N - 1];
 
-		// Apply rotation (before scaling)
-		rotate_3f(_rotation, integral(0), integral(1), integral(2));
-
-		// average
 		const float x = integral(0) / (float)N;
 		const float y = integral(1) / (float)N;
 		const float z = integral(2) / (float)N;
 
-		// Apply range scale and the calibration offset
-		const Vector3f val_calibrated{(Vector3f{x, y, z} * _scale) - _calibration_offset};
-
 		// publish
-		sensor_gyro_s report;
-
-		report.timestamp_sample = sample.timestamp_sample;
-		report.device_id = _device_id;
-		report.temperature = _temperature;
-		report.error_count = _error_count;
-		report.x = val_calibrated(0);
-		report.y = val_calibrated(1);
-		report.z = val_calibrated(2);
-		report.timestamp = hrt_absolute_time();
-
-		_sensor_pub.publish(report);
+		Publish(sample.timestamp_sample, x, y, z);
 	}
 
-
 	// publish fifo
 	sensor_gyro_fifo_s fifo{};
 
@@ -186,6 +121,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 	fifo.dt = dt;
 	fifo.scale = _scale;
 	fifo.samples = N;
+	fifo.rotation = _rotation;
 
 	memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N);
 	memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N);
@@ -195,12 +131,21 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 	_sensor_fifo_pub.publish(fifo);
 }
 
-void PX4Gyroscope::print_status()
+void PX4Gyroscope::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z)
 {
-#if !defined(CONSTRAINED_FLASH)
-	PX4_INFO(GYRO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
+	// Apply rotation (before scaling)
+	rotate_3f(_rotation, x, y, z);
 
-	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
-		 (double)_calibration_offset(2));
-#endif // !CONSTRAINED_FLASH
+	sensor_gyro_s report;
+
+	report.timestamp_sample = timestamp_sample;
+	report.device_id = _device_id;
+	report.temperature = _temperature;
+	report.error_count = _error_count;
+	report.x = x * _scale;
+	report.y = y * _scale;
+	report.z = z * _scale;
+	report.timestamp = hrt_absolute_time();
+
+	_sensor_pub.publish(report);
 }

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,24 +33,19 @@
 
 #pragma once
 
-#include <drivers/drv_gyro.h>
 #include <drivers/drv_hrt.h>
-#include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
 #include <px4_platform_common/module_params.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_gyro_fifo.h>
 
-class PX4Gyroscope : public cdev::CDev, public ModuleParams
+class PX4Gyroscope : public ModuleParams
 {
 public:
 	PX4Gyroscope(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Gyroscope() override;
 
-	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
-
 	uint32_t get_device_id() const { return _device_id; }
 
 	float get_max_rate_hz() const { return _param_imu_gyro_rate_max.get(); }
@@ -63,9 +58,7 @@ public:
 	void set_scale(float scale) { _scale = scale; }
 	void set_temperature(float temperature) { _temperature = temperature; }
 
-	void update(hrt_abstime timestamp_sample, float x, float y, float z);
-
-	void print_status();
+	void update(const hrt_abstime &timestamp_sample, float x, float y, float z);
 
 	struct FIFOSample {
 		hrt_abstime timestamp_sample;
@@ -83,12 +76,10 @@ public:
 	void updateFIFO(const FIFOSample &sample);
 
 private:
-	uORB::PublicationQueuedMulti<sensor_gyro_s>      _sensor_pub;
-	uORB::PublicationMulti<sensor_gyro_fifo_s>       _sensor_fifo_pub;
+	void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z);
 
-	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};
-
-	int			_class_device_instance{-1};
+	uORB::PublicationQueuedMulti<sensor_gyro_s> _sensor_pub;
+	uORB::PublicationMulti<sensor_gyro_fifo_s>  _sensor_fifo_pub;
 
 	uint32_t		_device_id{0};
 	const enum Rotation	_rotation;

src/lib/drivers/rangefinder/PX4Rangefinder.cpp

@@ -86,7 +86,3 @@ void PX4Rangefinder::update(const hrt_abstime &timestamp_sample, const float dis
 
 	_distance_sensor_pub.update();
 }
-
-void PX4Rangefinder::print_status()
-{
-}

src/lib/drivers/rangefinder/PX4Rangefinder.hpp

@@ -48,8 +48,6 @@ public:
 		       const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
 	~PX4Rangefinder();
 
-	void print_status();
-
 	void set_device_type(uint8_t device_type);
 	//void set_error_count(uint64_t error_count) { _distance_sensor_pub.get().error_count = error_count; }
 

src/modules/commander/accelerometer_calibration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -120,8 +120,6 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-// FIXME: Can some of these headers move out with detect_ move?
-
 #include "accelerometer_calibration.h"
 #include "calibration_messages.h"
 #include "calibration_routines.h"
@@ -130,316 +128,184 @@
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/time.h>
-#include <unistd.h>
-#include <stdio.h>
-#include <fcntl.h>
-#include <math.h>
-#include <float.h>
-#include <mathlib/mathlib.h>
-#include <string.h>
+
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_accel.h>
+#include <lib/mathlib/mathlib.h>
 #include <lib/ecl/geo/geo.h>
 #include <matrix/math.hpp>
-#include <conversion/rotation.h>
-#include <parameters/param.h>
+#include <lib/conversion/rotation.h>
+#include <lib/parameters/param.h>
 #include <systemlib/err.h>
 #include <systemlib/mavlink_log.h>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_correction.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionBlocking.hpp>
 
 using namespace time_literals;
 using namespace matrix;
+using math::radians;
 
-static const char *sensor_name = "accel";
+static constexpr char sensor_name[] {"accel"};
 
-static int32_t device_id[max_accel_sens];
-static int device_prio_max = 0;
-static int32_t device_id_primary = 0;
+static constexpr unsigned MAX_ACCEL_SENS = 3;
 
-calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub,
-		float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
-calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens],
-					float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
-int mat_invert3(float src[3][3], float dst[3][3]);
-calibrate_return calculate_calibration_values(unsigned sensor,
-		float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3],
-		float (&accel_offs)[max_accel_sens][3], float g);
+static calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub,
+		Vector3f(&accel_offs)[MAX_ACCEL_SENS],
+		Matrix3f(&accel_T)[MAX_ACCEL_SENS], unsigned active_sensors);
+
+static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3],
+		unsigned orient, unsigned samples_num);
+
+static calibrate_return calculate_calibration_values(unsigned sensor,
+		float (&accel_ref)[MAX_ACCEL_SENS][detect_orientation_side_count][3], Matrix3f(&accel_T)[MAX_ACCEL_SENS],
+		Vector3f(&accel_offs)[MAX_ACCEL_SENS]);
 
 /// Data passed to calibration worker routine
 typedef struct  {
-	orb_advert_t	*mavlink_log_pub;
-	unsigned	done_count;
-	int		subs[max_accel_sens];
-	float		accel_ref[max_accel_sens][detect_orientation_side_count][3];
-	int		sensor_correction_sub;
+	orb_advert_t	*mavlink_log_pub{nullptr};
+	unsigned	done_count{0};
+	float		accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {};
 } accel_worker_data_t;
 
 int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 {
-#if 1 // TODO: replace all IOCTL usage
-	int fd;
-#endif
-
 	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
 
-	struct accel_calibration_s accel_scale;
-	accel_scale.x_offset = 0.0f;
-	accel_scale.x_scale = 1.0f;
-	accel_scale.y_offset = 0.0f;
-	accel_scale.y_scale = 1.0f;
-	accel_scale.z_offset = 0.0f;
-	accel_scale.z_scale = 1.0f;
-
 	int res = PX4_OK;
 
-	char str[30];
-
-	/* reset all sensors */
-	for (unsigned s = 0; s < max_accel_sens; s++) {
-#if 1 // TODO: replace all IOCTL usage
-		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
-		/* reset all offsets to zero and all scales to one */
-		fd = px4_open(str, 0);
-
-		if (fd < 0) {
-			continue;
-		}
-
-		device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-
-		res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
-		px4_close(fd);
-
-		if (res != PX4_OK) {
-			calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
-		}
-
-#else
-		(void)sprintf(str, "CAL_ACC%u_XOFF", s);
-		res = param_set_no_notification(param_find(str), &accel_scale.x_offset);
-
-		if (res != PX4_OK) {
-			PX4_ERR("unable to reset %s", str);
-		}
-
-		(void)sprintf(str, "CAL_ACC%u_YOFF", s);
-		res = param_set_no_notification(param_find(str), &accel_scale.y_offset);
-
-		if (res != PX4_OK) {
-			PX4_ERR("unable to reset %s", str);
-		}
-
-		(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
-		res = param_set_no_notification(param_find(str), &accel_scale.z_offset);
-
-		if (res != PX4_OK) {
-			PX4_ERR("unable to reset %s", str);
-		}
-
-		(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
-		res = param_set_no_notification(param_find(str), &accel_scale.x_scale);
-
-		if (res != PX4_OK) {
-			PX4_ERR("unable to reset %s", str);
-		}
-
-		(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
-		res = param_set_no_notification(param_find(str), &accel_scale.y_scale);
-
-		if (res != PX4_OK) {
-			PX4_ERR("unable to reset %s", str);
-		}
-
-		(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
-		res = param_set_no_notification(param_find(str), &accel_scale.z_scale);
-
-		if (res != PX4_OK) {
-			PX4_ERR("unable to reset %s", str);
-		}
-
-		param_notify_changes();
-#endif
-	}
-
-	float accel_offs[max_accel_sens][3];
-	float accel_T[max_accel_sens][3][3];
+	int32_t device_id[MAX_ACCEL_SENS] {};
+	int device_prio_max = 0;
+	int32_t device_id_primary = 0;
 	unsigned active_sensors = 0;
 
-	/* measure and calculate offsets & scales */
-	if (res == PX4_OK) {
-		calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
+	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
+	const unsigned orb_accel_count = orb_group_count(ORB_ID(sensor_accel));
 
-		if (cal_return == calibrate_return_cancelled) {
-			// Cancel message already displayed, nothing left to do
-			return PX4_ERROR;
+	// Warn that we will not calibrate more than max_accels accelerometers
+	if (orb_accel_count > MAX_ACCEL_SENS) {
+		calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count,
+					 MAX_ACCEL_SENS);
+	}
 
-		} else if (cal_return == calibrate_return_ok) {
-			res = PX4_OK;
+	for (uint8_t cur_accel = 0; cur_accel < orb_accel_count && cur_accel < MAX_ACCEL_SENS; cur_accel++) {
+		uORB::SubscriptionData<sensor_accel_s> accel_sub{ORB_ID(sensor_accel), cur_accel};
+		device_id[cur_accel] = accel_sub.get().device_id;
+
+		if (device_id[cur_accel] != 0) {
+			// Get priority
+			int32_t prio = accel_sub.get_priority();
+
+			if (prio > device_prio_max) {
+				device_prio_max = prio;
+				device_id_primary = device_id[cur_accel];
+			}
+
+			active_sensors++;
 
 		} else {
-			res = PX4_ERROR;
+			calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel);
+			return PX4_ERROR;
 		}
 	}
 
-	if (res != PX4_OK) {
-		if (active_sensors == 0) {
-			calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
-		}
+	/* measure and calculate offsets & scales */
+	Vector3f accel_offs[MAX_ACCEL_SENS] {};
+	Matrix3f accel_T[MAX_ACCEL_SENS] {};
+	calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, active_sensors);
 
+	if (cal_return != calibrate_return_ok) {
+		// Cancel message already displayed, nothing left to do
+		return PX4_ERROR;
+	}
+
+	if (active_sensors == 0) {
+		calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
 		return PX4_ERROR;
 	}
 
 	/* measurements completed successfully, rotate calibration values */
-	param_t board_rotation_h = param_find("SENS_BOARD_ROT");
-	int32_t board_rotation_int;
-	param_get(board_rotation_h, &(board_rotation_int));
-	enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
-	Dcmf board_rotation = get_rot_matrix(board_rotation_id);
+	int32_t board_rotation_int = 0;
+	param_get(param_find("SENS_BOARD_ROT"), &board_rotation_int);
+	const Dcmf board_rotation = get_rot_matrix((enum Rotation)board_rotation_int);
+	const Dcmf board_rotation_t = board_rotation.transpose();
 
-	Dcmf board_rotation_t = board_rotation.transpose();
+	param_set_no_notification(param_find("CAL_ACC_PRIME"), &device_id_primary);
 
-	bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator
+	for (unsigned uorb_index = 0; uorb_index < MAX_ACCEL_SENS; uorb_index++) {
 
-	for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) {
+		if (uorb_index < active_sensors) {
+			/* handle individual sensors, one by one */
+			const Vector3f accel_offs_rotated = board_rotation_t *accel_offs[uorb_index];
+			const Matrix3f accel_T_rotated = board_rotation_t *accel_T[uorb_index] * board_rotation;
 
-		/* handle individual sensors, one by one */
-		Vector3f accel_offs_vec(accel_offs[uorb_index]);
-		Vector3f accel_offs_rotated = board_rotation_t *accel_offs_vec;
-		Matrix3f accel_T_mat(accel_T[uorb_index]);
-		Matrix3f accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
+			PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
+				 (double)accel_offs_rotated(0), (double)accel_offs_rotated(1), (double)accel_offs_rotated(2));
 
-		accel_scale.x_offset = accel_offs_rotated(0);
-		accel_scale.x_scale = accel_T_rotated(0, 0);
-		accel_scale.y_offset = accel_offs_rotated(1);
-		accel_scale.y_scale = accel_T_rotated(1, 1);
-		accel_scale.z_offset = accel_offs_rotated(2);
-		accel_scale.z_scale = accel_T_rotated(2, 2);
+			PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
+				 (double)accel_T_rotated(0, 0), (double)accel_T_rotated(1, 1), (double)accel_T_rotated(2, 2));
 
-		bool failed = false;
+			char str[30] {};
 
-		failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
+			// calibration offsets
+			float x_offset = accel_offs_rotated(0);
+			sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
+			param_set_no_notification(param_find(str), &x_offset);
+
+			float y_offset = accel_offs_rotated(1);
+			sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
+			param_set_no_notification(param_find(str), &y_offset);
+
+			float z_offset = accel_offs_rotated(2);
+			sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
+			param_set_no_notification(param_find(str), &z_offset);
 
 
-		PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
-			 (double)accel_scale.x_offset,
-			 (double)accel_scale.y_offset,
-			 (double)accel_scale.z_offset);
-		PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
-			 (double)accel_scale.x_scale,
-			 (double)accel_scale.y_scale,
-			 (double)accel_scale.z_scale);
+			// calibration scale
+			float x_scale = accel_T_rotated(0, 0);
+			sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
+			param_set_no_notification(param_find(str), &x_scale);
 
-		/* check if thermal compensation is enabled */
-		int32_t tc_enabled_int;
-		param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int));
+			float y_scale = accel_T_rotated(1, 1);
+			sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
+			param_set_no_notification(param_find(str), &y_scale);
 
-		if (tc_enabled_int == 1) {
-			/* Get struct containing sensor thermal compensation data */
-			sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */
-			uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
-			sensor_correction_sub.copy(&sensor_correction);
+			float z_scale = accel_T_rotated(2, 2);
+			sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
+			param_set_no_notification(param_find(str), &z_scale);
 
-			/* don't allow a parameter instance to be calibrated more than once by another uORB instance */
-			if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {
-				tc_locked[sensor_correction.accel_mapping[uorb_index]] = true;
-
-				/* update the _X0_ terms to include the additional offset */
-				int32_t handle;
-				float val;
-
-				for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
-					val = 0.0f;
-					(void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
-					handle = param_find(str);
-					param_get(handle, &val);
-
-					if (axis_index == 0) {
-						val += accel_scale.x_offset;
-
-					} else if (axis_index == 1) {
-						val += accel_scale.y_offset;
-
-					} else if (axis_index == 2) {
-						val += accel_scale.z_offset;
-					}
-
-					failed |= (PX4_OK != param_set_no_notification(handle, &val));
-				}
-
-				/* update the _SCL_ terms to include the scale factor */
-				for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
-					val = 1.0f;
-					(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
-					handle = param_find(str);
-
-					if (axis_index == 0) {
-						val = accel_scale.x_scale;
-
-					} else if (axis_index == 1) {
-						val = accel_scale.y_scale;
-
-					} else if (axis_index == 2) {
-						val = accel_scale.z_scale;
-					}
-
-					failed |= (PX4_OK != param_set_no_notification(handle, &val));
-				}
-
-				param_notify_changes();
-			}
-
-			// Ensure the calibration values used by the driver are at default settings when we are using thermal calibration data
-			accel_scale.x_offset = 0.f;
-			accel_scale.y_offset = 0.f;
-			accel_scale.z_offset = 0.f;
-			accel_scale.x_scale = 1.f;
-			accel_scale.y_scale = 1.f;
-			accel_scale.z_scale = 1.f;
-		}
-
-		// save the driver level calibration data
-		(void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
-		failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
-		(void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
-		failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
-		(void)sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
-		failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
-		(void)sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
-		failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
-		(void)sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
-		failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
-		(void)sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
-		failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
-		(void)sprintf(str, "CAL_ACC%u_ID", uorb_index);
-		failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index])));
-
-		if (failed) {
-			calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG);
-			return PX4_ERROR;
-		}
-
-#if 1 // TODO: replace all IOCTL usage
-		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, uorb_index);
-		fd = px4_open(str, 0);
-
-		if (fd < 0) {
-			calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
-			res = PX4_ERROR;
+			// calibration device ID
+			sprintf(str, "CAL_ACC%u_ID", uorb_index);
+			param_set_no_notification(param_find(str), &device_id[uorb_index]);
 
 		} else {
-			res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
-			px4_close(fd);
-		}
+			char str[30] {};
 
-		if (res != PX4_OK) {
-			calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG);
-		}
+			// reset calibration offsets
+			sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
+			param_reset(param_find(str));
+			sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
+			param_reset(param_find(str));
+			sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
+			param_reset(param_find(str));
 
-#endif
+			// reset calibration scale
+			sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
+			param_reset(param_find(str));
+			sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
+			param_reset(param_find(str));
+			sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
+			param_reset(param_find(str));
+
+			// reset calibration device ID
+			sprintf(str, "CAL_ACC%u_ID", uorb_index);
+			param_reset(param_find(str));
+		}
 	}
 
+	param_notify_changes();
+
 	if (res == PX4_OK) {
 		/* if there is a any preflight-check system response, let the barrage of messages through */
 		px4_usleep(200000);
@@ -464,8 +330,7 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
 	calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
 			     detect_orientation_str(orientation));
 
-	read_accelerometer_avg(worker_data->sensor_correction_sub, worker_data->subs, worker_data->accel_ref, orientation,
-			       samples_num);
+	read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num);
 
 	calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]",
 			     detect_orientation_str(orientation),
@@ -479,126 +344,28 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
 	return calibrate_return_ok;
 }
 
-calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub,
-		float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
+static calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub,
+		Vector3f(&accel_offs)[MAX_ACCEL_SENS], Matrix3f(&accel_T)[MAX_ACCEL_SENS], unsigned active_sensors)
 {
 	calibrate_return result = calibrate_return_ok;
 
-	*active_sensors = 0;
-
-	accel_worker_data_t worker_data;
-
+	accel_worker_data_t worker_data{};
 	worker_data.mavlink_log_pub = mavlink_log_pub;
-	worker_data.done_count = 0;
 
-	bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };
-
-	// Initialise sub to sensor thermal compensation data
-	worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
-
-	// Initialize subs to error condition so we know which ones are open and which are not
-	for (size_t i = 0; i < max_accel_sens; i++) {
-		worker_data.subs[i] = -1;
-	}
-
-	uint64_t timestamps[max_accel_sens] = {};
-
-	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
-	const unsigned orb_accel_count = orb_group_count(ORB_ID(sensor_accel));
-
-	// Warn that we will not calibrate more than max_accels accelerometers
-	if (orb_accel_count > max_accel_sens) {
-		calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count,
-					 max_accel_sens);
-	}
-
-	for (unsigned cur_accel = 0; cur_accel < orb_accel_count && cur_accel < max_accel_sens; cur_accel++) {
-
-		// Lock in to correct ORB instance
-		bool found_cur_accel = false;
-
-		for (unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) {
-			worker_data.subs[cur_accel] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
-
-			sensor_accel_s report = {};
-			orb_copy(ORB_ID(sensor_accel), worker_data.subs[cur_accel], &report);
-
-#if 1 // TODO: replace all IOCTL usage
-
-			// For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL
-			// and match it up with the one from the uORB subscription, because the
-			// instance ordering of uORB and the order of the FDs may not be the same.
-
-			if (report.device_id == (uint32_t)device_id[cur_accel]) {
-				// Device IDs match, correct ORB instance for this accel
-				found_cur_accel = true;
-				// store initial timestamp - used to infer which sensors are active
-				timestamps[cur_accel] = report.timestamp;
-
-			} else {
-				orb_unsubscribe(worker_data.subs[cur_accel]);
-			}
-
-#else
-
-			// For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report.
-			device_id[cur_accel] = report.device_id;
-			found_cur_accel = true;
-
-#endif
-		}
-
-		if (!found_cur_accel) {
-			calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]);
-			result = calibrate_return_error;
-			break;
-		}
-
-		if (device_id[cur_accel] != 0) {
-			// Get priority
-			ORB_PRIO prio = ORB_PRIO_UNINITIALIZED;
-			orb_priority(worker_data.subs[cur_accel], &prio);
-
-			if (prio > device_prio_max) {
-				device_prio_max = prio;
-				device_id_primary = device_id[cur_accel];
-			}
-
-		} else {
-			calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel);
-			result = calibrate_return_error;
-			break;
-		}
-	}
+	bool data_collected[detect_orientation_side_count] {};
 
 	if (result == calibrate_return_ok) {
 		int cancel_sub = calibrate_cancel_subscribe();
 		result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data,
-						    false /* normal still */);
+						    false);
+
 		calibrate_cancel_unsubscribe(cancel_sub);
 	}
 
-	/* close all subscriptions */
-	for (unsigned i = 0; i < max_accel_sens; i++) {
-		if (worker_data.subs[i] >= 0) {
-			/* figure out which sensors were active */
-			sensor_accel_s arp = {};
-			(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
-
-			if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
-				(*active_sensors)++;
-			}
-
-			px4_close(worker_data.subs[i]);
-		}
-	}
-
-	orb_unsubscribe(worker_data.sensor_correction_sub);
-
 	if (result == calibrate_return_ok) {
 		/* calculate offsets and transform matrix */
-		for (unsigned i = 0; i < (*active_sensors); i++) {
-			result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
+		for (unsigned i = 0; i < active_sensors; i++) {
+			result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs);
 
 			if (result != calibrate_return_ok) {
 				calibration_log_critical(mavlink_log_pub, "ERROR: calibration calculation error");
@@ -613,93 +380,69 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
 /*
  * Read specified number of accelerometer samples, calculate average and dispersion.
  */
-calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens],
-					float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
+static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3],
+		unsigned orient, unsigned samples_num)
 {
 	/* get total sensor board rotation matrix */
-	param_t board_rotation_h = param_find("SENS_BOARD_ROT");
-	param_t board_offset_x = param_find("SENS_BOARD_X_OFF");
-	param_t board_offset_y = param_find("SENS_BOARD_Y_OFF");
-	param_t board_offset_z = param_find("SENS_BOARD_Z_OFF");
+	float board_offset[3] {};
+	param_get(param_find("SENS_BOARD_X_OFF"), &board_offset[0]);
+	param_get(param_find("SENS_BOARD_Y_OFF"), &board_offset[1]);
+	param_get(param_find("SENS_BOARD_Z_OFF"), &board_offset[2]);
 
-	float board_offset[3];
-	param_get(board_offset_x, &board_offset[0]);
-	param_get(board_offset_y, &board_offset[1]);
-	param_get(board_offset_z, &board_offset[2]);
+	const Dcmf board_rotation_offset{Eulerf{math::radians(board_offset[0]), math::radians(board_offset[1]), math::radians(board_offset[2])}};
 
-	Dcmf board_rotation_offset = Eulerf(
-					     M_DEG_TO_RAD_F * board_offset[0],
-					     M_DEG_TO_RAD_F * board_offset[1],
-					     M_DEG_TO_RAD_F * board_offset[2]);
+	int32_t board_rotation_int = 0;
+	param_get(param_find("SENS_BOARD_ROT"), &board_rotation_int);
 
-	int32_t board_rotation_int;
-	param_get(board_rotation_h, &(board_rotation_int));
+	const Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int);
 
-	Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int);
-
-	px4_pollfd_struct_t fds[max_accel_sens];
-
-	for (unsigned i = 0; i < max_accel_sens; i++) {
-		fds[i].fd = subs[i];
-		fds[i].events = POLLIN;
-	}
-
-	unsigned counts[max_accel_sens] = { 0 };
-	float accel_sum[max_accel_sens][3] {};
+	Vector3f accel_sum[MAX_ACCEL_SENS] {};
+	unsigned counts[MAX_ACCEL_SENS] {};
 
 	unsigned errcount = 0;
-	struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
 
-	/* try to get latest thermal corrections */
-	if (orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction) != 0) {
-		/* use default values */
-		memset(&sensor_correction, 0, sizeof(sensor_correction));
+	// sensor thermal corrections
+	uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
+	sensor_correction_s sensor_correction{};
+	sensor_correction_sub.copy(&sensor_correction);
 
-		for (unsigned i = 0; i < 3; i++) {
-			sensor_correction.accel_scale_0[i] = 1.0f;
-			sensor_correction.accel_scale_1[i] = 1.0f;
-			sensor_correction.accel_scale_2[i] = 1.0f;
-		}
-	}
+	uORB::SubscriptionBlocking<sensor_accel_s> accel_sub[MAX_ACCEL_SENS] {
+		{ORB_ID(sensor_accel), 0, 0},
+		{ORB_ID(sensor_accel), 0, 1},
+		{ORB_ID(sensor_accel), 0, 2},
+	};
 
 	/* use the first sensor to pace the readout, but do per-sensor counts */
 	while (counts[0] < samples_num) {
-		int poll_ret = px4_poll(&fds[0], max_accel_sens, 1000);
+		if (accel_sub[0].updatedBlocking(100000)) {
+			for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
+				sensor_accel_s arp;
 
-		if (poll_ret > 0) {
+				if (accel_sub[accel_index].update(&arp)) {
+					// fetch optional thermal offset corrections in sensor/board frame
+					Vector3f offset{0, 0, 0};
+					sensor_correction_sub.update(&sensor_correction);
 
-			for (unsigned s = 0; s < max_accel_sens; s++) {
-				bool changed;
-				orb_check(subs[s], &changed);
-
-				if (changed) {
-
-					sensor_accel_s arp;
-					orb_copy(ORB_ID(sensor_accel), subs[s], &arp);
-
-					// Apply thermal offset corrections in sensor/board frame
-					if (s == 0) {
-						accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]);
-						accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]);
-						accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]);
-
-					} else if (s == 1) {
-						accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]);
-						accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]);
-						accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]);
-
-					} else if (s == 2) {
-						accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]);
-						accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]);
-						accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]);
-
-					} else {
-						accel_sum[s][0] += arp.x;
-						accel_sum[s][1] += arp.y;
-						accel_sum[s][2] += arp.z;
+					if (sensor_correction.timestamp > 0 && arp.device_id != 0) {
+						for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) {
+							if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) {
+								switch (correction_index) {
+								case 0:
+									offset = Vector3f{sensor_correction.accel_offset_0};
+									break;
+								case 1:
+									offset = Vector3f{sensor_correction.accel_offset_1};
+									break;
+								case 2:
+									offset = Vector3f{sensor_correction.accel_offset_2};
+									break;
+								}
+							}
+						}
 					}
 
-					counts[s]++;
+					accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset;
+					counts[accel_index]++;
 				}
 			}
 
@@ -714,79 +457,44 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m
 	}
 
 	// rotate sensor measurements from sensor to body frame using board rotation matrix
-	for (unsigned i = 0; i < max_accel_sens; i++) {
-		Vector3f accel_sum_vec(&accel_sum[i][0]);
-		accel_sum_vec = board_rotation * accel_sum_vec;
-
-		for (size_t j = 0; j < 3; j++) {
-			accel_sum[i][j] = accel_sum_vec(j);
-		}
+	for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
+		accel_sum[s] = board_rotation * accel_sum[s];
 	}
 
-	for (unsigned s = 0; s < max_accel_sens; s++) {
-		for (unsigned i = 0; i < 3; i++) {
-			accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
-		}
+	for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
+		const auto sum = accel_sum[s] / counts[s];
+		sum.copyTo(accel_avg[s][orient]);
 	}
 
 	return calibrate_return_ok;
 }
 
-int mat_invert3(float src[3][3], float dst[3][3])
-{
-	float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
-		    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
-		    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
-
-	if (fabsf(det) < FLT_EPSILON) {
-		return PX4_ERROR;        // Singular matrix
-	}
-
-	dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
-	dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
-	dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
-	dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det;
-	dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det;
-	dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det;
-	dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
-	dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
-	dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
-
-	return PX4_OK;
-}
-
-calibrate_return calculate_calibration_values(unsigned sensor,
-		float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3],
-		float (&accel_offs)[max_accel_sens][3], float g)
+static calibrate_return calculate_calibration_values(unsigned sensor,
+		float (&accel_ref)[MAX_ACCEL_SENS][detect_orientation_side_count][3], Matrix3f(&accel_T)[MAX_ACCEL_SENS],
+		Vector3f(&accel_offs)[MAX_ACCEL_SENS])
 {
 	/* calculate offsets */
 	for (unsigned i = 0; i < 3; i++) {
-		accel_offs[sensor][i] = (accel_ref[sensor][i * 2][i] + accel_ref[sensor][i * 2 + 1][i]) / 2;
+		accel_offs[sensor](i) = (accel_ref[sensor][i * 2][i] + accel_ref[sensor][i * 2 + 1][i]) / 2;
 	}
 
 	/* fill matrix A for linear equations system*/
-	float mat_A[3][3];
-	memset(mat_A, 0, sizeof(mat_A));
+	Matrix3f mat_A;
 
 	for (unsigned i = 0; i < 3; i++) {
 		for (unsigned j = 0; j < 3; j++) {
-			float a = accel_ref[sensor][i * 2][j] - accel_offs[sensor][j];
-			mat_A[i][j] = a;
+			mat_A(i, j) = accel_ref[sensor][i * 2][j] - accel_offs[sensor](j);
 		}
 	}
 
 	/* calculate inverse matrix for A */
-	float mat_A_inv[3][3];
-
-	if (mat_invert3(mat_A, mat_A_inv) != PX4_OK) {
-		return calibrate_return_error;
-	}
+	const Matrix3f mat_A_inv = mat_A.I();
 
 	/* copy results to accel_T */
 	for (unsigned i = 0; i < 3; i++) {
 		for (unsigned j = 0; j < 3; j++) {
 			/* simplify matrices mult because b has only one non-zero element == g at index i */
-			accel_T[sensor][j][i] = mat_A_inv[j][i] * g;
+			accel_T[sensor](j, i) = mat_A_inv(j, i) * CONSTANTS_ONE_G;
 		}
 	}
 
@@ -796,38 +504,31 @@ calibrate_return calculate_calibration_values(unsigned sensor,
 int do_level_calibration(orb_advert_t *mavlink_log_pub)
 {
 	bool success = false;
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	vehicle_attitude_s att{};
 
 	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level");
 
 	param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF");
 	param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF");
-	param_t board_rot_handle = param_find("SENS_BOARD_ROT");
 
 	// get old values
-	float roll_offset_current;
-	float pitch_offset_current;
-	int32_t board_rot_current = 0;
+	float roll_offset_current = 0.f;
+	float pitch_offset_current = 0.f;
 	param_get(roll_offset_handle, &roll_offset_current);
 	param_get(pitch_offset_handle, &pitch_offset_current);
-	param_get(board_rot_handle, &board_rot_current);
 
-	Dcmf board_rotation_offset = Eulerf(
-					     math::radians(roll_offset_current),
-					     math::radians(pitch_offset_current),
-					     0.f);
+	int32_t board_rot_current = 0;
+	param_get(param_find("SENS_BOARD_ROT"), &board_rot_current);
 
-	px4_pollfd_struct_t fds[1];
-	fds[0].fd = att_sub;
-	fds[0].events = POLLIN;
+	const Dcmf board_rotation_offset{Eulerf{radians(roll_offset_current), radians(pitch_offset_current), 0.f}};
 
-	float roll_mean = 0.0f;
-	float pitch_mean = 0.0f;
+	float roll_mean = 0.f;
+	float pitch_mean = 0.f;
 	unsigned counter = 0;
 	bool had_motion = true;
 	int num_retries = 0;
 
+	uORB::SubscriptionBlocking<vehicle_attitude_s> att_sub{ORB_ID(vehicle_attitude)};
+
 	while (had_motion && num_retries++ < 50) {
 		Vector2f min_angles{100.f, 100.f};
 		Vector2f max_angles{-100.f, -100.f};
@@ -839,9 +540,10 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub)
 		const hrt_abstime start = hrt_absolute_time();
 
 		while (hrt_elapsed_time(&start) < calibration_duration) {
-			int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
-			if (pollret <= 0) {
+			vehicle_attitude_s att{};
+
+			if (!att_sub.updateBlocking(att, 100000)) {
 				// attitude estimator is not running
 				calibration_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot");
 				calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
@@ -855,8 +557,7 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub)
 				last_progress_report = progress;
 			}
 
-			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-			Eulerf att_euler = Quatf(att.q);
+			Eulerf att_euler{Quatf{att.q}};
 
 			// keep min + max angles
 			for (int i = 0; i < 2; ++i) {
@@ -866,7 +567,8 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub)
 			}
 
 			att_euler(2) = 0.f; // ignore yaw
-			att_euler = Eulerf(board_rotation_offset * Dcmf(att_euler)); // subtract existing board rotation
+
+			att_euler = Eulerf{board_rotation_offset *Dcmf{att_euler}};  // subtract existing board rotation
 			roll_mean += att_euler.phi();
 			pitch_mean += att_euler.theta();
 			++counter;
@@ -876,6 +578,7 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub)
 		// The difference is typically <0.1 deg while at rest
 		if (max_angles(0) - min_angles(0) < math::radians(0.5f) &&
 		    max_angles(1) - min_angles(1) < math::radians(0.5f)) {
+
 			had_motion = false;
 		}
 	}
@@ -895,18 +598,16 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub)
 		calibration_log_critical(mavlink_log_pub, "excess pitch angle");
 
 	} else {
-		roll_mean *= (float)M_RAD_TO_DEG;
-		pitch_mean *= (float)M_RAD_TO_DEG;
-		param_set_no_notification(roll_offset_handle, &roll_mean);
-		param_set_no_notification(pitch_offset_handle, &pitch_mean);
+		float roll_mean_degrees = math::degrees(roll_mean);
+		float pitch_mean_degrees = math::degrees(pitch_mean);
+		param_set_no_notification(roll_offset_handle, &roll_mean_degrees);
+		param_set_no_notification(pitch_offset_handle, &pitch_mean_degrees);
 		param_notify_changes();
 		success = true;
 	}
 
 out:
 
-	orb_unsubscribe(att_sub);
-
 	if (success) {
 		calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
 		return 0;

src/modules/commander/calibration_routines.cpp

@@ -41,26 +41,25 @@
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/time.h>
-#include <stdio.h>
-#include <unistd.h>
-#include <math.h>
-#include <float.h>
+
 #include <drivers/drv_hrt.h>
-#include <systemlib/mavlink_log.h>
+#include <drivers/drv_tone_alarm.h>
+
 #include <lib/ecl/geo/geo.h>
-#include <string.h>
-#include <mathlib/mathlib.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/systemlib/mavlink_log.h>
 #include <matrix/math.hpp>
 
-#include <uORB/topics/vehicle_command.h>
+#include <uORB/SubscriptionBlocking.hpp>
 #include <uORB/topics/sensor_combined.h>
-
-#include <drivers/drv_tone_alarm.h>
+#include <uORB/topics/vehicle_command.h>
 
 #include "calibration_routines.h"
 #include "calibration_messages.h"
 #include "commander_helper.h"
 
+using namespace time_literals;
+
 int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
 			     float *sphere_radius)
@@ -523,28 +522,22 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
 	}
 }
 
-enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub,
-		bool lenient_still_position)
+enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, bool lenient_still_position)
 {
 	static constexpr unsigned ndim = 3;
 
-	float		accel_ema[ndim] = { 0.0f };		// exponential moving average of accel
-	float		accel_disp[3] = { 0.0f, 0.0f, 0.0f };	// max-hold dispersion of accel
-	static constexpr float		ema_len = 0.5f;				// EMA time constant in seconds
-	static constexpr float	normal_still_thr = 0.25;		// normal still threshold
-	float		still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
-	static constexpr float		accel_err_thr = 5.0f;			// set accel error threshold to 5m/s^2
-	const hrt_abstime	still_time = lenient_still_position ? 500000 : 1300000;	// still time required in us
+	float accel_ema[ndim] {};                       // exponential moving average of accel
+	float accel_disp[3] {};                         // max-hold dispersion of accel
+	static constexpr float ema_len = 0.5f;          // EMA time constant in seconds
+	static constexpr float normal_still_thr = 0.25; // normal still threshold
+	float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
+	static constexpr float accel_err_thr = 5.0f;    // set accel error threshold to 5m/s^2
+	const hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us
 
-	px4_pollfd_struct_t fds[1];
-	fds[0].fd = accel_sub;
-	fds[0].events = POLLIN;
+	/* set timeout to 90s */
+	static constexpr hrt_abstime timeout = 90_s;
 
 	const hrt_abstime t_start = hrt_absolute_time();
-
-	/* set timeout to 30s */
-	static constexpr hrt_abstime timeout = 90000000;
-
 	hrt_abstime t_timeout = t_start + timeout;
 	hrt_abstime t = t_start;
 	hrt_abstime t_prev = t_start;
@@ -552,13 +545,13 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 
 	unsigned poll_errcount = 0;
 
-	while (true) {
-		/* wait blocking for new data */
-		int poll_ret = px4_poll(fds, 1, 1000);
+	// Setup subscriptions to onboard accel sensor
+	uORB::SubscriptionBlocking<sensor_combined_s> sensor_sub{ORB_ID(sensor_combined)};
 
-		if (poll_ret) {
-			struct sensor_combined_s sensor;
-			orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor);
+	while (true) {
+		sensor_combined_s sensor;
+
+		if (sensor_sub.updateBlocking(sensor, 100000)) {
 			t = hrt_absolute_time();
 			float dt = (t - t_prev) / 1000000.0f;
 			t_prev = t;
@@ -586,6 +579,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 			if (accel_disp[0] < still_thr2 &&
 			    accel_disp[1] < still_thr2 &&
 			    accel_disp[2] < still_thr2) {
+
 				/* is still now */
 				if (t_still == 0) {
 					/* first time */
@@ -612,7 +606,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 				}
 			}
 
-		} else if (poll_ret == 0) {
+		} else {
 			poll_errcount++;
 		}
 
@@ -691,15 +685,6 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
 {
 	calibrate_return result = calibrate_return_ok;
 
-	// Setup subscriptions to onboard accel sensor
-
-	int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
-
-	if (sub_accel < 0) {
-		calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
-		return calibrate_return_error;
-	}
-
 	unsigned orientation_failures = 0;
 
 	// Rotate through all requested orientation
@@ -744,8 +729,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
 		px4_usleep(20000);
 		calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
 		px4_usleep(20000);
-		enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel,
-							lenient_still_position);
+		enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, lenient_still_position);
 
 		if (orient == DETECT_ORIENTATION_ERROR) {
 			orientation_failures++;
@@ -792,10 +776,6 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
 		px4_usleep(200000);
 	}
 
-	if (sub_accel >= 0) {
-		px4_close(sub_accel);
-	}
-
 	return result;
 }
 

src/modules/commander/calibration_routines.h

@@ -72,9 +72,6 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
 bool inverse4x4(float m[], float invOut[]);
 bool mat_inverse(float *A, float *inv, uint8_t n);
 
-// FIXME: Change the name
-static const unsigned max_accel_sens = 3;
-
 // The order of these cannot change since the calibration calculations depend on them in this order
 enum detect_orientation_return {
 	DETECT_ORIENTATION_TAIL_DOWN,
@@ -92,7 +89,6 @@ static const unsigned detect_orientation_side_count = 6;
 ///		and ready for measurements
 enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,	///< uORB handle to write output to
 		int	cancel_sub,			///< Cancel subscription from calibration_cancel_subscribe
-		int	accel_sub,			///< Orb subcription to accel sensor
 		bool	lenient_still_detection);	///< true: Use more lenient still position detection
 
 /// Returns the human readable string representation of the orientation

src/modules/commander/gyro_calibration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
-*   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+*   Copyright (c) 2013-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,32 +46,27 @@
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/time.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <math.h>
-#include <mathlib/mathlib.h>
-#include <string.h>
+
 #include <drivers/drv_hrt.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/parameters/param.h>
+#include <lib/systemlib/mavlink_log.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionBlocking.hpp>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_correction.h>
-#include <drivers/drv_gyro.h>
-#include <systemlib/mavlink_log.h>
-#include <parameters/param.h>
-#include <systemlib/err.h>
+#include <uORB/topics/sensor_gyro.h>
 
-static const char *sensor_name = "gyro";
+static constexpr char sensor_name[] {"gyro"};
+static constexpr unsigned MAX_GYROS = 3;
 
-static constexpr unsigned max_gyros = 3;
+using matrix::Vector3f;
 
 /// Data passed to calibration worker routine
 struct gyro_worker_data_t {
-	orb_advert_t		*mavlink_log_pub;
-	int32_t			device_id[max_gyros];
-	int			gyro_sensor_sub[max_gyros];
-	int			sensor_correction_sub;
-	struct gyro_calibration_s	gyro_scale[max_gyros];
+	orb_advert_t *mavlink_log_pub{nullptr};
+	int32_t device_id[MAX_GYROS] {};
+	Vector3f offset[MAX_GYROS] {};
 
 	static constexpr int last_num_samples = 9; ///< number of samples for the motion detection median filter
 	float last_sample_0_x[last_num_samples];
@@ -89,112 +84,88 @@ static int float_cmp(const void *elem1, const void *elem2)
 	return *(const float *)elem1 > *(const float *)elem2;
 }
 
-static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
+static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data_t &worker_data)
 {
-	gyro_worker_data_t	*worker_data = (gyro_worker_data_t *)(data);
-	unsigned		calibration_counter[max_gyros] = { 0 }, slow_count = 0;
-	const unsigned		calibration_count = 250;
-	sensor_gyro_s	gyro_report;
-	unsigned		poll_errcount = 0;
+	unsigned calibration_counter[MAX_GYROS] {};
+	static constexpr unsigned CALIBRATION_COUNT = 250;
+	unsigned poll_errcount = 0;
 
-	struct sensor_correction_s sensor_correction {}; /**< sensor thermal corrections */
+	uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
+	sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */
 
-	if (orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction) != 0) {
-		for (unsigned i = 0; i < 3; i++) {
-			sensor_correction.gyro_scale_0[i] = 1.0f;
-			sensor_correction.gyro_scale_1[i] = 1.0f;
-			sensor_correction.gyro_scale_2[i] = 1.0f;
-		}
-	}
+	uORB::SubscriptionBlocking<sensor_gyro_s> gyro_sub[MAX_GYROS] {
+		{ORB_ID(sensor_gyro), 0, 0},
+		{ORB_ID(sensor_gyro), 0, 1},
+		{ORB_ID(sensor_gyro), 0, 2},
+	};
 
-	px4_pollfd_struct_t fds[max_gyros];
-
-	for (unsigned s = 0; s < max_gyros; s++) {
-		fds[s].fd = worker_data->gyro_sensor_sub[s];
-		fds[s].events = POLLIN;
-	}
-
-	memset(&worker_data->last_sample_0_x, 0, sizeof(worker_data->last_sample_0_x));
-	memset(&worker_data->last_sample_0_y, 0, sizeof(worker_data->last_sample_0_y));
-	memset(&worker_data->last_sample_0_z, 0, sizeof(worker_data->last_sample_0_z));
-	worker_data->last_sample_0_idx = 0;
+	memset(&worker_data.last_sample_0_x, 0, sizeof(worker_data.last_sample_0_x));
+	memset(&worker_data.last_sample_0_y, 0, sizeof(worker_data.last_sample_0_y));
+	memset(&worker_data.last_sample_0_z, 0, sizeof(worker_data.last_sample_0_z));
+	worker_data.last_sample_0_idx = 0;
 
 	/* use slowest gyro to pace, but count correctly per-gyro for statistics */
-	while (slow_count < calibration_count) {
-		if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
+	unsigned slow_count = 0;
+
+	while (slow_count < CALIBRATION_COUNT) {
+		if (calibrate_cancel_check(worker_data.mavlink_log_pub, cancel_sub)) {
 			return calibrate_return_cancelled;
 		}
 
-		/* check if there are new thermal corrections */
-		bool updated;
-		orb_check(worker_data->sensor_correction_sub, &updated);
+		if (gyro_sub[0].updatedBlocking(100000)) {
+			unsigned update_count = CALIBRATION_COUNT;
 
-		if (updated) {
-			orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction);
-		}
-
-		int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
-
-		if (poll_ret > 0) {
-			unsigned update_count = calibration_count;
-
-			for (unsigned s = 0; s < max_gyros; s++) {
-				if (calibration_counter[s] >= calibration_count) {
+			for (unsigned gyro_index = 0; gyro_index < MAX_GYROS; gyro_index++) {
+				if (calibration_counter[gyro_index] >= CALIBRATION_COUNT) {
 					// Skip if instance has enough samples
 					continue;
 				}
 
-				bool changed;
-				orb_check(worker_data->gyro_sensor_sub[s], &changed);
+				sensor_gyro_s gyro_report;
 
-				if (changed) {
-					orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report);
-					float sample[3];
+				if (gyro_sub[gyro_index].update(&gyro_report)) {
 
-					if (s == 0) {
-						// take a working copy
-						sample[0] = (gyro_report.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0];
-						sample[1] = (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1];
-						sample[2] = (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2];
-
-						worker_data->last_sample_0_x[worker_data->last_sample_0_idx] = sample[0];
-						worker_data->last_sample_0_y[worker_data->last_sample_0_idx] = sample[1];
-						worker_data->last_sample_0_z[worker_data->last_sample_0_idx] = sample[2];
-						worker_data->last_sample_0_idx = (worker_data->last_sample_0_idx + 1) % gyro_worker_data_t::last_num_samples;
-
-					} else if (s == 1) {
-						sample[0] = (gyro_report.x - sensor_correction.gyro_offset_1[0]) * sensor_correction.gyro_scale_1[0];
-						sample[1] = (gyro_report.y - sensor_correction.gyro_offset_1[1]) * sensor_correction.gyro_scale_1[1];
-						sample[2] = (gyro_report.z - sensor_correction.gyro_offset_1[2]) * sensor_correction.gyro_scale_1[2];
-
-					} else if (s == 2) {
-						sample[0] = (gyro_report.x - sensor_correction.gyro_offset_2[0]) * sensor_correction.gyro_scale_2[0];
-						sample[1] = (gyro_report.y - sensor_correction.gyro_offset_2[1]) * sensor_correction.gyro_scale_2[1];
-						sample[2] = (gyro_report.z - sensor_correction.gyro_offset_2[2]) * sensor_correction.gyro_scale_2[2];
-
-					} else {
-						sample[0] = gyro_report.x;
-						sample[1] = gyro_report.y;
-						sample[2] = gyro_report.z;
+					// fetch optional thermal offset corrections in sensor/board frame
+					Vector3f offset{0, 0, 0};
+					sensor_correction_sub.update(&sensor_correction);
 
+					if (sensor_correction.timestamp > 0 && gyro_report.device_id != 0) {
+						for (uint8_t correction_index = 0; correction_index < MAX_GYROS; correction_index++) {
+							if (sensor_correction.gyro_device_ids[correction_index] == gyro_report.device_id) {
+								switch (correction_index) {
+								case 0:
+									offset = Vector3f{sensor_correction.gyro_offset_0};
+									break;
+								case 1:
+									offset = Vector3f{sensor_correction.gyro_offset_1};
+									break;
+								case 2:
+									offset = Vector3f{sensor_correction.gyro_offset_2};
+									break;
+								}
+							}
+						}
 					}
 
-					worker_data->gyro_scale[s].x_offset += sample[0];
-					worker_data->gyro_scale[s].y_offset += sample[1];
-					worker_data->gyro_scale[s].z_offset += sample[2];
-					calibration_counter[s]++;
+					worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - offset;
+					calibration_counter[gyro_index]++;
 
+					if (gyro_index == 0) {
+						worker_data.last_sample_0_x[worker_data.last_sample_0_idx] = gyro_report.x - offset(0);
+						worker_data.last_sample_0_y[worker_data.last_sample_0_idx] = gyro_report.y - offset(1);
+						worker_data.last_sample_0_z[worker_data.last_sample_0_idx] = gyro_report.z - offset(2);
+						worker_data.last_sample_0_idx = (worker_data.last_sample_0_idx + 1) % gyro_worker_data_t::last_num_samples;
+					}
 				}
 
 				// Maintain the sample count of the slowest sensor
-				if (calibration_counter[s] && calibration_counter[s] < update_count) {
-					update_count = calibration_counter[s];
+				if (calibration_counter[gyro_index] && calibration_counter[gyro_index] < update_count) {
+					update_count = calibration_counter[gyro_index];
 				}
-
 			}
 
-			if (update_count % (calibration_count / 20) == 0) {
-				calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (update_count * 100) / calibration_count);
+			if (update_count % (CALIBRATION_COUNT / 20) == 0) {
+				calibration_log_info(worker_data.mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (update_count * 100) / CALIBRATION_COUNT);
 			}
 
 			// Propagate out the slowest sensor's count
@@ -207,20 +178,18 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
 		}
 
 		if (poll_errcount > 1000) {
-			calibration_log_critical(worker_data->mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
+			calibration_log_critical(worker_data.mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
 			return calibrate_return_error;
 		}
 	}
 
-	for (unsigned s = 0; s < max_gyros; s++) {
-		if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
-			calibration_log_critical(worker_data->mavlink_log_pub, "ERROR: missing data, sensor %d", s)
+	for (unsigned s = 0; s < MAX_GYROS; s++) {
+		if ((worker_data.device_id[s] != 0) && (calibration_counter[s] < CALIBRATION_COUNT / 2)) {
+			calibration_log_critical(worker_data.mavlink_log_pub, "ERROR: missing data, sensor %d", s)
 			return calibrate_return_error;
 		}
 
-		worker_data->gyro_scale[s].x_offset /= calibration_counter[s];
-		worker_data->gyro_scale[s].y_offset /= calibration_counter[s];
-		worker_data->gyro_scale[s].z_offset /= calibration_counter[s];
+		worker_data.offset[s] /= calibration_counter[s];
 	}
 
 	return calibrate_return_ok;
@@ -228,131 +197,35 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
 
 int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 {
-	int			res = PX4_OK;
-	gyro_worker_data_t	worker_data = {};
+	int res = PX4_OK;
 
 	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
 
+	gyro_worker_data_t worker_data{};
 	worker_data.mavlink_log_pub = mavlink_log_pub;
 
-	gyro_calibration_s gyro_scale_zero{};
-	int device_prio_max = 0;
+	enum ORB_PRIO device_prio_max = ORB_PRIO_UNINITIALIZED;
 	int32_t device_id_primary = 0;
 
-	worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
-
-	for (unsigned s = 0; s < max_gyros; s++) {
-		char str[30];
-
-		// Reset gyro ids to unavailable.
-		worker_data.device_id[s] = 0;
-		// And set default subscriber values.
-		worker_data.gyro_sensor_sub[s] = -1;
-		(void)sprintf(str, "CAL_GYRO%u_ID", s);
-		res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
-
-		if (res != PX4_OK) {
-			calibration_log_critical(mavlink_log_pub, "Unable to reset CAL_GYRO%u_ID", s);
-			return PX4_ERROR;
-		}
-
-		// Reset all offsets to 0
-		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
-#if 1 // TODO: replace all IOCTL usage
-		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
-		int fd = px4_open(str, 0);
-
-		if (fd >= 0) {
-			worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-			res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
-			px4_close(fd);
-
-			if (res != PX4_OK) {
-				calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
-				return PX4_ERROR;
-			}
-		}
-
-#else
-		(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
-		res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_offset);
-
-		if (res != PX4_OK) {
-			PX4_ERR("unable to reset %s", str);
-		}
-
-		(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
-		res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_offset);
-
-		if (res != PX4_OK) {
-			PX4_ERR("unable to reset %s", str);
-		}
-
-		(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
-		res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_offset);
-
-		if (res != PX4_OK) {
-			PX4_ERR("unable to reset %s", str);
-		}
-
-		param_notify_changes();
-#endif
-
-	}
-
 	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
 	const unsigned orb_gyro_count = orb_group_count(ORB_ID(sensor_gyro));
 
-	// Warn that we will not calibrate more than max_gyros gyroscopes
-	if (orb_gyro_count > max_gyros) {
-		calibration_log_critical(mavlink_log_pub, "Detected %u gyros, but will calibrate only %u", orb_gyro_count, max_gyros);
+	// Warn that we will not calibrate more than MAX_GYROS gyroscopes
+	if (orb_gyro_count > MAX_GYROS) {
+		calibration_log_critical(mavlink_log_pub, "Detected %u gyros, but will calibrate only %u", orb_gyro_count, MAX_GYROS);
 	}
 
-	for (unsigned cur_gyro = 0; cur_gyro < orb_gyro_count && cur_gyro < max_gyros; cur_gyro++) {
+	for (uint8_t cur_gyro = 0; cur_gyro < orb_gyro_count && cur_gyro < MAX_GYROS; cur_gyro++) {
 
-		// Lock in to correct ORB instance
-		bool found_cur_gyro = false;
+		uORB::Subscription gyro_sensor_sub{ORB_ID(sensor_gyro), cur_gyro};
+		sensor_gyro_s report{};
+		gyro_sensor_sub.copy(&report);
 
-		for (unsigned i = 0; i < orb_gyro_count && !found_cur_gyro; i++) {
-			worker_data.gyro_sensor_sub[cur_gyro] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
-
-			sensor_gyro_s report{};
-			orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[cur_gyro], &report);
-
-#if 1 // TODO: replace all IOCTL usage
-
-			// For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL
-			// and match it up with the one from the uORB subscription, because the
-			// instance ordering of uORB and the order of the FDs may not be the same.
-
-			if (report.device_id == (uint32_t)worker_data.device_id[cur_gyro]) {
-				// Device IDs match, correct ORB instance for this gyro
-				found_cur_gyro = true;
-
-			} else {
-				orb_unsubscribe(worker_data.gyro_sensor_sub[cur_gyro]);
-			}
-
-#else
-
-			// For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report.
-			worker_data.device_id[cur_gyro] = report.device_id;
-			found_cur_gyro = true;
-
-#endif
-		}
-
-		if (!found_cur_gyro) {
-			calibration_log_critical(mavlink_log_pub, "Gyro #%u (ID %u) no matching uORB devid", cur_gyro,
-						 worker_data.device_id[cur_gyro]);
-			res = calibrate_return_error;
-			break;
-		}
+		worker_data.device_id[cur_gyro] = report.device_id;
 
 		if (worker_data.device_id[cur_gyro] != 0) {
 			// Get priority
-			ORB_PRIO prio = ORB_PRIO_UNINITIALIZED;
-			orb_priority(worker_data.gyro_sensor_sub[cur_gyro], &prio);
+			enum ORB_PRIO prio = gyro_sensor_sub.get_priority();
 
 			if (prio > device_prio_max) {
 				device_prio_max = prio;
@@ -364,7 +237,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 		}
 	}
 
-	int cancel_sub  = calibrate_cancel_subscribe();
+	int cancel_sub = calibrate_cancel_subscribe();
 
 	unsigned try_count = 0;
 	unsigned max_tries = 20;
@@ -372,7 +245,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 
 	do {
 		// Calibrate gyro and ensure user didn't move
-		calibrate_return cal_return = gyro_calibration_worker(cancel_sub, &worker_data);
+		calibrate_return cal_return = gyro_calibration_worker(cancel_sub, worker_data);
 
 		if (cal_return == calibrate_return_cancelled) {
 			// Cancel message already sent, we are done here
@@ -387,22 +260,18 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 			qsort(worker_data.last_sample_0_x, gyro_worker_data_t::last_num_samples, sizeof(float), float_cmp);
 			qsort(worker_data.last_sample_0_y, gyro_worker_data_t::last_num_samples, sizeof(float), float_cmp);
 			qsort(worker_data.last_sample_0_z, gyro_worker_data_t::last_num_samples, sizeof(float), float_cmp);
-			float xdiff = worker_data.last_sample_0_x[gyro_worker_data_t::last_num_samples / 2] -
-				      worker_data.gyro_scale[0].x_offset;
-			float ydiff = worker_data.last_sample_0_y[gyro_worker_data_t::last_num_samples / 2] -
-				      worker_data.gyro_scale[0].y_offset;
-			float zdiff = worker_data.last_sample_0_z[gyro_worker_data_t::last_num_samples / 2] -
-				      worker_data.gyro_scale[0].z_offset;
+
+			float xdiff = worker_data.last_sample_0_x[gyro_worker_data_t::last_num_samples / 2] - worker_data.offset[0](0);
+			float ydiff = worker_data.last_sample_0_y[gyro_worker_data_t::last_num_samples / 2] - worker_data.offset[0](1);
+			float zdiff = worker_data.last_sample_0_z[gyro_worker_data_t::last_num_samples / 2] - worker_data.offset[0](2);
 
 			/* maximum allowable calibration error */
 			const float maxoff = math::radians(0.6f);
 
-			if (!PX4_ISFINITE(worker_data.gyro_scale[0].x_offset) ||
-			    !PX4_ISFINITE(worker_data.gyro_scale[0].y_offset) ||
-			    !PX4_ISFINITE(worker_data.gyro_scale[0].z_offset) ||
-			    fabsf(xdiff) > maxoff ||
-			    fabsf(ydiff) > maxoff ||
-			    fabsf(zdiff) > maxoff) {
+			if (!PX4_ISFINITE(worker_data.offset[0](0)) ||
+			    !PX4_ISFINITE(worker_data.offset[0](1)) ||
+			    !PX4_ISFINITE(worker_data.offset[0](2)) ||
+			    fabsf(xdiff) > maxoff || fabsf(ydiff) > maxoff || fabsf(zdiff) > maxoff) {
 
 				calibration_log_critical(mavlink_log_pub, "motion, retrying..");
 				res = PX4_ERROR;
@@ -423,98 +292,44 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 
 	calibrate_cancel_unsubscribe(cancel_sub);
 
-	for (unsigned s = 0; s < max_gyros; s++) {
-		px4_close(worker_data.gyro_sensor_sub[s]);
-	}
-
 	if (res == PX4_OK) {
 
 		/* set offset parameters to new values */
-		bool failed = false;
+		bool failed = (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &device_id_primary));
 
-		failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary)));
+		for (unsigned uorb_index = 0; uorb_index < MAX_GYROS; uorb_index++) {
 
-		bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator
+			char str[30] {};
 
-		for (unsigned uorb_index = 0; uorb_index < max_gyros; uorb_index++) {
-			if (worker_data.device_id[uorb_index] != 0) {
-				char str[30];
+			if (uorb_index < orb_gyro_count) {
+				float x_offset = worker_data.offset[uorb_index](0);
+				sprintf(str, "CAL_GYRO%u_XOFF", uorb_index);
+				failed |= (PX4_OK != param_set_no_notification(param_find(str), &x_offset));
 
-				/* check if thermal compensation is enabled */
-				int32_t tc_enabled_int;
-				param_get(param_find("TC_G_ENABLE"), &(tc_enabled_int));
+				float y_offset = worker_data.offset[uorb_index](1);
+				sprintf(str, "CAL_GYRO%u_YOFF", uorb_index);
+				failed |= (PX4_OK != param_set_no_notification(param_find(str), &y_offset));
 
-				if (tc_enabled_int == 1) {
-					/* Get struct containing sensor thermal compensation data */
-					struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
-					memset(&sensor_correction, 0, sizeof(sensor_correction));
-					orb_copy(ORB_ID(sensor_correction), worker_data.sensor_correction_sub, &sensor_correction);
+				float z_offset = worker_data.offset[uorb_index](2);
+				sprintf(str, "CAL_GYRO%u_ZOFF", uorb_index);
+				failed |= (PX4_OK != param_set_no_notification(param_find(str), &z_offset));
 
-					/* don't allow a parameter instance to be calibrated again by another uORB instance */
-					if (!tc_locked[sensor_correction.gyro_mapping[uorb_index]]) {
-						tc_locked[sensor_correction.gyro_mapping[uorb_index]] = true;
+				int32_t device_id = worker_data.device_id[uorb_index];
+				sprintf(str, "CAL_GYRO%u_ID", uorb_index);
+				failed |= (PX4_OK != param_set_no_notification(param_find(str), &device_id));
 
-						/* update the _X0_ terms to include the additional offset */
-						int32_t handle;
-						float val;
+			} else {
+				// reset unused calibration offsets
+				sprintf(str, "CAL_GYRO%u_XOFF", uorb_index);
+				param_reset(param_find(str));
+				sprintf(str, "CAL_GYRO%u_YOFF", uorb_index);
+				param_reset(param_find(str));
+				sprintf(str, "CAL_GYRO%u_ZOFF", uorb_index);
+				param_reset(param_find(str));
 
-						for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
-							val = 0.0f;
-							(void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index);
-							handle = param_find(str);
-							param_get(handle, &val);
-
-							if (axis_index == 0) {
-								val += worker_data.gyro_scale[uorb_index].x_offset;
-
-							} else if (axis_index == 1) {
-								val += worker_data.gyro_scale[uorb_index].y_offset;
-
-							} else if (axis_index == 2) {
-								val += worker_data.gyro_scale[uorb_index].z_offset;
-
-							}
-
-							failed |= (PX4_OK != param_set_no_notification(handle, &val));
-						}
-
-						param_notify_changes();
-					}
-
-					// Ensure the calibration values used the driver are at default settings
-					worker_data.gyro_scale[uorb_index].x_offset = 0.f;
-					worker_data.gyro_scale[uorb_index].y_offset = 0.f;
-					worker_data.gyro_scale[uorb_index].z_offset = 0.f;
-				}
-
-				(void)sprintf(str, "CAL_GYRO%u_XOFF", uorb_index);
-				failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].x_offset)));
-				(void)sprintf(str, "CAL_GYRO%u_YOFF", uorb_index);
-				failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].y_offset)));
-				(void)sprintf(str, "CAL_GYRO%u_ZOFF", uorb_index);
-				failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[uorb_index].z_offset)));
-
-				(void)sprintf(str, "CAL_GYRO%u_ID", uorb_index);
-				failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.device_id[uorb_index])));
-
-#if 1 // TODO: replace all IOCTL usage
-				/* apply new scaling and offsets */
-				(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, uorb_index);
-				int fd = px4_open(str, 0);
-
-				if (fd < 0) {
-					failed = true;
-					continue;
-				}
-
-				res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[uorb_index]);
-				px4_close(fd);
-
-				if (res != PX4_OK) {
-					calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG);
-				}
-
-#endif
+				// reset unused calibration device ID
+				sprintf(str, "CAL_GYRO%u_ID", uorb_index);
+				param_reset(param_find(str));
 			}
 		}
 
@@ -524,6 +339,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 		}
 	}
 
+	param_notify_changes();
+
 	/* if there is a any preflight-check system response, let the barrage of messages through */
 	px4_usleep(200000);
 
@@ -534,8 +351,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 		calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
 	}
 
-	orb_unsubscribe(worker_data.sensor_correction_sub);
-
 	/* give this message enough time to propagate */
 	px4_usleep(600000);
 

src/modules/commander/mag_calibration.cpp

@@ -52,14 +52,12 @@
 #include <math.h>
 #include <fcntl.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_tone_alarm.h>
 #include <systemlib/mavlink_log.h>
 #include <parameters/param.h>
 #include <systemlib/err.h>
-#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/sensor_gyro.h>
 
 static const char *sensor_name = "mag";
 static constexpr unsigned max_mags = 4;

src/modules/sensors/CMakeLists.txt

@@ -31,7 +31,7 @@
 #
 ############################################################################
 
-add_subdirectory(sensor_corrections) # used by vehicle_{acceleration, angular_velocity, imu}
+add_subdirectory(sensor_calibration) # used by vehicle_{acceleration, angular_velocity, imu}
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)

src/modules/sensors/sensor_calibration/CMakeLists.txt

@@ -31,8 +31,9 @@
 #
 ############################################################################
 
-px4_add_library(sensor_corrections
-	SensorCorrections.cpp
-	SensorCorrections.hpp
+px4_add_library(sensor_calibration
+	SensorCalibration.cpp
+	SensorCalibration.hpp
 )
-target_compile_options(sensor_corrections PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_compile_options(sensor_calibration PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+target_include_directories(sensor_calibration PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/..)

src/modules/sensors/sensor_calibration/SensorCalibration.cpp

@@ -0,0 +1,252 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 "SensorCalibration.hpp"
+
+#include <lib/parameters/param.h>
+
+using namespace matrix;
+using namespace time_literals;
+using math::radians;
+
+namespace sensors
+{
+
+void SensorCalibration::set_device_id(uint32_t device_id)
+{
+	if (_device_id != device_id) {
+		_device_id = device_id;
+		SensorCorrectionsUpdate(true);
+		ParametersUpdate();
+	}
+}
+
+matrix::Vector3f SensorCalibration::Correct(const matrix::Vector3f &data)
+{
+	SensorCorrectionsUpdate();
+	return _rotation * matrix::Vector3f{(data - _thermal_offset - _offset).emult(_scale)};
+}
+
+const char *SensorCalibration::SensorString() const
+{
+	switch (_type) {
+	case SensorType::Accelerometer:
+		return "ACC";
+
+	case SensorType::Gyroscope:
+		return "GYRO";
+	}
+
+	return nullptr;
+}
+
+int SensorCalibration::FindCalibrationIndex(uint32_t device_id) const
+{
+	if (device_id == 0) {
+		return -1;
+	}
+
+	for (unsigned i = 0; i < MAX_SENSOR_COUNT; ++i) {
+		char str[16] {};
+		sprintf(str, "CAL_%s%u_ID", SensorString(), i);
+
+		int32_t device_id_val = 0;
+
+		if (param_get(param_find(str), &device_id_val) != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id_val == device_id) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+bool SensorCalibration::CalibrationEnabled(uint8_t calibration_index) const
+{
+	char str[20] {};
+	sprintf(str, "CAL_%s%u_EN", SensorString(), calibration_index);
+	int32_t enabled_val = 1;
+	param_get(param_find(str), &enabled_val);
+
+	return (enabled_val != 0);
+}
+
+Vector3f SensorCalibration::CalibrationOffset(uint8_t calibration_index) const
+{
+	// offsets (x, y, z)
+	Vector3f offset{0.f, 0.f, 0.f};
+	char str[20] {};
+
+	sprintf(str, "CAL_%s%u_XOFF", SensorString(), calibration_index);
+	param_get(param_find(str), &offset(0));
+
+	sprintf(str, "CAL_%s%u_YOFF", SensorString(), calibration_index);
+	param_get(param_find(str), &offset(1));
+
+	sprintf(str, "CAL_%s%u_ZOFF", SensorString(), calibration_index);
+	param_get(param_find(str), &offset(2));
+
+	return offset;
+}
+
+Vector3f SensorCalibration::CalibrationScale(uint8_t calibration_index) const
+{
+	// scale factors (x, y, z)
+	Vector3f scale{1.f, 1.f, 1.f};
+
+	// gyroscope doesn't have a scale factor calibration
+	if (_type != SensorType::Gyroscope) {
+		char str[20] {};
+
+		sprintf(str, "CAL_%s%u_XSCALE", SensorString(), calibration_index);
+		param_get(param_find(str), &scale(0));
+
+		sprintf(str, "CAL_%s%u_YSCALE", SensorString(), calibration_index);
+		param_get(param_find(str), &scale(1));
+
+		sprintf(str, "CAL_%s%u_ZSCALE", SensorString(), calibration_index);
+		param_get(param_find(str), &scale(2));
+	}
+
+	return scale;
+}
+
+void SensorCalibration::SensorCorrectionsUpdate(bool force)
+{
+	// check if the selected sensor has updated
+	if (_sensor_correction_sub.updated() || force) {
+
+		// valid device id required
+		if (_device_id == 0) {
+			return;
+		}
+
+		sensor_correction_s corrections;
+
+		if (_sensor_correction_sub.copy(&corrections)) {
+			// find sensor_corrections index
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if ((_type == SensorType::Accelerometer) && (corrections.accel_device_ids[i] == _device_id)) {
+					switch (i) {
+					case 0:
+						_thermal_offset = Vector3f{corrections.accel_offset_0};
+						return;
+					case 1:
+						_thermal_offset = Vector3f{corrections.accel_offset_1};
+						return;
+					case 2:
+						_thermal_offset = Vector3f{corrections.accel_offset_2};
+						return;
+					}
+
+				} else if ((_type == SensorType::Gyroscope) && (corrections.gyro_device_ids[i] == _device_id)) {
+					switch (i) {
+					case 0:
+						_thermal_offset = Vector3f{corrections.gyro_offset_0};
+						return;
+					case 1:
+						_thermal_offset = Vector3f{corrections.gyro_offset_1};
+						return;
+					case 2:
+						_thermal_offset = Vector3f{corrections.gyro_offset_2};
+						return;
+					}
+				}
+			}
+		}
+
+		// zero thermal offset if not found
+		_thermal_offset.zero();
+	}
+}
+
+void SensorCalibration::ParametersUpdate()
+{
+	if (!_external) {
+		// fine tune the rotation
+		float x_offset = 0.f;
+		float y_offset = 0.f;
+		float z_offset = 0.f;
+		param_get(param_find("SENS_BOARD_X_OFF"), &x_offset);
+		param_get(param_find("SENS_BOARD_Y_OFF"), &y_offset);
+		param_get(param_find("SENS_BOARD_Z_OFF"), &z_offset);
+
+		const Dcmf board_rotation_offset(Eulerf(radians(x_offset), radians(y_offset), radians(z_offset)));
+
+		// get transformation matrix from sensor/board to body frame
+		int32_t board_rot = 0;
+		param_get(param_find("SENS_BOARD_ROT"), &board_rot);
+		_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rot);
+
+	} else {
+		// TODO: per sensor external rotation
+		_rotation.setIdentity();
+	}
+
+
+	int calibration_index = FindCalibrationIndex(_device_id);
+
+	if (calibration_index >= 0) {
+		_enabled = CalibrationEnabled(calibration_index);
+		_offset = CalibrationOffset(calibration_index);
+		_scale = CalibrationScale(calibration_index);
+
+	} else {
+		_enabled = true;
+		_offset.zero();
+		_scale = Vector3f{1.f, 1.f, 1.f};
+	}
+}
+
+void SensorCalibration::PrintStatus()
+{
+	if (_type != SensorType::Gyroscope) {
+		PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f] scale: [%.4f %.4f %.4f]", SensorString(), _device_id, _enabled,
+			 (double)_offset(0), (double)_offset(1), (double)_offset(2), (double)_scale(0), (double)_scale(1), (double)_scale(2));
+
+	} else {
+		PX4_INFO("%s %d EN: %d, offset: [%.4f %.4f %.4f]", SensorString(), _device_id, _enabled,
+			 (double)_offset(0), (double)_offset(1), (double)_offset(2));
+	}
+
+	if (_thermal_offset.norm() > 0.f) {
+		PX4_INFO("%s %d temperature offset: [%.4f %.4f %.4f]", SensorString(), _device_id,
+			 (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2));
+	}
+}
+
+} // namespace sensors

src/modules/sensors/sensor_calibration/SensorCalibration.hpp

@@ -37,14 +37,13 @@
 #include <lib/matrix/matrix/math.hpp>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/log.h>
-#include <px4_platform_common/module_params.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/sensor_correction.h>
 
 namespace sensors
 {
 
-class SensorCorrections : public ModuleParams
+class SensorCalibration
 {
 public:
 
@@ -53,49 +52,55 @@ public:
 		Gyroscope,
 	};
 
-	SensorCorrections() = delete;
-	SensorCorrections(ModuleParams *parent, SensorType type);
-	~SensorCorrections() override = default;
+	SensorCalibration() = delete;
+	explicit SensorCalibration(SensorType type) : _type(type) {}
+	~SensorCalibration() = default;
 
 	void PrintStatus();
 
 	void set_device_id(uint32_t device_id);
-	uint32_t get_device_id() const { return _device_id; }
+	void set_external(bool external = true) { _external = external; }
+
+	uint32_t device_id() const { return _device_id; }
+	bool enabled() const { return _enabled; }
+	bool external() const { return _external; }
 
 	// apply offsets and scale
 	// rotate corrected measurements from sensor to body frame
-	matrix::Vector3f Correct(const matrix::Vector3f &data) const { return _board_rotation * matrix::Vector3f{(data - _offset).emult(_scale)}; }
+	matrix::Vector3f Correct(const matrix::Vector3f &data);
 
 	void ParametersUpdate();
 	void SensorCorrectionsUpdate(bool force = false);
 
-	const matrix::Dcmf &getBoardRotation() const { return _board_rotation; }
+	const matrix::Dcmf &getBoardRotation() const { return _rotation; }
 
 private:
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
+	int FindCalibrationIndex(uint32_t device_id) const;
+
+	bool CalibrationEnabled(uint8_t calibration_index) const;
+	matrix::Vector3f CalibrationOffset(uint8_t calibration_index) const;
+	matrix::Vector3f CalibrationScale(uint8_t calibration_index) const;
+
 	const char *SensorString() const;
 
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
-	matrix::Dcmf _board_rotation;
+	matrix::Dcmf _rotation;
 
 	matrix::Vector3f _offset{0.f, 0.f, 0.f};
 	matrix::Vector3f _scale{1.f, 1.f, 1.f};
 
+	matrix::Vector3f _thermal_offset{0.f, 0.f, 0.f};
+
 	uint32_t _device_id{0};
-	int8_t _corrections_selected_instance{-1};
 
 	const SensorType _type;
 
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
-
-		(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
-		(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
-		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
-	)
+	bool _enabled{true};
+	bool _external{false};
 };
 
 } // namespace sensors

src/modules/sensors/sensor_corrections/SensorCorrections.cpp

@@ -1,170 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 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 "SensorCorrections.hpp"
-
-using namespace matrix;
-using namespace time_literals;
-using math::radians;
-
-namespace sensors
-{
-
-SensorCorrections::SensorCorrections(ModuleParams *parent, SensorType type) :
-	ModuleParams(parent),
-	_type(type)
-{
-}
-
-void SensorCorrections::set_device_id(uint32_t device_id)
-{
-	if (_device_id != device_id) {
-		_device_id = device_id;
-		SensorCorrectionsUpdate(true);
-	}
-}
-
-const char *SensorCorrections::SensorString() const
-{
-	switch (_type) {
-	case SensorType::Accelerometer:
-		return "ACC";
-
-	case SensorType::Gyroscope:
-		return "GYRO";
-	}
-
-	return nullptr;
-}
-
-void SensorCorrections::SensorCorrectionsUpdate(bool force)
-{
-	// check if the selected sensor has updated
-	if (_sensor_correction_sub.updated() || force) {
-
-		sensor_correction_s corrections;
-
-		if (_sensor_correction_sub.copy(&corrections)) {
-
-			// selected sensor has changed, find updated index
-			if ((_corrections_selected_instance < 0) || force) {
-				_corrections_selected_instance = -1;
-
-				// find sensor_corrections index
-				for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
-
-					switch (_type) {
-					case SensorType::Accelerometer:
-						if (corrections.accel_device_ids[i] == _device_id) {
-							_corrections_selected_instance = i;
-						}
-
-						break;
-
-					case SensorType::Gyroscope:
-						if (corrections.gyro_device_ids[i] == _device_id) {
-							_corrections_selected_instance = i;
-						}
-
-						break;
-					}
-				}
-			}
-
-			switch (_type) {
-			case SensorType::Accelerometer:
-				switch (_corrections_selected_instance) {
-				case 0:
-					_offset = Vector3f{corrections.accel_offset_0};
-					_scale = Vector3f{corrections.accel_scale_0};
-					return;
-				case 1:
-					_offset = Vector3f{corrections.accel_offset_1};
-					_scale = Vector3f{corrections.accel_scale_1};
-					return;
-				case 2:
-					_offset = Vector3f{corrections.accel_offset_2};
-					_scale = Vector3f{corrections.accel_scale_2};
-					return;
-				}
-
-				break;
-
-			case SensorType::Gyroscope:
-				switch (_corrections_selected_instance) {
-				case 0:
-					_offset = Vector3f{corrections.gyro_offset_0};
-					_scale = Vector3f{corrections.gyro_scale_0};
-					return;
-				case 1:
-					_offset = Vector3f{corrections.gyro_offset_1};
-					_scale = Vector3f{corrections.gyro_scale_1};
-					return;
-				case 2:
-					_offset = Vector3f{corrections.gyro_offset_2};
-					_scale = Vector3f{corrections.gyro_scale_2};
-					return;
-				}
-
-				break;
-			}
-		}
-	}
-}
-
-void SensorCorrections::ParametersUpdate()
-{
-	// fine tune the rotation
-	const Dcmf board_rotation_offset(Eulerf(
-			radians(_param_sens_board_x_off.get()),
-			radians(_param_sens_board_y_off.get()),
-			radians(_param_sens_board_z_off.get())));
-
-	// get transformation matrix from sensor/board to body frame
-	_board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
-}
-
-void SensorCorrections::PrintStatus()
-{
-	if (_offset.norm() > 0.f) {
-		PX4_INFO("%s %d offset: [%.3f %.3f %.3f]", SensorString(), _device_id, (double)_offset(0), (double)_offset(1),
-			 (double)_offset(2));
-	}
-
-	if (fabsf(_scale.norm_squared() - 3.f) > FLT_EPSILON) {
-		PX4_INFO("%s %d scale: [%.3f %.3f %.3f]", SensorString(), _device_id, (double)_scale(0), (double)_scale(1),
-			 (double)_scale(2));
-	}
-}
-
-} // namespace sensors

src/modules/sensors/sensors.cpp

@@ -239,6 +239,8 @@ Sensors::~Sensors()
 			i->Stop();
 		}
 	}
+
+	perf_free(_loop_perf);
 }
 
 bool Sensors::init()

src/modules/sensors/vehicle_acceleration/CMakeLists.txt

@@ -41,6 +41,6 @@ target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_link_libraries(vehicle_acceleration
 	PRIVATE
 		mathlib
-		sensor_corrections
+		sensor_calibration
 		px4_work_queue
 )

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -43,8 +43,7 @@ namespace sensors
 
 VehicleAcceleration::VehicleAcceleration() :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers),
-	_corrections(this, SensorCorrections::SensorType::Accelerometer)
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers)
 {
 	_lp_filter.set_cutoff_frequency(kInitialRateHz, _param_imu_accel_cutoff.get());
 }
@@ -182,7 +181,7 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
 						// clear bias and corrections
 						_bias.zero();
 
-						_corrections.set_device_id(report.device_id);
+						_calibration.set_device_id(report.device_id);
 
 						// reset sample interval accumulator on sensor change
 						_timestamp_sample_last = 0;
@@ -212,7 +211,7 @@ void VehicleAcceleration::ParametersUpdate(bool force)
 
 		updateParams();
 
-		_corrections.ParametersUpdate();
+		_calibration.ParametersUpdate();
 	}
 }
 
@@ -224,7 +223,7 @@ void VehicleAcceleration::Run()
 	// update corrections first to set _selected_sensor
 	bool selection_updated = SensorSelectionUpdate();
 
-	_corrections.SensorCorrectionsUpdate(selection_updated);
+	_calibration.SensorCorrectionsUpdate(selection_updated);
 	SensorBiasUpdate(selection_updated);
 	ParametersUpdate();
 
@@ -264,7 +263,7 @@ void VehicleAcceleration::Run()
 
 			if (!sensor_updated) {
 				// correct for in-run bias errors
-				const Vector3f accel = _corrections.Correct(accel_filtered) - _bias;
+				const Vector3f accel = _calibration.Correct(accel_filtered) - _bias;
 
 				// Publish vehicle_acceleration
 				vehicle_acceleration_s v_acceleration;
@@ -283,8 +282,9 @@ void VehicleAcceleration::PrintStatus()
 {
 	PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
 		 _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
-	PX4_INFO("estimated bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
-	_corrections.PrintStatus();
+	PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
+
+	_calibration.PrintStatus();
 }
 
 } // namespace sensors

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include <sensor_corrections/SensorCorrections.hpp>
+#include <sensor_calibration/SensorCalibration.hpp>
 
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
@@ -88,7 +88,7 @@ private:
 		{this, ORB_ID(sensor_accel), 2}
 	};
 
-	SensorCorrections _corrections;
+	SensorCalibration _calibration{SensorCalibration::SensorType::Accelerometer};
 
 	matrix::Vector3f _bias{0.f, 0.f, 0.f};
 

src/modules/sensors/vehicle_air_data/VehicleAirData.cpp

@@ -72,44 +72,36 @@ void VehicleAirData::Stop()
 	}
 }
 
-void VehicleAirData::SensorCorrectionsUpdate()
+void VehicleAirData::SensorCorrectionsUpdate(bool force)
 {
-	// check if the selected sensor has updated
-	sensor_correction_s corrections;
+	if (_sensor_correction_sub.updated() || force) {
+		sensor_correction_s corrections;
 
-	if (_sensor_correction_sub.update(&corrections)) {
-		for (int baro_index = 0; baro_index < MAX_SENSOR_COUNT; baro_index++) {
-			const sensor_baro_s &baro = _last_data[baro_index];
+		if (_sensor_correction_sub.copy(&corrections)) {
+			for (int baro_index = 0; baro_index < MAX_SENSOR_COUNT; baro_index++) {
+				// find sensor (by device id) in sensor_correction
+				const uint32_t device_id = _last_data[baro_index].device_id;
 
-			// find sensor (by device id) in sensor_correction
-			if (_sensor_correction_index[baro_index] < 0) {
-				for (int correction_index = 0; correction_index < MAX_SENSOR_COUNT; correction_index++) {
-					if ((baro.device_id > 0) && (corrections.baro_device_ids[correction_index] == baro.device_id)) {
-						_sensor_correction_index[baro_index] = correction_index;
+				if (device_id != 0) {
+					for (int correction_index = 0; correction_index < MAX_SENSOR_COUNT; correction_index++) {
+						if (corrections.baro_device_ids[correction_index] == device_id) {
+							switch (correction_index) {
+							case 0:
+								_thermal_offset[baro_index] = corrections.baro_offset_0;
+								break;
+
+							case 1:
+								_thermal_offset[baro_index] = corrections.baro_offset_1;
+								break;
+
+							case 2:
+								_thermal_offset[baro_index] = corrections.baro_offset_2;
+								break;
+							}
+						}
 					}
 				}
 			}
-
-			switch (_sensor_correction_index[baro_index]) {
-			case 0:
-				_offset[baro_index] = corrections.baro_offset_0;
-				_scale[baro_index] = corrections.baro_scale_0;
-				break;
-
-			case 1:
-				_offset[baro_index] = corrections.baro_offset_1;
-				_scale[baro_index] = corrections.baro_scale_1;
-				break;
-
-			case 2:
-				_offset[baro_index] = corrections.baro_offset_2;
-				_scale[baro_index] = corrections.baro_scale_2;
-				break;
-
-			default:
-				_offset[baro_index] = 0.f;
-				_scale[baro_index] = 1.f;
-			}
 		}
 	}
 }
@@ -130,6 +122,8 @@ void VehicleAirData::Run()
 {
 	perf_begin(_cycle_perf);
 
+	SensorCorrectionsUpdate();
+
 	bool updated[MAX_SENSOR_COUNT] {};
 
 	for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
@@ -147,6 +141,9 @@ void VehicleAirData::Run()
 
 					_advertised[uorb_index] = true;
 
+					// force temperature correction update
+					SensorCorrectionsUpdate(true);
+
 				} else {
 					_last_data[uorb_index].timestamp = hrt_absolute_time();
 				}
@@ -164,8 +161,8 @@ void VehicleAirData::Run()
 				// millibar to Pa
 				const float raw_pressure_pascals = _last_data[uorb_index].pressure * 100.f;
 
-				// pressure corrected with offset and scale (if available)
-				const float pressure_corrected = (raw_pressure_pascals - _offset[uorb_index]) * _scale[uorb_index];
+				// pressure corrected with offset (if available)
+				const float pressure_corrected = (raw_pressure_pascals - _thermal_offset[uorb_index]);
 
 				float vect[3] {pressure_corrected, _last_data[uorb_index].temperature, 0.f};
 				_voter.put(uorb_index, _last_data[uorb_index].timestamp, vect, _last_data[uorb_index].error_count,
@@ -192,7 +189,6 @@ void VehicleAirData::Run()
 
 	if ((_selected_sensor_sub_index >= 0) && updated[_selected_sensor_sub_index]) {
 		ParametersUpdate();
-		SensorCorrectionsUpdate();
 
 		const sensor_baro_s &baro = _last_data[_selected_sensor_sub_index];
 
@@ -203,8 +199,7 @@ void VehicleAirData::Run()
 		out.baro_temp_celcius = baro.temperature;
 
 		// Convert from millibar to Pa and apply temperature compensation
-		out.baro_pressure_pa = (100.0f * baro.pressure - _offset[_selected_sensor_sub_index]) *
-				       _scale[_selected_sensor_sub_index];
+		out.baro_pressure_pa = 100.0f * baro.pressure - _thermal_offset[_selected_sensor_sub_index];
 
 		// calculate altitude using the hypsometric equation
 		static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin
@@ -281,6 +276,11 @@ void VehicleAirData::PrintStatus()
 {
 	if (_selected_sensor_sub_index >= 0) {
 		PX4_INFO("selected barometer: %d (%d)", _last_data[_selected_sensor_sub_index].device_id, _selected_sensor_sub_index);
+
+		if (fabsf(_thermal_offset[_selected_sensor_sub_index]) > 0.f) {
+			PX4_INFO("%d temperature offset: %.4f", _last_data[_selected_sensor_sub_index].device_id,
+				 (double)_thermal_offset[_selected_sensor_sub_index]);
+		}
 	}
 
 	_voter.print();

src/modules/sensors/vehicle_air_data/VehicleAirData.hpp

@@ -66,7 +66,7 @@ private:
 	void Run() override;
 
 	void ParametersUpdate();
-	void SensorCorrectionsUpdate();
+	void SensorCorrectionsUpdate(bool force = false);
 
 	static constexpr int MAX_SENSOR_COUNT = 3;
 
@@ -96,10 +96,8 @@ private:
 	sensor_baro_s _last_data[MAX_SENSOR_COUNT] {};
 	bool _advertised[MAX_SENSOR_COUNT] {};
 
-	float _offset[MAX_SENSOR_COUNT] {0.f, 0.f, 0.f};
-	float _scale[MAX_SENSOR_COUNT] {1.f, 1.f, 1.f};
+	float _thermal_offset[MAX_SENSOR_COUNT] {0.f, 0.f, 0.f};
 
-	int8_t _sensor_correction_index[MAX_SENSOR_COUNT] {-1, -1, -1};
 	uint8_t _priority[MAX_SENSOR_COUNT] {};
 
 	int8_t _selected_sensor_sub_index{-1};

src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt

@@ -41,6 +41,6 @@ target_compile_options(vehicle_angular_velocity PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
 target_link_libraries(vehicle_angular_velocity
 	PRIVATE
 		mathlib
-		sensor_corrections
+		sensor_calibration
 		px4_work_queue
 )

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -43,8 +43,7 @@ namespace sensors
 
 VehicleAngularVelocity::VehicleAngularVelocity() :
 	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
-	_corrections(this, SensorCorrections::SensorType::Gyroscope)
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
 	_lp_filter_velocity.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get());
 	_notch_filter_velocity.setParameters(kInitialRateHz, _param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
@@ -202,7 +201,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 						// clear bias and corrections
 						_bias.zero();
 
-						_corrections.set_device_id(report.device_id);
+						_calibration.set_device_id(report.device_id);
 
 						// reset sample interval accumulator on sensor change
 						_timestamp_sample_last = 0;
@@ -232,7 +231,7 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 
 		updateParams();
 
-		_corrections.ParametersUpdate();
+		_calibration.ParametersUpdate();
 	}
 }
 
@@ -244,7 +243,7 @@ void VehicleAngularVelocity::Run()
 	// update corrections first to set _selected_sensor
 	bool selection_updated = SensorSelectionUpdate();
 
-	_corrections.SensorCorrectionsUpdate(selection_updated);
+	_calibration.SensorCorrectionsUpdate(selection_updated);
 	SensorBiasUpdate(selection_updated);
 	ParametersUpdate();
 
@@ -280,7 +279,7 @@ void VehicleAngularVelocity::Run()
 			const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
 
 			// correct for in-run bias errors
-			const Vector3f angular_velocity_raw = _corrections.Correct(val) - _bias;
+			const Vector3f angular_velocity_raw = _calibration.Correct(val) - _bias;
 
 			// Gyro filtering:
 			// - Apply general notch filter (IMU_GYRO_NF_FREQ)
@@ -339,8 +338,8 @@ void VehicleAngularVelocity::PrintStatus()
 {
 	PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
 		 _selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
-	PX4_INFO("estimated bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
-	_corrections.PrintStatus();
+	PX4_INFO("estimated bias: [%.4f %.4f %.4f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
+	_calibration.PrintStatus();
 }
 
 } // namespace sensors

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include <sensor_corrections/SensorCorrections.hpp>
+#include <sensor_calibration/SensorCalibration.hpp>
 
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
@@ -91,7 +91,7 @@ private:
 		{this, ORB_ID(sensor_gyro), 2}
 	};
 
-	SensorCorrections _corrections;
+	SensorCalibration _calibration{SensorCalibration::SensorType::Gyroscope};
 
 	matrix::Vector3f _bias{0.f, 0.f, 0.f};
 

src/modules/sensors/vehicle_imu/CMakeLists.txt

@@ -38,4 +38,4 @@ px4_add_library(vehicle_imu
 	VehicleIMU.hpp
 )
 target_compile_options(vehicle_imu PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_link_libraries(vehicle_imu PRIVATE sensor_corrections px4_work_queue)
+target_link_libraries(vehicle_imu PRIVATE sensor_calibration px4_work_queue)

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -49,9 +49,7 @@ VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::navigation_and_controllers),
 	_sensor_accel_sub(this, ORB_ID(sensor_accel), accel_index),
-	_sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index),
-	_accel_corrections(this, SensorCorrections::SensorType::Accelerometer),
-	_gyro_corrections(this, SensorCorrections::SensorType::Gyroscope)
+	_sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index)
 {
 	const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
 
@@ -107,8 +105,8 @@ void VehicleIMU::ParametersUpdate(bool force)
 
 		updateParams();
 
-		_accel_corrections.ParametersUpdate();
-		_gyro_corrections.ParametersUpdate();
+		_accel_calibration.ParametersUpdate();
+		_gyro_calibration.ParametersUpdate();
 
 		// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
 		int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), 50, 1000);
@@ -191,7 +189,7 @@ void VehicleIMU::Run()
 
 		_gyro_last_generation = _sensor_gyro_sub.get_last_generation();
 
-		_gyro_corrections.set_device_id(gyro.device_id);
+		_gyro_calibration.set_device_id(gyro.device_id);
 
 		if (gyro.error_count != _status.gyro_error_count) {
 			publish_status = true;
@@ -229,7 +227,7 @@ void VehicleIMU::Run()
 
 		_accel_last_generation = _sensor_accel_sub.get_last_generation();
 
-		_accel_corrections.set_device_id(accel.device_id);
+		_accel_calibration.set_device_id(accel.device_id);
 
 		if (accel.error_count != _status.accel_error_count) {
 			publish_status = true;
@@ -249,7 +247,7 @@ void VehicleIMU::Run()
 		if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) {
 
 			// rotate sensor clip counts into vehicle body frame
-			const Vector3f clipping{_accel_corrections.getBoardRotation() *
+			const Vector3f clipping{_accel_calibration.getBoardRotation() *
 				Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}};
 
 			// round to get reasonble clip counts per axis (after board rotation)
@@ -301,14 +299,14 @@ void VehicleIMU::Run()
 		    && _gyro_integrator.reset(delta_angle, gyro_integral_dt)) {
 
 			// delta angle: apply offsets, scale, and board rotation
-			_gyro_corrections.SensorCorrectionsUpdate();
-			const float gyro_dt = 1.e-6f * gyro_integral_dt;
-			const Vector3f delta_angle_corrected{_gyro_corrections.Correct(delta_angle * gyro_dt) / gyro_dt};
+			_gyro_calibration.SensorCorrectionsUpdate();
+			const float gyro_dt_inv = 1.e6f / gyro_integral_dt;
+			const Vector3f delta_angle_corrected{_gyro_calibration.Correct(delta_angle * gyro_dt_inv) / gyro_dt_inv};
 
 			// delta velocity: apply offsets, scale, and board rotation
-			_accel_corrections.SensorCorrectionsUpdate();
-			const float accel_dt = 1.e-6f * accel_integral_dt;
-			Vector3f delta_velocity_corrected{_accel_corrections.Correct(delta_velocity * accel_dt) / accel_dt};
+			_accel_calibration.SensorCorrectionsUpdate();
+			const float accel_dt_inv = 1.e6f / accel_integral_dt;
+			Vector3f delta_velocity_corrected{_accel_calibration.Correct(delta_velocity * accel_dt_inv) / accel_dt_inv};
 
 			UpdateAccelVibrationMetrics(delta_velocity_corrected);
 			UpdateGyroVibrationMetrics(delta_angle_corrected);
@@ -316,8 +314,8 @@ void VehicleIMU::Run()
 			// vehicle_imu_status
 			//  publish before vehicle_imu so that error counts are available synchronously if needed
 			if (publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms)) {
-				_status.accel_device_id = _accel_corrections.get_device_id();
-				_status.gyro_device_id = _gyro_corrections.get_device_id();
+				_status.accel_device_id = _accel_calibration.device_id();
+				_status.gyro_device_id = _gyro_calibration.device_id();
 				_status.timestamp = hrt_absolute_time();
 				_vehicle_imu_status_pub.publish(_status);
 			}
@@ -326,8 +324,8 @@ void VehicleIMU::Run()
 			// publish vehicle_imu
 			vehicle_imu_s imu;
 			imu.timestamp_sample = _last_timestamp_sample_gyro;
-			imu.accel_device_id = _accel_corrections.get_device_id();
-			imu.gyro_device_id = _gyro_corrections.get_device_id();
+			imu.accel_device_id = _accel_calibration.device_id();
+			imu.gyro_device_id = _gyro_calibration.device_id();
 			delta_angle_corrected.copyTo(imu.delta_angle);
 			delta_velocity_corrected.copyTo(imu.delta_velocity);
 			imu.delta_angle_dt = gyro_integral_dt;
@@ -376,7 +374,7 @@ void VehicleIMU::UpdateIntegratorConfiguration()
 		_intervals_update = false; // stop monitoring topic publication rate
 
 		PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d, accel interval: %.1f, gyro interval: %.1f",
-			  _accel_corrections.get_device_id(), _gyro_corrections.get_device_id(), accel_integral_samples, gyro_integral_samples,
+			  _accel_calibration.device_id(), _gyro_calibration.device_id(), accel_integral_samples, gyro_integral_samples,
 			  (double)_accel_interval.update_interval, (double)_gyro_interval.update_interval);
 	}
 }
@@ -406,16 +404,16 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &delta_angle)
 void VehicleIMU::PrintStatus()
 {
 	PX4_INFO("Accel ID: %d, interval: %.1f us, Gyro ID: %d, interval: %.1f us",
-		 _accel_corrections.get_device_id(), (double)_accel_interval.update_interval,
-		 _gyro_corrections.get_device_id(), (double)_gyro_interval.update_interval);
+		 _accel_calibration.device_id(), (double)_accel_interval.update_interval,
+		 _gyro_calibration.device_id(), (double)_gyro_interval.update_interval);
 
 	perf_print_counter(_accel_generation_gap_perf);
 	perf_print_counter(_gyro_generation_gap_perf);
 	perf_print_counter(_accel_update_perf);
 	perf_print_counter(_gyro_update_perf);
 
-	_accel_corrections.PrintStatus();
-	_gyro_corrections.PrintStatus();
+	_accel_calibration.PrintStatus();
+	_gyro_calibration.PrintStatus();
 }
 
 } // namespace sensors

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -35,7 +35,7 @@
 
 #include "Integrator.hpp"
 
-#include <sensor_corrections/SensorCorrections.hpp>
+#include <sensor_calibration/SensorCalibration.hpp>
 
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
@@ -91,8 +91,8 @@ private:
 	uORB::SubscriptionCallbackWorkItem _sensor_accel_sub;
 	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub;
 
-	SensorCorrections _accel_corrections;
-	SensorCorrections _gyro_corrections;
+	SensorCalibration _accel_calibration{SensorCalibration::SensorType::Accelerometer};
+	SensorCalibration _gyro_calibration{SensorCalibration::SensorType::Gyroscope};
 
 	Integrator _accel_integrator{}; // 200 Hz default
 	Integrator _gyro_integrator{true};   // 200 Hz default, coning compensation enabled

src/modules/sensors/voted_sensors_update.cpp

@@ -39,11 +39,12 @@
 
 #include "voted_sensors_update.h"
 
-#include <systemlib/mavlink_log.h>
-
+#include <lib/conversion/rotation.h>
+#include <lib/ecl/geo/geo.h>
+#include <lib/systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
-#include <conversion/rotation.h>
-#include <ecl/geo/geo.h>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_gyro.h>
 
 #define MAG_ROT_VAL_INTERNAL		-1
 #define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
@@ -91,6 +92,8 @@ void VotedSensorsUpdate::initializeSensors()
 
 void VotedSensorsUpdate::parametersUpdate()
 {
+	updateParams();
+
 	/* fine tune board offset */
 	const Dcmf board_rotation_offset{Eulerf{radians(_parameters.board_offset[0]), radians(_parameters.board_offset[1]), radians(_parameters.board_offset[2])}};
 
@@ -101,211 +104,44 @@ void VotedSensorsUpdate::parametersUpdate()
 		_mag_rotation[topic_instance] = _board_rotation;
 	}
 
-	updateParams();
-
 	/* set offset parameters to new values */
 	bool failed = false;
 
-	/* run through all gyro sensors */
-	unsigned gyro_count = 0;
-	unsigned gyro_cal_found_count = 0;
+	// run through all IMUs
+	for (uint8_t uorb_index = 0; uorb_index < math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX); uorb_index++) {
+		uORB::SubscriptionData<vehicle_imu_s> imu{ORB_ID(vehicle_imu), uorb_index};
+		imu.update();
 
-	for (unsigned driver_index = 0; driver_index < GYRO_COUNT_MAX; driver_index++) {
-		_gyro.enabled[driver_index] = true;
+		if (imu.get().timestamp > 0 && imu.get().accel_device_id > 0 && imu.get().gyro_device_id > 0) {
 
-		char str[30] {};
-		(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, driver_index);
+			if (_accel.priority[uorb_index] == ORB_PRIO_UNINITIALIZED) {
+				// find corresponding sensor_accel publication
+				for (uint8_t i = 0; i < ACCEL_COUNT_MAX; i++) {
+					uORB::SubscriptionData<sensor_accel_s> sensor_accel{ORB_ID(sensor_accel), i};
+					sensor_accel.update();
 
-		int fd = px4_open(str, O_RDWR);
-
-		if (fd < 0) {
-			continue;
-		}
-
-		uint32_t driver_device_id = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-		bool config_ok = false;
-
-		/* run through all stored calibrations that are applied at the driver level*/
-		for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) {
-			/* initially status is ok per config */
-			failed = false;
-
-			(void)sprintf(str, "CAL_GYRO%u_ID", i);
-			int32_t device_id = 0;
-			failed = failed || (PX4_OK != param_get(param_find(str), &device_id));
-
-			(void)sprintf(str, "CAL_GYRO%u_EN", i);
-			int32_t device_enabled = 1;
-			failed = failed || (PX4_OK != param_get(param_find(str), &device_enabled));
-
-			if (failed) {
-				continue;
-			}
-
-			if (driver_index == 0 && device_id > 0) {
-				gyro_cal_found_count++;
-			}
-
-			/* if the calibration is for this device, apply it */
-			if ((uint32_t)device_id == driver_device_id) {
-				_gyro.enabled[driver_index] = (device_enabled == 1);
-
-				if (!_gyro.enabled[driver_index]) {
-					_gyro.priority[driver_index] = ORB_PRIO_UNINITIALIZED;
-				}
-
-				gyro_calibration_s gscale{};
-
-				(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &gscale.x_offset));
-
-				(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &gscale.y_offset));
-
-				(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &gscale.z_offset));
-
-				if (failed) {
-					PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
-
-				} else {
-					/* apply new scaling and offsets */
-					config_ok = (px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale) == 0);
-
-					if (!config_ok) {
-						PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i);
+					if (imu.get().accel_device_id == sensor_accel.get().device_id) {
+						_accel.priority[uorb_index] = sensor_accel.get_priority();
+						break;
 					}
 				}
-
-				break;
-			}
-		}
-
-		if (config_ok) {
-			gyro_count++;
-		}
-
-		px4_close(fd);
-	}
-
-	// There are fewer gyros than calibrations
-	// reset the board calibration and fail the initial load
-	if (gyro_count < gyro_cal_found_count) {
-		PX4_ERR("fewer accels than calibrations, resetting all CAL_GYROx_ID");
-
-		// run through all stored calibrations and reset them
-		for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) {
-			char str[30] {};
-			(void)sprintf(str, "CAL_GYRO%u_ID", i);
-			int32_t device_id = 0;
-			(void)param_set(param_find(str), &device_id);
-		}
-	}
-
-	/* run through all accel sensors */
-	unsigned accel_count = 0;
-	unsigned accel_cal_found_count = 0;
-
-	for (unsigned driver_index = 0; driver_index < ACCEL_COUNT_MAX; driver_index++) {
-		_accel.enabled[driver_index] = true;
-
-		char str[30] {};
-		(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, driver_index);
-
-		int fd = px4_open(str, O_RDWR);
-
-		if (fd < 0) {
-			continue;
-		}
-
-		uint32_t driver_device_id = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-		bool config_ok = false;
-
-		/* run through all stored calibrations */
-		for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) {
-			/* initially status is ok per config */
-			failed = false;
-
-			(void)sprintf(str, "CAL_ACC%u_ID", i);
-			int32_t device_id = 0;
-			failed = failed || (PX4_OK != param_get(param_find(str), &device_id));
-
-			(void)sprintf(str, "CAL_ACC%u_EN", i);
-			int32_t device_enabled = 1;
-			failed = failed || (PX4_OK != param_get(param_find(str), &device_enabled));
-
-			if (failed) {
-				continue;
 			}
 
-			if (driver_index == 0 && device_id > 0) {
-				accel_cal_found_count++;
-			}
+			if (_gyro.priority[uorb_index] == ORB_PRIO_UNINITIALIZED) {
+				// find corresponding sensor_gyro publication
+				for (uint8_t i = 0; i < GYRO_COUNT_MAX; i++) {
+					uORB::SubscriptionData<sensor_accel_s> sensor_gyro{ORB_ID(sensor_gyro), i};
+					sensor_gyro.update();
 
-			/* if the calibration is for this device, apply it */
-			if ((uint32_t)device_id == driver_device_id) {
-				_accel.enabled[driver_index] = (device_enabled == 1);
-
-				if (!_accel.enabled[driver_index]) {
-					_accel.priority[driver_index] = ORB_PRIO_UNINITIALIZED;
-				}
-
-				accel_calibration_s ascale{};
-
-				(void)sprintf(str, "CAL_ACC%u_XOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &ascale.x_offset));
-
-				(void)sprintf(str, "CAL_ACC%u_YOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &ascale.y_offset));
-
-				(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &ascale.z_offset));
-
-				(void)sprintf(str, "CAL_ACC%u_XSCALE", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &ascale.x_scale));
-
-				(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &ascale.y_scale));
-
-				(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
-				failed = failed || (PX4_OK != param_get(param_find(str), &ascale.z_scale));
-
-				if (failed) {
-					PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
-
-				} else {
-					/* apply new scaling and offsets */
-					config_ok = (px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale) == 0);
-
-					if (!config_ok) {
-						PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel ", i);
+					if (imu.get().gyro_device_id == sensor_gyro.get().device_id) {
+						_gyro.priority[uorb_index] = sensor_gyro.get_priority();
+						break;
 					}
 				}
-
-				break;
 			}
 		}
-
-		if (config_ok) {
-			accel_count++;
-		}
-
-		px4_close(fd);
 	}
 
-	// There are fewer accels than calibrations
-	// reset the board calibration and fail the initial load
-	if (accel_count < accel_cal_found_count) {
-		PX4_ERR("fewer accels than calibrations, resetting all CAL_ACCx_ID");
-
-		// run through all stored calibrations and reset them
-		for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) {
-			char str[30] {};
-			(void)sprintf(str, "CAL_ACC%u_ID", i);
-			int32_t device_id = 0;
-			(void)param_set(param_find(str), &device_id);
-		}
-	}
 
 	/* run through all mag sensors
 	 * Because we store the device id in _mag_device_id, we need to get the id via uorb topic since

src/modules/sensors/voted_sensors_update.h

@@ -41,8 +41,6 @@
 
 #include "parameters.h"
 
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
 

src/modules/simulator/simulator.cpp

@@ -66,17 +66,8 @@ void Simulator::parameters_update(bool force)
 
 void Simulator::print_status()
 {
-	PX4_INFO("accelerometer");
-	_px4_accel.print_status();
-
-	PX4_INFO("gyroscope");
-	_px4_gyro.print_status();
-
 	PX4_INFO("magnetometer");
 	_px4_mag.print_status();
-
-	PX4_INFO("barometer");
-	_px4_baro.print_status();
 }
 
 int Simulator::start(int argc, char *argv[])

src/modules/temperature_compensation/TemperatureCompensation.cpp

@@ -71,8 +71,6 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
 				parameter_handles.gyro_cal_handles[j].x1[i] = param_find(nbuf);
 				sprintf(nbuf, "TC_G%d_X0_%d", j, i);
 				parameter_handles.gyro_cal_handles[j].x0[i] = param_find(nbuf);
-				sprintf(nbuf, "TC_G%d_SCL_%d", j, i);
-				parameter_handles.gyro_cal_handles[j].scale[i] = param_find(nbuf);
 			}
 
 			sprintf(nbuf, "TC_G%d_TREF", j);
@@ -103,8 +101,6 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
 				parameter_handles.accel_cal_handles[j].x1[i] = param_find(nbuf);
 				sprintf(nbuf, "TC_A%d_X0_%d", j, i);
 				parameter_handles.accel_cal_handles[j].x0[i] = param_find(nbuf);
-				sprintf(nbuf, "TC_A%d_SCL_%d", j, i);
-				parameter_handles.accel_cal_handles[j].scale[i] = param_find(nbuf);
 			}
 
 			sprintf(nbuf, "TC_A%d_TREF", j);
@@ -137,8 +133,6 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
 			parameter_handles.baro_cal_handles[j].x1 = param_find(nbuf);
 			sprintf(nbuf, "TC_B%d_X0", j);
 			parameter_handles.baro_cal_handles[j].x0 = param_find(nbuf);
-			sprintf(nbuf, "TC_B%d_SCL", j);
-			parameter_handles.baro_cal_handles[j].scale = param_find(nbuf);
 			sprintf(nbuf, "TC_B%d_TREF", j);
 			parameter_handles.baro_cal_handles[j].ref_temp = param_find(nbuf);
 			sprintf(nbuf, "TC_B%d_TMIN", j);
@@ -175,18 +169,12 @@ int TemperatureCompensation::parameters_update()
 					param_get(parameter_handles.gyro_cal_handles[j].x2[i], &(_parameters.gyro_cal_data[j].x2[i]));
 					param_get(parameter_handles.gyro_cal_handles[j].x1[i], &(_parameters.gyro_cal_data[j].x1[i]));
 					param_get(parameter_handles.gyro_cal_handles[j].x0[i], &(_parameters.gyro_cal_data[j].x0[i]));
-					param_get(parameter_handles.gyro_cal_handles[j].scale[i], &(_parameters.gyro_cal_data[j].scale[i]));
 				}
 
 			} else {
-				// Set all cal values to zero and scale factor to unity
+				// Set all cal values to zero
 				memset(&_parameters.gyro_cal_data[j], 0, sizeof(_parameters.gyro_cal_data[j]));
 
-				// Set the scale factor to unity
-				for (unsigned int i = 0; i < 3; i++) {
-					_parameters.gyro_cal_data[j].scale[i] = 1.0f;
-				}
-
 				PX4_WARN("FAIL GYRO %d CAL PARAM LOAD - USING DEFAULTS", j);
 				ret = PX4_ERROR;
 			}
@@ -208,18 +196,12 @@ int TemperatureCompensation::parameters_update()
 					param_get(parameter_handles.accel_cal_handles[j].x2[i], &(_parameters.accel_cal_data[j].x2[i]));
 					param_get(parameter_handles.accel_cal_handles[j].x1[i], &(_parameters.accel_cal_data[j].x1[i]));
 					param_get(parameter_handles.accel_cal_handles[j].x0[i], &(_parameters.accel_cal_data[j].x0[i]));
-					param_get(parameter_handles.accel_cal_handles[j].scale[i], &(_parameters.accel_cal_data[j].scale[i]));
 				}
 
 			} else {
-				// Set all cal values to zero and scale factor to unity
+				// Set all cal values to zero
 				memset(&_parameters.accel_cal_data[j], 0, sizeof(_parameters.accel_cal_data[j]));
 
-				// Set the scale factor to unity
-				for (unsigned int i = 0; i < 3; i++) {
-					_parameters.accel_cal_data[j].scale[i] = 1.0f;
-				}
-
 				PX4_WARN("FAIL ACCEL %d CAL PARAM LOAD - USING DEFAULTS", j);
 				ret = PX4_ERROR;
 			}
@@ -241,22 +223,18 @@ int TemperatureCompensation::parameters_update()
 				param_get(parameter_handles.baro_cal_handles[j].x2, &(_parameters.baro_cal_data[j].x2));
 				param_get(parameter_handles.baro_cal_handles[j].x1, &(_parameters.baro_cal_data[j].x1));
 				param_get(parameter_handles.baro_cal_handles[j].x0, &(_parameters.baro_cal_data[j].x0));
-				param_get(parameter_handles.baro_cal_handles[j].scale, &(_parameters.baro_cal_data[j].scale));
 
 			} else {
-				// Set all cal values to zero and scale factor to unity
+				// Set all cal values to zero
 				memset(&_parameters.baro_cal_data[j], 0, sizeof(_parameters.baro_cal_data[j]));
 
-				// Set the scale factor to unity
-				_parameters.baro_cal_data[j].scale = 1.0f;
-
 				PX4_WARN("FAIL BARO %d CAL PARAM LOAD - USING DEFAULTS", j);
 				ret = PX4_ERROR;
 			}
 		}
 	}
 
-	/* the offsets & scales might have changed, so make sure to report that change later when applying the
+	/* the offsets might have changed, so make sure to report that change later when applying the
 	 * next corrections
 	 */
 	_gyro_data.reset_temperature();
@@ -374,8 +352,7 @@ int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instanc
 	return -1;
 }
 
-int TemperatureCompensation::update_scales_and_offsets_gyro(int topic_instance, float temperature, float *offsets,
-		float *scales)
+int TemperatureCompensation::update_offsets_gyro(int topic_instance, float temperature, float *offsets)
 {
 	// Check if temperature compensation is enabled
 	if (_parameters.gyro_tc_enable != 1) {
@@ -392,11 +369,6 @@ int TemperatureCompensation::update_scales_and_offsets_gyro(int topic_instance,
 	// Calculate and update the offsets
 	calc_thermal_offsets_3D(_parameters.gyro_cal_data[mapping], temperature, offsets);
 
-	// Update the scales
-	for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
-		scales[axis_index] = _parameters.gyro_cal_data[mapping].scale[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;
@@ -406,8 +378,7 @@ int TemperatureCompensation::update_scales_and_offsets_gyro(int topic_instance,
 	return 1;
 }
 
-int TemperatureCompensation::update_scales_and_offsets_accel(int topic_instance, float temperature, float *offsets,
-		float *scales)
+int TemperatureCompensation::update_offsets_accel(int topic_instance, float temperature, float *offsets)
 {
 	// Check if temperature compensation is enabled
 	if (_parameters.accel_tc_enable != 1) {
@@ -424,11 +395,6 @@ int TemperatureCompensation::update_scales_and_offsets_accel(int topic_instance,
 	// Calculate and update the offsets
 	calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets);
 
-	// Update the scales
-	for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
-		scales[axis_index] = _parameters.accel_cal_data[mapping].scale[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;
@@ -438,8 +404,7 @@ int TemperatureCompensation::update_scales_and_offsets_accel(int topic_instance,
 	return 1;
 }
 
-int TemperatureCompensation::update_scales_and_offsets_baro(int topic_instance, float temperature, float *offsets,
-		float *scales)
+int TemperatureCompensation::update_offsets_baro(int topic_instance, float temperature, float *offsets)
 {
 	// Check if temperature compensation is enabled
 	if (_parameters.baro_tc_enable != 1) {
@@ -456,9 +421,6 @@ int TemperatureCompensation::update_scales_and_offsets_baro(int topic_instance,
 	// Calculate and update the offsets
 	calc_thermal_offsets_1D(_parameters.baro_cal_data[mapping], temperature, *offsets);
 
-	// Update the scales
-	*scales = _parameters.baro_cal_data[mapping].scale;
-
 	// 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;

src/modules/temperature_compensation/TemperatureCompensation.h

@@ -84,15 +84,14 @@ public:
 	 * @param sensor_data input sensor data, output sensor data with applied corrections
 	 * @param temperature measured current temperature
 	 * @param offsets returns offsets that were applied (length = 3, except for baro), depending on return value
-	 * @param scales returns scales that were applied (length = 3), depending on return value
 	 * @return -1: error: correction enabled, but no sensor mapping set (@see set_sendor_id_gyro)
 	 *         0: no changes (correction not enabled),
-	 *         1: corrections applied but no changes to offsets & scales,
-	 *         2: corrections applied and offsets & scales updated
+	 *         1: corrections applied but no changes to offsets,
+	 *         2: corrections applied and offsets updated
 	 */
-	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);
+	int update_offsets_gyro(int topic_instance, float temperature, float *offsets);
+	int update_offsets_accel(int topic_instance, float temperature, float *offsets);
+	int update_offsets_baro(int topic_instance, float temperature, float *offsets);
 
 	/** output current configuration status to console */
 	void print_status();
@@ -110,7 +109,7 @@ private:
 
 	delta_temp = measured_temp - ref_temp
 	offset = x5 * delta_temp^5 + x4 * delta_temp^4 + x3 * delta_temp^3 + x2 * delta_temp^2 + x1 * delta_temp + x0
-	corrected_value = (raw_value - offset) * scale
+	corrected_value = raw_value - offset
 
 	*/
 	struct SensorCalData1D {
@@ -121,7 +120,6 @@ private:
 		float x2;
 		float x1;
 		float x0;
-		float scale;
 		float ref_temp;
 		float min_temp;
 		float max_temp;
@@ -135,7 +133,6 @@ private:
 		param_t x2;
 		param_t x1;
 		param_t x0;
-		param_t scale;
 		param_t ref_temp;
 		param_t min_temp;
 		param_t max_temp;
@@ -154,7 +151,7 @@ private:
 
 	delta_temp = measured_temp - ref_temp
 	offset = x3 * delta_temp^3 + x2 * delta_temp^2 + x1 * delta_temp + x0
-	corrected_value = (raw_value - offset) * scale
+	corrected_value = raw_value - offset
 
 	 */
 	struct SensorCalData3D {
@@ -163,7 +160,6 @@ private:
 		float x2[3];		/**< x^2 term of polynomial */
 		float x1[3];		/**< x^1 term of polynomial */
 		float x0[3];		/**< x^0 / offset term of polynomial */
-		float scale[3];		/**< scale factor correction */
 		float ref_temp;		/**< reference temperature used by the curve-fit */
 		float min_temp;		/**< minimum temperature with valid compensation data */
 		float max_temp;		/**< maximum temperature with valid compensation data */
@@ -175,7 +171,6 @@ private:
 		param_t x2[3];
 		param_t x1[3];
 		param_t x0[3];
-		param_t scale[3];
 		param_t ref_temp;
 		param_t min_temp;
 		param_t max_temp;

src/modules/temperature_compensation/TemperatureCompensationModule.cpp

@@ -48,19 +48,6 @@ TemperatureCompensationModule::TemperatureCompensationModule() :
 	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()
@@ -81,11 +68,9 @@ void TemperatureCompensationModule::parameters_update()
 
 			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;
 			}
 		}
@@ -101,11 +86,9 @@ void TemperatureCompensationModule::parameters_update()
 			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;
 			}
 		}
@@ -120,13 +103,10 @@ void TemperatureCompensationModule::parameters_update()
 
 			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;
-
 			}
 		}
 	}
@@ -135,7 +115,6 @@ void TemperatureCompensationModule::parameters_update()
 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++) {
@@ -143,13 +122,13 @@ void TemperatureCompensationModule::accelPoll()
 
 		// Grab temperature from report
 		if (_accel_subs[uorb_index].update(&report)) {
+			if (PX4_ISFINITE(report.temperature)) {
+				// Update the offsets and mark for publication if they've changed
+				if (_temperature_compensation.update_offsets_accel(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
 
-			// 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;
+					_corrections.accel_device_ids[uorb_index] = report.device_id;
+					_corrections_changed = true;
+				}
 			}
 		}
 	}
@@ -158,7 +137,6 @@ void TemperatureCompensationModule::accelPoll()
 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++) {
@@ -166,13 +144,13 @@ void TemperatureCompensationModule::gyroPoll()
 
 		// Grab temperature from report
 		if (_gyro_subs[uorb_index].update(&report)) {
+			if (PX4_ISFINITE(report.temperature)) {
+				// Update the offsets and mark for publication if they've changed
+				if (_temperature_compensation.update_offsets_gyro(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
 
-			// 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;
+					_corrections.gyro_device_ids[uorb_index] = report.device_id;
+					_corrections_changed = true;
+				}
 			}
 		}
 	}
@@ -181,7 +159,6 @@ void TemperatureCompensationModule::gyroPoll()
 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++) {
@@ -189,13 +166,13 @@ void TemperatureCompensationModule::baroPoll()
 
 		// Grab temperature from report
 		if (_baro_subs[uorb_index].update(&report)) {
+			if (PX4_ISFINITE(report.temperature)) {
+				// Update the offsets and mark for publication if they've changed
+				if (_temperature_compensation.update_offsets_baro(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
 
-			// 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;
+					_corrections.baro_device_ids[uorb_index] = report.device_id;
+					_corrections_changed = true;
+				}
 			}
 		}
 	}

src/modules/temperature_compensation/temp_comp_params_accel.c

@@ -154,30 +154,6 @@ PARAM_DEFINE_FLOAT(TC_A0_X0_1, 0.0f);
  */
 PARAM_DEFINE_FLOAT(TC_A0_X0_2, 0.0f);
 
-/**
- * Accelerometer scale factor - X axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_A0_SCL_0, 1.0f);
-
-/**
- * Accelerometer scale factor - Y axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_A0_SCL_1, 1.0f);
-
-/**
- * Accelerometer scale factor - Z axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_A0_SCL_2, 1.0f);
-
 /**
  * Accelerometer calibration reference temperature.
  *
@@ -308,30 +284,6 @@ PARAM_DEFINE_FLOAT(TC_A1_X0_1, 0.0f);
  */
 PARAM_DEFINE_FLOAT(TC_A1_X0_2, 0.0f);
 
-/**
- * Accelerometer scale factor - X axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_A1_SCL_0, 1.0f);
-
-/**
- * Accelerometer scale factor - Y axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_A1_SCL_1, 1.0f);
-
-/**
- * Accelerometer scale factor - Z axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_A1_SCL_2, 1.0f);
-
 /**
  * Accelerometer calibration reference temperature.
  *
@@ -462,30 +414,6 @@ PARAM_DEFINE_FLOAT(TC_A2_X0_1, 0.0f);
  */
 PARAM_DEFINE_FLOAT(TC_A2_X0_2, 0.0f);
 
-/**
- * Accelerometer scale factor - X axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_A2_SCL_0, 1.0f);
-
-/**
- * Accelerometer scale factor - Y axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_A2_SCL_1, 1.0f);
-
-/**
- * Accelerometer scale factor - Z axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_A2_SCL_2, 1.0f);
-
 /**
  * Accelerometer calibration reference temperature.
  *

src/modules/temperature_compensation/temp_comp_params_baro.c

@@ -106,14 +106,6 @@ PARAM_DEFINE_FLOAT(TC_B0_X1, 0.0f);
  */
 PARAM_DEFINE_FLOAT(TC_B0_X0, 0.0f);
 
-/**
- * Barometer scale factor - X axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_B0_SCL, 1.0f);
-
 /**
  * Barometer calibration reference temperature.
  *
@@ -196,14 +188,6 @@ PARAM_DEFINE_FLOAT(TC_B1_X1, 0.0f);
  */
 PARAM_DEFINE_FLOAT(TC_B1_X0, 0.0f);
 
-/**
- * Barometer scale factor - X axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_B1_SCL, 1.0f);
-
 /**
  * Barometer calibration reference temperature.
  *
@@ -286,14 +270,6 @@ PARAM_DEFINE_FLOAT(TC_B2_X1, 0.0f);
  */
 PARAM_DEFINE_FLOAT(TC_B2_X0, 0.0f);
 
-/**
- * Barometer scale factor - X axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_B2_SCL, 1.0f);
-
 /**
  * Barometer calibration reference temperature.
  *

src/modules/temperature_compensation/temp_comp_params_gyro.c

@@ -154,30 +154,6 @@ PARAM_DEFINE_FLOAT(TC_G0_X0_1, 0.0f);
  */
 PARAM_DEFINE_FLOAT(TC_G0_X0_2, 0.0f);
 
-/**
- * Gyro scale factor - X axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_G0_SCL_0, 1.0f);
-
-/**
- * Gyro scale factor - Y axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_G0_SCL_1, 1.0f);
-
-/**
- * Gyro scale factor - Z axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_G0_SCL_2, 1.0f);
-
 /**
  * Gyro calibration reference temperature.
  *
@@ -308,30 +284,6 @@ PARAM_DEFINE_FLOAT(TC_G1_X0_1, 0.0f);
  */
 PARAM_DEFINE_FLOAT(TC_G1_X0_2, 0.0f);
 
-/**
- * Gyro scale factor - X axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_G1_SCL_0, 1.0f);
-
-/**
- * Gyro scale factor - Y axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_G1_SCL_1, 1.0f);
-
-/**
- * Gyro scale factor - Z axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_G1_SCL_2, 1.0f);
-
 /**
  * Gyro calibration reference temperature.
  *
@@ -462,30 +414,6 @@ PARAM_DEFINE_FLOAT(TC_G2_X0_1, 0.0f);
  */
 PARAM_DEFINE_FLOAT(TC_G2_X0_2, 0.0f);
 
-/**
- * Gyro scale factor - X axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_G2_SCL_0, 1.0f);
-
-/**
- * Gyro scale factor - Y axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_G2_SCL_1, 1.0f);
-
-/**
- * Gyro scale factor - Z axis.
- *
- * @group Thermal Compensation
- * @category system
- */
-PARAM_DEFINE_FLOAT(TC_G2_SCL_2, 1.0f);
-
 /**
  * Gyro calibration reference temperature.
  *

src/modules/temperature_compensation/temperature_calibration/accel.cpp

@@ -68,22 +68,6 @@ TemperatureCalibrationAccel::~TemperatureCalibrationAccel()
 	}
 }
 
-void TemperatureCalibrationAccel::reset_calibration()
-{
-	/* reset all driver level calibrations */
-	float offset = 0.0f;
-	float scale = 1.0f;
-
-	for (unsigned s = 0; s < 3; s++) {
-		set_parameter("CAL_ACC%u_XOFF", s, &offset);
-		set_parameter("CAL_ACC%u_YOFF", s, &offset);
-		set_parameter("CAL_ACC%u_ZOFF", s, &offset);
-		set_parameter("CAL_ACC%u_XSCALE", s, &scale);
-		set_parameter("CAL_ACC%u_YSCALE", s, &scale);
-		set_parameter("CAL_ACC%u_ZSCALE", s, &scale);
-	}
-}
-
 int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int sensor_sub)
 {
 	bool finished = data.hot_soaked;

src/modules/temperature_compensation/temperature_calibration/accel.h

@@ -47,11 +47,6 @@ public:
 	 */
 	int finish();
 
-	/**
-	 * @see TemperatureCalibrationBase::reset_calibration()
-	 */
-	void reset_calibration();
-
 private:
 
 	virtual inline int update_sensor_instance(PerSensorData &data, int sensor_sub);

src/modules/temperature_compensation/temperature_calibration/baro.cpp

@@ -68,11 +68,6 @@ TemperatureCalibrationBaro::~TemperatureCalibrationBaro()
 	}
 }
 
-void TemperatureCalibrationBaro::reset_calibration()
-{
-	// nothing to do
-}
-
 int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int sensor_sub)
 {
 	bool finished = data.hot_soaked;

src/modules/temperature_compensation/temperature_calibration/baro.h

@@ -50,11 +50,6 @@ public:
 	 */
 	int finish();
 
-	/**
-	 * @see TemperatureCalibrationBase::reset_calibration()
-	 */
-	void reset_calibration();
-
 private:
 
 	virtual int update_sensor_instance(PerSensorData &data, int sensor_sub);

src/modules/temperature_compensation/temperature_calibration/common.h

@@ -78,9 +78,6 @@ public:
 	 */
 	virtual int finish() = 0;
 
-	/** reset all driver-level calibration parameters */
-	virtual void reset_calibration() = 0;
-
 protected:
 
 	/**

src/modules/temperature_compensation/temperature_calibration/gyro.cpp

@@ -55,18 +55,6 @@ TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_ris
 	_num_sensor_instances = num_gyros;
 }
 
-void TemperatureCalibrationGyro::reset_calibration()
-{
-	/* reset all driver level calibrations */
-	float offset = 0.0f;
-
-	for (unsigned s = 0; s < 3; s++) {
-		set_parameter("CAL_GYRO%u_XOFF", s, &offset);
-		set_parameter("CAL_GYRO%u_YOFF", s, &offset);
-		set_parameter("CAL_GYRO%u_ZOFF", s, &offset);
-	}
-}
-
 int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int sensor_sub)
 {
 	bool finished = data.hot_soaked;

src/modules/temperature_compensation/temperature_calibration/gyro.h

@@ -48,11 +48,6 @@ public:
 	 */
 	int finish();
 
-	/**
-	 * @see TemperatureCalibrationBase::reset_calibration()
-	 */
-	void reset_calibration();
-
 private:
 
 	virtual int update_sensor_instance(PerSensorData &data, int sensor_sub);

src/modules/temperature_compensation/temperature_calibration/task.cpp

@@ -163,16 +163,6 @@ void TemperatureCalibration::task_main()
 		}
 	}
 
-	// reset params
-	for (int i = 0; i < num_calibrators; ++i) {
-		calibrators[i]->reset_calibration();
-	}
-
-	// make sure the system updates the changed parameters
-	param_notify_changes();
-
-	px4_usleep(300000); // wait a bit for the system to apply the parameters
-
 	hrt_abstime next_progress_output = hrt_absolute_time() + 1e6;
 
 	// control LED's: blink, then turn solid according to progress

src/modules/uORB/SubscriptionBlocking.hpp

@@ -64,13 +64,17 @@ public:
 			PX4_ERR("pthread_mutexattr_init failed, status=%d", ret_attr_init);
 		}
 
+#if defined(PTHREAD_PRIO_NONE)
 		// pthread_mutexattr_settype
+		//  PTHREAD_PRIO_NONE not available on cygwin
 		int ret_mutexattr_settype = pthread_mutexattr_settype(&attr, PTHREAD_PRIO_NONE);
 
 		if (ret_mutexattr_settype != 0) {
 			PX4_ERR("pthread_mutexattr_settype failed, status=%d", ret_mutexattr_settype);
 		}
 
+#endif // PTHREAD_PRIO_NONE
+
 		// pthread_mutex_init
 		int ret_mutex_init = pthread_mutex_init(&_mutex, &attr);
 
@@ -94,21 +98,20 @@ public:
 	}
 
 	/**
-	 * Copy the struct if updated.
-	 * @param data The data reference where the struct will be copied.
+	 * Block until updated.
 	 * @param timeout_us The requested timeout in microseconds, or 0 to wait indefinitely.
 	 *
-	 * @return true only if topic was updated and copied successfully.
+	 * @return true only if topic was updated
 	 */
-	bool updateBlocking(T &data, uint32_t timeout_us = 0)
+	bool updatedBlocking(uint32_t timeout_us = 0)
 	{
 		if (!_registered) {
 			registerCallback();
 		}
 
 		if (updated()) {
-			// copy immediately if updated
-			return copy(&data);
+			// return immediately if updated
+			return true;
 
 		} else {
 			// otherwise wait
@@ -118,7 +121,7 @@ public:
 			if (timeout_us == 0) {
 				// wait with no timeout
 				if (pthread_cond_wait(&_cv, &_mutex) == 0) {
-					return update(&data);
+					return updated();
 				}
 
 			} else {
@@ -134,7 +137,7 @@ public:
 				ts.tv_nsec = nsecs;
 
 				if (px4_pthread_cond_timedwait(&_cv, &_mutex, &ts) == 0) {
-					return update(&data);
+					return updated();
 				}
 			}
 		}
@@ -142,6 +145,22 @@ public:
 		return false;
 	}
 
+	/**
+	 * Copy the struct if updated.
+	 * @param data The data reference where the struct will be copied.
+	 * @param timeout_us The requested timeout in microseconds, or 0 to wait indefinitely.
+	 *
+	 * @return true only if topic was updated and copied successfully.
+	 */
+	bool updateBlocking(T &data, uint32_t timeout_us = 0)
+	{
+		if (updatedBlocking(timeout_us)) {
+			return copy(&data);
+		}
+
+		return false;
+	}
+
 private:
 
 	pthread_mutex_t _mutex = PTHREAD_MUTEX_INITIALIZER;