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

sensors temp compensation: use SENSOR_COUNT_MAX instead of just 3

Browse Source
This commit is contained in:
Beat Küng 2017-01-19 08:00:09 +01:00 committed by Lorenz Meier
parent a3515893f3
commit 916a04bc56
4 changed files with 70 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
src/modules/sensors/common.h Normal file
View File

@ -0,0 +1,48 @@
/****************************************************************************
*
* 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
/**
* @file common.h
* common definitions used in sensors module
*
* @author Beat Kueng <beat-kueng@gmx.net>
*/
namespace sensors
{
static const int SENSOR_COUNT_MAX = 3;
} /* namespace sensors */

14
src/modules/sensors/temperature_compensation.cpp
View File

@ -54,7 +54,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
/* rate gyro calibration parameters */
parameter_handles.gyro_tc_enable = param_find("TC_G_ENABLE");
for (unsigned j = 0; j < 3; j++) {
for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
sprintf(nbuf, "TC_G%d_ID", j);
parameter_handles.gyro_cal_handles[j].ID = param_find(nbuf);
@ -82,7 +82,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
/* accelerometer calibration parameters */
parameter_handles.accel_tc_enable = param_find("TC_A_ENABLE");
for (unsigned j = 0; j < 3; j++) {
for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
sprintf(nbuf, "TC_A%d_ID", j);
parameter_handles.accel_cal_handles[j].ID = param_find(nbuf);
@ -110,7 +110,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
/* barometer calibration parameters */
parameter_handles.baro_tc_enable = param_find("TC_B_ENABLE");
for (unsigned j = 0; j < 3; j++) {
for (unsigned j = 0; j < SENSOR_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);
@ -152,7 +152,7 @@ int TemperatureCompensation::parameters_update()
/* rate gyro calibration parameters */
param_get(parameter_handles.gyro_tc_enable, &(_parameters.gyro_tc_enable));
for (unsigned j = 0; j < 3; j++) {
for (unsigned j = 0; j < SENSOR_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));
@ -183,7 +183,7 @@ int TemperatureCompensation::parameters_update()
/* accelerometer calibration parameters */
param_get(parameter_handles.accel_tc_enable, &(_parameters.accel_tc_enable));
for (unsigned j = 0; j < 3; j++) {
for (unsigned j = 0; j < SENSOR_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));
@ -214,7 +214,7 @@ int TemperatureCompensation::parameters_update()
/* barometer calibration parameters */
param_get(parameter_handles.baro_tc_enable, &(_parameters.baro_tc_enable));
for (unsigned j = 0; j < 3; j++) {
for (unsigned j = 0; j < SENSOR_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));
@ -332,7 +332,7 @@ template<typename T>
int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data,
const T *sensor_cal_data)
{
for (int i = 0; i < 3; ++i) {
for (int i = 0; i < SENSOR_COUNT_MAX; ++i) {
if (device_id == sensor_cal_data[i].ID) {
sensor_data.device_mapping[topic_instance] = i;
return 0;

22
src/modules/sensors/temperature_compensation.h
View File

@ -43,9 +43,15 @@
#include <systemlib/param/param.h>
#include <mathlib/mathlib.h>
#include "common.h"
namespace sensors
{
static_assert(SENSOR_COUNT_MAX == 3,
"SENSOR_COUNT_MAX must be 3 (if changed, add/remove TC_* params to match the count)");
/**
** class TemperatureCompensation
* Applies temperature compensation to sensor data. Loads the parameters from PX4 param storage.
@ -173,21 +179,21 @@ private:
// create a struct containing all thermal calibration parameters
struct Parameters {
int gyro_tc_enable;
SensorCalData3D gyro_cal_data[3];
SensorCalData3D gyro_cal_data[SENSOR_COUNT_MAX];
int accel_tc_enable;
SensorCalData3D accel_cal_data[3];
SensorCalData3D accel_cal_data[SENSOR_COUNT_MAX];
int baro_tc_enable;
SensorCalData1D baro_cal_data[3];
SensorCalData1D baro_cal_data[SENSOR_COUNT_MAX];
};
// create a struct containing the handles required to access all calibration parameters
struct ParameterHandles {
param_t gyro_tc_enable;
SensorCalHandles3D gyro_cal_handles[3];
SensorCalHandles3D gyro_cal_handles[SENSOR_COUNT_MAX];
param_t accel_tc_enable;
SensorCalHandles3D accel_cal_handles[3];
SensorCalHandles3D accel_cal_handles[SENSOR_COUNT_MAX];
param_t baro_tc_enable;
SensorCalHandles1D baro_cal_handles[3];
SensorCalHandles1D baro_cal_handles[SENSOR_COUNT_MAX];
};
@ -243,9 +249,9 @@ private:
struct PerSensorData {
PerSensorData()
{
device_mapping[0] = device_mapping[1] = device_mapping[2] = 255;
for (int i = 0; i < SENSOR_COUNT_MAX; ++i) { device_mapping[i] = 255; }
}
uint8_t device_mapping[3]; /// map a topic instance to the parameters index
uint8_t device_mapping[SENSOR_COUNT_MAX]; /// map a topic instance to the parameters index
int8_t last_temperature = -100;
};
PerSensorData _gyro_data;

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

@ -59,12 +59,11 @@
#include <DevMgr.hpp>
#include "temperature_compensation.h"
#include "common.h"
namespace sensors
{
static const int SENSOR_COUNT_MAX = 3;
/**
** class VotedSensorsUpdate
*