From 8c23b7fd8b1bd42bbce7a6ec004e74ecde1cd5e0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 31 May 2020 21:25:13 -0400 Subject: [PATCH 1/2] accel and gyro calibration refactor and cleanup - remove all remaining IOCTLs for accel and gyro - move accel/gyro calibration entirely to sensors module - calibration no longer needs to clear existing 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) --- .ci/Jenkinsfile-hardware | 4 + Tools/process_sensor_caldata.py | 34 +- msg/sensor_accel_fifo.msg | 2 + msg/sensor_correction.msg | 53 +- msg/sensor_gyro_fifo.msg | 2 + src/drivers/barometer/bmp280/BMP280.cpp | 2 - src/drivers/barometer/bmp388/bmp388.cpp | 2 - src/drivers/barometer/dps310/DPS310.cpp | 2 - src/drivers/barometer/lps22hb/LPS22HB.cpp | 1 - src/drivers/barometer/lps25h/LPS25H.cpp | 2 - src/drivers/barometer/mpl3115a2/MPL3115A2.cpp | 2 - src/drivers/barometer/ms5611/ms5611.cpp | 2 - .../distance_sensor/cm8jl65/CM8JL65.cpp | 2 - .../distance_sensor/leddar_one/LeddarOne.cpp | 2 - src/drivers/distance_sensor/sf0x/SF0X.cpp | 2 - src/drivers/distance_sensor/sf1xx/sf1xx.cpp | 1 - src/drivers/distance_sensor/srf02/SRF02.cpp | 2 - .../distance_sensor/teraranger/TERARANGER.cpp | 2 - src/drivers/distance_sensor/tfmini/TFMINI.cpp | 2 - .../ulanding_radar/AerotennaULanding.cpp | 2 - .../distance_sensor/vl53l0x/VL53L0X.cpp | 1 - src/drivers/drv_accel.h | 75 -- src/drivers/drv_gyro.h | 70 -- src/drivers/drv_mixer.h | 2 +- src/drivers/imu/adis16448/ADIS16448.cpp | 3 - src/drivers/imu/adis16477/ADIS16477.cpp | 2 - src/drivers/imu/adis16497/ADIS16497.cpp | 2 - src/drivers/imu/bma180/bma180.cpp | 1 - src/drivers/imu/bmi055/BMI055_accel.cpp | 1 - src/drivers/imu/bmi055/BMI055_gyro.cpp | 1 - src/drivers/imu/bmi088/BMI088_accel.cpp | 1 - src/drivers/imu/bmi088/BMI088_gyro.cpp | 1 - src/drivers/imu/bmi160/bmi160.cpp | 2 - src/drivers/imu/fxas21002c/FXAS21002C.cpp | 1 - src/drivers/imu/icm20948/icm20948.cpp | 2 - .../imu/invensense/icm20602/ICM20602.cpp | 2 - .../imu/invensense/icm20608g/ICM20608G.cpp | 2 - .../imu/invensense/icm20649/ICM20649.cpp | 2 - .../imu/invensense/icm20689/ICM20689.cpp | 3 - .../imu/invensense/icm20948/ICM20948.cpp | 2 - .../imu/invensense/icm40609d/ICM40609D.cpp | 1 - .../imu/invensense/icm42688p/ICM42688P.cpp | 2 - .../imu/invensense/mpu6000/MPU6000.cpp | 2 - .../imu/invensense/mpu6500/MPU6500.cpp | 2 - .../imu/invensense/mpu9250/MPU9250.cpp | 2 - src/drivers/imu/l3gd20/L3GD20.cpp | 1 - src/drivers/imu/mpu6000/MPU6000.cpp | 2 - src/drivers/imu/mpu9250/mpu9250.cpp | 2 - src/drivers/imu/st/ism330dlc/ISM330DLC.cpp | 2 - src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp | 2 - .../matlab_csv_serial/matlab_csv_serial.c | 2 - .../accelerometer/PX4Accelerometer.cpp | 149 ++-- .../accelerometer/PX4Accelerometer.hpp | 24 +- src/lib/drivers/barometer/PX4Barometer.cpp | 8 - src/lib/drivers/barometer/PX4Barometer.hpp | 2 - src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 99 +-- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 21 +- .../drivers/rangefinder/PX4Rangefinder.cpp | 4 - .../drivers/rangefinder/PX4Rangefinder.hpp | 2 - .../commander/accelerometer_calibration.cpp | 714 +++++------------- .../commander/calibration_routines.cpp | 74 +- src/modules/commander/calibration_routines.h | 4 - src/modules/commander/gyro_calibration.cpp | 420 +++-------- src/modules/commander/mag_calibration.cpp | 4 +- src/modules/sensors/CMakeLists.txt | 2 +- .../CMakeLists.txt | 8 +- .../sensor_calibration/SensorCalibration.cpp | 252 +++++++ .../SensorCalibration.hpp} | 39 +- .../sensor_corrections/SensorCorrections.cpp | 170 ----- src/modules/sensors/sensors.cpp | 2 + .../vehicle_acceleration/CMakeLists.txt | 2 +- .../VehicleAcceleration.cpp | 14 +- .../VehicleAcceleration.hpp | 4 +- .../vehicle_air_data/VehicleAirData.cpp | 78 +- .../vehicle_air_data/VehicleAirData.hpp | 6 +- .../vehicle_angular_velocity/CMakeLists.txt | 2 +- .../VehicleAngularVelocity.cpp | 13 +- .../VehicleAngularVelocity.hpp | 4 +- .../sensors/vehicle_imu/CMakeLists.txt | 2 +- .../sensors/vehicle_imu/VehicleIMU.cpp | 63 +- .../sensors/vehicle_imu/VehicleIMU.hpp | 6 +- src/modules/sensors/voted_sensors_update.cpp | 220 +----- src/modules/sensors/voted_sensors_update.h | 2 - src/modules/simulator/simulator.cpp | 9 - .../TemperatureCompensation.cpp | 52 +- .../TemperatureCompensation.h | 19 +- .../TemperatureCompensationModule.cpp | 59 +- .../temp_comp_params_accel.c | 72 -- .../temp_comp_params_baro.c | 24 - .../temp_comp_params_gyro.c | 72 -- .../temperature_calibration/accel.cpp | 16 - .../temperature_calibration/accel.h | 5 - .../temperature_calibration/baro.cpp | 5 - .../temperature_calibration/baro.h | 5 - .../temperature_calibration/common.h | 3 - .../temperature_calibration/gyro.cpp | 12 - .../temperature_calibration/gyro.h | 5 - .../temperature_calibration/task.cpp | 10 - src/modules/uORB/SubscriptionBlocking.hpp | 35 +- 99 files changed, 924 insertions(+), 2212 deletions(-) delete mode 100644 src/drivers/drv_accel.h delete mode 100644 src/drivers/drv_gyro.h rename src/modules/sensors/{sensor_corrections => sensor_calibration}/CMakeLists.txt (91%) create mode 100644 src/modules/sensors/sensor_calibration/SensorCalibration.cpp rename src/modules/sensors/{sensor_corrections/SensorCorrections.hpp => sensor_calibration/SensorCalibration.hpp} (75%) delete mode 100644 src/modules/sensors/sensor_corrections/SensorCorrections.cpp diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index dbfbf5091666..6bf8f3270f11 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.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"' @@ -818,6 +820,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"' diff --git a/Tools/process_sensor_caldata.py b/Tools/process_sensor_caldata.py index 1065c64358c1..d270c2ab6cab 100644 --- a/Tools/process_sensor_caldata.py +++ b/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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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: diff --git a/msg/sensor_accel_fifo.msg b/msg/sensor_accel_fifo.msg index 0f3eaf465d90..0745dbf1b6ae 100644 --- a/msg/sensor_accel_fifo.msg +++ b/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) diff --git a/msg/sensor_correction.msg b/msg/sensor_correction.msg index a61caf15e6ee..c492c3cf3fa4 100644 --- a/msg/sensor_correction.msg +++ b/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 diff --git a/msg/sensor_gyro_fifo.msg b/msg/sensor_gyro_fifo.msg index 24cc131806d8..567d2adbbed3 100644 --- a/msg/sensor_gyro_fifo.msg +++ b/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) diff --git a/src/drivers/barometer/bmp280/BMP280.cpp b/src/drivers/barometer/bmp280/BMP280.cpp index 95d92cd3f17a..dc2fc51ed652 100644 --- a/src/drivers/barometer/bmp280/BMP280.cpp +++ b/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(); } diff --git a/src/drivers/barometer/bmp388/bmp388.cpp b/src/drivers/barometer/bmp388/bmp388.cpp index b9609f6462b3..82ee70560e60 100644 --- a/src/drivers/barometer/bmp388/bmp388.cpp +++ b/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 diff --git a/src/drivers/barometer/dps310/DPS310.cpp b/src/drivers/barometer/dps310/DPS310.cpp index 978481c3860f..7e1b66a89926 100644 --- a/src/drivers/barometer/dps310/DPS310.cpp +++ b/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 diff --git a/src/drivers/barometer/lps22hb/LPS22HB.cpp b/src/drivers/barometer/lps22hb/LPS22HB.cpp index e43155a0802d..1c09faac15d9 100644 --- a/src/drivers/barometer/lps22hb/LPS22HB.cpp +++ b/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(); } diff --git a/src/drivers/barometer/lps25h/LPS25H.cpp b/src/drivers/barometer/lps25h/LPS25H.cpp index e68b37a4978e..c08aeaa39268 100644 --- a/src/drivers/barometer/lps25h/LPS25H.cpp +++ b/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(); } diff --git a/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp b/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp index 804efafa5ab8..7c3ca88b9090 100644 --- a/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp +++ b/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(); } diff --git a/src/drivers/barometer/ms5611/ms5611.cpp b/src/drivers/barometer/ms5611/ms5611.cpp index c44b2ac2fea6..480237a958d8 100644 --- a/src/drivers/barometer/ms5611/ms5611.cpp +++ b/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 diff --git a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp index 5ede856f7013..0b8c0fc3f940 100644 --- a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp +++ b/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 diff --git a/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp b/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp index edc447ac3ce6..e0012ea686d6 100644 --- a/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp +++ b/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 diff --git a/src/drivers/distance_sensor/sf0x/SF0X.cpp b/src/drivers/distance_sensor/sf0x/SF0X.cpp index eefdb781f86a..43ead83d27ce 100644 --- a/src/drivers/distance_sensor/sf0x/SF0X.cpp +++ b/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(); } diff --git a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp index 362bdba71608..1a483133e9d3 100644 --- a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp +++ b/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() diff --git a/src/drivers/distance_sensor/srf02/SRF02.cpp b/src/drivers/distance_sensor/srf02/SRF02.cpp index 9be84e6e5263..1b6256755b90 100644 --- a/src/drivers/distance_sensor/srf02/SRF02.cpp +++ b/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(); } diff --git a/src/drivers/distance_sensor/teraranger/TERARANGER.cpp b/src/drivers/distance_sensor/teraranger/TERARANGER.cpp index c5f566a4b114..ca4d399fde1f 100644 --- a/src/drivers/distance_sensor/teraranger/TERARANGER.cpp +++ b/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(); } diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.cpp b/src/drivers/distance_sensor/tfmini/TFMINI.cpp index 25c5b80d55de..99e0e134d4a8 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.cpp +++ b/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(); } diff --git a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp index 6963d291c223..b9da4f6d3717 100644 --- a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp +++ b/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(); } diff --git a/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp b/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp index 5ac848606f77..0f06ee5306f3 100644 --- a/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp +++ b/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() diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h deleted file mode 100644 index 00002ad7b223..000000000000 --- a/src/drivers/drv_accel.h +++ /dev/null @@ -1,75 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file drv_accel.h - * - * Accelerometer driver interface. - */ - -#ifndef _DRV_ACCEL_H -#define _DRV_ACCEL_H - -#include -#include - -#include "drv_sensor.h" -#include "drv_orb_dev.h" - -#define ACCEL_BASE_DEVICE_PATH "/dev/accel" - -#include - -/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ -struct accel_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; -}; -/* - * ioctl() definitions - * - * Accelerometer drivers also implement the generic sensor driver - * interfaces from drv_sensor.h - */ - -#define _ACCELIOCBASE (0x2100) -#define _ACCELIOC(_n) (_PX4_IOC(_ACCELIOCBASE, _n)) - -/** set the accel scaling constants to the structure pointed to by (arg) */ -#define ACCELIOCSSCALE _ACCELIOC(5) - -#endif /* _DRV_ACCEL_H */ diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h deleted file mode 100644 index 945ee92e97ae..000000000000 --- a/src/drivers/drv_gyro.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file drv_gyro.h - * - * Gyroscope driver interface. - */ - -#ifndef _DRV_GYRO_H -#define _DRV_GYRO_H - -#include -#include - -#include "drv_sensor.h" -#include "drv_orb_dev.h" - -#define GYRO_BASE_DEVICE_PATH "/dev/gyro" - -#include - -/** gyro scaling factors; Vout = Vin + Voffset */ -struct gyro_calibration_s { - float x_offset; - float y_offset; - float z_offset; -}; - -/* - * ioctl() definitions - */ - -#define _GYROIOCBASE (0x2300) -#define _GYROIOC(_n) (_PX4_IOC(_GYROIOCBASE, _n)) - -/** set the gyro scaling constants to (arg) */ -#define GYROIOCSSCALE _GYROIOC(4) - -#endif /* _DRV_GYRO_H */ diff --git a/src/drivers/drv_mixer.h b/src/drivers/drv_mixer.h index 317eb33942e9..2c54885d7909 100644 --- a/src/drivers/drv_mixer.h +++ b/src/drivers/drv_mixer.h @@ -85,4 +85,4 @@ * - save/serialise for saving tuned mixers. */ -#endif /* _DRV_ACCEL_H */ +#endif /* _DRV_MIXER_H */ diff --git a/src/drivers/imu/adis16448/ADIS16448.cpp b/src/drivers/imu/adis16448/ADIS16448.cpp index 0555820a4592..3c88d3e8d89a 100644 --- a/src/drivers/imu/adis16448/ADIS16448.cpp +++ b/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(); } diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index 32831588583a..db68a4e40ea5 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/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(); } diff --git a/src/drivers/imu/adis16497/ADIS16497.cpp b/src/drivers/imu/adis16497/ADIS16497.cpp index 38aa2c58647d..f0f30463f27b 100644 --- a/src/drivers/imu/adis16497/ADIS16497.cpp +++ b/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(); } diff --git a/src/drivers/imu/bma180/bma180.cpp b/src/drivers/imu/bma180/bma180.cpp index f829c07a658e..df1e33276ffa 100644 --- a/src/drivers/imu/bma180/bma180.cpp +++ b/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() diff --git a/src/drivers/imu/bmi055/BMI055_accel.cpp b/src/drivers/imu/bmi055/BMI055_accel.cpp index e84029a50b15..a5c5d300d6d9 100644 --- a/src/drivers/imu/bmi055/BMI055_accel.cpp +++ b/src/drivers/imu/bmi055/BMI055_accel.cpp @@ -412,7 +412,6 @@ BMI055_accel::print_status() } } - _px4_accel.print_status(); } void diff --git a/src/drivers/imu/bmi055/BMI055_gyro.cpp b/src/drivers/imu/bmi055/BMI055_gyro.cpp index fa9398feced3..4472649c1ed2 100644 --- a/src/drivers/imu/bmi055/BMI055_gyro.cpp +++ b/src/drivers/imu/bmi055/BMI055_gyro.cpp @@ -437,7 +437,6 @@ BMI055_gyro::print_status() } } - _px4_gyro.print_status(); } void diff --git a/src/drivers/imu/bmi088/BMI088_accel.cpp b/src/drivers/imu/bmi088/BMI088_accel.cpp index 68b86583b189..e2902d8ce51c 100644 --- a/src/drivers/imu/bmi088/BMI088_accel.cpp +++ b/src/drivers/imu/bmi088/BMI088_accel.cpp @@ -555,7 +555,6 @@ BMI088_accel::print_status() } } - _px4_accel.print_status(); } void diff --git a/src/drivers/imu/bmi088/BMI088_gyro.cpp b/src/drivers/imu/bmi088/BMI088_gyro.cpp index 2485e5f1d569..8da909c85114 100644 --- a/src/drivers/imu/bmi088/BMI088_gyro.cpp +++ b/src/drivers/imu/bmi088/BMI088_gyro.cpp @@ -442,7 +442,6 @@ BMI088_gyro::print_status() } } - _px4_gyro.print_status(); } void diff --git a/src/drivers/imu/bmi160/bmi160.cpp b/src/drivers/imu/bmi160/bmi160.cpp index 1ef0e3ed016e..89bd001e2aa1 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/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(); } diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.cpp b/src/drivers/imu/fxas21002c/FXAS21002C.cpp index 15312cd5fce8..a0d5a12da843 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.cpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.cpp @@ -418,7 +418,6 @@ void FXAS21002C::print_status() } } - _px4_gyro.print_status(); } void diff --git a/src/drivers/imu/icm20948/icm20948.cpp b/src/drivers/imu/icm20948/icm20948.cpp index 4756b8d0d517..e51cad98f9fd 100644 --- a/src/drivers/imu/icm20948/icm20948.cpp +++ b/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(); } diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index bb36426e0eec..f7a011abac2e 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/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() diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp index d1b8a78729ed..4531427006ae 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp @@ -102,8 +102,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() diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp index 1161b3bbc13b..de3f23a33b1f 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp +++ b/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() diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index 3ca7817923a7..3a85b0ffa035 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -101,9 +101,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() diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index 5e10f237b424..4c7f3b15a4ee 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/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(); diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 4f05c09c1802..571b9da92f6a 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -103,7 +103,6 @@ void ICM40609D::print_status() perf_print_counter(_drdy_interval_perf); _px4_accel.print_status(); - _px4_gyro.print_status(); } int ICM40609D::probe() diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index 27b079e42594..ffa5f8d619dc 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/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() diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index 1887ffd4e462..ce42309ba5cd 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -102,8 +102,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() diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp index 28c85a7066bc..4648e13f8e5e 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp +++ b/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() diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index 238cc19e1a29..f10dabfc339e 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/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(); diff --git a/src/drivers/imu/l3gd20/L3GD20.cpp b/src/drivers/imu/l3gd20/L3GD20.cpp index efc5dade3e35..2ac26277e3a0 100644 --- a/src/drivers/imu/l3gd20/L3GD20.cpp +++ b/src/drivers/imu/l3gd20/L3GD20.cpp @@ -414,7 +414,6 @@ L3GD20::print_status() } } - _px4_gyro.print_status(); } void diff --git a/src/drivers/imu/mpu6000/MPU6000.cpp b/src/drivers/imu/mpu6000/MPU6000.cpp index b815a63af19a..55551c82e21d 100644 --- a/src/drivers/imu/mpu6000/MPU6000.cpp +++ b/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 diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index efd5670918d0..1bcf8b5ece7f 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/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(); } diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp index ba44d975a074..9f5c36c217c9 100644 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp +++ b/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(); } diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp index 5965398fb05b..ec17f0449116 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp +++ b/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(); } diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index c1c14f0d448f..78a7b79a0e0c 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -56,8 +56,6 @@ #include #include #include -#include -#include #include #include #include diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 16a7fb316388..91c10f7b68e2 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/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,52 +64,21 @@ 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(); } 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 +92,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 ×tamp_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 +111,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 +122,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); - - 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)); + uint8_t clip_count[3] { + clipping(sample.x, _clip_limit, N), + clipping(sample.y, _clip_limit, N), + clipping(sample.z, _clip_limit, N), + }; - // 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 +144,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 +154,35 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample) _sensor_fifo_pub.publish(fifo); } -void PX4Accelerometer::UpdateClipLimit() +void PX4Accelerometer::Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3]) { - // 99.9% of potential max - _clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX); + // 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::print_status() +void PX4Accelerometer::UpdateClipLimit() { -#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 + // 99.9% of potential max + _clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX); } diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 1b13d713cc19..df977cd68533 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -33,24 +33,18 @@ #pragma once -#include #include -#include #include #include -#include -#include #include #include #include -class PX4Accelerometer : public cdev::CDev, public ModuleParams +class PX4Accelerometer { 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; + ~PX4Accelerometer(); uint32_t get_device_id() const { return _device_id; } @@ -62,9 +56,7 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams 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 ×tamp_sample, float x, float y, float z); struct FIFOSample { hrt_abstime timestamp_sample; @@ -82,15 +74,11 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams void updateFIFO(const FIFOSample &sample); private: + void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z, uint8_t clip_count[3]); void UpdateClipLimit(); - uORB::PublicationQueuedMulti _sensor_pub; - uORB::PublicationMulti _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_pub; + uORB::PublicationMulti _sensor_fifo_pub; uint32_t _device_id{0}; const enum Rotation _rotation; diff --git a/src/lib/drivers/barometer/PX4Barometer.cpp b/src/lib/drivers/barometer/PX4Barometer.cpp index 1f13bc4e2f18..aada22a64222 100644 --- a/src/lib/drivers/barometer/PX4Barometer.cpp +++ b/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()); -} diff --git a/src/lib/drivers/barometer/PX4Barometer.hpp b/src/lib/drivers/barometer/PX4Barometer.hpp index 7589cbd09705..cfa98985696a 100644 --- a/src/lib/drivers/barometer/PX4Barometer.hpp +++ b/src/lib/drivers/barometer/PX4Barometer.hpp @@ -59,8 +59,6 @@ class PX4Barometer : public cdev::CDev int get_class_instance() { return _class_device_instance; }; - void print_status(); - private: uORB::PublicationMultiData _sensor_baro_pub; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 44298f662220..56e3df72c599 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/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 ×tamp_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 ×tamp_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); } diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 2b94919c6446..d4d159f3c027 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,24 +33,19 @@ #pragma once -#include #include -#include #include #include #include -#include #include #include -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 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams 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 ×tamp_sample, float x, float y, float z); struct FIFOSample { hrt_abstime timestamp_sample; @@ -83,12 +76,10 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams void updateFIFO(const FIFOSample &sample); private: - uORB::PublicationQueuedMulti _sensor_pub; - uORB::PublicationMulti _sensor_fifo_pub; - - matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f}; + void Publish(const hrt_abstime ×tamp_sample, float x, float y, float z); - int _class_device_instance{-1}; + uORB::PublicationQueuedMulti _sensor_pub; + uORB::PublicationMulti _sensor_fifo_pub; uint32_t _device_id{0}; const enum Rotation _rotation; diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp index c058d81839e9..928e17c5aadf 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp @@ -86,7 +86,3 @@ void PX4Rangefinder::update(const hrt_abstime ×tamp_sample, const float dis _distance_sensor_pub.update(); } - -void PX4Rangefinder::print_status() -{ -} diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index f5f8b1181132..36b7af111022 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -48,8 +48,6 @@ class PX4Rangefinder 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; } diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 50819f2b44a9..e072a395125e 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/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 */ -// 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 #include #include -#include -#include -#include -#include -#include -#include -#include + #include -#include +#include #include #include -#include -#include +#include +#include #include #include -#include +#include #include +#include #include +#include using namespace time_literals; using namespace matrix; +using math::radians; + +static constexpr char sensor_name[] {"accel"}; + +static constexpr unsigned MAX_ACCEL_SENS = 3; -static const char *sensor_name = "accel"; +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 int32_t device_id[max_accel_sens]; -static int device_prio_max = 0; -static int32_t device_id_primary = 0; +static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3], + unsigned orient, unsigned samples_num); -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 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); + int32_t device_id[MAX_ACCEL_SENS] {}; + int device_prio_max = 0; + int32_t device_id_primary = 0; + unsigned active_sensors = 0; - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } + // 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)); - (void)sprintf(str, "CAL_ACC%u_XSCALE", s); - res = param_set_no_notification(param_find(str), &accel_scale.x_scale); + // 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); + } - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } + for (uint8_t cur_accel = 0; cur_accel < orb_accel_count && cur_accel < MAX_ACCEL_SENS; cur_accel++) { + uORB::SubscriptionData accel_sub{ORB_ID(sensor_accel), cur_accel}; + device_id[cur_accel] = accel_sub.get().device_id; - (void)sprintf(str, "CAL_ACC%u_YSCALE", s); - res = param_set_no_notification(param_find(str), &accel_scale.y_scale); + if (device_id[cur_accel] != 0) { + // Get priority + int32_t prio = accel_sub.get_priority(); - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); - } + if (prio > device_prio_max) { + device_prio_max = prio; + device_id_primary = device_id[cur_accel]; + } - (void)sprintf(str, "CAL_ACC%u_ZSCALE", s); - res = param_set_no_notification(param_find(str), &accel_scale.z_scale); + active_sensors++; - if (res != PX4_OK) { - PX4_ERR("unable to reset %s", str); + } else { + calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel); + return PX4_ERROR; } - - param_notify_changes(); -#endif } - float accel_offs[max_accel_sens][3]; - float accel_T[max_accel_sens][3][3]; - 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); + 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_cancelled) { - // Cancel message already displayed, nothing left to do - return PX4_ERROR; - - } else if (cal_return == calibrate_return_ok) { - res = PX4_OK; - - } else { - res = PX4_ERROR; - } + if (cal_return != calibrate_return_ok) { + // Cancel message already displayed, nothing left to do + return PX4_ERROR; } - if (res != PX4_OK) { - if (active_sensors == 0) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG); - } - + 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); - - Dcmf board_rotation_t = board_rotation.transpose(); - - bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator - - for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) { - - /* 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; - - 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); - - bool failed = false; - - failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); + 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(); + param_set_no_notification(param_find("CAL_ACC_PRIME"), &device_id_primary); - PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, - (double)accel_scale.x_offset, - (double)accel_scale.y_offset, - (double)accel_scale.z_offset); - PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, - (double)accel_scale.x_scale, - (double)accel_scale.y_scale, - (double)accel_scale.z_scale); + for (unsigned uorb_index = 0; uorb_index < MAX_ACCEL_SENS; uorb_index++) { - /* check if thermal compensation is enabled */ - int32_t tc_enabled_int; - param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int)); + 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; - 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); + 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)); - /* 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; + 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)); - /* update the _X0_ terms to include the additional offset */ - int32_t handle; - float val; + char str[30] {}; - 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); + // 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); - if (axis_index == 0) { - val += accel_scale.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); - } else if (axis_index == 1) { - val += accel_scale.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); - } 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; - } + // 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); - // 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; - } + 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 1 // TODO: replace all IOCTL usage - sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, uorb_index); - fd = px4_open(str, 0); + 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); - 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] {}; + + // 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)); + + // 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)); } - - if (res != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG); - } - -#endif } + 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,29 @@ 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 */); - 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)++; - } + false); - px4_close(worker_data.subs[i]); - } + calibrate_cancel_unsubscribe(cancel_sub); } - 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,92 +381,68 @@ 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(board_offset_x, &board_offset[0]); - param_get(board_offset_y, &board_offset[1]); - param_get(board_offset_z, &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]); + 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]); - int32_t board_rotation_int; - param_get(board_rotation_h, &(board_rotation_int)); + const Dcmf board_rotation_offset{Eulerf{math::radians(board_offset[0]), math::radians(board_offset[1]), math::radians(board_offset[2])}}; - Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int); + int32_t board_rotation_int = 0; + param_get(param_find("SENS_BOARD_ROT"), &board_rotation_int); - px4_pollfd_struct_t fds[max_accel_sens]; + const Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int); - 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 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 (poll_ret > 0) { - - 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 (accel_sub[0].updatedBlocking(100000)) { + for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) { + sensor_accel_s arp; + + if (accel_sub[s].update(&arp)) { + // 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 && arp.device_id != 0) { + for (uint8_t i = 0; i < MAX_ACCEL_SENS; i++) { + if (sensor_correction.accel_device_ids[i] == arp.device_id) { + switch (i) { + 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; + } + } + } } + accel_sum[s] += Vector3f{arp.x, arp.y, arp.z} - offset; counts[s]++; } } @@ -714,79 +458,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 +505,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 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 +541,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 +558,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 +568,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 +579,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 +599,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; diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 429bdce152cb..054e1bdada70 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -41,26 +41,25 @@ #include #include #include -#include -#include -#include -#include + #include -#include +#include + #include -#include -#include +#include +#include #include -#include +#include #include - -#include +#include #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; + // Setup subscriptions to onboard accel sensor + uORB::SubscriptionBlocking sensor_sub{ORB_ID(sensor_combined)}; + while (true) { - /* wait blocking for new data */ - int poll_ret = px4_poll(fds, 1, 1000); + sensor_combined_s sensor; - if (poll_ret) { - struct sensor_combined_s sensor; - orb_copy(ORB_ID(sensor_combined), accel_sub, &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; } diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 73be0d27e2a3..3fc38ad95c86 100644 --- a/src/modules/commander/calibration_routines.h +++ b/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 diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 71f39de3b611..0dfbb4c24cbb 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/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,136 +46,106 @@ #include #include #include -#include -#include -#include -#include -#include -#include + #include +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include +#include -static const char *sensor_name = "gyro"; +static constexpr char sensor_name[] {"gyro"}; +static constexpr unsigned MAX_GYROS = 3; -static constexpr unsigned max_gyros = 3; +using matrix::Vector3f; /// Data passed to calibration worker routine -typedef struct { - orb_advert_t *mavlink_log_pub; - int32_t device_id[max_gyros]; - int gyro_sensor_sub[max_gyros]; - int sensor_correction_sub; - struct gyro_calibration_s gyro_scale[max_gyros]; - float last_sample_0[3]; -} gyro_worker_data_t; - -static calibrate_return gyro_calibration_worker(int cancel_sub, void *data) +struct gyro_worker_data_t { + orb_advert_t *mavlink_log_pub{nullptr}; + int32_t device_id[MAX_GYROS] {}; + Vector3f offset[MAX_GYROS] {}; + Vector3f last_sample_0{}; +}; + +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; - - struct 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; - } - } + unsigned calibration_counter[MAX_GYROS] {}; + static constexpr unsigned calibration_count = 250; + unsigned poll_errcount = 0; - px4_pollfd_struct_t fds[max_gyros]; + uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; + sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */ - for (unsigned s = 0; s < max_gyros; s++) { - fds[s].fd = worker_data->gyro_sensor_sub[s]; - fds[s].events = POLLIN; - } + uORB::SubscriptionBlocking gyro_sub[MAX_GYROS] { + {ORB_ID(sensor_gyro), 0, 0}, + {ORB_ID(sensor_gyro), 0, 1}, + {ORB_ID(sensor_gyro), 0, 2}, + }; - memset(&worker_data->last_sample_0, 0, sizeof(worker_data->last_sample_0)); + worker_data.last_sample_0.zero(); /* use slowest gyro to pace, but count correctly per-gyro for statistics */ + unsigned slow_count = 0; + while (slow_count < calibration_count) { - if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) { + if (calibrate_cancel_check(worker_data.mavlink_log_pub, cancel_sub)) { return calibrate_return_cancelled; } - /* check if there are new thermal corrections */ - bool updated; - orb_check(worker_data->sensor_correction_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction); - } - - int poll_ret = px4_poll(&fds[0], max_gyros, 1000); - - if (poll_ret > 0) { + if (gyro_sub[0].updatedBlocking(100000)) { unsigned update_count = calibration_count; - for (unsigned s = 0; s < max_gyros; s++) { + for (unsigned s = 0; s < MAX_GYROS; s++) { if (calibration_counter[s] >= calibration_count) { // Skip if instance has enough samples continue; } - bool changed; - orb_check(worker_data->gyro_sensor_sub[s], &changed); - - if (changed) { - orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report); - float sample[3]; - - 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]; - - for (int i = 0; i < 3; ++i) { - worker_data->last_sample_0[i] = sample[i]; + sensor_gyro_s gyro_report; + + if (gyro_sub[s].update(&gyro_report)) { + + // 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 i = 0; i < MAX_GYROS; i++) { + if (sensor_correction.gyro_device_ids[i] == gyro_report.device_id) { + switch (i) { + 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; + } + } } - - } 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; - } - 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]; + worker_data.offset[s] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - offset; calibration_counter[s]++; + if (s == 0) { + worker_data.last_sample_0 = Vector3f{gyro_report.x, gyro_report.y, gyro_report.z}; + } } // Maintain the sample count of the slowest sensor if (calibration_counter[s] && calibration_counter[s] < update_count) { update_count = calibration_counter[s]; } - } if (update_count % (calibration_count / 20) == 0) { - calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (update_count * 100) / calibration_count); + calibration_log_info(worker_data.mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (update_count * 100) / calibration_count); } // Propagate out the slowest sensor's count @@ -188,20 +158,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; @@ -209,131 +177,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++) { - - // Lock in to correct ORB instance - bool found_cur_gyro = false; - - for (unsigned i = 0; i < orb_gyro_count && !found_cur_gyro; i++) { - worker_data.gyro_sensor_sub[cur_gyro] = orb_subscribe_multi(ORB_ID(sensor_gyro), i); + for (uint8_t cur_gyro = 0; cur_gyro < orb_gyro_count && cur_gyro < MAX_GYROS; cur_gyro++) { - sensor_gyro_s report{}; - orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[cur_gyro], &report); + uORB::Subscription gyro_sensor_sub{ORB_ID(sensor_gyro), cur_gyro}; + sensor_gyro_s report{}; + gyro_sensor_sub.copy(&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; @@ -345,7 +217,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; @@ -353,7 +225,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 @@ -365,19 +237,15 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) } else { /* check offsets */ - float xdiff = worker_data.last_sample_0[0] - worker_data.gyro_scale[0].x_offset; - float ydiff = worker_data.last_sample_0[1] - worker_data.gyro_scale[0].y_offset; - float zdiff = worker_data.last_sample_0[2] - worker_data.gyro_scale[0].z_offset; + Vector3f diff = worker_data.last_sample_0 - worker_data.offset[0]; /* maximum allowable calibration error */ const float maxoff = math::radians(0.4f); - 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(diff(0)) > maxoff || fabsf(diff(1)) > maxoff || fabsf(diff(2)) > maxoff) { calibration_log_critical(mavlink_log_pub, "motion, retrying.."); res = PX4_ERROR; @@ -398,98 +266,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; - - failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary))); - - bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator - - for (unsigned uorb_index = 0; uorb_index < max_gyros; uorb_index++) { - if (worker_data.device_id[uorb_index] != 0) { - char str[30]; - - /* check if thermal compensation is enabled */ - int32_t tc_enabled_int; - param_get(param_find("TC_G_ENABLE"), &(tc_enabled_int)); + bool failed = (PX4_OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &device_id_primary)); - 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); + for (unsigned uorb_index = 0; uorb_index < MAX_GYROS; uorb_index++) { - /* 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; + char str[30] {}; - /* update the _X0_ terms to include the additional offset */ - int32_t handle; - float val; + 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)); - 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); + 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 (axis_index == 0) { - val += worker_data.gyro_scale[uorb_index].x_offset; + 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)); - } else if (axis_index == 1) { - val += worker_data.gyro_scale[uorb_index].y_offset; + 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)); - } 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 + } 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)); + + // reset unused calibration device ID + sprintf(str, "CAL_GYRO%u_ID", uorb_index); + param_reset(param_find(str)); } } @@ -499,6 +313,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) } } + param_notify_changes(); + /* if there is a any preflight-check system response, let the barrage of messages through */ px4_usleep(200000); @@ -509,8 +325,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); } - orb_unsubscribe(worker_data.sensor_correction_sub); - /* give this message enough time to propagate */ px4_usleep(600000); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index ed149b869e94..857d044d4203 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -52,14 +52,12 @@ #include #include #include -#include -#include #include #include #include #include #include -#include +#include static const char *sensor_name = "mag"; static constexpr unsigned max_mags = 4; diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 3316d5b1f7c9..7859d4d839cd 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/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) diff --git a/src/modules/sensors/sensor_corrections/CMakeLists.txt b/src/modules/sensors/sensor_calibration/CMakeLists.txt similarity index 91% rename from src/modules/sensors/sensor_corrections/CMakeLists.txt rename to src/modules/sensors/sensor_calibration/CMakeLists.txt index 932e2029c5bd..8d0a6c0161b8 100644 --- a/src/modules/sensors/sensor_corrections/CMakeLists.txt +++ b/src/modules/sensors/sensor_calibration/CMakeLists.txt @@ -31,7 +31,9 @@ # ############################################################################ -px4_add_library(sensor_corrections - SensorCorrections.cpp - SensorCorrections.hpp +px4_add_library(sensor_calibration + SensorCalibration.cpp + SensorCalibration.hpp ) + +target_include_directories(sensor_calibration PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/..) diff --git a/src/modules/sensors/sensor_calibration/SensorCalibration.cpp b/src/modules/sensors/sensor_calibration/SensorCalibration.cpp new file mode 100644 index 000000000000..7afd28c60bcc --- /dev/null +++ b/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 + +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: [%.3f %.3f %.3f]", SensorString(), _device_id, + (double)_thermal_offset(0), (double)_thermal_offset(1), (double)_thermal_offset(2)); + } +} + +} // namespace sensors diff --git a/src/modules/sensors/sensor_corrections/SensorCorrections.hpp b/src/modules/sensors/sensor_calibration/SensorCalibration.hpp similarity index 75% rename from src/modules/sensors/sensor_corrections/SensorCorrections.hpp rename to src/modules/sensors/sensor_calibration/SensorCalibration.hpp index 88c146b73052..b4a7f95cd074 100644 --- a/src/modules/sensors/sensor_corrections/SensorCorrections.hpp +++ b/src/modules/sensors/sensor_calibration/SensorCalibration.hpp @@ -37,14 +37,13 @@ #include #include #include -#include #include #include namespace sensors { -class SensorCorrections : public ModuleParams +class SensorCalibration { public: @@ -53,49 +52,55 @@ class SensorCorrections : public ModuleParams 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) _param_sens_board_rot, - - (ParamFloat) _param_sens_board_x_off, - (ParamFloat) _param_sens_board_y_off, - (ParamFloat) _param_sens_board_z_off - ) + bool _enabled{true}; + bool _external{false}; }; } // namespace sensors diff --git a/src/modules/sensors/sensor_corrections/SensorCorrections.cpp b/src/modules/sensors/sensor_corrections/SensorCorrections.cpp deleted file mode 100644 index f5b51d523bcb..000000000000 --- a/src/modules/sensors/sensor_corrections/SensorCorrections.cpp +++ /dev/null @@ -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 diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 09d329769134..42b6fc3f06bd 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -239,6 +239,8 @@ Sensors::~Sensors() i->Stop(); } } + + perf_free(_loop_perf); } bool Sensors::init() diff --git a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt index 7a345b12fa8f..aac8558cde58 100644 --- a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt +++ b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt @@ -38,6 +38,6 @@ px4_add_library(vehicle_acceleration target_link_libraries(vehicle_acceleration PRIVATE mathlib - sensor_corrections + sensor_calibration px4_work_queue ) diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 82e31e304410..0aa3240b5d50 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/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()); } @@ -166,7 +165,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; @@ -195,7 +194,7 @@ void VehicleAcceleration::ParametersUpdate(bool force) updateParams(); - _corrections.ParametersUpdate(); + _calibration.ParametersUpdate(); } } @@ -207,7 +206,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(); @@ -247,7 +246,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; @@ -268,7 +267,8 @@ 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(); + + _calibration.PrintStatus(); } } // namespace sensors diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 0fc52419b402..cd0fd7fdd497 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -33,7 +33,7 @@ #pragma once -#include +#include #include #include @@ -88,7 +88,7 @@ class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem {this, ORB_ID(sensor_accel), 2} }; - SensorCorrections _corrections; + SensorCalibration _calibration{SensorCalibration::SensorType::Accelerometer}; matrix::Vector3f _bias{0.f, 0.f, 0.f}; diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 6650225dcf40..b3ed6f11a1d5 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/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.update(&corrections)) { - for (int baro_index = 0; baro_index < MAX_SENSOR_COUNT; baro_index++) { - const sensor_baro_s &baro = _last_data[baro_index]; - - // 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 (_sensor_correction_sub.updated() || force) { + sensor_correction_s corrections; + + 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; + + 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: %.3f", _last_data[_selected_sensor_sub_index].device_id, + (double)_thermal_offset[_selected_sensor_sub_index]); + } } _voter.print(); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index d31dc204b2df..9cb89b821f64 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -66,7 +66,7 @@ class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem void Run() override; void ParametersUpdate(); - void SensorCorrectionsUpdate(); + void SensorCorrectionsUpdate(bool force = false); static constexpr int MAX_SENSOR_COUNT = 3; @@ -96,10 +96,8 @@ class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem 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}; diff --git a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt index a32fa705bb46..ec3c95b627ce 100644 --- a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt +++ b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt @@ -38,6 +38,6 @@ px4_add_library(vehicle_angular_velocity target_link_libraries(vehicle_angular_velocity PRIVATE mathlib - sensor_corrections + sensor_calibration px4_work_queue ) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 9fe653c75501..6adb8ad021bb 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/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()); @@ -186,7 +185,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; @@ -215,7 +214,7 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) updateParams(); - _corrections.ParametersUpdate(); + _calibration.ParametersUpdate(); } } @@ -227,7 +226,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(); @@ -263,7 +262,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; // Differentiate angular velocity (after notch filter) const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)}; @@ -320,7 +319,7 @@ 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(); + _calibration.PrintStatus(); } } // namespace sensors diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index df524998d97f..6b700618dcf8 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -33,7 +33,7 @@ #pragma once -#include +#include #include #include @@ -91,7 +91,7 @@ class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkIte {this, ORB_ID(sensor_gyro), 2} }; - SensorCorrections _corrections; + SensorCalibration _calibration{SensorCalibration::SensorType::Gyroscope}; matrix::Vector3f _bias{0.f, 0.f, 0.f}; diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt index 7ce66b836bb9..f5b3bc4b96a6 100644 --- a/src/modules/sensors/vehicle_imu/CMakeLists.txt +++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt @@ -37,4 +37,4 @@ px4_add_library(vehicle_imu VehicleIMU.cpp VehicleIMU.hpp ) -target_link_libraries(vehicle_imu PRIVATE sensor_corrections px4_work_queue) +target_link_libraries(vehicle_imu PRIVATE sensor_calibration px4_work_queue) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 3187fcdf3c62..150af746d4ee 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/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(); @@ -105,8 +103,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); @@ -157,8 +155,8 @@ void VehicleIMU::Run() ScheduleDelayed(10_ms); ParametersUpdate(); - _accel_corrections.SensorCorrectionsUpdate(); - _gyro_corrections.SensorCorrectionsUpdate(); + _accel_calibration.SensorCorrectionsUpdate(); + _gyro_calibration.SensorCorrectionsUpdate(); bool update_integrator_config = false; @@ -174,10 +172,10 @@ 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); _gyro_error_count = gyro.error_count; - const Vector3f gyro_corrected{_gyro_corrections.Correct(Vector3f{gyro.x, gyro.y, gyro.z})}; + const Vector3f gyro_corrected{_gyro_calibration.Correct(Vector3f{gyro.x, gyro.y, gyro.z})}; _gyro_integrator.put(gyro.timestamp_sample, gyro_corrected); _last_timestamp_sample_gyro = gyro.timestamp_sample; @@ -203,10 +201,10 @@ 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); _accel_error_count = accel.error_count; - const Vector3f accel_corrected{_accel_corrections.Correct(Vector3f{accel.x, accel.y, accel.z})}; + const Vector3f accel_corrected{_accel_calibration.Correct(Vector3f{accel.x, accel.y, accel.z})}; _accel_integrator.put(accel.timestamp_sample, accel_corrected); _last_timestamp_sample_accel = accel.timestamp_sample; @@ -218,7 +216,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) @@ -267,14 +265,16 @@ void VehicleIMU::Run() if (_accel_integrator.reset(delta_velocity, accel_integral_dt) && _gyro_integrator.reset(delta_angle, gyro_integral_dt)) { + + UpdateAccelVibrationMetrics(delta_velocity); UpdateGyroVibrationMetrics(delta_angle); // vehicle_imu_status // publish first so that error counts are available synchronously if needed vehicle_imu_status_s status; - 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.accel_error_count = _accel_error_count; status.gyro_error_count = _gyro_error_count; status.accel_rate_hz = roundf(1e6f / _accel_interval.update_interval); @@ -290,17 +290,20 @@ 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(); - delta_angle.copyTo(imu.delta_angle); - delta_velocity.copyTo(imu.delta_velocity); - imu.delta_angle_dt = gyro_integral_dt; - imu.delta_velocity_dt = accel_integral_dt; - imu.delta_velocity_clipping = _delta_velocity_clipping; - imu.timestamp = hrt_absolute_time(); - _vehicle_imu_pub.publish(imu); + if (_accel_calibration.enabled() && _gyro_calibration.enabled()) { + // only publish if both accel and gyro are enabled + vehicle_imu_s imu; + imu.timestamp_sample = _last_timestamp_sample_gyro; + imu.accel_device_id = _accel_calibration.device_id(); + imu.gyro_device_id = _gyro_calibration.device_id(); + delta_angle.copyTo(imu.delta_angle); + delta_velocity.copyTo(imu.delta_velocity); + imu.delta_angle_dt = gyro_integral_dt; + imu.delta_velocity_dt = accel_integral_dt; + imu.delta_velocity_clipping = _delta_velocity_clipping; + imu.timestamp = hrt_absolute_time(); + _vehicle_imu_pub.publish(imu); + } // reset clip counts _delta_velocity_clipping = 0; @@ -341,7 +344,7 @@ void VehicleIMU::UpdateIntegratorConfiguration() _intervals_configured = true; 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); } } @@ -371,16 +374,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 diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index feceacdf7626..6c03406ac2e3 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -35,7 +35,7 @@ #include "Integrator.hpp" -#include +#include #include #include @@ -91,8 +91,8 @@ class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem 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 diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index a8e5328e9e13..1049b3737c95 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -39,11 +39,12 @@ #include "voted_sensors_update.h" -#include - +#include +#include +#include #include -#include -#include +#include +#include #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; - - for (unsigned driver_index = 0; driver_index < GYRO_COUNT_MAX; driver_index++) { - _gyro.enabled[driver_index] = true; - - char str[30] {}; - (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, driver_index); - - int fd = px4_open(str, O_RDWR); + // run through all IMUs + for (uint8_t uorb_index = 0; uorb_index < math::max(ACCEL_COUNT_MAX, GYRO_COUNT_MAX); uorb_index++) { + uORB::SubscriptionData imu{ORB_ID(vehicle_imu), uorb_index}; + imu.update(); - if (fd < 0) { - continue; - } + if (imu.get().timestamp > 0 && imu.get().accel_device_id > 0 && imu.get().gyro_device_id > 0) { - 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 (_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{ORB_ID(sensor_accel), i}; + sensor_accel.update(); - /* 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_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 diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index c7f613e7f4e4..24595a91caf7 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -41,8 +41,6 @@ #include "parameters.h" -#include -#include #include #include diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index 2e7dd351321f..3f1784773a77 100644 --- a/src/modules/simulator/simulator.cpp +++ b/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[]) diff --git a/src/modules/temperature_compensation/TemperatureCompensation.cpp b/src/modules/temperature_compensation/TemperatureCompensation.cpp index 1cc305405485..502f40cd2151 100644 --- a/src/modules/temperature_compensation/TemperatureCompensation.cpp +++ b/src/modules/temperature_compensation/TemperatureCompensation.cpp @@ -71,8 +71,6 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles ¶ 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 ¶ 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 ¶ 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; diff --git a/src/modules/temperature_compensation/TemperatureCompensation.h b/src/modules/temperature_compensation/TemperatureCompensation.h index c5ee39a7a32c..6ac6955b145e 100644 --- a/src/modules/temperature_compensation/TemperatureCompensation.h +++ b/src/modules/temperature_compensation/TemperatureCompensation.h @@ -84,15 +84,14 @@ class TemperatureCompensation * @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 @@ class TemperatureCompensation 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 @@ class TemperatureCompensation float x2; float x1; float x0; - float scale; float ref_temp; float min_temp; float max_temp; @@ -135,7 +133,6 @@ class TemperatureCompensation 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 @@ class TemperatureCompensation 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 @@ class TemperatureCompensation 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 @@ class TemperatureCompensation 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; diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp index 9b04e19377ee..a205703692b4 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp +++ b/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; + } } } } diff --git a/src/modules/temperature_compensation/temp_comp_params_accel.c b/src/modules/temperature_compensation/temp_comp_params_accel.c index 95c674c15dcb..6c24759e5889 100644 --- a/src/modules/temperature_compensation/temp_comp_params_accel.c +++ b/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. * diff --git a/src/modules/temperature_compensation/temp_comp_params_baro.c b/src/modules/temperature_compensation/temp_comp_params_baro.c index e11bd06a29c2..651c3ab9f31e 100644 --- a/src/modules/temperature_compensation/temp_comp_params_baro.c +++ b/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. * diff --git a/src/modules/temperature_compensation/temp_comp_params_gyro.c b/src/modules/temperature_compensation/temp_comp_params_gyro.c index 32f2855a6c62..7bd9ebf0ffca 100644 --- a/src/modules/temperature_compensation/temp_comp_params_gyro.c +++ b/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. * diff --git a/src/modules/temperature_compensation/temperature_calibration/accel.cpp b/src/modules/temperature_compensation/temperature_calibration/accel.cpp index a0464591eab2..4d87c3249cd0 100644 --- a/src/modules/temperature_compensation/temperature_calibration/accel.cpp +++ b/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; diff --git a/src/modules/temperature_compensation/temperature_calibration/accel.h b/src/modules/temperature_compensation/temperature_calibration/accel.h index 2cb1fac348ad..f464bb0cb564 100644 --- a/src/modules/temperature_compensation/temperature_calibration/accel.h +++ b/src/modules/temperature_compensation/temperature_calibration/accel.h @@ -47,11 +47,6 @@ class TemperatureCalibrationAccel : public TemperatureCalibrationCommon<3, 3> */ int finish(); - /** - * @see TemperatureCalibrationBase::reset_calibration() - */ - void reset_calibration(); - private: virtual inline int update_sensor_instance(PerSensorData &data, int sensor_sub); diff --git a/src/modules/temperature_compensation/temperature_calibration/baro.cpp b/src/modules/temperature_compensation/temperature_calibration/baro.cpp index 7927cd26ad4b..10f6889ff047 100644 --- a/src/modules/temperature_compensation/temperature_calibration/baro.cpp +++ b/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; diff --git a/src/modules/temperature_compensation/temperature_calibration/baro.h b/src/modules/temperature_compensation/temperature_calibration/baro.h index 2b91f3380a51..fc74e3033da4 100644 --- a/src/modules/temperature_compensation/temperature_calibration/baro.h +++ b/src/modules/temperature_compensation/temperature_calibration/baro.h @@ -50,11 +50,6 @@ class TemperatureCalibrationBaro : public TemperatureCalibrationCommon<1, POLYFI */ int finish(); - /** - * @see TemperatureCalibrationBase::reset_calibration() - */ - void reset_calibration(); - private: virtual int update_sensor_instance(PerSensorData &data, int sensor_sub); diff --git a/src/modules/temperature_compensation/temperature_calibration/common.h b/src/modules/temperature_compensation/temperature_calibration/common.h index 27ec331c9486..f45e7597d0e1 100644 --- a/src/modules/temperature_compensation/temperature_calibration/common.h +++ b/src/modules/temperature_compensation/temperature_calibration/common.h @@ -78,9 +78,6 @@ class TemperatureCalibrationBase */ virtual int finish() = 0; - /** reset all driver-level calibration parameters */ - virtual void reset_calibration() = 0; - protected: /** diff --git a/src/modules/temperature_compensation/temperature_calibration/gyro.cpp b/src/modules/temperature_compensation/temperature_calibration/gyro.cpp index 27a7f92587e0..714837862c77 100644 --- a/src/modules/temperature_compensation/temperature_calibration/gyro.cpp +++ b/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; diff --git a/src/modules/temperature_compensation/temperature_calibration/gyro.h b/src/modules/temperature_compensation/temperature_calibration/gyro.h index 702849e89054..c6f9a5df47f3 100644 --- a/src/modules/temperature_compensation/temperature_calibration/gyro.h +++ b/src/modules/temperature_compensation/temperature_calibration/gyro.h @@ -48,11 +48,6 @@ class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3> */ int finish(); - /** - * @see TemperatureCalibrationBase::reset_calibration() - */ - void reset_calibration(); - private: virtual int update_sensor_instance(PerSensorData &data, int sensor_sub); diff --git a/src/modules/temperature_compensation/temperature_calibration/task.cpp b/src/modules/temperature_compensation/temperature_calibration/task.cpp index a361b7b59395..61ab722010bd 100644 --- a/src/modules/temperature_compensation/temperature_calibration/task.cpp +++ b/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 diff --git a/src/modules/uORB/SubscriptionBlocking.hpp b/src/modules/uORB/SubscriptionBlocking.hpp index 3d9de3f023a6..7ec29e329b6e 100644 --- a/src/modules/uORB/SubscriptionBlocking.hpp +++ b/src/modules/uORB/SubscriptionBlocking.hpp @@ -64,13 +64,17 @@ class SubscriptionBlocking : public SubscriptionCallback 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 @@ class SubscriptionBlocking : public SubscriptionCallback } /** - * 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 @@ class SubscriptionBlocking : public SubscriptionCallback 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 @@ class SubscriptionBlocking : public SubscriptionCallback ts.tv_nsec = nsecs; if (px4_pthread_cond_timedwait(&_cv, &_mutex, &ts) == 0) { - return update(&data); + return updated(); } } } @@ -142,6 +145,22 @@ class SubscriptionBlocking : public SubscriptionCallback 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; From 86d1311a354eb838bc06f258805ecf018c6ec00d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 2 Jun 2020 08:36:34 -0400 Subject: [PATCH 2/2] accel & gyro cal improve loop iterator naming --- .../commander/accelerometer_calibration.cpp | 14 ++++++------ src/modules/commander/gyro_calibration.cpp | 22 +++++++++---------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index e072a395125e..dc31fec9f65f 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -416,18 +416,18 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS /* use the first sensor to pace the readout, but do per-sensor counts */ while (counts[0] < samples_num) { if (accel_sub[0].updatedBlocking(100000)) { - for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) { + for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { sensor_accel_s arp; - if (accel_sub[s].update(&arp)) { + 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); if (sensor_correction.timestamp > 0 && arp.device_id != 0) { - for (uint8_t i = 0; i < MAX_ACCEL_SENS; i++) { - if (sensor_correction.accel_device_ids[i] == arp.device_id) { - switch (i) { + 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; @@ -442,8 +442,8 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS } } - accel_sum[s] += Vector3f{arp.x, arp.y, arp.z} - offset; - counts[s]++; + accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset; + counts[accel_index]++; } } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 0dfbb4c24cbb..0c9ed6b52008 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -98,24 +98,24 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data if (gyro_sub[0].updatedBlocking(100000)) { 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; } sensor_gyro_s gyro_report; - if (gyro_sub[s].update(&gyro_report)) { + if (gyro_sub[gyro_index].update(&gyro_report)) { // 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 i = 0; i < MAX_GYROS; i++) { - if (sensor_correction.gyro_device_ids[i] == gyro_report.device_id) { - switch (i) { + 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; @@ -130,17 +130,17 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, gyro_worker_data } } - worker_data.offset[s] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - offset; - calibration_counter[s]++; + worker_data.offset[gyro_index] += Vector3f{gyro_report.x, gyro_report.y, gyro_report.z} - offset; + calibration_counter[gyro_index]++; - if (s == 0) { + if (gyro_index == 0) { worker_data.last_sample_0 = Vector3f{gyro_report.x, gyro_report.y, gyro_report.z}; } } // 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]; } }