fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 10:13:33 +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/temperature_compensation/CMakeLists.txt
+46
View File
@@ -0,0 +1,46 @@
############################################################################
#
# Copyright (c) 2015-2019 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.
#
############################################################################
px4_add_module(
MODULE modules__temperature_compensation
MAIN temperature_compensation
SRCS
TemperatureCompensationModule.cpp
TemperatureCompensation.cpp
temperature_calibration/accel.cpp
temperature_calibration/baro.cpp
temperature_calibration/gyro.cpp
temperature_calibration/task.cpp
DEPENDS
mathlib
)
src/modules/temperature_compensation/TemperatureCompensation.cpp
+511
View File
@@ -0,0 +1,511 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file temperature_compensation.cpp
*
* Sensors temperature compensation methods
*
* @author Paul Riseborough <gncsolns@gmail.com>
*/
#include "TemperatureCompensation.h"
#include <parameters/param.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/log.h>
namespace temperature_compensation
{
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()
{
ParameterHandles parameter_handles;
int ret = initialize_parameter_handles(parameter_handles);
if (ret != 0) {
return ret;
}
/* rate gyro calibration parameters */
param_get(parameter_handles.gyro_tc_enable, &_parameters.gyro_tc_enable);
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 */
param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable);
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 */
param_get(parameter_handles.baro_tc_enable, &_parameters.baro_tc_enable);
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::update_scales_and_offsets_gyro(int topic_instance, float temperature, float *offsets,
float *scales)
{
// Check if temperature compensation is enabled
if (_parameters.gyro_tc_enable != 1) {
return 0;
}
// Map device ID to uORB topic instance
uint8_t mapping = _gyro_data.device_mapping[topic_instance];
if (mapping == 255) {
return -1;
}
// Calculate and update the offsets
calc_thermal_offsets_3D(_parameters.gyro_cal_data[mapping], temperature, offsets);
// Update the scales
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
scales[axis_index] = _parameters.gyro_cal_data[mapping].scale[axis_index];
}
// Check if temperature delta is large enough to warrant a new publication
if (fabsf(temperature - _gyro_data.last_temperature[topic_instance]) > 1.0f) {
_gyro_data.last_temperature[topic_instance] = temperature;
return 2;
}
return 1;
}
int TemperatureCompensation::update_scales_and_offsets_accel(int topic_instance, float temperature, float *offsets,
float *scales)
{
// Check if temperature compensation is enabled
if (_parameters.accel_tc_enable != 1) {
return 0;
}
// Map device ID to uORB topic instance
uint8_t mapping = _accel_data.device_mapping[topic_instance];
if (mapping == 255) {
return -1;
}
// Calculate and update the offsets
calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets);
// Update the scales
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
scales[axis_index] = _parameters.accel_cal_data[mapping].scale[axis_index];
}
// Check if temperature delta is large enough to warrant a new publication
if (fabsf(temperature - _accel_data.last_temperature[topic_instance]) > 1.0f) {
_accel_data.last_temperature[topic_instance] = temperature;
return 2;
}
return 1;
}
int TemperatureCompensation::update_scales_and_offsets_baro(int topic_instance, float temperature, float *offsets,
float *scales)
{
// Check if temperature compensation is enabled
if (_parameters.baro_tc_enable != 1) {
return 0;
}
// Map device ID to uORB topic instance
uint8_t mapping = _baro_data.device_mapping[topic_instance];
if (mapping == 255) {
return -1;
}
// Calculate and update the offsets
calc_thermal_offsets_1D(_parameters.baro_cal_data[mapping], temperature, *offsets);
// Update the scales
*scales = _parameters.baro_cal_data[mapping].scale;
// Check if temperature delta is large enough to warrant a new publication
if (fabsf(temperature - _baro_data.last_temperature[topic_instance]) > 1.0f) {
_baro_data.last_temperature[topic_instance] = temperature;
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);
}
}
}
}
} // namespace temperature_compensation
src/modules/temperature_compensation/TemperatureCompensation.h
+288
View File
@@ -0,0 +1,288 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @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>
namespace temperature_compensation
{
static constexpr uint8_t GYRO_COUNT_MAX = 3;
static constexpr uint8_t ACCEL_COUNT_MAX = 3;
static constexpr uint8_t BARO_COUNT_MAX = 3;
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)");
static constexpr uint8_t SENSOR_COUNT_MAX = 3;
/**
** 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();
/** 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 update_scales_and_offsets_gyro(int topic_instance, float temperature, float *offsets, float *scales);
int update_scales_and_offsets_accel(int topic_instance, float temperature, float *offsets, float *scales);
int update_scales_and_offsets_baro(int topic_instance, float temperature, float *offsets, float *scales);
/** 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{0};
SensorCalData3D gyro_cal_data[GYRO_COUNT_MAX] {};
int32_t accel_tc_enable{0};
SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX] {};
int32_t baro_tc_enable{0};
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{PARAM_INVALID};
SensorCalHandles3D gyro_cal_handles[GYRO_COUNT_MAX] {};
param_t accel_tc_enable{PARAM_INVALID};
SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX] {};
param_t baro_tc_enable{PARAM_INVALID};
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/temperature_compensation/TemperatureCompensationModule.cpp
+421
View File
@@ -0,0 +1,421 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "TemperatureCompensationModule.h"
#include "temperature_calibration/temperature_calibration.h"
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/vehicle_command.h>
#include <systemlib/mavlink_log.h>
using namespace temperature_compensation;
using namespace time_literals;
TemperatureCompensationModule::TemperatureCompensationModule() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
_loop_perf(perf_alloc(PC_ELAPSED, "temperature_compensation"))
{
// Initialize the publication variables
for (unsigned i = 0; i < 3; i++) {
_corrections.gyro_scale_0[i] = 1.0f;
_corrections.accel_scale_0[i] = 1.0f;
_corrections.gyro_scale_1[i] = 1.0f;
_corrections.accel_scale_1[i] = 1.0f;
_corrections.gyro_scale_2[i] = 1.0f;
_corrections.accel_scale_2[i] = 1.0f;
}
_corrections.baro_scale_0 = 1.0f;
_corrections.baro_scale_1 = 1.0f;
_corrections.baro_scale_2 = 1.0f;
}
TemperatureCompensationModule::~TemperatureCompensationModule()
{
perf_free(_loop_perf);
}
void TemperatureCompensationModule::parameters_update()
{
_temperature_compensation.parameters_update();
// Gyro
for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) {
sensor_gyro_s report;
if (_gyro_subs[uorb_index].copy(&report)) {
int temp = _temperature_compensation.set_sensor_id_gyro(report.device_id, uorb_index);
if (temp < 0) {
PX4_ERR("%s init: failed to find device ID %u for instance %i", "gyro", report.device_id, uorb_index);
_corrections.gyro_mapping[uorb_index] = 0;
_corrections.gyro_device_ids[uorb_index] = 0;
} else {
_corrections.gyro_mapping[uorb_index] = temp;
_corrections.gyro_device_ids[uorb_index] = report.device_id;
}
}
}
// Accel
for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) {
sensor_accel_s report;
if (_accel_subs[uorb_index].copy(&report)) {
int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, uorb_index);
if (temp < 0) {
PX4_ERR("%s init: failed to find device ID %u for instance %i", "accel", report.device_id, uorb_index);
_corrections.accel_mapping[uorb_index] = 0;
_corrections.accel_device_ids[uorb_index] = 0;
} else {
_corrections.accel_mapping[uorb_index] = temp;
_corrections.accel_device_ids[uorb_index] = report.device_id;
}
}
}
// Baro
for (uint8_t uorb_index = 0; uorb_index < BARO_COUNT_MAX; uorb_index++) {
sensor_baro_s report;
if (_baro_subs[uorb_index].copy(&report)) {
int temp = _temperature_compensation.set_sensor_id_baro(report.device_id, uorb_index);
if (temp < 0) {
PX4_ERR("%s init: failed to find device ID %u for instance %i", "baro", report.device_id, uorb_index);
_corrections.baro_mapping[uorb_index] = 0;
_corrections.baro_device_ids[uorb_index] = 0;
} else {
_corrections.baro_mapping[uorb_index] = temp;
_corrections.baro_device_ids[uorb_index] = temp;
}
}
}
}
void TemperatureCompensationModule::accelPoll()
{
float *offsets[] = {_corrections.accel_offset_0, _corrections.accel_offset_1, _corrections.accel_offset_2 };
float *scales[] = {_corrections.accel_scale_0, _corrections.accel_scale_1, _corrections.accel_scale_2 };
// For each accel instance
for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) {
sensor_accel_s report;
// Grab temperature from report
if (_accel_subs[uorb_index].update(&report)) {
// Update the scales and offsets and mark for publication if they've changed
if (_temperature_compensation.update_scales_and_offsets_accel(uorb_index, report.temperature, offsets[uorb_index],
scales[uorb_index]) == 2) {
_corrections.accel_device_ids[uorb_index] = report.device_id;
_corrections_changed = true;
}
}
}
}
void TemperatureCompensationModule::gyroPoll()
{
float *offsets[] = {_corrections.gyro_offset_0, _corrections.gyro_offset_1, _corrections.gyro_offset_2 };
float *scales[] = {_corrections.gyro_scale_0, _corrections.gyro_scale_1, _corrections.gyro_scale_2 };
// For each gyro instance
for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) {
sensor_gyro_s report;
// Grab temperature from report
if (_gyro_subs[uorb_index].update(&report)) {
// Update the scales and offsets and mark for publication if they've changed
if (_temperature_compensation.update_scales_and_offsets_gyro(uorb_index, report.temperature, offsets[uorb_index],
scales[uorb_index]) == 2) {
_corrections.gyro_device_ids[uorb_index] = report.device_id;
_corrections_changed = true;
}
}
}
}
void TemperatureCompensationModule::baroPoll()
{
float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2 };
float *scales[] = {&_corrections.baro_scale_0, &_corrections.baro_scale_1, &_corrections.baro_scale_2 };
// For each baro instance
for (uint8_t uorb_index = 0; uorb_index < BARO_COUNT_MAX; uorb_index++) {
sensor_baro_s report;
// Grab temperature from report
if (_baro_subs[uorb_index].update(&report)) {
// Update the scales and offsets and mark for publication if they've changed
if (_temperature_compensation.update_scales_and_offsets_baro(uorb_index, report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections.baro_device_ids[uorb_index] = report.device_id;
_corrections_changed = true;
}
}
}
}
void TemperatureCompensationModule::Run()
{
perf_begin(_loop_perf);
// Check if user has requested to run the calibration routine
if (_vehicle_command_sub.updated()) {
vehicle_command_s cmd;
if (_vehicle_command_sub.copy(&cmd)) {
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION) {
bool got_temperature_calibration_command = false;
bool accel = false;
bool baro = false;
bool gyro = false;
if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
gyro = true;
got_temperature_calibration_command = true;
}
if ((int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
accel = true;
got_temperature_calibration_command = true;
}
if ((int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
baro = true;
got_temperature_calibration_command = true;
}
if (got_temperature_calibration_command) {
int ret = run_temperature_calibration(accel, baro, gyro);
// publish ACK
vehicle_command_ack_s command_ack{};
if (ret == 0) {
command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED;
}
command_ack.timestamp = hrt_absolute_time();
command_ack.command = cmd.command;
command_ack.target_system = cmd.source_system;
command_ack.target_component = cmd.source_component;
uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
command_ack_pub.publish(command_ack);
}
}
}
}
// Check if any parameter has changed
if (_params_sub.updated()) {
// Read from param to clear updated flag
parameter_update_s update;
_params_sub.copy(&update);
parameters_update();
}
accelPoll();
gyroPoll();
baroPoll();
// publish sensor corrections if necessary
if (_corrections_changed) {
_corrections.timestamp = hrt_absolute_time();
_sensor_correction_pub.publish(_corrections);
_corrections_changed = false;
}
perf_end(_loop_perf);
}
int TemperatureCompensationModule::task_spawn(int argc, char *argv[])
{
TemperatureCompensationModule *instance = new TemperatureCompensationModule();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
bool TemperatureCompensationModule::init()
{
ScheduleOnInterval(1_s);
return true;
}
int TemperatureCompensationModule::custom_command(int argc, char *argv[])
{
if (!strcmp(argv[0], "calibrate")) {
bool accel_calib = false;
bool baro_calib = false;
bool gyro_calib = false;
bool calib_all = true;
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "abg", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'a':
accel_calib = true;
calib_all = false;
break;
case 'b':
baro_calib = true;
calib_all = false;
break;
case 'g':
gyro_calib = true;
calib_all = false;
break;
default:
print_usage("unrecognized flag");
return PX4_ERROR;
}
}
if (!is_running()) {
PX4_WARN("background task not running");
if (task_spawn(0, nullptr) != PX4_OK) {
return PX4_ERROR;
}
}
vehicle_command_s vcmd{};
vcmd.timestamp = hrt_absolute_time();
vcmd.param1 = (float)((gyro_calib
|| calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
vcmd.param2 = NAN;
vcmd.param3 = NAN;
vcmd.param4 = NAN;
vcmd.param5 = ((accel_calib
|| calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : (double)NAN);
vcmd.param6 = (double)NAN;
vcmd.param7 = (float)((baro_calib
|| calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION;
uORB::PublicationQueued<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd_pub.publish(vcmd);
return PX4_OK;
} else {
print_usage("unrecognized command");
return PX4_ERROR;
}
}
int TemperatureCompensationModule::print_status()
{
_temperature_compensation.print_status();
return PX4_OK;
}
int TemperatureCompensationModule::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
The temperature compensation module allows all of the gyro(s), accel(s), and baro(s) in the system to be temperature
compensated. The module monitors the data coming from the sensors and updates the associated sensor_thermal_cal topic
whenever a change in temperature is detected. The module can also be configured to perform the coeffecient calculation
routine at next boot, which allows the thermal calibration coeffecients to be calculated while the vehicle undergoes
a temperature cycle.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("temperature_compensation", "system");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the module, which monitors the sensors and updates the sensor_thermal_cal topic");
PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run temperature calibration process");
PRINT_MODULE_USAGE_PARAM_FLAG('g', "calibrate the gyro", true);
PRINT_MODULE_USAGE_PARAM_FLAG('a', "calibrate the accel", true);
PRINT_MODULE_USAGE_PARAM_FLAG('b', "calibrate the baro (if none of these is given, all will be calibrated)", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int temperature_compensation_main(int argc, char *argv[])
{
return TemperatureCompensationModule::main(argc, argv);
}
src/modules/temperature_compensation/TemperatureCompensationModule.h
+139
View File
@@ -0,0 +1,139 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <drivers/drv_hrt.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/time.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include "TemperatureCompensation.h"
namespace temperature_compensation
{
class TemperatureCompensationModule : public ModuleBase<TemperatureCompensationModule>, public ModuleParams,
public px4::ScheduledWorkItem
{
public:
TemperatureCompensationModule();
~TemperatureCompensationModule() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static TemperatureCompensationModule *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
void Run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
/**
* Initializes scheduling on work queue.
*/
bool init();
private:
void accelPoll();
void gyroPoll();
void baroPoll();
/**
* call this whenever parameters got updated. Make sure to have initialize_sensors() called at least
* once before calling this.
*/
void parameters_update();
uORB::Subscription _accel_subs[ACCEL_COUNT_MAX] {
{ORB_ID(sensor_accel), 0},
{ORB_ID(sensor_accel), 1},
{ORB_ID(sensor_accel), 2}
};
uORB::Subscription _gyro_subs[GYRO_COUNT_MAX] {
{ORB_ID(sensor_gyro), 0},
{ORB_ID(sensor_gyro), 1},
{ORB_ID(sensor_gyro), 2}
};
uORB::Subscription _baro_subs[BARO_COUNT_MAX] {
{ORB_ID(sensor_baro), 0},
{ORB_ID(sensor_baro), 1},
{ORB_ID(sensor_baro), 2}
};
uORB::Subscription _params_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
perf_counter_t _loop_perf; /**< loop performance counter */
orb_advert_t _mavlink_log_pub{nullptr};
/* sensor thermal compensation */
TemperatureCompensation _temperature_compensation;
sensor_correction_s _corrections{}; /**< struct containing the sensor corrections to be published to the uORB*/
uORB::Publication<sensor_correction_s> _sensor_correction_pub{ORB_ID(sensor_correction)};
bool _corrections_changed{true};
};
} // namespace temperature_compensation
src/modules/temperature_compensation/temp_comp_params_accel.c
+512
View File
@@ -0,0 +1,512 @@
/****************************************************************************
*
* 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/temperature_compensation/temp_comp_params_baro.c
+320
View File
@@ -0,0 +1,320 @@
/****************************************************************************
*
* 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/temperature_compensation/temp_comp_params_gyro.c
+512
View File
@@ -0,0 +1,512 @@
/****************************************************************************
*
* 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/temperature_compensation/temperature_calibration/accel.cpp
+230
View File
@@ -0,0 +1,230 @@
/****************************************************************************
*
* 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 accel.cpp
* Implementation of the Accel Temperature Calibration for onboard sensors.
*
* @author Siddharth Bharat Purohit
* @author Paul Riseborough
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include "accel.h"
#include <uORB/topics/sensor_accel.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature,
float max_start_temperature)
: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
{
// init subscriptions
_num_sensor_instances = orb_group_count(ORB_ID(sensor_accel));
if (_num_sensor_instances > SENSOR_COUNT_MAX) {
_num_sensor_instances = SENSOR_COUNT_MAX;
}
for (unsigned i = 0; i < _num_sensor_instances; i++) {
_sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
}
}
TemperatureCalibrationAccel::~TemperatureCalibrationAccel()
{
for (unsigned i = 0; i < _num_sensor_instances; i++) {
orb_unsubscribe(_sensor_subs[i]);
}
}
void TemperatureCalibrationAccel::reset_calibration()
{
/* reset all driver level calibrations */
float offset = 0.0f;
float scale = 1.0f;
for (unsigned s = 0; s < 3; s++) {
set_parameter("CAL_ACC%u_XOFF", s, &offset);
set_parameter("CAL_ACC%u_YOFF", s, &offset);
set_parameter("CAL_ACC%u_ZOFF", s, &offset);
set_parameter("CAL_ACC%u_XSCALE", s, &scale);
set_parameter("CAL_ACC%u_YSCALE", s, &scale);
set_parameter("CAL_ACC%u_ZSCALE", s, &scale);
}
}
int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int sensor_sub)
{
bool finished = data.hot_soaked;
bool updated;
orb_check(sensor_sub, &updated);
if (!updated) {
return finished ? 0 : 1;
}
sensor_accel_s accel_data;
orb_copy(ORB_ID(sensor_accel), sensor_sub, &accel_data);
if (finished) {
// if we're done, return, but we need to return after orb_copy because of poll()
return 0;
}
data.device_id = accel_data.device_id;
data.sensor_sample_filt[0] = accel_data.x;
data.sensor_sample_filt[1] = accel_data.y;
data.sensor_sample_filt[2] = accel_data.z;
data.sensor_sample_filt[3] = accel_data.temperature;
// wait for min start temp to be reached before starting calibration
if (data.sensor_sample_filt[3] < _min_start_temperature) {
return 1;
}
if (!data.cold_soaked) {
// allow time for sensors and filters to settle
if (hrt_absolute_time() > 10E6) {
// If intial temperature exceeds maximum declare an error condition and exit
if (data.sensor_sample_filt[3] > _max_start_temperature) {
return -TC_ERROR_INITIAL_TEMP_TOO_HIGH;
} else {
data.cold_soaked = true;
data.low_temp = data.sensor_sample_filt[3]; // Record the low temperature
data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature
data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
return 1;
}
} else {
return 1;
}
}
// check if temperature increased
if (data.sensor_sample_filt[3] > data.high_temp) {
data.high_temp = data.sensor_sample_filt[3];
data.hot_soak_sat = 0;
} else {
return 1;
}
//TODO: Detect when temperature has stopped rising for more than TBD seconds
if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
data.hot_soaked = true;
}
if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
TC_DEBUG("\nAccel: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
(double)data.sensor_sample_filt[1],
(double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp,
(double)(data.high_temp - data.low_temp));
}
//update linear fit matrices
double relative_temperature = (double)data.sensor_sample_filt[3] - (double)data.ref_temp;
data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
data.P[1].update(relative_temperature, (double)data.sensor_sample_filt[1]);
data.P[2].update(relative_temperature, (double)data.sensor_sample_filt[2]);
return 1;
}
int TemperatureCalibrationAccel::finish()
{
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
finish_sensor_instance(_data[uorb_index], uorb_index);
}
int32_t enabled = 1;
int result = param_set_no_notification(param_find("TC_A_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_A_ENABLE (%i)", result);
}
return result;
}
int TemperatureCalibrationAccel::finish_sensor_instance(PerSensorData &data, int sensor_index)
{
if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
double res[3][4] = {};
data.P[0].fit(res[0]);
res[0][3] = 0.0; // normalise the correction to be zero at the reference temperature
PX4_INFO("Result Accel %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1],
(double)res[0][2],
(double)res[0][3]);
data.P[1].fit(res[1]);
res[1][3] = 0.0; // normalise the correction to be zero at the reference temperature
PX4_INFO("Result Accel %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1],
(double)res[1][2],
(double)res[1][3]);
data.P[2].fit(res[2]);
res[2][3] = 0.0; // normalise the correction to be zero at the reference temperature
PX4_INFO("Result Accel %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1],
(double)res[2][2],
(double)res[2][3]);
data.tempcal_complete = true;
char str[30];
float param = 0.0f;
int result = PX4_OK;
set_parameter("TC_A%d_ID", sensor_index, &data.device_id);
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
sprintf(str, "TC_A%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
param = (float)res[axis_index][coef_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
}
set_parameter("TC_A%d_TMAX", sensor_index, &data.high_temp);
set_parameter("TC_A%d_TMIN", sensor_index, &data.low_temp);
set_parameter("TC_A%d_TREF", sensor_index, &data.ref_temp);
return 0;
}
src/modules/temperature_compensation/temperature_calibration/accel.h
+60
View File
@@ -0,0 +1,60 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
#include "common.h"
#include "polyfit.hpp"
class TemperatureCalibrationAccel : public TemperatureCalibrationCommon<3, 3>
{
public:
TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature, float max_start_temperature);
virtual ~TemperatureCalibrationAccel();
/**
* @see TemperatureCalibrationBase::finish()
*/
int finish();
/**
* @see TemperatureCalibrationBase::reset_calibration()
*/
void reset_calibration();
private:
virtual inline int update_sensor_instance(PerSensorData &data, int sensor_sub);
inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
};
src/modules/temperature_compensation/temperature_calibration/baro.cpp
+203
View File
@@ -0,0 +1,203 @@
/****************************************************************************
*
* 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 baro.cpp
* Implementation of the Baro Temperature Calibration for onboard sensors.
*
* @author Siddharth Bharat Purohit
* @author Paul Riseborough
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include "baro.h"
#include <uORB/topics/sensor_baro.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature,
float max_start_temperature)
: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
{
// init subscriptions
_num_sensor_instances = orb_group_count(ORB_ID(sensor_baro));
if (_num_sensor_instances > SENSOR_COUNT_MAX) {
_num_sensor_instances = SENSOR_COUNT_MAX;
}
for (unsigned i = 0; i < _num_sensor_instances; i++) {
_sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_baro), i);
}
}
TemperatureCalibrationBaro::~TemperatureCalibrationBaro()
{
for (unsigned i = 0; i < _num_sensor_instances; i++) {
orb_unsubscribe(_sensor_subs[i]);
}
}
void TemperatureCalibrationBaro::reset_calibration()
{
// nothing to do
}
int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int sensor_sub)
{
bool finished = data.hot_soaked;
bool updated;
orb_check(sensor_sub, &updated);
if (!updated) {
return finished ? 0 : 1;
}
sensor_baro_s baro_data;
orb_copy(ORB_ID(sensor_baro), sensor_sub, &baro_data);
if (finished) {
// if we're done, return, but we need to return after orb_copy because of poll()
return 0;
}
data.device_id = baro_data.device_id;
data.sensor_sample_filt[0] = 100.0f * baro_data.pressure; // convert from hPA to Pa
data.sensor_sample_filt[1] = baro_data.temperature;
// wait for min start temp to be reached before starting calibration
if (data.sensor_sample_filt[1] < _min_start_temperature) {
return 1;
}
if (!data.cold_soaked) {
// allow time for sensors and filters to settle
if (hrt_absolute_time() > 10E6) {
// If intial temperature exceeds maximum declare an error condition and exit
if (data.sensor_sample_filt[1] > _max_start_temperature) {
return -TC_ERROR_INITIAL_TEMP_TOO_HIGH;
} else {
data.cold_soaked = true;
data.low_temp = data.sensor_sample_filt[1]; // Record the low temperature
data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature
data.ref_temp = data.sensor_sample_filt[1] + 0.5f * _min_temperature_rise;
return 1;
}
} else {
return 1;
}
}
// check if temperature increased
if (data.sensor_sample_filt[1] > data.high_temp) {
data.high_temp = data.sensor_sample_filt[1];
data.hot_soak_sat = 0;
} else {
return 1;
}
// TODO: Detect when temperature has stopped rising for more than TBD seconds
if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
data.hot_soaked = true;
}
if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
TC_DEBUG("\nBaro: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
(double)data.sensor_sample_filt[1], (double)data.low_temp, (double)data.high_temp,
(double)(data.high_temp - data.low_temp));
}
// update linear fit matrices
double relative_temperature = (double)data.sensor_sample_filt[1] - (double)data.ref_temp;
data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
return 1;
}
int TemperatureCalibrationBaro::finish()
{
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
finish_sensor_instance(_data[uorb_index], uorb_index);
}
int32_t enabled = 1;
int result = param_set_no_notification(param_find("TC_B_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_B_ENABLE (%i)", result);
}
return result;
}
int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int sensor_index)
{
if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
double res[POLYFIT_ORDER + 1] {};
data.P[0].fit(res);
res[POLYFIT_ORDER] =
0.0; // normalise the correction to be zero at the reference temperature by setting the X^0 coefficient to zero
PX4_INFO("Result baro %u %.20f %.20f %.20f %.20f %.20f %.20f", sensor_index, (double)res[0],
(double)res[1], (double)res[2], (double)res[3], (double)res[4], (double)res[5]);
data.tempcal_complete = true;
char str[30];
float param = 0.0f;
int result = PX4_OK;
set_parameter("TC_B%d_ID", sensor_index, &data.device_id);
for (unsigned coef_index = 0; coef_index <= POLYFIT_ORDER; coef_index++) {
sprintf(str, "TC_B%d_X%d", sensor_index, (POLYFIT_ORDER - coef_index));
param = (float)res[coef_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
set_parameter("TC_B%d_TMAX", sensor_index, &data.high_temp);
set_parameter("TC_B%d_TMIN", sensor_index, &data.low_temp);
set_parameter("TC_B%d_TREF", sensor_index, &data.ref_temp);
return 0;
}
src/modules/temperature_compensation/temperature_calibration/baro.h
+63
View File
@@ -0,0 +1,63 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
#include "common.h"
#include "polyfit.hpp"
#define POLYFIT_ORDER 5
class TemperatureCalibrationBaro : public TemperatureCalibrationCommon<1, POLYFIT_ORDER>
{
public:
TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature, float max_start_temperature);
virtual ~TemperatureCalibrationBaro();
/**
* @see TemperatureCalibrationBase::finish()
*/
int finish();
/**
* @see TemperatureCalibrationBase::reset_calibration()
*/
void reset_calibration();
private:
virtual int update_sensor_instance(PerSensorData &data, int sensor_sub);
inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
};
src/modules/temperature_compensation/temperature_calibration/common.h
+193
View File
@@ -0,0 +1,193 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
#define TC_PRINT_DEBUG 0
#if TC_PRINT_DEBUG
#define TC_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
#else
#define TC_DEBUG(fmt, ...)
#endif
#include <px4_platform_common/log.h>
#include <mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include "polyfit.hpp"
#define SENSOR_COUNT_MAX 3
#define TC_ERROR_INITIAL_TEMP_TOO_HIGH 110 ///< starting temperature was above the configured allowed temperature
#define TC_ERROR_COMMUNICATION 112 ///< no sensors found
/**
* Base class for temperature calibration types with abstract methods (for all different sensor types)
*/
class TemperatureCalibrationBase
{
public:
TemperatureCalibrationBase(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
: _min_temperature_rise(min_temperature_rise), _min_start_temperature(min_start_temperature),
_max_start_temperature(max_start_temperature) {}
virtual ~TemperatureCalibrationBase() = default;
/**
* check & update new sensor data.
* @return progress in range [0, 100], 110 when finished, <0 on error,
* -TC_ERROR_INITIAL_TEMP_TOO_HIGH if starting temperature is too hot
* -TC_ERROR_COMMUNICATION if no sensors found
*/
virtual int update() = 0;
/**
* do final fitting & write the parameters. Call this exactly once after update() returned 110
* @return 0 on success, <0 otherwise
*/
virtual int finish() = 0;
/** reset all driver-level calibration parameters */
virtual void reset_calibration() = 0;
protected:
/**
* set a system parameter (without system notification) and print an error if it fails
* @param format_str for example "CAL_GYRO%u_XOFF"
* @param index which index (will replace %u in format_str)
* @param value
* @return 0 on success
*/
inline int set_parameter(const char *format_str, unsigned index, const void *value);
float _min_temperature_rise; ///< minimum difference in temperature before the process finishes
float _min_start_temperature; ///< minimum temperature before the process starts
float _max_start_temperature; ///< maximum temperature above which the process does not start and an error is declared
};
int TemperatureCalibrationBase::set_parameter(const char *format_str, unsigned index, const void *value)
{
char param_str[30] {};
(void)sprintf(param_str, format_str, index);
int result = param_set_no_notification(param_find(param_str), value);
if (result != 0) {
PX4_ERR("unable to reset %s (%i)", param_str, result);
}
return result;
}
/**
** class TemperatureCalibrationCommon
* Common base class for all sensor types, contains shared code & data.
*/
template <int Dim, int PolyfitOrder>
class TemperatureCalibrationCommon : public TemperatureCalibrationBase
{
public:
TemperatureCalibrationCommon(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
: TemperatureCalibrationBase(min_temperature_rise, min_start_temperature, max_start_temperature) {}
virtual ~TemperatureCalibrationCommon() = default;
/**
* @see TemperatureCalibrationBase::update()
*/
int update()
{
int num_not_complete = 0;
if (_num_sensor_instances == 0) {
return -TC_ERROR_COMMUNICATION;
}
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
int status = update_sensor_instance(_data[uorb_index], _sensor_subs[uorb_index]);
if (status < 0) {
return status;
}
num_not_complete += status;
}
if (num_not_complete > 0) {
// calculate progress
float min_diff = _min_temperature_rise;
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp;
if (cur_diff < min_diff) {
min_diff = cur_diff;
}
}
return math::min(100, (int)(min_diff / _min_temperature_rise * 100.f));
}
return 110;
}
protected:
struct PerSensorData {
float sensor_sample_filt[Dim + 1]; ///< last value is the temperature
polyfitter < PolyfitOrder + 1 > P[Dim];
unsigned hot_soak_sat = 0; /**< counter that increments every time the sensor temperature reduces
from the last reading */
uint32_t device_id = 0; ///< ID for the sensor being calibrated
bool cold_soaked = false; ///< true when the sensor cold soak starting temperature condition had been
/// verified and the starting temperature set
bool hot_soaked = false; ///< true when the sensor has achieved the specified temperature increase
bool tempcal_complete = false; ///< true when the calibration has been completed
float low_temp = 0.f; ///< low temperature recorded at start of calibration (deg C)
float high_temp = 0.f; ///< highest temperature recorded during calibration (deg C)
float ref_temp = 0.f; /**< calibration reference temperature, nominally in the middle of the
calibration temperature range (deg C) */
};
PerSensorData _data[SENSOR_COUNT_MAX];
/**
* update a single sensor instance
* @return 0 when done, 1 not finished yet, <0 for an error
*/
virtual int update_sensor_instance(PerSensorData &data, int sensor_sub) = 0;
unsigned _num_sensor_instances{0};
int _sensor_subs[SENSOR_COUNT_MAX];
};
src/modules/temperature_compensation/temperature_calibration/gyro.cpp
+211
View File
@@ -0,0 +1,211 @@
/****************************************************************************
*
* 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 gyro.cpp
* Implementation of the Gyro Temperature Calibration for onboard sensors.
*
* @author Siddharth Bharat Purohit
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include <mathlib/mathlib.h>
#include <uORB/topics/sensor_gyro.h>
#include "gyro.h"
#include <drivers/drv_hrt.h>
TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature,
float max_start_temperature, int gyro_subs[], int num_gyros)
: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
{
for (int i = 0; i < num_gyros; ++i) {
_sensor_subs[i] = gyro_subs[i];
}
_num_sensor_instances = num_gyros;
}
void TemperatureCalibrationGyro::reset_calibration()
{
/* reset all driver level calibrations */
float offset = 0.0f;
for (unsigned s = 0; s < 3; s++) {
set_parameter("CAL_GYRO%u_XOFF", s, &offset);
set_parameter("CAL_GYRO%u_YOFF", s, &offset);
set_parameter("CAL_GYRO%u_ZOFF", s, &offset);
}
}
int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int sensor_sub)
{
bool finished = data.hot_soaked;
bool updated;
orb_check(sensor_sub, &updated);
if (!updated) {
return finished ? 0 : 1;
}
sensor_gyro_s gyro_data{};
orb_copy(ORB_ID(sensor_gyro), sensor_sub, &gyro_data);
if (finished) {
// if we're done, return, but we need to return after orb_copy because of poll()
return 0;
}
data.device_id = gyro_data.device_id;
data.sensor_sample_filt[0] = gyro_data.x;
data.sensor_sample_filt[1] = gyro_data.y;
data.sensor_sample_filt[2] = gyro_data.z;
data.sensor_sample_filt[3] = gyro_data.temperature;
// wait for min start temp to be reached before starting calibration
if (data.sensor_sample_filt[3] < _min_start_temperature) {
return 1;
}
if (!data.cold_soaked) {
// allow time for sensors and filters to settle
if (hrt_absolute_time() > 10E6) {
// If intial temperature exceeds maximum declare an error condition and exit
if (data.sensor_sample_filt[3] > _max_start_temperature) {
return -TC_ERROR_INITIAL_TEMP_TOO_HIGH;
} else {
data.cold_soaked = true;
data.low_temp = data.sensor_sample_filt[3]; // Record the low temperature
data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature
data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
return 1;
}
} else {
return 1;
}
}
// check if temperature increased
if (data.sensor_sample_filt[3] > data.high_temp) {
data.high_temp = data.sensor_sample_filt[3];
data.hot_soak_sat = 0;
} else {
return 1;
}
// TODO: Detect when temperature has stopped rising for more than TBD seconds
if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
data.hot_soaked = true;
}
if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
TC_DEBUG("\nGyro: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
(double)data.sensor_sample_filt[1],
(double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp,
(double)(data.high_temp - data.low_temp));
}
//update linear fit matrices
double relative_temperature = (double)data.sensor_sample_filt[3] - (double)data.ref_temp;
data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
data.P[1].update(relative_temperature, (double)data.sensor_sample_filt[1]);
data.P[2].update(relative_temperature, (double)data.sensor_sample_filt[2]);
return 1;
}
int TemperatureCalibrationGyro::finish()
{
for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
finish_sensor_instance(_data[uorb_index], uorb_index);
}
int32_t enabled = 1;
int result = param_set_no_notification(param_find("TC_G_ENABLE"), &enabled);
if (result != PX4_OK) {
PX4_ERR("unable to reset TC_G_ENABLE (%i)", result);
}
return result;
}
int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int sensor_index)
{
if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
double res[3][4] {};
data.P[0].fit(res[0]);
PX4_INFO("Result Gyro %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1],
(double)res[0][2],
(double)res[0][3]);
data.P[1].fit(res[1]);
PX4_INFO("Result Gyro %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1],
(double)res[1][2],
(double)res[1][3]);
data.P[2].fit(res[2]);
PX4_INFO("Result Gyro %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1],
(double)res[2][2],
(double)res[2][3]);
data.tempcal_complete = true;
char str[30] {};
float param = 0.0f;
int result = PX4_OK;
set_parameter("TC_G%d_ID", sensor_index, &data.device_id);
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
sprintf(str, "TC_G%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
param = (float)res[axis_index][coef_index];
result = param_set_no_notification(param_find(str), &param);
if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
}
}
set_parameter("TC_G%d_TMAX", sensor_index, &data.high_temp);
set_parameter("TC_G%d_TMIN", sensor_index, &data.low_temp);
set_parameter("TC_G%d_TREF", sensor_index, &data.ref_temp);
return 0;
}
src/modules/temperature_compensation/temperature_calibration/gyro.h
+61
View File
@@ -0,0 +1,61 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
#include "common.h"
#include "polyfit.hpp"
class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3>
{
public:
TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, float max_start_temperature,
int gyro_subs[], int num_gyros);
virtual ~TemperatureCalibrationGyro() {}
/**
* @see TemperatureCalibrationBase::finish()
*/
int finish();
/**
* @see TemperatureCalibrationBase::reset_calibration()
*/
void reset_calibration();
private:
virtual int update_sensor_instance(PerSensorData &data, int sensor_sub);
inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
};
src/modules/temperature_compensation/temperature_calibration/polyfit.hpp
+196
View File
@@ -0,0 +1,196 @@
/****************************************************************************
*
* Copyright (c) 2015-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.
*
****************************************************************************/
/*
This algorithm performs a curve fit of m x,y data points using a polynomial
equation of the following form:
yi = a0 + a1.xi + a2.xi^2 + a3.xi^3 + .... + an.xi^n + ei , where:
i = [0,m]
xi is the x coordinate (independant variable) of the i'th measurement
yi is the y coordinate (dependant variable) of the i'th measurement
ei is a random fit error being the difference between the i'th y coordinate
and the value predicted by the polynomial.
In vector form this is represented as:
Y = V.A + E , where:
V is Vandermonde matrix in x -> https://en.wikipedia.org/wiki/Vandermonde_matrix
Y is a vector of length m containing the y measurements
E is a vector of length m containing the fit errors for each measurement
Use an Ordinary Least Squares derivation to minimise ∑(i=0..m)ei^2 -> https://en.wikipedia.org/wiki/Ordinary_least_squares
Note: In the wikipedia reference, the X matrix in reference is equivalent to our V matrix and the Beta matrix is equivalent to our A matrix
A = inv(transpose(V)*V)*(transpose(V)*Y)
We can accumulate VTV and VTY recursively as they are of fixed size, where:
VTV = transpose(V)*V =
__ __
| m+1 x0+x1+...+xm x0^2+x1^2+...+xm^3 .......... x0^n+x1^n+...+xm^n |
|x0+x1+...+xm x0^2+x1^2+...+xm^3 x0^3+x1^3+...+xm^3 .......... x0^(n+1)+x1^(n+1)+...+xm^(n+1) |
| . . . . |
| . . . . |
| . . . . |
|x0^n+x1^n+...+xm^n x0^(n+1)+x1^(n+1)+...+xm^(n+1) x0^(n+2)+x1^(n+2)+...+xm^(n+2) .... x0^(2n)+x1^(2n)+...+xm^(2n) |
|__ __|
and VTY = transpose(V)*Y =
__ __
| ∑(i=0..m)yi |
| ∑(i=0..m)yi*xi |
| . |
| . |
| . |
|∑(i=0..m)yi*xi^n|
|__ __|
*/
/*
Polygon linear fit
Author: Siddharth Bharat Purohit
*/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <float.h>
#include <matrix/math.hpp>
#define DEBUG 0
#if DEBUG
#define PF_DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__);
#else
#define PF_DEBUG(fmt, ...)
#endif
template<int _forder>
class polyfitter
{
public:
polyfitter() {}
void update(double x, double y)
{
update_VTV(x);
update_VTY(x, y);
}
bool fit(double res[])
{
//Do inverse of VTV
matrix::SquareMatrix<double, _forder> IVTV;
IVTV = _VTV.I();
for (int i = 0; i < _forder; i++) {
for (int j = 0; j < _forder; j++) {
PF_DEBUG("%.10f ", (double)IVTV(i, j));
}
PF_DEBUG("\n");
}
for (int i = 0; i < _forder; i++) {
res[i] = 0.0;
for (int j = 0; j < _forder; j++) {
res[i] += IVTV(i, j) * (double)_VTY(j);
}
PF_DEBUG("%.10f ", res[i]);
}
return true;
}
private:
matrix::SquareMatrix<double, _forder> _VTV;
matrix::Vector<double, _forder> _VTY;
void update_VTY(double x, double y)
{
double temp = 1.0;
PF_DEBUG("O %.6f\n", (double)x);
for (int i = _forder - 1; i >= 0; i--) {
_VTY(i) += y * temp;
temp *= x;
PF_DEBUG("%.6f ", (double)_VTY(i));
}
PF_DEBUG("\n");
}
void update_VTV(double x)
{
double temp = 1.0;
int8_t z;
for (int i = 0; i < _forder; i++) {
for (int j = 0; j < _forder; j++) {
PF_DEBUG("%.10f ", (double)_VTV(i, j));
}
PF_DEBUG("\n");
}
for (int i = 2 * _forder - 2; i >= 0; i--) {
if (i < _forder) {
z = 0.0f;
} else {
z = i - _forder + 1;
}
for (int j = i - z; j >= z; j--) {
int row = j;
int col = i - j;
_VTV(row, col) += (double)temp;
}
temp *= x;
}
}
};
src/modules/temperature_compensation/temperature_calibration/task.cpp
+362
View File
@@ -0,0 +1,362 @@
/****************************************************************************
*
* 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 task.cpp
*
* Main task handling the temperature calibration process
*
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include <uORB/PublicationQueued.hpp>
#include <uORB/topics/sensor_gyro.h>
#include <mathlib/mathlib.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_led.h>
#include <unistd.h>
#include "common.h"
#include "temperature_calibration.h"
#include "accel.h"
#include "baro.h"
#include "gyro.h"
class TemperatureCalibration;
namespace temperature_calibration
{
px4::atomic<TemperatureCalibration *> instance{nullptr};
}
class TemperatureCalibration
{
public:
TemperatureCalibration(bool accel, bool baro, bool gyro) : _accel(accel), _baro(baro), _gyro(gyro) {}
~TemperatureCalibration() = default;
/**
* Start task.
*
* @return OK on success.
*/
int start();
static int do_temperature_calibration(int argc, char *argv[]);
void task_main();
void exit_task() { _force_task_exit = true; }
private:
void publish_led_control(led_control_s &led_control);
uORB::PublicationQueued<led_control_s> _led_control_pub{ORB_ID(led_control)};
bool _force_task_exit = false;
int _control_task = -1; // task handle for task
const bool _accel; ///< enable accel calibration?
const bool _baro; ///< enable baro calibration?
const bool _gyro; ///< enable gyro calibration?
};
void TemperatureCalibration::task_main()
{
// subscribe to all gyro instances
int gyro_sub[SENSOR_COUNT_MAX] {-1, -1, -1};
px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] {};
unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro));
if (num_gyro > SENSOR_COUNT_MAX) {
num_gyro = SENSOR_COUNT_MAX;
}
for (unsigned i = 0; i < num_gyro; i++) {
gyro_sub[i] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
fds[i].fd = gyro_sub[i];
fds[i].events = POLLIN;
}
int32_t min_temp_rise = 24;
param_get(param_find("SYS_CAL_TDEL"), &min_temp_rise);
PX4_INFO("Waiting for %i degrees difference in sensor temperature", min_temp_rise);
int32_t min_start_temp = 5;
param_get(param_find("SYS_CAL_TMIN"), &min_start_temp);
int32_t max_start_temp = 10;
param_get(param_find("SYS_CAL_TMAX"), &max_start_temp);
//init calibrators
TemperatureCalibrationBase *calibrators[3] {};
bool error_reported[3] {};
int num_calibrators = 0;
if (_accel) {
calibrators[num_calibrators] = new TemperatureCalibrationAccel(min_temp_rise, min_start_temp, max_start_temp);
if (calibrators[num_calibrators]) {
++num_calibrators;
} else {
PX4_ERR("alloc failed");
}
}
if (_baro) {
calibrators[num_calibrators] = new TemperatureCalibrationBaro(min_temp_rise, min_start_temp, max_start_temp);
if (calibrators[num_calibrators]) {
++num_calibrators;
} else {
PX4_ERR("alloc failed");
}
}
if (_gyro) {
calibrators[num_calibrators] = new TemperatureCalibrationGyro(min_temp_rise, min_start_temp, max_start_temp, gyro_sub,
num_gyro);
if (calibrators[num_calibrators]) {
++num_calibrators;
} else {
PX4_ERR("alloc failed");
}
}
// reset params
for (int i = 0; i < num_calibrators; ++i) {
calibrators[i]->reset_calibration();
}
// make sure the system updates the changed parameters
param_notify_changes();
px4_usleep(300000); // wait a bit for the system to apply the parameters
hrt_abstime next_progress_output = hrt_absolute_time() + 1e6;
// control LED's: blink, then turn solid according to progress
led_control_s led_control{};
led_control.led_mask = 0xff;
led_control.mode = led_control_s::MODE_BLINK_NORMAL;
led_control.priority = led_control_s::MAX_PRIORITY;
led_control.color = led_control_s::COLOR_YELLOW;
led_control.num_blinks = 0;
publish_led_control(led_control);
int leds_completed = 0;
bool abort_calibration = false;
while (!_force_task_exit) {
/* we poll on the gyro(s), since this is the sensor with the highest update rate.
* Each individual sensor will then check on its own if there's new data.
*/
int ret = px4_poll(fds, num_gyro, 1000);
if (ret < 0) {
// Poll error, sleep and try again
px4_usleep(10000);
continue;
} else if (ret == 0) {
// Poll timeout or no new data, do nothing
continue;
}
//if gyro is not enabled: we must do an orb_copy here, so that poll() does not immediately return again
if (!_gyro) {
sensor_gyro_s gyro_data;
for (unsigned i = 0; i < num_gyro; ++i) {
orb_copy(ORB_ID(sensor_gyro), gyro_sub[i], &gyro_data);
}
}
int min_progress = 110;
for (int i = 0; i < num_calibrators; ++i) {
ret = calibrators[i]->update();
if (ret == -TC_ERROR_COMMUNICATION) {
abort_calibration = true;
PX4_ERR("Calibration won't start - sensor bad or communication error");
_force_task_exit = true;
break;
} else if (ret == -TC_ERROR_INITIAL_TEMP_TOO_HIGH) {
abort_calibration = true;
PX4_ERR("Calibration won't start - sensor temperature too high");
_force_task_exit = true;
break;
} else if (ret < 0 && !error_reported[i]) {
// temperature has decreased so calibration is not being updated
error_reported[i] = true;
PX4_ERR("Calibration update step failed (%i)", ret);
} else if (ret < min_progress) {
// temperature is stable or increasing
min_progress = ret;
}
}
if (min_progress == 110 || abort_calibration) {
break; // we are done
}
int led_progress = min_progress * BOARD_MAX_LEDS / 100;
for (; leds_completed < led_progress; ++leds_completed) {
led_control.led_mask = 1 << leds_completed;
led_control.mode = led_control_s::MODE_ON;
publish_led_control(led_control);
}
//print progress each second
hrt_abstime now = hrt_absolute_time();
if (now > next_progress_output) {
PX4_INFO("Calibration progress: %i%%", min_progress);
next_progress_output = now + 1e6;
}
}
if (abort_calibration) {
led_control.color = led_control_s::COLOR_RED;
} else {
PX4_INFO("Sensor Measurments completed");
// save params immediately so that we can check the result and don't have to wait for param save timeout
param_control_autosave(false);
// do final calculations & parameter storage
for (int i = 0; i < num_calibrators; ++i) {
int ret = calibrators[i]->finish();
if (ret < 0) {
PX4_ERR("Failed to finish calibration process (%i)", ret);
}
}
param_notify_changes();
int ret = param_save_default();
if (ret != 0) {
PX4_ERR("Failed to save params (%i)", ret);
}
param_control_autosave(true);
led_control.color = led_control_s::COLOR_GREEN;
}
// blink the LED's according to success/failure
led_control.led_mask = 0xff;
led_control.mode = led_control_s::MODE_BLINK_FAST;
led_control.num_blinks = 0;
publish_led_control(led_control);
for (int i = 0; i < num_calibrators; ++i) {
delete calibrators[i];
}
for (unsigned i = 0; i < num_gyro; i++) {
orb_unsubscribe(gyro_sub[i]);
}
delete temperature_calibration::instance.load();
temperature_calibration::instance.store(nullptr);
PX4_INFO("Exiting temperature calibration task");
}
int TemperatureCalibration::do_temperature_calibration(int argc, char *argv[])
{
temperature_calibration::instance.load()->task_main();
return 0;
}
int TemperatureCalibration::start()
{
_control_task = px4_task_spawn_cmd("temperature_calib",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
5800,
(px4_main_t)&TemperatureCalibration::do_temperature_calibration,
nullptr);
if (_control_task < 0) {
delete temperature_calibration::instance.load();
temperature_calibration::instance.store(nullptr);
PX4_ERR("start failed");
return -errno;
}
return 0;
}
void TemperatureCalibration::publish_led_control(led_control_s &led_control)
{
led_control.timestamp = hrt_absolute_time();
_led_control_pub.publish(led_control);
}
int run_temperature_calibration(bool accel, bool baro, bool gyro)
{
if (temperature_calibration::instance.load() == nullptr) {
PX4_INFO("Starting temperature calibration task (accel=%i, baro=%i, gyro=%i)", (int)accel, (int)baro, (int)gyro);
temperature_calibration::instance.store(new TemperatureCalibration(accel, baro, gyro));
if (temperature_calibration::instance.load() == nullptr) {
PX4_ERR("alloc failed");
return 1;
}
return temperature_calibration::instance.load()->start();
} else {
PX4_WARN("temperature calibration task already running");
}
return PX4_ERROR;
}
src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h
+39
View File
@@ -0,0 +1,39 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
/** start temperature calibration in a new task for one or multiple sensors
* @return 0 on success, <0 error otherwise */
int run_temperature_calibration(bool accel, bool baro, bool gyro);