accel and gyro calibration refactor and cleanup

.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 @@ def is_valid_directory(parser, arg):
 '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 @@ def is_valid_directory(parser, arg):
     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 @@ def is_valid_directory(parser, arg):
 '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 @@ def is_valid_directory(parser, arg):
     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 @@ def is_valid_directory(parser, arg):
 '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 @@ def is_valid_directory(parser, arg):
     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 @@ def is_valid_directory(parser, arg):
 '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 @@ def is_valid_directory(parser, arg):
     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 @@ def is_valid_directory(parser, arg):
 '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 @@ def is_valid_directory(parser, arg):
     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 @@ def is_valid_directory(parser, arg):
 '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 @@ def is_valid_directory(parser, arg):
     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 @@ def is_valid_directory(parser, arg):
 '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 @@ def is_valid_directory(parser, arg):
 # 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 @@ def is_valid_directory(parser, arg):
 '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 @@ def is_valid_directory(parser, arg):
     # 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 @@ def is_valid_directory(parser, arg):
     # 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()