fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add prototype IMU temperature compensation

Browse Source 
Enabled using TC_A_ENABLE and TC_G_ENABLE parameters
Disabled by default.
IMU offsets and scale factors for selected sensor published to sensor_correction topic
TODO Parameter storage method is cumbersome
This commit is contained in:
Paul Riseborough 2016-12-20 16:38:45 +11:00 committed by Lorenz Meier
parent 9599f58c5d
commit 63f032832f
7 changed files with 1565 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/sensors/CMakeLists.txt
View File

@ -43,6 +43,7 @@ px4_add_module(
sensors.cpp
sensors_init.cpp
parameters.cpp
temperature_compensation.cpp
DEPENDS
platforms__common

454
src/modules/sensors/temp_comp_params_accel.c Normal file
View File

@ -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 <gncsolns@gmail.com>
*/
/**
* 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);

454
src/modules/sensors/temp_comp_params_gyro.c Normal file
View File

@ -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 <gncsolns@gmail.com>
*/
/**
* 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);

249
src/modules/sensors/temperature_compensation.cpp Normal file
View File

@ -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 <gncsolns@gmail.com>
*/
#include "temperature_compensation.h"
#include <systemlib/param/param.h>
#include <stdio.h>
#include <px4_defines.h>
namespace sensors_temp_comp
{
int initialize_parameter_handles(ParameterHandles &parameter_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 &parameter_handles, Parameters &parameters)
{
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(&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;
}
}
/* 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(&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;
}
}
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;
}
}

176
src/modules/sensors/temperature_compensation.h Normal file
View File

@ -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 <gncsolns@gmail.com>
*/
#include <systemlib/param/param.h>
#include <mathlib/mathlib.h>
/* 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 &parameter_handles);
/**
* Read out the parameters using the handles into the parameters struct.
* @return 0 on succes, <0 on error
*/
int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters);
/*
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[]);
}

278
src/modules/sensors/voted_sensors_update.cpp
View File

@ -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()

12
src/modules/sensors/voted_sensors_update.h
View File

@ -54,9 +54,11 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_correction.h>
#include <DevMgr.hpp>
#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 */
};