From 916a04bc56d757d3ee400eb7249b895961e5bd5e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 19 Jan 2017 08:00:09 +0100 Subject: [PATCH] sensors temp compensation: use SENSOR_COUNT_MAX instead of just 3 --- src/modules/sensors/common.h | 48 +++++++++++++++++++ .../sensors/temperature_compensation.cpp | 14 +++--- .../sensors/temperature_compensation.h | 22 +++++---- src/modules/sensors/voted_sensors_update.h | 3 +- 4 files changed, 70 insertions(+), 17 deletions(-) create mode 100644 src/modules/sensors/common.h diff --git a/src/modules/sensors/common.h b/src/modules/sensors/common.h new file mode 100644 index 0000000000..6d6f6b6c56 --- /dev/null +++ b/src/modules/sensors/common.h @@ -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 + */ + +namespace sensors +{ + +static const int SENSOR_COUNT_MAX = 3; + +} /* namespace sensors */ diff --git a/src/modules/sensors/temperature_compensation.cpp b/src/modules/sensors/temperature_compensation.cpp index 7cebe035ad..b35e57825e 100644 --- a/src/modules/sensors/temperature_compensation.cpp +++ b/src/modules/sensors/temperature_compensation.cpp @@ -54,7 +54,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles ¶ /* 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 ¶ /* 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 ¶ /* 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 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; diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/sensors/temperature_compensation.h index 46d019f78c..2daa065680 100644 --- a/src/modules/sensors/temperature_compensation.h +++ b/src/modules/sensors/temperature_compensation.h @@ -43,9 +43,15 @@ #include #include +#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; diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index c45fd14927..930f17e75f 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -59,12 +59,11 @@ #include #include "temperature_compensation.h" +#include "common.h" namespace sensors { -static const int SENSOR_COUNT_MAX = 3; - /** ** class VotedSensorsUpdate *