zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

sensors temp compensation: refactor into a separate class 

- reduces some code duplication
- provides clear API & separation for temp compensation

additional changes:
- added timestamp to sensor_correction topic
- reduced its publication rate, to only when voting index or scales or
  offsets change (if there is more than 1deg change in temperature)
sbg
Beat Küng 8 years ago committed by Lorenz Meier
parent
23d278cc43
commit
117826a31f
4 changed files with 466 additions and 340 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 209
      src/modules/sensors/temperature_compensation.cpp
  2. 346
      src/modules/sensors/temperature_compensation.h
  3. 242
      src/modules/sensors/voted_sensors_update.cpp
  4. 9
      src/modules/sensors/voted_sensors_update.h

209
src/modules/sensors/temperature_compensation.cpp
Unescape View File

@ -44,10 +44,10 @@ @@ -44,10 +44,10 @@
#include <stdio.h>
#include <px4_defines.h>
namespace sensors_temp_comp
namespace sensors
{
int initialize_parameter_handles(ParameterHandles &parameter_handles)
int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &parameter_handles)
{
char nbuf[16];
@ -138,34 +138,41 @@ int initialize_parameter_handles(ParameterHandles &parameter_handles) @@ -138,34 +138,41 @@ int initialize_parameter_handles(ParameterHandles &parameter_handles)
return PX4_OK;
}
int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters)
int TemperatureCompensation::parameters_update()
{
int ret = PX4_OK;
int ret = 0;
ParameterHandles parameter_handles;
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));
param_get(parameter_handles.gyro_tc_enable, &(_parameters.gyro_tc_enable));
for (unsigned j = 0; j < 3; j++) {
if (param_get(parameter_handles.gyro_cal_handles[j].ID, &(parameters.gyro_cal_data[j].ID)) == PX4_OK) {
param_get(parameter_handles.gyro_cal_handles[j].ref_temp, &(parameters.gyro_cal_data[j].ref_temp));
param_get(parameter_handles.gyro_cal_handles[j].min_temp, &(parameters.gyro_cal_data[j].min_temp));
param_get(parameter_handles.gyro_cal_handles[j].min_temp, &(parameters.gyro_cal_data[j].min_temp));
if (param_get(parameter_handles.gyro_cal_handles[j].ID, &(_parameters.gyro_cal_data[j].ID)) == PX4_OK) {
param_get(parameter_handles.gyro_cal_handles[j].ref_temp, &(_parameters.gyro_cal_data[j].ref_temp));
param_get(parameter_handles.gyro_cal_handles[j].min_temp, &(_parameters.gyro_cal_data[j].min_temp));
param_get(parameter_handles.gyro_cal_handles[j].min_temp, &(_parameters.gyro_cal_data[j].min_temp));
for (unsigned int i = 0; i < 3; i++) {
param_get(parameter_handles.gyro_cal_handles[j].x3[i], &(parameters.gyro_cal_data[j].x3[i]));
param_get(parameter_handles.gyro_cal_handles[j].x2[i], &(parameters.gyro_cal_data[j].x2[i]));
param_get(parameter_handles.gyro_cal_handles[j].x1[i], &(parameters.gyro_cal_data[j].x1[i]));
param_get(parameter_handles.gyro_cal_handles[j].x0[i], &(parameters.gyro_cal_data[j].x0[i]));
param_get(parameter_handles.gyro_cal_handles[j].scale[i], &(parameters.gyro_cal_data[j].scale[i]));
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]));
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;
_parameters.gyro_cal_data[j].scale[i] = 1.0f;
}
PX4_WARN("FAIL GYRO %d CAL PARAM LOAD - USING DEFAULTS", j);
@ -174,29 +181,29 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par @@ -174,29 +181,29 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par
}
/* accelerometer calibration parameters */
param_get(parameter_handles.accel_tc_enable, &(parameters.accel_tc_enable));
param_get(parameter_handles.accel_tc_enable, &(_parameters.accel_tc_enable));
for (unsigned j = 0; j < 3; j++) {
if (param_get(parameter_handles.accel_cal_handles[j].ID, &(parameters.accel_cal_data[j].ID)) == PX4_OK) {
param_get(parameter_handles.accel_cal_handles[j].ref_temp, &(parameters.accel_cal_data[j].ref_temp));
param_get(parameter_handles.accel_cal_handles[j].min_temp, &(parameters.accel_cal_data[j].min_temp));
param_get(parameter_handles.accel_cal_handles[j].min_temp, &(parameters.accel_cal_data[j].min_temp));
if (param_get(parameter_handles.accel_cal_handles[j].ID, &(_parameters.accel_cal_data[j].ID)) == PX4_OK) {
param_get(parameter_handles.accel_cal_handles[j].ref_temp, &(_parameters.accel_cal_data[j].ref_temp));
param_get(parameter_handles.accel_cal_handles[j].min_temp, &(_parameters.accel_cal_data[j].min_temp));
param_get(parameter_handles.accel_cal_handles[j].min_temp, &(_parameters.accel_cal_data[j].min_temp));
for (unsigned int i = 0; i < 3; i++) {
param_get(parameter_handles.accel_cal_handles[j].x3[i], &(parameters.accel_cal_data[j].x3[i]));
param_get(parameter_handles.accel_cal_handles[j].x2[i], &(parameters.accel_cal_data[j].x2[i]));
param_get(parameter_handles.accel_cal_handles[j].x1[i], &(parameters.accel_cal_data[j].x1[i]));
param_get(parameter_handles.accel_cal_handles[j].x0[i], &(parameters.accel_cal_data[j].x0[i]));
param_get(parameter_handles.accel_cal_handles[j].scale[i], &(parameters.accel_cal_data[j].scale[i]));
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]));
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;
_parameters.accel_cal_data[j].scale[i] = 1.0f;
}
PX4_WARN("FAIL ACCEL %d CAL PARAM LOAD - USING DEFAULTS", j);
@ -205,35 +212,37 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par @@ -205,35 +212,37 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par
}
/* barometer calibration parameters */
param_get(parameter_handles.baro_tc_enable, &(parameters.baro_tc_enable));
param_get(parameter_handles.baro_tc_enable, &(_parameters.baro_tc_enable));
for (unsigned j = 0; j < 3; 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].min_temp, &(parameters.baro_cal_data[j].min_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));
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].min_temp, &(_parameters.baro_cal_data[j].min_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]));
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;
_parameters.baro_cal_data[j].scale = 1.0f;
PX4_WARN("FAIL BARO %d CAL PARAM LOAD - USING DEFAULTS", j);
ret = PX4_ERROR;
}
} return ret;
}
return ret;
}
bool calc_thermal_offsets_1D(struct SENSOR_CAL_DATA_1D &coef, const float &measured_temp, float &offset)
bool TemperatureCompensation::calc_thermal_offsets_1D(SensorCalData1D &coef, float measured_temp, float &offset)
{
bool ret = true;
@ -270,7 +279,7 @@ bool calc_thermal_offsets_1D(struct SENSOR_CAL_DATA_1D &coef, const float &measu @@ -270,7 +279,7 @@ bool calc_thermal_offsets_1D(struct SENSOR_CAL_DATA_1D &coef, const float &measu
}
bool calc_thermal_offsets_3D(struct SENSOR_CAL_DATA_3D &coef, const float &measured_temp, float offset[])
bool TemperatureCompensation::calc_thermal_offsets_3D(SensorCalData3D &coef, float measured_temp, float offset[])
{
bool ret = true;
@ -298,11 +307,119 @@ bool calc_thermal_offsets_3D(struct SENSOR_CAL_DATA_3D &coef, const float &measu @@ -298,11 +307,119 @@ bool calc_thermal_offsets_3D(struct SENSOR_CAL_DATA_3D &coef, const float &measu
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)
{
return set_sensor_id(device_id, topic_instance, _gyro_data, _parameters.gyro_cal_data);
}
int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_instance)
{
return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data);
}
int TemperatureCompensation::set_sensor_id_baro(uint32_t device_id, int topic_instance)
{
return set_sensor_id(device_id, topic_instance, _baro_data, _parameters.baro_cal_data);
}
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) {
if (device_id == sensor_cal_data[i].ID) {
sensor_data.device_mapping[topic_instance] = i;
return 0;
}
}
return -1;
}
int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Vector<3> &sensor_data, float temperature,
float *offsets, float *scales)
{
uint8_t mapping = _gyro_data.device_mapping[topic_instance];
if (mapping == 255 || _parameters.gyro_tc_enable != 1) {
return 0;
}
calc_thermal_offsets_3D(_parameters.gyro_cal_data[mapping], temperature, offsets);
// get the sensor scale factors and correct the data
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
scales[axis_index] = _parameters.gyro_cal_data[mapping].scale[axis_index];
sensor_data(axis_index) = sensor_data(axis_index) * scales[axis_index] + offsets[axis_index];
}
int8_t temperaturei = (int8_t)temperature;
if (temperaturei != _gyro_data.last_temperature) {
_gyro_data.last_temperature = temperaturei;
return 2;
}
return 1;
}
int TemperatureCompensation::apply_corrections_accel(int topic_instance, math::Vector<3> &sensor_data,
float temperature, float *offsets, float *scales)
{
uint8_t mapping = _accel_data.device_mapping[topic_instance];
if (mapping == 255 || _parameters.accel_tc_enable != 1) {
return 0;
}
calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets);
// get the sensor scale factors and correct the data
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
scales[axis_index] = _parameters.accel_cal_data[mapping].scale[axis_index];
sensor_data(axis_index) = sensor_data(axis_index) * scales[axis_index] + offsets[axis_index];
}
int8_t temperaturei = (int8_t)temperature;
if (temperaturei != _accel_data.last_temperature) {
_accel_data.last_temperature = temperaturei;
return 2;
}
return 1;
}
int TemperatureCompensation::apply_corrections_baro(int topic_instance, float &sensor_data, float temperature,
float *offsets, float *scales)
{
uint8_t mapping = _baro_data.device_mapping[topic_instance];
if (mapping == 255 || _parameters.baro_tc_enable != 1) {
return 0;
}
calc_thermal_offsets_1D(_parameters.baro_cal_data[mapping], temperature, *offsets);
// get the sensor scale factors and correct the data
*scales = _parameters.baro_cal_data[mapping].scale;
sensor_data = sensor_data * *scales + *offsets;
int8_t temperaturei = (int8_t)temperature;
if (temperaturei != _baro_data.last_temperature) {
_baro_data.last_temperature = temperaturei;
return 2;
}
return 1;
}
}

