fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 16:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

create temperature_compensation module

Browse Source 
- this is a new module for temperature compensation that consolidates the functionality previously handled in the sensors module (calculating runtime thermal corrections) and the events module (online thermal calibration)
 - by collecting this functionality into a single module we can optionally disable it on systems where it's not used and save some flash (if disabled at build time) or memory (disabled at run time)
This commit is contained in:
Daniel Agar
2020-01-20 21:42:42 -05:00
committed by GitHub
parent dedb4e8267
commit dc05ceaad2
87 changed files with 978 additions and 739 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/CMakeLists.txt
-2
View File
@@ -42,8 +42,6 @@ px4_add_module(
voted_sensors_update.cpp
sensors.cpp
parameters.cpp
temperature_compensation.cpp
DEPENDS
airspeed
conversion
src/modules/sensors/temp_comp_params_accel.c
-512
View File
@@ -1,512 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 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>
*/
/**
* Thermal compensation for accelerometer sensors.
*
* @group Thermal Compensation
* @min 0
* @max 1
* @boolean
*/
PARAM_DEFINE_INT32(TC_A_ENABLE, 0);
/* Accelerometer 0 */
/**
* ID of Accelerometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_A0_ID, 0);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X3_0, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X3_1, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X3_2, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X2_0, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X2_1, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X2_2, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X1_0, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X1_1, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X1_2, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X0_0, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X0_1, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_X0_2, 0.0f);
/**
* Accelerometer scale factor - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_SCL_0, 1.0f);
/**
* Accelerometer scale factor - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_SCL_1, 1.0f);
/**
* Accelerometer scale factor - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_SCL_2, 1.0f);
/**
* Accelerometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_TREF, 25.0f);
/**
* Accelerometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_TMIN, 0.0f);
/**
* Accelerometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A0_TMAX, 100.0f);
/* Accelerometer 1 */
/**
* ID of Accelerometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_A1_ID, 0);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X3_0, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X3_1, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X3_2, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X2_0, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X2_1, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X2_2, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X1_0, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X1_1, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X1_2, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X0_0, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X0_1, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_X0_2, 0.0f);
/**
* Accelerometer scale factor - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_SCL_0, 1.0f);
/**
* Accelerometer scale factor - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_SCL_1, 1.0f);
/**
* Accelerometer scale factor - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_SCL_2, 1.0f);
/**
* Accelerometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_TREF, 25.0f);
/**
* Accelerometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_TMIN, 0.0f);
/**
* Accelerometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A1_TMAX, 100.0f);
/* Accelerometer 2 */
/**
* ID of Accelerometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_A2_ID, 0);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X3_0, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X3_1, 0.0f);
/**
* Accelerometer offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X3_2, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X2_0, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X2_1, 0.0f);
/**
* Accelerometer offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X2_2, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X1_0, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X1_1, 0.0f);
/**
* Accelerometer offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X1_2, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X0_0, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X0_1, 0.0f);
/**
* Accelerometer offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_X0_2, 0.0f);
/**
* Accelerometer scale factor - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_SCL_0, 1.0f);
/**
* Accelerometer scale factor - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_SCL_1, 1.0f);
/**
* Accelerometer scale factor - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_SCL_2, 1.0f);
/**
* Accelerometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_TREF, 25.0f);
/**
* Accelerometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_TMIN, 0.0f);
/**
* Accelerometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_A2_TMAX, 100.0f);
src/modules/sensors/temp_comp_params_baro.c
-320
View File
@@ -1,320 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 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_baro.c
*
* Parameters required for temperature compensation of barometers.
*
* @author Paul Riseborough <gncsolns@gmail.com>
*/
/**
* Thermal compensation for barometric pressure sensors.
*
* @group Thermal Compensation
* @min 0
* @max 1
* @boolean
*/
PARAM_DEFINE_INT32(TC_B_ENABLE, 0);
/* Barometer 0 */
/**
* ID of Barometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_B0_ID, 0);
/**
* Barometer offset temperature ^5 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X5, 0.0f);
/**
* Barometer offset temperature ^4 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X4, 0.0f);
/**
* Barometer offset temperature ^3 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X3, 0.0f);
/**
* Barometer offset temperature ^2 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X2, 0.0f);
/**
* Barometer offset temperature ^1 polynomial coefficients.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X1, 0.0f);
/**
* Barometer offset temperature ^0 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_X0, 0.0f);
/**
* Barometer scale factor - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_SCL, 1.0f);
/**
* Barometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_TREF, 40.0f);
/**
* Barometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_TMIN, 5.0f);
/**
* Barometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B0_TMAX, 75.0f);
/* Barometer 1 */
/**
* ID of Barometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_B1_ID, 0);
/**
* Barometer offset temperature ^5 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X5, 0.0f);
/**
* Barometer offset temperature ^4 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X4, 0.0f);
/**
* Barometer offset temperature ^3 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X3, 0.0f);
/**
* Barometer offset temperature ^2 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X2, 0.0f);
/**
* Barometer offset temperature ^1 polynomial coefficients.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X1, 0.0f);
/**
* Barometer offset temperature ^0 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_X0, 0.0f);
/**
* Barometer scale factor - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_SCL, 1.0f);
/**
* Barometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_TREF, 40.0f);
/**
* Barometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_TMIN, 5.0f);
/**
* Barometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B1_TMAX, 75.0f);
/* Barometer 2 */
/**
* ID of Barometer that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_B2_ID, 0);
/**
* Barometer offset temperature ^5 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X5, 0.0f);
/**
* Barometer offset temperature ^4 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X4, 0.0f);
/**
* Barometer offset temperature ^3 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X3, 0.0f);
/**
* Barometer offset temperature ^2 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X2, 0.0f);
/**
* Barometer offset temperature ^1 polynomial coefficients.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X1, 0.0f);
/**
* Barometer offset temperature ^0 polynomial coefficient.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_X0, 0.0f);
/**
* Barometer scale factor - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_SCL, 1.0f);
/**
* Barometer calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_TREF, 40.0f);
/**
* Barometer calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_TMIN, 5.0f);
/**
* Barometer calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_B2_TMAX, 75.0f);
src/modules/sensors/temp_comp_params_gyro.c
-512
View File
@@ -1,512 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2017 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>
*/
/**
* Thermal compensation for rate gyro sensors.
*
* @group Thermal Compensation
* @min 0
* @max 1
* @boolean
*/
PARAM_DEFINE_INT32(TC_G_ENABLE, 0);
/* Gyro 0 */
/**
* ID of Gyro that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_G0_ID, 0);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X3_0, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X3_1, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X3_2, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X2_0, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X2_1, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X2_2, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X1_0, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X1_1, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X1_2, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X0_0, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X0_1, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_X0_2, 0.0f);
/**
* Gyro scale factor - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_SCL_0, 1.0f);
/**
* Gyro scale factor - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_SCL_1, 1.0f);
/**
* Gyro scale factor - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_SCL_2, 1.0f);
/**
* Gyro calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_TREF, 25.0f);
/**
* Gyro calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_TMIN, 0.0f);
/**
* Gyro calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G0_TMAX, 100.0f);
/* Gyro 1 */
/**
* ID of Gyro that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_G1_ID, 0);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X3_0, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X3_1, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X3_2, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X2_0, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X2_1, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X2_2, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X1_0, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X1_1, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X1_2, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X0_0, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X0_1, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_X0_2, 0.0f);
/**
* Gyro scale factor - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_SCL_0, 1.0f);
/**
* Gyro scale factor - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_SCL_1, 1.0f);
/**
* Gyro scale factor - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_SCL_2, 1.0f);
/**
* Gyro calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_TREF, 25.0f);
/**
* Gyro calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_TMIN, 0.0f);
/**
* Gyro calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G1_TMAX, 100.0f);
/* Gyro 2 */
/**
* ID of Gyro that the calibration is for.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_INT32(TC_G2_ID, 0);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X3_0, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X3_1, 0.0f);
/**
* Gyro rate offset temperature ^3 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X3_2, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X2_0, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X2_1, 0.0f);
/**
* Gyro rate offset temperature ^2 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X2_2, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X1_0, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X1_1, 0.0f);
/**
* Gyro rate offset temperature ^1 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X1_2, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X0_0, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X0_1, 0.0f);
/**
* Gyro rate offset temperature ^0 polynomial coefficient - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_X0_2, 0.0f);
/**
* Gyro scale factor - X axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_SCL_0, 1.0f);
/**
* Gyro scale factor - Y axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_SCL_1, 1.0f);
/**
* Gyro scale factor - Z axis.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_SCL_2, 1.0f);
/**
* Gyro calibration reference temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_TREF, 25.0f);
/**
* Gyro calibration minimum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_TMIN, 0.0f);
/**
* Gyro calibration maximum temperature.
*
* @group Thermal Compensation
* @category system
*/
PARAM_DEFINE_FLOAT(TC_G2_TMAX, 100.0f);
src/modules/sensors/temperature_compensation.cpp
-520
View File
@@ -1,520 +0,0 @@
/****************************************************************************
*
* 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 <parameters/param.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/log.h>
namespace sensors
{
int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &parameter_handles)
{
char nbuf[16];
int ret = PX4_ERROR;
/* rate gyro calibration parameters */
parameter_handles.gyro_tc_enable = param_find("TC_G_ENABLE");
int32_t gyro_tc_enabled = 0;
ret = param_get(parameter_handles.gyro_tc_enable, &gyro_tc_enabled);
if (ret == PX4_OK && gyro_tc_enabled) {
for (unsigned j = 0; j < GYRO_COUNT_MAX; 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 */
parameter_handles.accel_tc_enable = param_find("TC_A_ENABLE");
int32_t accel_tc_enabled = 0;
ret = param_get(parameter_handles.accel_tc_enable, &accel_tc_enabled);
if (ret == PX4_OK && accel_tc_enabled) {
for (unsigned j = 0; j < ACCEL_COUNT_MAX; 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);
}
}
/* barometer calibration parameters */
parameter_handles.baro_tc_enable = param_find("TC_B_ENABLE");
int32_t baro_tc_enabled = 0;
ret = param_get(parameter_handles.baro_tc_enable, &baro_tc_enabled);
if (ret == PX4_OK && baro_tc_enabled) {
for (unsigned j = 0; j < BARO_COUNT_MAX; j++) {
sprintf(nbuf, "TC_B%d_ID", j);
parameter_handles.baro_cal_handles[j].ID = param_find(nbuf);
sprintf(nbuf, "TC_B%d_X5", j);
parameter_handles.baro_cal_handles[j].x5 = param_find(nbuf);
sprintf(nbuf, "TC_B%d_X4", j);
parameter_handles.baro_cal_handles[j].x4 = param_find(nbuf);
sprintf(nbuf, "TC_B%d_X3", j);
parameter_handles.baro_cal_handles[j].x3 = param_find(nbuf);
sprintf(nbuf, "TC_B%d_X2", j);
parameter_handles.baro_cal_handles[j].x2 = param_find(nbuf);
sprintf(nbuf, "TC_B%d_X1", j);
parameter_handles.baro_cal_handles[j].x1 = param_find(nbuf);
sprintf(nbuf, "TC_B%d_X0", j);
parameter_handles.baro_cal_handles[j].x0 = param_find(nbuf);
sprintf(nbuf, "TC_B%d_SCL", j);
parameter_handles.baro_cal_handles[j].scale = param_find(nbuf);
sprintf(nbuf, "TC_B%d_TREF", j);
parameter_handles.baro_cal_handles[j].ref_temp = param_find(nbuf);
sprintf(nbuf, "TC_B%d_TMIN", j);
parameter_handles.baro_cal_handles[j].min_temp = param_find(nbuf);
sprintf(nbuf, "TC_B%d_TMAX", j);
parameter_handles.baro_cal_handles[j].max_temp = param_find(nbuf);
}
}
return PX4_OK;
}
int TemperatureCompensation::parameters_update(bool hil_enabled)
{
int ret = 0;
ParameterHandles parameter_handles;
ret = initialize_parameter_handles(parameter_handles);
if (ret != 0) {
return ret;
}
/* rate gyro calibration parameters */
if (!hil_enabled) {
param_get(parameter_handles.gyro_tc_enable, &_parameters.gyro_tc_enable);
} else {
_parameters.gyro_tc_enable = 0;
}
if (_parameters.gyro_tc_enable == 1) {
for (unsigned j = 0; j < GYRO_COUNT_MAX; 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].max_temp, &(_parameters.gyro_cal_data[j].max_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 */
if (!hil_enabled) {
param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable);
} else {
_parameters.accel_tc_enable = 0;
}
if (_parameters.accel_tc_enable == 1) {
for (unsigned j = 0; j < ACCEL_COUNT_MAX; 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].max_temp, &(_parameters.accel_cal_data[j].max_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;
}
}
}
/* barometer calibration parameters */
if (!hil_enabled) {
param_get(parameter_handles.baro_tc_enable, &_parameters.baro_tc_enable);
} else {
_parameters.baro_tc_enable = 0;
}
if (_parameters.baro_tc_enable == 1) {
for (unsigned j = 0; j < BARO_COUNT_MAX; j++) {
if (param_get(parameter_handles.baro_cal_handles[j].ID, &(_parameters.baro_cal_data[j].ID)) == PX4_OK) {
param_get(parameter_handles.baro_cal_handles[j].ref_temp, &(_parameters.baro_cal_data[j].ref_temp));
param_get(parameter_handles.baro_cal_handles[j].min_temp, &(_parameters.baro_cal_data[j].min_temp));
param_get(parameter_handles.baro_cal_handles[j].max_temp, &(_parameters.baro_cal_data[j].max_temp));
param_get(parameter_handles.baro_cal_handles[j].x5, &(_parameters.baro_cal_data[j].x5));
param_get(parameter_handles.baro_cal_handles[j].x4, &(_parameters.baro_cal_data[j].x4));
param_get(parameter_handles.baro_cal_handles[j].x3, &(_parameters.baro_cal_data[j].x3));
param_get(parameter_handles.baro_cal_handles[j].x2, &(_parameters.baro_cal_data[j].x2));
param_get(parameter_handles.baro_cal_handles[j].x1, &(_parameters.baro_cal_data[j].x1));
param_get(parameter_handles.baro_cal_handles[j].x0, &(_parameters.baro_cal_data[j].x0));
param_get(parameter_handles.baro_cal_handles[j].scale, &(_parameters.baro_cal_data[j].scale));
} else {
// Set all cal values to zero and scale factor to unity
memset(&_parameters.baro_cal_data[j], 0, sizeof(_parameters.baro_cal_data[j]));
// Set the scale factor to unity
_parameters.baro_cal_data[j].scale = 1.0f;
PX4_WARN("FAIL BARO %d CAL PARAM LOAD - USING DEFAULTS", j);
ret = PX4_ERROR;
}
}
}
/* the offsets & scales might have changed, so make sure to report that change later when applying the
* next corrections
*/
_gyro_data.reset_temperature();
_accel_data.reset_temperature();
_baro_data.reset_temperature();
return ret;
}
bool TemperatureCompensation::calc_thermal_offsets_1D(SensorCalData1D &coef, 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 - coef.ref_temp;
ret = false;
} else if (measured_temp < coef.min_temp) {
delta_temp = coef.min_temp - coef.ref_temp;
ret = false;
} else {
delta_temp = measured_temp - coef.ref_temp;
}
// calulate the offset
float temp_var = delta_temp;
offset = coef.x0 + coef.x1 * temp_var;
temp_var *= delta_temp;
offset += coef.x2 * temp_var;
temp_var *= delta_temp;
offset += coef.x3 * temp_var;
temp_var *= delta_temp;
offset += coef.x4 * temp_var;
temp_var *= delta_temp;
offset += coef.x5 * temp_var;
return ret;
}
bool TemperatureCompensation::calc_thermal_offsets_3D(const SensorCalData3D &coef, 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 - coef.ref_temp;
ret = false;
} else if (measured_temp < coef.min_temp) {
delta_temp = coef.min_temp - coef.ref_temp;
ret = false;
} else {
delta_temp = measured_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;
}
int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_instance)
{
if (_parameters.gyro_tc_enable != 1) {
return 0;
}
return set_sensor_id(device_id, topic_instance, _gyro_data, _parameters.gyro_cal_data, GYRO_COUNT_MAX);
}
int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_instance)
{
if (_parameters.accel_tc_enable != 1) {
return 0;
}
return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data, ACCEL_COUNT_MAX);
}
int TemperatureCompensation::set_sensor_id_baro(uint32_t device_id, int topic_instance)
{
if (_parameters.baro_tc_enable != 1) {
return 0;
}
return set_sensor_id(device_id, topic_instance, _baro_data, _parameters.baro_cal_data, BARO_COUNT_MAX);
}
template<typename T>
int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data,
const T *sensor_cal_data, uint8_t sensor_count_max)
{
for (int i = 0; i < sensor_count_max; ++i) {
if (device_id == (uint32_t)sensor_cal_data[i].ID) {
sensor_data.device_mapping[topic_instance] = i;
return i;
}
}
return -1;
}
int TemperatureCompensation::apply_corrections_gyro(int topic_instance, matrix::Vector3f &sensor_data,
float temperature, float *offsets, float *scales)
{
if (_parameters.gyro_tc_enable != 1) {
return 0;
}
uint8_t mapping = _gyro_data.device_mapping[topic_instance];
if (mapping == 255) {
return -1;
}
calc_thermal_offsets_3D(_parameters.gyro_cal_data[mapping], temperature, offsets);
// get the sensor scale factors and correct the data
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
scales[axis_index] = _parameters.gyro_cal_data[mapping].scale[axis_index];
sensor_data(axis_index) = (sensor_data(axis_index) - offsets[axis_index]) * scales[axis_index];
}
if (fabsf(temperature - _gyro_data.last_temperature[topic_instance]) > 1.0f) {
_gyro_data.last_temperature[topic_instance] = temperature;
return 2;
}
return 1;
}
int TemperatureCompensation::apply_corrections_accel(int topic_instance, matrix::Vector3f &sensor_data,
float temperature, float *offsets, float *scales)
{
if (_parameters.accel_tc_enable != 1) {
return 0;
}
uint8_t mapping = _accel_data.device_mapping[topic_instance];
if (mapping == 255) {
return -1;
}
calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets);
// get the sensor scale factors and correct the data
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
scales[axis_index] = _parameters.accel_cal_data[mapping].scale[axis_index];
sensor_data(axis_index) = (sensor_data(axis_index) - offsets[axis_index]) * scales[axis_index];
}
if (fabsf(temperature - _accel_data.last_temperature[topic_instance]) > 1.0f) {
_accel_data.last_temperature[topic_instance] = temperature;
return 2;
}
return 1;
}
int TemperatureCompensation::apply_corrections_baro(int topic_instance, float &sensor_data, float temperature,
float *offsets, float *scales)
{
if (_parameters.baro_tc_enable != 1) {
return 0;
}
uint8_t mapping = _baro_data.device_mapping[topic_instance];
if (mapping == 255) {
return -1;
}
calc_thermal_offsets_1D(_parameters.baro_cal_data[mapping], temperature, *offsets);
// get the sensor scale factors and correct the data
*scales = _parameters.baro_cal_data[mapping].scale;
sensor_data = (sensor_data - *offsets) * *scales;
if (fabsf(temperature - _baro_data.last_temperature[topic_instance]) > 1.0f) {
_baro_data.last_temperature[topic_instance] = temperature;
return 2;
}
return 1;
}
void TemperatureCompensation::print_status()
{
PX4_INFO("Temperature Compensation:");
PX4_INFO(" gyro: enabled: %i", _parameters.gyro_tc_enable);
if (_parameters.gyro_tc_enable == 1) {
for (int i = 0; i < GYRO_COUNT_MAX; ++i) {
uint8_t mapping = _gyro_data.device_mapping[i];
if (_gyro_data.device_mapping[i] != 255) {
PX4_INFO(" using device ID %i for topic instance %i", _parameters.gyro_cal_data[mapping].ID, i);
}
}
}
PX4_INFO(" accel: enabled: %i", _parameters.accel_tc_enable);
if (_parameters.accel_tc_enable == 1) {
for (int i = 0; i < ACCEL_COUNT_MAX; ++i) {
uint8_t mapping = _accel_data.device_mapping[i];
if (_accel_data.device_mapping[i] != 255) {
PX4_INFO(" using device ID %i for topic instance %i", _parameters.accel_cal_data[mapping].ID, i);
}
}
}
PX4_INFO(" baro: enabled: %i", _parameters.baro_tc_enable);
if (_parameters.baro_tc_enable == 1) {
for (int i = 0; i < BARO_COUNT_MAX; ++i) {
uint8_t mapping = _baro_data.device_mapping[i];
if (_baro_data.device_mapping[i] != 255) {
PX4_INFO(" using device ID %i for topic instance %i", _parameters.baro_cal_data[mapping].ID, i);
}
}
}
}
}
src/modules/sensors/temperature_compensation.h
-280
View File
@@ -1,280 +0,0 @@
/****************************************************************************
*
* 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>
* @author Beat Küng <beat-kueng@gmx.net>
*/
#pragma once
#include <parameters/param.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include "common.h"
namespace sensors
{
static_assert(GYRO_COUNT_MAX == 3,
"GYRO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
static_assert(ACCEL_COUNT_MAX == 3,
"ACCEL_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
static_assert(BARO_COUNT_MAX == 3,
"BARO_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
/**
** class TemperatureCompensation
* Applies temperature compensation to sensor data. Loads the parameters from PX4 param storage.
*/
class TemperatureCompensation
{
public:
/** (re)load the parameters. Make sure to call this on startup as well */
int parameters_update(bool hil_enabled = false);
/** supply information which device_id matches a specific uORB topic_instance
* (needed if a system has multiple sensors of the same type)
* @return index for compensation parameter entry containing matching device ID on success, <0 otherwise */
int set_sensor_id_gyro(uint32_t device_id, int topic_instance);
int set_sensor_id_accel(uint32_t device_id, int topic_instance);
int set_sensor_id_baro(uint32_t device_id, int topic_instance);
/**
* Apply Thermal corrections to gyro (& other) sensor data.
* @param topic_instance uORB topic instance
* @param sensor_data input sensor data, output sensor data with applied corrections
* @param temperature measured current temperature
* @param offsets returns offsets that were applied (length = 3, except for baro), depending on return value
* @param scales returns scales that were applied (length = 3), depending on return value
* @return -1: error: correction enabled, but no sensor mapping set (@see set_sendor_id_gyro)
* 0: no changes (correction not enabled),
* 1: corrections applied but no changes to offsets & scales,
* 2: corrections applied and offsets & scales updated
*/
int apply_corrections_gyro(int topic_instance, matrix::Vector3f &sensor_data, float temperature, float *offsets,
float *scales);
int apply_corrections_accel(int topic_instance, matrix::Vector3f &sensor_data, float temperature, float *offsets,
float *scales);
int apply_corrections_baro(int topic_instance, float &sensor_data, float temperature, float *offsets, float *scales);
/** output current configuration status to console */
void print_status();
private:
/* Struct containing parameters used by the single axis 5th order 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 = x5 * delta_temp^5 + x4 * delta_temp^4 + x3 * delta_temp^3 + x2 * delta_temp^2 + x1 * delta_temp + x0
corrected_value = (raw_value - offset) * scale
*/
struct SensorCalData1D {
int32_t ID;
float x5;
float x4;
float x3;
float x2;
float x1;
float x0;
float scale;
float ref_temp;
float min_temp;
float max_temp;
};
struct SensorCalHandles1D {
param_t ID;
param_t x5;
param_t x4;
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 containing parameters used by the 3-axis 3rd order temperature compensation algorithm
Input:
measured_temp : temperature measured at the sensor (deg C)
raw_value[3] : XYZ readings from the sensor before compensation
corrected_value[3] : XYZ readings from the sensor after compensation for errors
Compute for each measurement index:
delta_temp = measured_temp - ref_temp
offset = x3 * delta_temp^3 + x2 * delta_temp^2 + x1 * delta_temp + x0
corrected_value = (raw_value - offset) * scale
*/
struct SensorCalData3D {
int32_t 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 SensorCalHandles3D {
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;
};
// create a struct containing all thermal calibration parameters
struct Parameters {
int32_t gyro_tc_enable;
SensorCalData3D gyro_cal_data[GYRO_COUNT_MAX];
int32_t accel_tc_enable;
SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX];
int32_t baro_tc_enable;
SensorCalData1D baro_cal_data[BARO_COUNT_MAX];
};
// create a struct containing the handles required to access all calibration parameters
struct ParameterHandles {
param_t gyro_tc_enable;
SensorCalHandles3D gyro_cal_handles[GYRO_COUNT_MAX];
param_t accel_tc_enable;
SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX];
param_t baro_tc_enable;
SensorCalHandles1D baro_cal_handles[BARO_COUNT_MAX];
};
/**
* initialize ParameterHandles struct
* @return 0 on succes, <0 on error
*/
static int initialize_parameter_handles(ParameterHandles &parameter_handles);
/**
Calculate the offset required to compensate the sensor for temperature effects using a 5th order method
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 calc_thermal_offsets_1D(SensorCalData1D &coef, 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(const SensorCalData3D &coef, float measured_temp, float offset[]);
Parameters _parameters;
struct PerSensorData {
PerSensorData()
{
for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { device_mapping[i] = 255; last_temperature[i] = -100.0f; }
}
void reset_temperature()
{
for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { last_temperature[i] = -100.0f; }
}
uint8_t device_mapping[SENSOR_COUNT_MAX]; /// map a topic instance to the parameters index
float last_temperature[SENSOR_COUNT_MAX];
};
PerSensorData _gyro_data;
PerSensorData _accel_data;
PerSensorData _baro_data;
template<typename T>
static inline int set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data,
const T *sensor_cal_data, uint8_t sensor_count_max);
};
}
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
+37 -28
View File
@@ -37,6 +37,7 @@
using namespace matrix;
using namespace time_literals;
using math::radians;
VehicleAcceleration::VehicleAcceleration() :
ModuleParams(nullptr),
@@ -53,11 +54,15 @@ bool VehicleAcceleration::Start()
{
// force initial updates
ParametersUpdate(true);
SensorBiasUpdate(true);
SensorCorrectionsUpdate(true);
// needed to change the active sensor if the primary stops updating
return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
// sensor_selection needed to change the active sensor if the primary stops updating
if (!_sensor_selection_sub.registerCallback()) {
PX4_ERR("sensor_selection callback registration failed");
return false;
}
ScheduleNow();
return true;
}
void VehicleAcceleration::Stop()
@@ -97,7 +102,7 @@ void VehicleAcceleration::SensorCorrectionsUpdate(bool force)
_sensor_correction_sub.copy(&corrections);
// selected sensor has changed, find updated index
if ((_corrections_selected_instance != corrections.selected_accel_instance) || force) {
if ((_corrections_selected_instance < 0) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
@@ -122,8 +127,8 @@ void VehicleAcceleration::SensorCorrectionsUpdate(bool force)
_scale = Vector3f{corrections.accel_scale_2};
break;
default:
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
}
}
}
@@ -154,8 +159,11 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
// clear bias and corrections
_bias.zero();
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
// force corrections reselection
_corrections_selected_instance = -1;
return true;
}
@@ -182,13 +190,13 @@ void VehicleAcceleration::ParametersUpdate(bool force)
updateParams();
// get transformation matrix from sensor/board to body frame
const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
// fine tune the rotation
const Dcmf board_rotation_offset(Eulerf(
math::radians(_param_sens_board_x_off.get()),
math::radians(_param_sens_board_y_off.get()),
math::radians(_param_sens_board_z_off.get())));
radians(_param_sens_board_x_off.get()),
radians(_param_sens_board_y_off.get()),
radians(_param_sens_board_z_off.get())));
_board_rotation = board_rotation_offset * board_rotation;
}
@@ -204,28 +212,29 @@ void VehicleAcceleration::Run()
if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
sensor_accel_s sensor_data;
_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// apply offsets and scale
Vector3f accel{(val - _offset).emult(_scale)};
// apply offsets and scale
Vector3f accel{(val - _offset).emult(_scale)};
// rotate corrected measurements from sensor to body frame
accel = _board_rotation * accel;
// rotate corrected measurements from sensor to body frame
accel = _board_rotation * accel;
// correct for in-run bias errors
accel -= _bias;
// correct for in-run bias errors
accel -= _bias;
// publish
vehicle_acceleration_s out;
// publish
vehicle_acceleration_s out;
out.timestamp_sample = sensor_data.timestamp_sample;
accel.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
out.timestamp_sample = sensor_data.timestamp_sample;
accel.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
_vehicle_acceleration_pub.publish(out);
_vehicle_acceleration_pub.publish(out);
}
}
}
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
+1 -1
View File
@@ -96,9 +96,9 @@ private:
matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
matrix::Vector3f _bias{0.f, 0.f, 0.f};
matrix::Vector3f _offset{0.f, 0.f, 0.f};
matrix::Vector3f _scale{1.f, 1.f, 1.f};
matrix::Vector3f _bias{0.f, 0.f, 0.f};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+37 -28
View File
@@ -37,6 +37,7 @@
using namespace matrix;
using namespace time_literals;
using math::radians;
VehicleAngularVelocity::VehicleAngularVelocity() :
ModuleParams(nullptr),
@@ -53,11 +54,15 @@ bool VehicleAngularVelocity::Start()
{
// force initial updates
ParametersUpdate(true);
SensorBiasUpdate(true);
SensorCorrectionsUpdate(true);
// needed to change the active sensor if the primary stops updating
return _sensor_selection_sub.registerCallback() && SensorSelectionUpdate(true);
// sensor_selection needed to change the active sensor if the primary stops updating
if (!_sensor_selection_sub.registerCallback()) {
PX4_ERR("sensor_selection callback registration failed");
return false;
}
ScheduleNow();
return true;
}
void VehicleAngularVelocity::Stop()
@@ -97,7 +102,7 @@ void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
_sensor_correction_sub.copy(&corrections);
// selected sensor has changed, find updated index
if ((_corrections_selected_instance != corrections.selected_gyro_instance) || force) {
if ((_corrections_selected_instance < 0) || force) {
_corrections_selected_instance = -1;
// find sensor_corrections index
@@ -122,8 +127,8 @@ void VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
_scale = Vector3f{corrections.gyro_scale_2};
break;
default:
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
}
}
}
@@ -154,8 +159,11 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
// clear bias and corrections
_bias.zero();
_offset = Vector3f{0.0f, 0.0f, 0.0f};
_scale = Vector3f{1.0f, 1.0f, 1.0f};
_offset = Vector3f{0.f, 0.f, 0.f};
_scale = Vector3f{1.f, 1.f, 1.f};
// force corrections reselection
_corrections_selected_instance = -1;
return true;
}
@@ -182,13 +190,13 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
updateParams();
// get transformation matrix from sensor/board to body frame
const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
const Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
// fine tune the rotation
const Dcmf board_rotation_offset(Eulerf(
math::radians(_param_sens_board_x_off.get()),
math::radians(_param_sens_board_y_off.get()),
math::radians(_param_sens_board_z_off.get())));
radians(_param_sens_board_x_off.get()),
radians(_param_sens_board_y_off.get()),
radians(_param_sens_board_z_off.get())));
_board_rotation = board_rotation_offset * board_rotation;
}
@@ -204,28 +212,29 @@ void VehicleAngularVelocity::Run()
if (_sensor_sub[_selected_sensor_sub_index].updated() || sensor_select_update) {
sensor_gyro_s sensor_data;
_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data);
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
// get the sensor data and correct for thermal errors (apply offsets and scale)
const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z};
// apply offsets and scale
Vector3f rates{(val - _offset).emult(_scale)};
// apply offsets and scale
Vector3f rates{(val - _offset).emult(_scale)};
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// correct for in-run bias errors
rates -= _bias;
// correct for in-run bias errors
rates -= _bias;
// publish
vehicle_angular_velocity_s out;
// publish
vehicle_angular_velocity_s out;
out.timestamp_sample = sensor_data.timestamp_sample;
rates.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
out.timestamp_sample = sensor_data.timestamp_sample;
rates.copyTo(out.xyz);
out.timestamp = hrt_absolute_time();
_vehicle_angular_velocity_pub.publish(out);
_vehicle_angular_velocity_pub.publish(out);
}
}
}
src/modules/sensors/voted_sensors_update.cpp
+12 -103
View File
@@ -84,7 +84,6 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
initializeSensors();
_corrections_changed = true; //make sure to initially publish the corrections topic
_selection_changed = true;
return 0;
@@ -132,76 +131,6 @@ void VotedSensorsUpdate::parametersUpdate()
_mag_rotation[topic_instance] = _board_rotation;
}
/* Load & apply the sensor calibrations.
* IMPORTANT: we assume all sensor drivers are running and published sensor data at this point
*/
/* temperature compensation */
_temperature_compensation.parameters_update(_hil_enabled);
/* gyro */
for (uint8_t topic_instance = 0; topic_instance < _gyro.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_gyro_integrated_s> report{ORB_ID(sensor_gyro_integrated), topic_instance};
int temp = _temperature_compensation.set_sensor_id_gyro(report.get().device_id, topic_instance);
if (temp < 0) {
PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "gyro", report.get().device_id,
topic_instance);
_corrections.gyro_mapping[topic_instance] = 0;
_corrections.gyro_device_ids[topic_instance] = 0;
} else {
_corrections.gyro_mapping[topic_instance] = temp;
_corrections.gyro_device_ids[topic_instance] = report.get().device_id;
}
}
/* accel */
for (uint8_t topic_instance = 0; topic_instance < _accel.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_accel_integrated_s> report{ORB_ID(sensor_accel_integrated), topic_instance};
int temp = _temperature_compensation.set_sensor_id_accel(report.get().device_id, topic_instance);
if (temp < 0) {
PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "accel", report.get().device_id,
topic_instance);
_corrections.accel_mapping[topic_instance] = 0;
_corrections.accel_device_ids[topic_instance] = 0;
} else {
_corrections.accel_mapping[topic_instance] = temp;
_corrections.accel_device_ids[topic_instance] = report.get().device_id;
}
}
/* baro */
for (uint8_t topic_instance = 0; topic_instance < _baro.subscription_count; ++topic_instance) {
uORB::SubscriptionData<sensor_baro_s> report{ORB_ID(sensor_baro), topic_instance};
int temp = _temperature_compensation.set_sensor_id_baro(report.get().device_id, topic_instance);
if (temp < 0) {
PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "baro", report.get().device_id,
topic_instance);
_corrections.baro_mapping[topic_instance] = 0;
_corrections.baro_device_ids[topic_instance] = 0;
} else {
_corrections.baro_mapping[topic_instance] = temp;
_corrections.baro_device_ids[topic_instance] = report.get().device_id;
}
}
/* set offset parameters to new values */
bool failed = false;
@@ -601,11 +530,10 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.dt;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// apply temperature compensation
accel_data(0) = (accel_data(0) - offsets[uorb_index][0]) * scales[uorb_index][0]; // X
accel_data(1) = (accel_data(1) - offsets[uorb_index][1]) * scales[uorb_index][1]; // Y
accel_data(2) = (accel_data(2) - offsets[uorb_index][2]) * scales[uorb_index][2]; // Z
// rotate corrected measurements from sensor to body frame
accel_data = _board_rotation * accel_data;
@@ -631,8 +559,6 @@ void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw)
if (best_index != _accel.last_best_vote) {
_accel.last_best_vote = (uint8_t)best_index;
_corrections.selected_accel_instance = (uint8_t)best_index;
_corrections_changed = true;
}
if (_selection.accel_device_id != _accel_device_id[best_index]) {
@@ -685,11 +611,10 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.dt;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// apply temperature compensation
gyro_rate(0) = (gyro_rate(0) - offsets[uorb_index][0]) * scales[uorb_index][0]; // X
gyro_rate(1) = (gyro_rate(1) - offsets[uorb_index][1]) * scales[uorb_index][1]; // Y
gyro_rate(2) = (gyro_rate(2) - offsets[uorb_index][2]) * scales[uorb_index][2]; // Z
// rotate corrected measurements from sensor to body frame
gyro_rate = _board_rotation * gyro_rate;
@@ -716,8 +641,6 @@ void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw)
if (_gyro.last_best_vote != best_index) {
_gyro.last_best_vote = (uint8_t)best_index;
_corrections.selected_gyro_instance = (uint8_t)best_index;
_corrections_changed = true;
}
if (_selection.gyro_device_id != _gyro_device_id[best_index]) {
@@ -814,11 +737,8 @@ void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata)
// Convert from millibar to Pa
float corrected_pressure = 100.0f * baro_report.pressure;
// handle temperature compensation
if (_temperature_compensation.apply_corrections_baro(uorb_index, corrected_pressure, baro_report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections_changed = true;
}
// apply temperature compensation
corrected_pressure = (corrected_pressure - *offsets[uorb_index]) * *scales[uorb_index];
// First publication with data
if (_baro.priority[uorb_index] == 0) {
@@ -850,8 +770,6 @@ void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata)
if (_baro.last_best_vote != best_index) {
_baro.last_best_vote = (uint8_t)best_index;
_corrections.selected_baro_instance = (uint8_t)best_index;
_corrections_changed = true;
}
if (_selection.baro_device_id != _baro_device_id[best_index]) {
@@ -1001,27 +919,18 @@ void VotedSensorsUpdate::printStatus()
_mag.voter.print();
PX4_INFO("baro status:");
_baro.voter.print();
_temperature_compensation.print_status();
}
void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata,
vehicle_magnetometer_s &magnetometer)
{
_corrections_sub.update(&_corrections);
accelPoll(raw);
gyroPoll(raw);
magPoll(magnetometer);
baroPoll(airdata);
// publish sensor corrections if necessary
if (_corrections_changed) {
_corrections.timestamp = hrt_absolute_time();
_sensor_correction_pub.publish(_corrections);
_corrections_changed = false;
}
// publish sensor selection if changed
if (_selection_changed) {
_selection.timestamp = hrt_absolute_time();
src/modules/sensors/voted_sensors_update.h
+7 -8
View File
@@ -48,12 +48,14 @@
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <lib/ecl/validation/data_validator.h>
#include <lib/ecl/validation/data_validator_group.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/sensor_preflight.h>
@@ -64,7 +66,6 @@
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/subsystem_info.h>
#include "temperature_compensation.h"
#include "common.h"
namespace sensors
@@ -218,8 +219,11 @@ private:
orb_advert_t _mavlink_log_pub{nullptr};
uORB::Publication<sensor_correction_s> _sensor_correction_pub{ORB_ID(sensor_correction)}; /**< handle to the sensor correction uORB topic */
uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */
uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */
uORB::PublicationQueued<subsystem_info_s> _info_pub{ORB_ID(subsystem_info)}; /* subsystem info publication */
/* sensor thermal compensation */
uORB::Subscription _corrections_sub{ORB_ID(sensor_correction)};
sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
vehicle_air_data_s _last_airdata[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */
@@ -231,7 +235,6 @@ private:
const Parameters &_parameters;
const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */
bool _corrections_changed{false};
bool _selection_changed{false}; /**< true when a sensor selection has changed and not been published */
float _accel_diff[3][2] {}; /**< filtered accel differences between IMU units (m/s/s) */
@@ -248,10 +251,6 @@ private:
sensor_correction_s _corrections {}; /**< struct containing the sensor corrections to be published to the uORB */
sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */
subsystem_info_s _info {}; /**< subsystem info publication */
TemperatureCompensation _temperature_compensation{}; /**< sensor thermal compensation */
uORB::PublicationQueued<subsystem_info_s> _info_pub{ORB_ID(subsystem_info)}; /* subsystem info publication */
};
} /* namespace sensors */