diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 8cedab7b66..40c801236f 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -43,6 +43,7 @@ px4_add_module( sensors.cpp sensors_init.cpp parameters.cpp + temperature_compensation.cpp DEPENDS platforms__common diff --git a/src/modules/sensors/temp_comp_params_accel.c b/src/modules/sensors/temp_comp_params_accel.c new file mode 100644 index 0000000000..8d9dcfd0db --- /dev/null +++ b/src/modules/sensors/temp_comp_params_accel.c @@ -0,0 +1,454 @@ +/**************************************************************************** + * + * 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 temp_comp_params_accel.c + * + * Parameters required for temperature compensation of rate Accelerometers. + * + * @author Paul Riseborough + */ + +/** + * Set to 1 to enable thermal compensation for accelerometer sensors. Set to 0 to disable. + * + * @group Sensor Thermal Compensation + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(TC_A_ENABLE, 0); + +/* Accelerometer 0 */ + +/** + * ID of Accelerometer that the calibration is for. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_INT32(TC_A0_ID, 0); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_X3_0, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_X3_1, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_X3_2, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_X2_0, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_X2_1, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_X2_2, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_X1_0, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_X1_1, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_X1_2, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_X0_0, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_X0_1, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_X0_2, 0.0f); + +/** + * Accelerometer scale factor - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_SCL_0, 1.0f); + +/** + * Accelerometer scale factor - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_SCL_1, 1.0f); + +/** + * Accelerometer scale factor - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_SCL_2, 1.0f); + +/** + * Accelerometer calibration reference temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_TREF, 25.0f); + +/** + * Accelerometer calibration minimum temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_TMIN, 0.0f); + +/** + * Accelerometer calibration maximum temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A0_TMAX, 100.0f); + +/* Accelerometer 1 */ + +/** + * ID of Accelerometer that the calibration is for. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_INT32(TC_A1_ID, 0); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_X3_0, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_X3_1, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_X3_2, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_X2_0, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_X2_1, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_X2_2, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_X1_0, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_X1_1, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_X1_2, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_X0_0, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_X0_1, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_X0_2, 0.0f); + +/** + * Accelerometer scale factor - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_SCL_0, 1.0f); + +/** + * Accelerometer scale factor - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_SCL_1, 1.0f); + +/** + * Accelerometer scale factor - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_SCL_2, 1.0f); + +/** + * Accelerometer calibration reference temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_TREF, 25.0f); + +/** + * Accelerometer calibration minimum temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_TMIN, 0.0f); + +/** + * Accelerometer calibration maximum temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A1_TMAX, 100.0f); + +/* Accelerometer 2 */ + +/** + * ID of Accelerometer that the calibration is for. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_INT32(TC_A2_ID, 0); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_X3_0, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_X3_1, 0.0f); + +/** + * Accelerometer offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_X3_2, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_X2_0, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_X2_1, 0.0f); + +/** + * Accelerometer offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_X2_2, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_X1_0, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_X1_1, 0.0f); + +/** + * Accelerometer offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_X1_2, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_X0_0, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_X0_1, 0.0f); + +/** + * Accelerometer offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_X0_2, 0.0f); + +/** + * Accelerometer scale factor - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_SCL_0, 1.0f); + +/** + * Accelerometer scale factor - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_SCL_1, 1.0f); + +/** + * Accelerometer scale factor - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_SCL_2, 1.0f); + +/** + * Accelerometer calibration reference temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_TREF, 25.0f); + +/** + * Accelerometer calibration minimum temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_TMIN, 0.0f); + +/** + * Accelerometer calibration maximum temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_A2_TMAX, 100.0f); diff --git a/src/modules/sensors/temp_comp_params_gyro.c b/src/modules/sensors/temp_comp_params_gyro.c new file mode 100644 index 0000000000..82e5475b28 --- /dev/null +++ b/src/modules/sensors/temp_comp_params_gyro.c @@ -0,0 +1,454 @@ +/**************************************************************************** + * + * 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 temp_comp_params_gyro.c + * + * Parameters required for temperature compensation of rate gyros. + * + * @author Paul Riseborough + */ + +/** + * Set to 1 to enable thermal compensation for rate gyro sensors. Set to 0 to disable. + * + * @group Sensor Thermal Compensation + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(TC_G_ENABLE, 0); + +/* Gyro 0 */ + +/** + * ID of Gyro that the calibration is for. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_INT32(TC_G0_ID, 0); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_X3_0, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_X3_1, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_X3_2, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_X2_0, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_X2_1, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_X2_2, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_X1_0, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_X1_1, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_X1_2, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_X0_0, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_X0_1, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_X0_2, 0.0f); + +/** + * Gyro scale factor - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_SCL_0, 1.0f); + +/** + * Gyro scale factor - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_SCL_1, 1.0f); + +/** + * Gyro scale factor - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_SCL_2, 1.0f); + +/** + * Gyro calibration reference temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_TREF, 25.0f); + +/** + * Gyro calibration minimum temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_TMIN, 0.0f); + +/** + * Gyro calibration maximum temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G0_TMAX, 100.0f); + +/* Gyro 1 */ + +/** + * ID of Gyro that the calibration is for. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_INT32(TC_G1_ID, 0); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_X3_0, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_X3_1, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_X3_2, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_X2_0, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_X2_1, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_X2_2, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_X1_0, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_X1_1, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_X1_2, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_X0_0, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_X0_1, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_X0_2, 0.0f); + +/** + * Gyro scale factor - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_SCL_0, 1.0f); + +/** + * Gyro scale factor - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_SCL_1, 1.0f); + +/** + * Gyro scale factor - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_SCL_2, 1.0f); + +/** + * Gyro calibration reference temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_TREF, 25.0f); + +/** + * Gyro calibration minimum temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_TMIN, 0.0f); + +/** + * Gyro calibration maximum temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G1_TMAX, 100.0f); + +/* Gyro 2 */ + +/** + * ID of Gyro that the calibration is for. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_INT32(TC_G2_ID, 0); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_X3_0, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_X3_1, 0.0f); + +/** + * Gyro rate offset temperature ^3 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_X3_2, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_X2_0, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_X2_1, 0.0f); + +/** + * Gyro rate offset temperature ^2 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_X2_2, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_X1_0, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_X1_1, 0.0f); + +/** + * Gyro rate offset temperature ^1 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_X1_2, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_X0_0, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_X0_1, 0.0f); + +/** + * Gyro rate offset temperature ^0 polynomial coefficient - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_X0_2, 0.0f); + +/** + * Gyro scale factor - X axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_SCL_0, 1.0f); + +/** + * Gyro scale factor - Y axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_SCL_1, 1.0f); + +/** + * Gyro scale factor - Z axis. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_SCL_2, 1.0f); + +/** + * Gyro calibration reference temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_TREF, 25.0f); + +/** + * Gyro calibration minimum temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_TMIN, 0.0f); + +/** + * Gyro calibration maximum temperature. + * + * @group Sensor Thermal Compensation + */ +PARAM_DEFINE_FLOAT(TC_G2_TMAX, 100.0f); diff --git a/src/modules/sensors/temperature_compensation.cpp b/src/modules/sensors/temperature_compensation.cpp new file mode 100644 index 0000000000..acc834ba6b --- /dev/null +++ b/src/modules/sensors/temperature_compensation.cpp @@ -0,0 +1,249 @@ +/**************************************************************************** + * + * Copyright (c) 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 temperature_compensation.cpp + * + * Sensors temperature compensation methods + * + * @author Paul Riseborough + */ + +#include "temperature_compensation.h" +#include +#include +#include + +namespace sensors_temp_comp +{ + +int initialize_parameter_handles(ParameterHandles ¶meter_handles) +{ + char nbuf[16]; + + /* rate gyro calibration parameters */ + sprintf(nbuf, "TC_G_ENABLE"); + parameter_handles.gyro_tc_enable = param_find(nbuf); + + for (unsigned j = 0; j < 3; j++) { + sprintf(nbuf, "TC_G%d_ID", j); + parameter_handles.gyro_cal_handles[j].ID = param_find(nbuf); + + for (unsigned i = 0; i < 3; i++) { + sprintf(nbuf, "TC_G%d_X3_%d", j, i); + parameter_handles.gyro_cal_handles[j].x3[i] = param_find(nbuf); + sprintf(nbuf, "TC_G%d_X2_%d", j, i); + parameter_handles.gyro_cal_handles[j].x2[i] = param_find(nbuf); + sprintf(nbuf, "TC_G%d_X1_%d", j, i); + 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); + parameter_handles.gyro_cal_handles[j].ref_temp = param_find(nbuf); + sprintf(nbuf, "TC_G%d_TMIN", j); + parameter_handles.gyro_cal_handles[j].min_temp = param_find(nbuf); + sprintf(nbuf, "TC_G%d_TMAX", j); + parameter_handles.gyro_cal_handles[j].max_temp = param_find(nbuf); + } + + /* accelerometer calibration parameters */ + sprintf(nbuf, "TC_A_ENABLE"); + parameter_handles.accel_tc_enable = param_find(nbuf); + + for (unsigned j = 0; j < 3; j++) { + sprintf(nbuf, "TC_A%d_ID", j); + parameter_handles.accel_cal_handles[j].ID = param_find(nbuf); + + for (unsigned i = 0; i < 3; i++) { + sprintf(nbuf, "TC_A%d_X3_%d", j, i); + parameter_handles.accel_cal_handles[j].x3[i] = param_find(nbuf); + sprintf(nbuf, "TC_A%d_X2_%d", j, i); + parameter_handles.accel_cal_handles[j].x2[i] = param_find(nbuf); + sprintf(nbuf, "TC_A%d_X1_%d", j, i); + 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); + parameter_handles.accel_cal_handles[j].ref_temp = param_find(nbuf); + sprintf(nbuf, "TC_A%d_TMIN", j); + parameter_handles.accel_cal_handles[j].min_temp = param_find(nbuf); + sprintf(nbuf, "TC_A%d_TMAX", j); + parameter_handles.accel_cal_handles[j].max_temp = param_find(nbuf); + } + + return PX4_OK; +} + +int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters) +{ + int ret = PX4_OK; + + /* rate gyro calibration parameters */ + param_get(parameter_handles.gyro_tc_enable, &(parameters.gyro_tc_enable)); + + for (unsigned j = 0; j < 3; j++) { + if (param_get(parameter_handles.gyro_cal_handles[j].ID, &(parameters.gyro_cal_data[j].ID)) == PX4_OK) { + param_get(parameter_handles.gyro_cal_handles[j].ref_temp, &(parameters.gyro_cal_data[j].ref_temp)); + param_get(parameter_handles.gyro_cal_handles[j].min_temp, &(parameters.gyro_cal_data[j].min_temp)); + param_get(parameter_handles.gyro_cal_handles[j].min_temp, &(parameters.gyro_cal_data[j].min_temp)); + + for (unsigned int i = 0; i < 3; i++) { + param_get(parameter_handles.gyro_cal_handles[j].x3[i], &(parameters.gyro_cal_data[j].x3[i])); + 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 + memset(¶meters.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; + } + } + + /* accelerometer calibration parameters */ + param_get(parameter_handles.accel_tc_enable, &(parameters.accel_tc_enable)); + + for (unsigned j = 0; j < 3; j++) { + if (param_get(parameter_handles.accel_cal_handles[j].ID, &(parameters.accel_cal_data[j].ID)) == PX4_OK) { + param_get(parameter_handles.accel_cal_handles[j].ref_temp, &(parameters.accel_cal_data[j].ref_temp)); + param_get(parameter_handles.accel_cal_handles[j].min_temp, &(parameters.accel_cal_data[j].min_temp)); + param_get(parameter_handles.accel_cal_handles[j].min_temp, &(parameters.accel_cal_data[j].min_temp)); + + for (unsigned int i = 0; i < 3; i++) { + param_get(parameter_handles.accel_cal_handles[j].x3[i], &(parameters.accel_cal_data[j].x3[i])); + 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 + memset(¶meters.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; + } + } + + return ret; +} + +bool correct_data_1D(struct SENSOR_CAL_DATA_1D &coef, const float &measured_temp, float &offset) +{ + bool ret = true; + + // clip the measured temperature to remain within the calibration range + float delta_temp; + + if (measured_temp > coef.max_temp) { + delta_temp = coef.max_temp; + ret = false; + + } else if (measured_temp < coef.min_temp) { + delta_temp = coef.min_temp; + ret = false; + + } else { + delta_temp = measured_temp; + + } + + delta_temp -= coef.ref_temp; + + // calulate the offset + offset = coef.x0 + coef.x1 * delta_temp + coef.x2 * delta_temp * delta_temp + coef.x3 * delta_temp * delta_temp * + delta_temp; + + return ret; + +} + +bool calc_thermal_offsets_3D(struct SENSOR_CAL_DATA_3D &coef, const float &measured_temp, float offset[]) +{ + bool ret = true; + + // clip the measured temperature to remain within the calibration range + float delta_temp; + + if (measured_temp > coef.max_temp) { + delta_temp = coef.max_temp; + ret = false; + + } else if (measured_temp < coef.min_temp) { + delta_temp = coef.min_temp; + ret = false; + + } else { + delta_temp = measured_temp; + + } + + delta_temp -= coef.ref_temp; + + // calulate the offsets + float delta_temp_2 = delta_temp * delta_temp; + float delta_temp_3 = delta_temp_2 * delta_temp; + + for (uint8_t i = 0; i < 3; i++) { + offset[i] = coef.x0[i] + coef.x1[i] * delta_temp + coef.x2[i] * delta_temp_2 + coef.x3[i] * delta_temp_3; + + } + + return ret; + +} + +} diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/sensors/temperature_compensation.h new file mode 100644 index 0000000000..51d30e7119 --- /dev/null +++ b/src/modules/sensors/temperature_compensation.h @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 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 temperature_compensation.h + * + * Sensor correction methods + * + * @author Paul Riseborough + */ + +#include +#include + +/* These structs struct define the parameters used by the temperature compensation algorithm + +Input: + +measured_temp : temperature measured at the sensor (deg C) +raw_value : reading from the sensor before compensation +corrected_value : reading from the sensor after compensation for errors + +Compute: + +delta_temp = measured_temp - ref_temp +offset = x3 * delta_temp^3 + x2 * delta_temp^2 + x1 * delta_temp + x0 +corrected_value = raw_value * scale + offset + +*/ +namespace sensors_temp_comp +{ + +struct SENSOR_CAL_DATA_1D { + int ID; /**< sensor device ID*/ + float x3; /**< x^3 term of polynomial */ + float x2; /**< x^2 term of polynomial */ + float x1; /**< x^1 term of polynomial */ + float x0; /**< x^0 / offset term of polynomial */ + float scale; /**< 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 */ +}; + +struct SENSOR_CAL_DATA_3D { + int ID; /**< sensor device ID*/ + float x3[3]; /**< x^3 term of polynomial */ + 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 */ +}; + +struct SENSOR_CAL_HANDLES_1D { + param_t ID; + param_t x3; + param_t x2; + param_t x1; + param_t x0; + param_t scale; + param_t ref_temp; + param_t min_temp; + param_t max_temp; +}; + +struct SENSOR_CAL_HANDLES_3D { + param_t ID; + param_t x3[3]; + 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; +}; + +struct Parameters { + int gyro_tc_enable; + SENSOR_CAL_DATA_3D gyro_cal_data[3]; + int accel_tc_enable; + SENSOR_CAL_DATA_3D accel_cal_data[3]; +}; + +struct ParameterHandles { + param_t gyro_tc_enable; + SENSOR_CAL_HANDLES_3D gyro_cal_handles[3]; + param_t accel_tc_enable; + SENSOR_CAL_HANDLES_3D accel_cal_handles[3]; +}; + +/** + * initialize ParameterHandles struct + * @return 0 on succes, <0 on error + */ +int initialize_parameter_handles(ParameterHandles ¶meter_handles); + + +/** + * Read out the parameters using the handles into the parameters struct. + * @return 0 on succes, <0 on error + */ +int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters); + +/* + +Calculate the offset required to compensate the sensor for temperature effects +If the measured temperature is outside the calibration range, clip the temperature to remain within the range and return false. +If the measured temperature is within the calibration range, return true. + +Arguments: + +coef : reference to struct containing calibration coefficients +measured_temp : temperature measured at the sensor (deg C) +offset : reference to sensor offset + +Returns: + +Boolean true if the measured temperature is inside the valid range for the compensation + +*/ +bool correct_data_1D(SENSOR_CAL_DATA_1D &coef, const float &measured_temp, float &offset); + +/* + +Calculate the offsets required to compensate the sensor for temperature effects +If the measured temperature is outside the calibration range, clip the temperature to remain within the range and return false. +If the measured temperature is within the calibration range, return true. + +Arguments: + +coef : reference to struct containing calibration coefficients +measured_temp : temperature measured at the sensor (deg C) +offset : reference to sensor offset - array of 3 + +Returns: + +Boolean true if the measured temperature is inside the valid range for the compensation + +*/ +bool calc_thermal_offsets_3D(SENSOR_CAL_DATA_3D &coef, const float &measured_temp, float offset[]); + +} diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 38be7f8f3f..a4e25d26ba 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -73,6 +73,27 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw) raw.timestamp = 0; initialize_sensors(); + + // get the parameter handles for the gyro temperature compensation parameters + sensors_temp_comp::initialize_parameter_handles(_thermal_correction_param_handles); + + // ensure pointer to sensor corrections publiations is defined + _sensor_correction_pub = nullptr; + + // initialise the corrections + memset(&_corrections, 0, sizeof(_corrections)); + memset(&_accel_offset, 0, sizeof(_accel_offset)); + memset(&_gyro_offset, 0, sizeof(_gyro_offset)); + + for (unsigned i = 0; i < 3; i++) { + _corrections.gyro_scale[i] = 1.0f; + _corrections.accel_scale[i] = 1.0f; + for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) { + _accel_scale[j][i] = 1.0f; + _gyro_scale[j][i] = 1.0f; + } + } + return 0; } @@ -121,6 +142,9 @@ void VotedSensorsUpdate::parameters_update() unsigned gyro_count = 0; unsigned accel_count = 0; + /* get stored sensor hub temperature compensation parameters */ + sensors_temp_comp::update_parameters(_thermal_correction_param_handles, _thermal_correction_param); + /* run through all gyro sensors */ for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { @@ -135,7 +159,7 @@ void VotedSensorsUpdate::parameters_update() bool config_ok = false; - /* run through all stored calibrations */ + /* run through all stored calibrations that are applied at the driver level*/ for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -388,136 +412,263 @@ void VotedSensorsUpdate::parameters_update() void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) { - for (unsigned i = 0; i < _accel.subscription_count; i++) { + for (unsigned uorb_index = 0; uorb_index < _accel.subscription_count; uorb_index++) { bool accel_updated; - orb_check(_accel.subscription[i], &accel_updated); + orb_check(_accel.subscription[uorb_index], &accel_updated); if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel), _accel.subscription[i], &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report); if (accel_report.timestamp == 0) { continue; //ignore invalid data } // First publication with data - if (_accel.priority[i] == 0) { + if (_accel.priority[uorb_index] == 0) { int32_t priority = 0; - orb_priority(_accel.subscription[i], &priority); - _accel.priority[i] = (uint8_t)priority; + orb_priority(_accel.subscription[uorb_index], &priority); + _accel.priority[uorb_index] = (uint8_t)priority; } if (accel_report.integral_dt != 0) { - math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral); - vect_int = _board_rotation * vect_int; + /* + * Using data that has been integrated in the driver before downsampling is preferred + * becasue it reduces aliasing errors. Correct the raw sensor data for scale factor errors + * and offsets due to temperature variation. It is assumed that any filtering of input + * data required is performed in the sensor driver, preferably before downsampling. + */ - float dt = accel_report.integral_dt / 1.e6f; - _last_sensor_data[i].accelerometer_integral_dt = dt; + // convert the delta velocities to an equivalent acceleration before application of corrections + float dt_inv = 1.e6f / accel_report.integral_dt; + math::Vector<3> accel_data(accel_report.x_integral * dt_inv , accel_report.y_integral * dt_inv , + accel_report.z_integral * dt_inv); - _last_sensor_data[i].accelerometer_m_s2[0] = vect_int(0) / dt; - _last_sensor_data[i].accelerometer_m_s2[1] = vect_int(1) / dt; - _last_sensor_data[i].accelerometer_m_s2[2] = vect_int(2) / dt; + if (_thermal_correction_param.accel_tc_enable == 1) { + // search through the available compensation parameter sets looking for one with a matching sensor ID and corect data if found + for (unsigned param_index = 0; param_index < 3; param_index++) { + if (accel_report.device_id == _thermal_correction_param.accel_cal_data[param_index].ID) { + // get the offsets + sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.accel_cal_data[param_index], accel_report.temperature, _accel_offset[uorb_index]); - } else { - //using the value instead of the integral (the integral is the prefered choice) - math::Vector<3> vect_val(accel_report.x, accel_report.y, accel_report.z); - vect_val = _board_rotation * vect_val; + // get the scale factors and correct the data + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + _accel_scale[uorb_index][axis_index] = _thermal_correction_param.accel_cal_data[param_index].scale[axis_index]; + accel_data(axis_index) = accel_data(axis_index) * _accel_scale[uorb_index][axis_index] + _accel_offset[uorb_index][axis_index]; + } - if (_last_accel_timestamp[i] == 0) { - _last_accel_timestamp[i] = accel_report.timestamp - 1000; + break; + + } + } } - _last_sensor_data[i].accelerometer_integral_dt = - (accel_report.timestamp - _last_accel_timestamp[i]) / 1.e6f; - _last_sensor_data[i].accelerometer_m_s2[0] = vect_val(0); - _last_sensor_data[i].accelerometer_m_s2[1] = vect_val(1); - _last_sensor_data[i].accelerometer_m_s2[2] = vect_val(2); + // rotate corrected measurements from sensor to body frame + accel_data = _board_rotation * accel_data; + + // write corrected data to uORB struct + _last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt / 1.e6f; + _last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0); + _last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1); + _last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2); + + } else { + // using the value instead of the integral (the integral is the prefered choice) + + // Correct each sensor for temperature effects + // Filtering and/or downsampling of temperature should be performed in the driver layer + math::Vector<3> accel_data(accel_report.x , accel_report.y , accel_report.z); + + if (_thermal_correction_param.accel_tc_enable == 1) { + // search through the available compensation parameter sets looking for one with a matching sensor ID + for (unsigned param_index = 0; param_index < 3; param_index++) { + if (accel_report.device_id == _thermal_correction_param.accel_cal_data[param_index].ID) { + // get the offsets + sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.accel_cal_data[param_index], accel_report.temperature, _accel_offset[uorb_index]); + + // get the scale factors and correct the data + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + _accel_scale[uorb_index][axis_index] = _thermal_correction_param.gyro_cal_data[param_index].scale[axis_index]; + accel_data(axis_index) = accel_data(axis_index) * _accel_scale[uorb_index][axis_index] + _accel_offset[uorb_index][axis_index]; + } + + break; + + } + } + } + + // rotate corrected measurements from sensor to body frame + accel_data = _board_rotation * accel_data; + + // handle the cse where this is our first output + if (_last_accel_timestamp[uorb_index] == 0) { + _last_accel_timestamp[uorb_index] = accel_report.timestamp - 1000; + } + + // approximate the delta time using the difference in accel data time stamps + // and write to uORB struct + _last_sensor_data[uorb_index].accelerometer_integral_dt = + (accel_report.timestamp - _last_accel_timestamp[uorb_index]) / 1.e6f; + + // write corrected body frame acceleration to uORB struct + _last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0); + _last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1); + _last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2); } - _last_accel_timestamp[i] = accel_report.timestamp; - _accel.voter.put(i, accel_report.timestamp, _last_sensor_data[i].accelerometer_m_s2, - accel_report.error_count, _accel.priority[i]); + _last_accel_timestamp[uorb_index] = accel_report.timestamp; + _accel.voter.put(uorb_index, accel_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2, accel_report.error_count, _accel.priority[uorb_index]); } } + // find the best sensor int best_index; _accel.voter.get_best(hrt_absolute_time(), &best_index); + // write the best sensor data to the output variables if (best_index >= 0) { - raw.accelerometer_m_s2[0] = _last_sensor_data[best_index].accelerometer_m_s2[0]; - raw.accelerometer_m_s2[1] = _last_sensor_data[best_index].accelerometer_m_s2[1]; - raw.accelerometer_m_s2[2] = _last_sensor_data[best_index].accelerometer_m_s2[2]; raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt; _accel.last_best_vote = (uint8_t)best_index; + _corrections.accel_select = (uint8_t)best_index; + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + raw.accelerometer_m_s2[axis_index] = _last_sensor_data[best_index].accelerometer_m_s2[axis_index]; + _corrections.accel_offset[axis_index] = _accel_offset[best_index][axis_index]; + _corrections.accel_scale[axis_index] = _accel_scale[best_index][axis_index]; + } } } void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) { - for (unsigned i = 0; i < _gyro.subscription_count; i++) { + for (unsigned uorb_index = 0; uorb_index < _gyro.subscription_count; uorb_index++) { bool gyro_updated; - orb_check(_gyro.subscription[i], &gyro_updated); + orb_check(_gyro.subscription[uorb_index], &gyro_updated); if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[i], &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report); if (gyro_report.timestamp == 0) { continue; //ignore invalid data } // First publication with data - if (_gyro.priority[i] == 0) { + if (_gyro.priority[uorb_index] == 0) { int32_t priority = 0; - orb_priority(_gyro.subscription[i], &priority); - _gyro.priority[i] = (uint8_t)priority; + orb_priority(_gyro.subscription[uorb_index], &priority); + _gyro.priority[uorb_index] = (uint8_t)priority; } if (gyro_report.integral_dt != 0) { - math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral); - vect_int = _board_rotation * vect_int; + /* + * Using data that has been integrated in the driver before downsampling is preferred + * becasue it reduces aliasing errors. Correct the raw sensor data for scale factor errors + * and offsets due to temperature variation. It is assumed that any filtering of input + * data required is performed in the sensor driver, preferably before downsampling. + */ - float dt = gyro_report.integral_dt / 1.e6f; - _last_sensor_data[i].gyro_integral_dt = dt; + // convert the delta angles to an equivalent angular rate before application of corrections + float dt_inv = 1.e6f / gyro_report.integral_dt; + math::Vector<3> gyro_rate(gyro_report.x_integral * dt_inv , gyro_report.y_integral * dt_inv , + gyro_report.z_integral * dt_inv); - _last_sensor_data[i].gyro_rad[0] = vect_int(0) / dt; - _last_sensor_data[i].gyro_rad[1] = vect_int(1) / dt; - _last_sensor_data[i].gyro_rad[2] = vect_int(2) / dt; + if (_thermal_correction_param.gyro_tc_enable == 1) { + // search through the available compensation parameter sets looking for one with a matching sensor ID and correct data if found + for (unsigned param_index = 0; param_index < 3; param_index++) { + if (gyro_report.device_id == _thermal_correction_param.gyro_cal_data[param_index].ID) { + // get the offsets + sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.gyro_cal_data[param_index], gyro_report.temperature, _gyro_offset[uorb_index]); + + // get the sensor scale factors and correct the data + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + _gyro_scale[uorb_index][axis_index] = _thermal_correction_param.gyro_cal_data[param_index].scale[axis_index]; + gyro_rate(axis_index) = gyro_rate(axis_index) * _gyro_scale[uorb_index][axis_index] + _gyro_offset[uorb_index][axis_index]; + } + + break; + + } + } + } + + // rotate corrected measurements from sensor to body frame + gyro_rate = _board_rotation * gyro_rate; + + // write to uORB struct + _last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt / 1.e6f; + _last_sensor_data[uorb_index].gyro_rad[0] = gyro_rate(0); + _last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1); + _last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2); } else { //using the value instead of the integral (the integral is the prefered choice) - math::Vector<3> vect_val(gyro_report.x, gyro_report.y, gyro_report.z); - vect_val = _board_rotation * vect_val; - if (_last_sensor_data[i].timestamp == 0) { - _last_sensor_data[i].timestamp = gyro_report.timestamp - 1000; + // Correct each sensor for temperature effects + // Filtering and/or downsampling of temperature should be performed in the driver layer + math::Vector<3> gyro_rate(gyro_report.x, gyro_report.y, gyro_report.z); + + if (_thermal_correction_param.gyro_tc_enable == 1) { + // search through the available compensation parameter sets looking for one with a matching sensor ID + for (unsigned param_index = 0; param_index < 3; param_index++) { + if (gyro_report.device_id == _thermal_correction_param.gyro_cal_data[param_index].ID) { + // get the offsets + sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.gyro_cal_data[param_index], gyro_report.temperature, _gyro_offset[uorb_index]); + + // get the sensor scale factors and correct the data + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + _gyro_scale[uorb_index][axis_index] = _thermal_correction_param.gyro_cal_data[param_index].scale[axis_index]; + gyro_rate(axis_index) = gyro_rate(axis_index) * _gyro_scale[uorb_index][axis_index] + _gyro_offset[uorb_index][axis_index]; + } + + break; + + } + } } - _last_sensor_data[i].gyro_integral_dt = - (gyro_report.timestamp - _last_sensor_data[i].timestamp) / 1.e6f; - _last_sensor_data[i].gyro_rad[0] = vect_val(0); - _last_sensor_data[i].gyro_rad[1] = vect_val(1); - _last_sensor_data[i].gyro_rad[2] = vect_val(2); + // rotate corrected measurements from sensor to body frame + gyro_rate = _board_rotation * gyro_rate; + + // handle the case where this is our first output + if (_last_sensor_data[uorb_index].timestamp == 0) { + _last_sensor_data[uorb_index].timestamp = gyro_report.timestamp - 1000; + } + + // approximate the delta time using the difference in gyro data time stamps + // and write to uORB struct + _last_sensor_data[uorb_index].gyro_integral_dt = + (gyro_report.timestamp - _last_sensor_data[uorb_index].timestamp) / 1.e6f; + + // write corrected body frame rates to uORB struct + _last_sensor_data[uorb_index].gyro_rad[0] = gyro_rate(0); + _last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1); + _last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2); } - _last_sensor_data[i].timestamp = gyro_report.timestamp; - _gyro.voter.put(i, gyro_report.timestamp, _last_sensor_data[i].gyro_rad, - gyro_report.error_count, _gyro.priority[i]); + _last_sensor_data[uorb_index].timestamp = gyro_report.timestamp; + _gyro.voter.put(uorb_index, gyro_report.timestamp, _last_sensor_data[uorb_index].gyro_rad, + gyro_report.error_count, _gyro.priority[uorb_index]); } } + // find the best sensor int best_index; _gyro.voter.get_best(hrt_absolute_time(), &best_index); + // write data for the best sensor to output variables if (best_index >= 0) { - raw.gyro_rad[0] = _last_sensor_data[best_index].gyro_rad[0]; - raw.gyro_rad[1] = _last_sensor_data[best_index].gyro_rad[1]; - raw.gyro_rad[2] = _last_sensor_data[best_index].gyro_rad[2]; raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt; raw.timestamp = _last_sensor_data[best_index].timestamp; _gyro.last_best_vote = (uint8_t)best_index; + _corrections.gyro_select = (uint8_t)best_index; + for (unsigned axis_index = 0; axis_index < 3; axis_index++) { + raw.gyro_rad[axis_index] = _last_sensor_data[best_index].gyro_rad[axis_index]; + _corrections.gyro_offset[axis_index] = _gyro_offset[best_index][axis_index]; + _corrections.gyro_scale[axis_index] = _gyro_scale[best_index][axis_index]; + } } } @@ -757,6 +908,15 @@ void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw) gyro_poll(raw); mag_poll(raw); baro_poll(raw); + + // publish sensor corrections + if (_sensor_correction_pub == nullptr) { + _sensor_correction_pub = orb_advertise(ORB_ID(sensor_correction), &_corrections); + + } else { + orb_publish(ORB_ID(sensor_correction), _sensor_correction_pub, &_corrections); + + } } void VotedSensorsUpdate::check_failover() diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index d5797112ef..47823b2e17 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -54,9 +54,11 @@ #include #include +#include #include +#include "temperature_compensation.h" namespace sensors { @@ -261,6 +263,16 @@ private: float _accel_diff[3][2]; /**< filtered accel differences between IMU units (m/s/s) */ float _gyro_diff[3][2]; /**< filtered gyro differences between IMU uinits (rad/s) */ + /* sensor thermal compensation */ + sensors_temp_comp::Parameters _thermal_correction_param; /**< copy of the thermal correction parameters*/ + sensors_temp_comp::ParameterHandles _thermal_correction_param_handles; /**< handles for the thermal correction parameters */ + struct sensor_correction_s _corrections = {}; /**< struct containing the sensor corrections to be published to the uORB*/ + orb_advert_t _sensor_correction_pub; /**< handle to the sensor correction uORB topic */ + float _accel_offset[SENSOR_COUNT_MAX][3]; /**< offsets to be added to the raw accel data after scale factor correction */ + float _accel_scale[SENSOR_COUNT_MAX][3]; /**< scale factor corrections to be applied to the raw accel data before offsets are added */ + float _gyro_offset[SENSOR_COUNT_MAX][3]; /**< offsets to be added to the raw angular rate data after scale factor correction */ + float _gyro_scale[SENSOR_COUNT_MAX][3]; /**< scale factor corrections to be applied to the raw angular rate data before offsets are added */ + };