346
src/modules/sensors/temperature_compensation.h
Unescape View File

@ -37,165 +37,225 @@ @@ -37,165 +37,225 @@
* Sensor correction methods
*
* @author Paul Riseborough <gncsolns@gmail.com>
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include <systemlib/param/param.h>
#include <mathlib/mathlib.h>
/* 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 * scale + offset
*/
namespace sensors_temp_comp
namespace sensors
{
struct SENSOR_CAL_DATA_1D {
int 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 SENSOR_CAL_HANDLES_1D {
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 * scale + offset
*/
struct SENSOR_CAL_DATA_3D {
int ID; /**< sensor device ID*/
float x3[3]; /**< x^3 term of polynomial */
float x2[3]; /**< x^2 term of polynomial */
float x1[3]; /**< x^1 term of polynomial */
float x0[3]; /**< x^0 / offset term of polynomial */
float scale[3]; /**< scale factor correction */
float ref_temp; /**< reference temperature used by the curve-fit */
float min_temp; /**< minimum temperature with valid compensation data */
float max_temp; /**< maximum temperature with valid compensation data */
};
struct SENSOR_CAL_HANDLES_3D {
param_t ID;
param_t x3[3];
param_t x2[3];
param_t x1[3];
param_t x0[3];
param_t scale[3];
param_t ref_temp;
param_t min_temp;
param_t max_temp;
};
// create a struct containing all thermal calibration parameters
struct Parameters {
int gyro_tc_enable;
SENSOR_CAL_DATA_3D gyro_cal_data[3];
int accel_tc_enable;
SENSOR_CAL_DATA_3D accel_cal_data[3];
int baro_tc_enable;
SENSOR_CAL_DATA_1D baro_cal_data[3];
};
// create a struct containing the handles required to access all calibration parameters
struct ParameterHandles {
param_t gyro_tc_enable;
SENSOR_CAL_HANDLES_3D gyro_cal_handles[3];
param_t accel_tc_enable;
SENSOR_CAL_HANDLES_3D accel_cal_handles[3];
param_t baro_tc_enable;
SENSOR_CAL_HANDLES_1D baro_cal_handles[3];
};
/**
* initialize ParameterHandles struct
* @return 0 on succes, <0 on error
*/
int initialize_parameter_handles(ParameterHandles &parameter_handles);
/**
* Read out the parameters using the handles into the parameters struct.
* @return 0 on succes, <0 on error
** class TemperatureCompensation
* Applies temperature compensation to sensor data. Loads the parameters from PX4 param storage.
*/
int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters);
/*
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
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 0 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 0: no changes (correction not enabled),
* 1: corrections applied but no changes to offsets & scales,
* 2: corrections applied and offsets & scales updated
*/
int apply_corrections_gyro(int topic_instance, math::Vector<3> &sensor_data, float temperature,
float *offsets, float *scales);
int apply_corrections_accel(int topic_instance, math::Vector<3> &sensor_data, float temperature,
float *offsets, float *scales);
int apply_corrections_baro(int topic_instance, float &sensor_data, float temperature,
float *offsets, float *scales);
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 * scale + offset
*/
struct SensorCalData1D {
int 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 * scale + offset
*/
struct SensorCalData3D {
int ID; /**< sensor device ID*/
float x3[3]; /**< x^3 term of polynomial */
float x2[3]; /**< x^2 term of polynomial */
float x1[3]; /**< x^1 term of polynomial */
float x0[3]; /**< x^0 / offset term of polynomial */
float scale[3]; /**< scale factor correction */
float ref_temp; /**< reference temperature used by the curve-fit */
float min_temp; /**< minimum temperature with valid compensation data */
float max_temp; /**< maximum temperature with valid compensation data */
};
struct 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 {
int gyro_tc_enable;
SensorCalData3D gyro_cal_data[3];
int accel_tc_enable;
SensorCalData3D accel_cal_data[3];
int baro_tc_enable;
SensorCalData1D baro_cal_data[3];
};
// create a struct containing the handles required to access all calibration parameters
struct ParameterHandles {
param_t gyro_tc_enable;
SensorCalHandles3D gyro_cal_handles[3];
param_t accel_tc_enable;
SensorCalHandles3D accel_cal_handles[3];
param_t baro_tc_enable;
SensorCalHandles1D baro_cal_handles[3];
};
/**
* 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.
*/
bool calc_thermal_offsets_1D(SENSOR_CAL_DATA_1D &coef, const float &measured_temp, float &offset);
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
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.
Returns:
Arguments:
Boolean true if the measured temperature is inside the valid range for the compensation
coef : reference to struct containing calibration coefficients
measured_temp : temperature measured at the sensor (deg C)
offset : reference to sensor offset - array of 3
*/
bool calc_thermal_offsets_3D(SensorCalData3D &coef, float measured_temp, float offset[]);
Parameters _parameters;
Returns:
Boolean true if the measured temperature is inside the valid range for the compensation
struct PerSensorData {
PerSensorData()
{
device_mapping[0] = device_mapping[1] = device_mapping[2] = 255;
}
uint8_t device_mapping[3]; /// map a topic instance to the parameters index
int8_t last_temperature = -100;
};
PerSensorData _gyro_data;
PerSensorData _accel_data;
PerSensorData _baro_data;
*/
bool calc_thermal_offsets_3D(SENSOR_CAL_DATA_3D &coef, const float &measured_temp, float offset[]);
template<typename T>
static inline int set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data,
const T *sensor_cal_data);
};
}

242
src/modules/sensors/voted_sensors_update.cpp
Unescape View File

@ -63,36 +63,12 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters) @@ -63,36 +63,12 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters)
memset(&_accel_diff, 0, sizeof(_accel_diff));
memset(&_gyro_diff, 0, sizeof(_gyro_diff));
_baro.voter.set_timeout(300000);
_mag.voter.set_timeout(300000);
_mag.voter.set_equal_value_threshold(1000);
}
int VotedSensorsUpdate::init(sensor_combined_s &raw)
{
raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
raw.magnetometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
raw.baro_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
raw.timestamp = 0;
initialize_sensors();
// get the parameter handles for the gyro temperature compensation parameters
sensors_temp_comp::initialize_parameter_handles(_thermal_correction_param_handles);
// ensure pointer to sensor corrections publiations is defined
_sensor_correction_pub = nullptr;
// initialise the corrections
memset(&_corrections, 0, sizeof(_corrections));
memset(&_accel_offset, 0, sizeof(_accel_offset));
memset(&_gyro_offset, 0, sizeof(_gyro_offset));
memset(&_baro_offset, 0, sizeof(_baro_offset));
for (unsigned i = 0; i < 3; i++) {
_corrections.gyro_scale[i] = 1.0f;
_corrections.accel_scale[i] = 1.0f;
_corrections.baro_scale = 1.0f;
for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
_accel_scale[j][i] = 1.0f;
@ -101,7 +77,27 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw) @@ -101,7 +77,27 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
}
}
_msl_pressure = 101325.0f;
_corrections.baro_scale = 1.0f;
memset(&_accel_offset, 0, sizeof(_accel_offset));
memset(&_gyro_offset, 0, sizeof(_gyro_offset));
memset(&_baro_offset, 0, sizeof(_baro_offset));
_baro.voter.set_timeout(300000);
_mag.voter.set_timeout(300000);
_mag.voter.set_equal_value_threshold(1000);
}
int VotedSensorsUpdate::init(sensor_combined_s &raw)
{
raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
raw.magnetometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
raw.baro_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
raw.timestamp = 0;
initialize_sensors();
_corrections_changed = true; //make sure to initially publish the corrections topic
return 0;
}
@ -144,6 +140,8 @@ void VotedSensorsUpdate::parameters_update() @@ -144,6 +140,8 @@ void VotedSensorsUpdate::parameters_update()
_board_rotation = board_rotation_offset * _board_rotation;
_temperature_compensation.parameters_update();
/* set offset parameters to new values */
bool failed;
char str[30];
@ -151,9 +149,6 @@ void VotedSensorsUpdate::parameters_update() @@ -151,9 +149,6 @@ void VotedSensorsUpdate::parameters_update()
unsigned gyro_count = 0;
unsigned accel_count = 0;
/* get stored sensor hub temperature compensation parameters */
sensors_temp_comp::update_parameters(_thermal_correction_param_handles, _thermal_correction_param);
/* run through all gyro sensors */
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
@ -166,6 +161,9 @@ void VotedSensorsUpdate::parameters_update() @@ -166,6 +161,9 @@ void VotedSensorsUpdate::parameters_update()
continue;
}
uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
_temperature_compensation.set_sensor_id_gyro(driver_device_id, s);
bool config_ok = false;
/* run through all stored calibrations that are applied at the driver level*/
@ -183,7 +181,7 @@ void VotedSensorsUpdate::parameters_update() @@ -183,7 +181,7 @@ void VotedSensorsUpdate::parameters_update()
}
/* if the calibration is for this device, apply it */
if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
if (device_id == driver_device_id) {
struct gyro_calibration_s gscale = {};
(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
failed = failed || (OK != param_get(param_find(str), &gscale.x_offset));
@ -231,6 +229,9 @@ void VotedSensorsUpdate::parameters_update() @@ -231,6 +229,9 @@ void VotedSensorsUpdate::parameters_update()
continue;
}
uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
_temperature_compensation.set_sensor_id_accel(driver_device_id, s);
bool config_ok = false;
/* run through all stored calibrations */
@ -248,7 +249,7 @@ void VotedSensorsUpdate::parameters_update() @@ -248,7 +249,7 @@ void VotedSensorsUpdate::parameters_update()
}
/* if the calibration is for this device, apply it */
if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
if (device_id == driver_device_id) {
struct accel_calibration_s ascale = {};
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
failed = failed || (OK != param_get(param_find(str), &ascale.x_offset));
@ -303,6 +304,8 @@ void VotedSensorsUpdate::parameters_update() @@ -303,6 +304,8 @@ void VotedSensorsUpdate::parameters_update()
continue;
}
uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
bool config_ok = false;
/* run through all stored calibrations */
@ -325,7 +328,7 @@ void VotedSensorsUpdate::parameters_update() @@ -325,7 +328,7 @@ void VotedSensorsUpdate::parameters_update()
// PX4_WARN("sensors: device ID: %s: %d, %u", str, id, (unsigned)id);
/* if the calibration is for this device, apply it */
if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) {
if (device_id == driver_device_id) {
struct mag_calibration_s mscale = {};
(void)sprintf(str, "CAL_MAG%u_XOFF", i);
failed = failed || (OK != param_get(param_find(str), &mscale.x_offset));
@ -417,6 +420,22 @@ void VotedSensorsUpdate::parameters_update() @@ -417,6 +420,22 @@ void VotedSensorsUpdate::parameters_update()
mag_count++;
}
}
/* run through all baro sensors */
for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) {
(void)sprintf(str, "%s%u", BARO_BASE_DEVICE_PATH, s);
DevHandle h;
DevMgr::getHandle(str, h);
if (!h.isValid()) {
continue;
}
uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0);
_temperature_compensation.set_sensor_id_baro(driver_device_id, s);
}
}
void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
@ -456,27 +475,6 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) @@ -456,27 +475,6 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
accel_data = math::Vector<3>(accel_report.x_integral * dt_inv , accel_report.y_integral * dt_inv ,
accel_report.z_integral * dt_inv);
if (_thermal_correction_param.accel_tc_enable == 1) {
// search through the available compensation parameter sets looking for one with a matching sensor ID and corect data if found
for (unsigned param_index = 0; param_index < 3; param_index++) {
if (accel_report.device_id == _thermal_correction_param.accel_cal_data[param_index].ID) {
// get the offsets
sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.accel_cal_data[param_index],
accel_report.temperature, _accel_offset[uorb_index]);
// get the scale factors and correct the data
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
_accel_scale[uorb_index][axis_index] = _thermal_correction_param.accel_cal_data[param_index].scale[axis_index];
accel_data(axis_index) = accel_data(axis_index) * _accel_scale[uorb_index][axis_index] +
_accel_offset[uorb_index][axis_index];
}
break;
}
}
}
_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt / 1.e6f;
} else {
@ -486,38 +484,22 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) @@ -486,38 +484,22 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
// Filtering and/or downsampling of temperature should be performed in the driver layer
accel_data = math::Vector<3>(accel_report.x , accel_report.y , accel_report.z);
if (_thermal_correction_param.accel_tc_enable == 1) {
// search through the available compensation parameter sets looking for one with a matching sensor ID
for (unsigned param_index = 0; param_index < 3; param_index++) {
if (accel_report.device_id == _thermal_correction_param.accel_cal_data[param_index].ID) {
// get the offsets
sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.accel_cal_data[param_index],
accel_report.temperature, _accel_offset[uorb_index]);
// get the scale factors and correct the data
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
_accel_scale[uorb_index][axis_index] = _thermal_correction_param.accel_cal_data[param_index].scale[axis_index];
accel_data(axis_index) = accel_data(axis_index) * _accel_scale[uorb_index][axis_index] +
_accel_offset[uorb_index][axis_index];
}
break;
}
}
}
// handle the cse where this is our first output
if (_last_accel_timestamp[uorb_index] == 0) {
_last_accel_timestamp[uorb_index] = accel_report.timestamp - 1000;
}
// approximate the delta time using the difference in accel data time stamps
// and write to uORB struct
_last_sensor_data[uorb_index].accelerometer_integral_dt =
(accel_report.timestamp - _last_accel_timestamp[uorb_index]) / 1.e6f;
}
// handle temperature compensation
if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature,
_accel_offset[uorb_index], _accel_scale[uorb_index]) == 2) {
_corrections_changed = true;
}
// rotate corrected measurements from sensor to body frame
accel_data = _board_rotation * accel_data;
@ -538,8 +520,12 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) @@ -538,8 +520,12 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
// write the best sensor data to the output variables
if (best_index >= 0) {
raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
_accel.last_best_vote = (uint8_t)best_index;
_corrections.selected_accel_instance = (uint8_t)best_index;
if (best_index != _accel.last_best_vote) {
_accel.last_best_vote = (uint8_t)best_index;
_corrections.selected_accel_instance = (uint8_t)best_index;
_corrections_changed = true;
}
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
raw.accelerometer_m_s2[axis_index] = _last_sensor_data[best_index].accelerometer_m_s2[axis_index];
@ -586,27 +572,6 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) @@ -586,27 +572,6 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
gyro_rate = math::Vector<3>(gyro_report.x_integral * dt_inv , gyro_report.y_integral * dt_inv ,
gyro_report.z_integral * dt_inv);
if (_thermal_correction_param.gyro_tc_enable == 1) {
// search through the available compensation parameter sets looking for one with a matching sensor ID and correct data if found
for (unsigned param_index = 0; param_index < 3; param_index++) {
if (gyro_report.device_id == _thermal_correction_param.gyro_cal_data[param_index].ID) {
// get the offsets
sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.gyro_cal_data[param_index],
gyro_report.temperature, _gyro_offset[uorb_index]);
// get the sensor scale factors and correct the data
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
_gyro_scale[uorb_index][axis_index] = _thermal_correction_param.gyro_cal_data[param_index].scale[axis_index];
gyro_rate(axis_index) = gyro_rate(axis_index) * _gyro_scale[uorb_index][axis_index] +
_gyro_offset[uorb_index][axis_index];
}
break;
}
}
}
_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt / 1.e6f;
} else {
@ -616,38 +581,22 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) @@ -616,38 +581,22 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
// Filtering and/or downsampling of temperature should be performed in the driver layer
gyro_rate = math::Vector<3>(gyro_report.x, gyro_report.y, gyro_report.z);
if (_thermal_correction_param.gyro_tc_enable == 1) {
// search through the available compensation parameter sets looking for one with a matching sensor ID
for (unsigned param_index = 0; param_index < 3; param_index++) {
if (gyro_report.device_id == _thermal_correction_param.gyro_cal_data[param_index].ID) {
// get the offsets
sensors_temp_comp::calc_thermal_offsets_3D(_thermal_correction_param.gyro_cal_data[param_index],
gyro_report.temperature, _gyro_offset[uorb_index]);
// get the sensor scale factors and correct the data
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
_gyro_scale[uorb_index][axis_index] = _thermal_correction_param.gyro_cal_data[param_index].scale[axis_index];
gyro_rate(axis_index) = gyro_rate(axis_index) * _gyro_scale[uorb_index][axis_index] +
_gyro_offset[uorb_index][axis_index];
}
break;
}
}
}
// handle the case where this is our first output
if (_last_sensor_data[uorb_index].timestamp == 0) {
_last_sensor_data[uorb_index].timestamp = gyro_report.timestamp - 1000;
}
// approximate the delta time using the difference in gyro data time stamps
// and write to uORB struct
_last_sensor_data[uorb_index].gyro_integral_dt =
(gyro_report.timestamp - _last_sensor_data[uorb_index].timestamp) / 1.e6f;
}
// handle temperature compensation
if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature,
_gyro_offset[uorb_index], _gyro_scale[uorb_index]) == 2) {
_corrections_changed = true;
}
// rotate corrected measurements from sensor to body frame
gyro_rate = _board_rotation * gyro_rate;
@ -669,8 +618,12 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) @@ -669,8 +618,12 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
if (best_index >= 0) {
raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
raw.timestamp = _last_sensor_data[best_index].timestamp;
_gyro.last_best_vote = (uint8_t)best_index;
_corrections.selected_gyro_instance = (uint8_t)best_index;
if (_gyro.last_best_vote != best_index) {
_gyro.last_best_vote = (uint8_t)best_index;
_corrections.selected_gyro_instance = (uint8_t)best_index;
_corrections_changed = true;
}
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
raw.gyro_rad[axis_index] = _last_sensor_data[best_index].gyro_rad[axis_index];
@ -746,24 +699,10 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) @@ -746,24 +699,10 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
// Convert from millibar to Pa
float corrected_pressure = 100.0f * baro_report.pressure;
// Apply thermal compensation if available
if (_thermal_correction_param.baro_tc_enable == 1) {
// search through the available compensation parameter sets looking for one with a matching sensor ID and correct data if found
for (unsigned param_index = 0; param_index < 3; param_index++) {
if (baro_report.device_id == _thermal_correction_param.baro_cal_data[param_index].ID) {
// get the offsets
sensors_temp_comp::calc_thermal_offsets_1D(_thermal_correction_param.baro_cal_data[param_index],
baro_report.temperature, _baro_offset[uorb_index]);
// get the sensor scale factors and correct the data
// convert pressure reading from millibar to Pa
_baro_scale[uorb_index] = _thermal_correction_param.baro_cal_data[param_index].scale;
corrected_pressure = corrected_pressure * _baro_scale[uorb_index] + _baro_offset[uorb_index];
break;
}
}
// handle temperature compensation
if (_temperature_compensation.apply_corrections_baro(uorb_index, corrected_pressure, baro_report.temperature,
&_baro_offset[uorb_index], &_baro_scale[uorb_index]) == 2) {
_corrections_changed = true;
}
// First publication with data
@ -793,8 +732,13 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) @@ -793,8 +732,13 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
if (best_index >= 0) {
raw.baro_temp_celcius = _last_sensor_data[best_index].baro_temp_celcius;
_last_best_baro_pressure = _last_baro_pressure[best_index];
_baro.last_best_vote = (uint8_t)best_index;
_corrections.selected_baro_instance = (uint8_t)best_index;
if (_baro.last_best_vote != best_index) {
_baro.last_best_vote = (uint8_t)best_index;
_corrections.selected_baro_instance = (uint8_t)best_index;
_corrections_changed = true;
}
_corrections.baro_offset = _baro_offset[best_index];
_corrections.baro_scale = _baro_scale[best_index];
@ -980,13 +924,19 @@ void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw) @@ -980,13 +924,19 @@ void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw)
mag_poll(raw);
baro_poll(raw);
// publish sensor corrections
if (_sensor_correction_pub == nullptr) {
_sensor_correction_pub = orb_advertise(ORB_ID(sensor_correction), &_corrections);
// publish sensor corrections if necessary
if (_corrections_changed) {
_corrections.timestamp = hrt_absolute_time();
} else {
orb_publish(ORB_ID(sensor_correction), _sensor_correction_pub, &_corrections);
if (_sensor_correction_pub == nullptr) {
_sensor_correction_pub = orb_advertise(ORB_ID(sensor_correction), &_corrections);
} else {
orb_publish(ORB_ID(sensor_correction), _sensor_correction_pub, &_corrections);
}
_corrections_changed = false;
}
}

9
src/modules/sensors/voted_sensors_update.h
Unescape View File

@ -264,17 +264,16 @@ private: @@ -264,17 +264,16 @@ private:
float _gyro_diff[3][2]; /**< filtered gyro differences between IMU uinits (rad/s) */
/* sensor thermal compensation */
sensors_temp_comp::Parameters _thermal_correction_param; /**< copy of the thermal correction parameters*/
sensors_temp_comp::ParameterHandles
_thermal_correction_param_handles; /**< handles for the thermal correction parameters */
struct sensor_correction_s _corrections = {}; /**< struct containing the sensor corrections to be published to the uORB*/
orb_advert_t _sensor_correction_pub; /**< handle to the sensor correction uORB topic */
TemperatureCompensation _temperature_compensation;
float _accel_offset[SENSOR_COUNT_MAX][3]; /**< offsets to be added to the raw accel data after scale factor correction */
float _accel_scale[SENSOR_COUNT_MAX][3]; /**< scale factor corrections to be applied to the raw accel data before offsets are added */
float _gyro_offset[SENSOR_COUNT_MAX][3]; /**< offsets to be added to the raw angular rate data after scale factor correction */
float _gyro_scale[SENSOR_COUNT_MAX][3]; /**< scale factor corrections to be applied to the raw angular rate data before offsets are added */
float _baro_offset[SENSOR_COUNT_MAX]; /**< offsets to be added to the raw baro pressure data after scale factor correction */
float _baro_scale[SENSOR_COUNT_MAX]; /**< scale factor corrections to be applied to the raw barp pressure data before offsets are added */
struct sensor_correction_s _corrections; /**< struct containing the sensor corrections to be published to the uORB*/
orb_advert_t _sensor_correction_pub = nullptr; /**< handle to the sensor correction uORB topic */
bool _corrections_changed = false;
static const double _msl_pressure; /** average sea-level pressure in kPa */
};

Write Preview
Loading…
Cancel
Save