forked from Archive/PX4-Autopilot
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)
This commit is contained in:
Beat Küng
Lorenz Meier
parent
23d278cc43
commit
117826a31f
|
|
@ -44,10 +44,10 @@
|
|||
#include <stdio.h>
|
||||
#include <px4_defines.h>
|
||||
|
||||
namespace sensors_temp_comp
|
||||
namespace sensors
|
||||
{
|
||||
|
||||
int initialize_parameter_handles(ParameterHandles ¶meter_handles)
|
||||
int TemperatureCompensation::initialize_parameter_handles(ParameterHandles ¶meter_handles)
|
||||
{
|
||||
char nbuf[16];
|
||||
|
||||
|
|
@ -138,34 +138,41 @@ int initialize_parameter_handles(ParameterHandles ¶meter_handles)
|
|||
return PX4_OK;
|
||||
}
|
||||
|
||||
int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters)
|
||||
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(¶meters.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 ¶meter_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(¶meters.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 ¶meter_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(¶meters.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
|
|||
|
||||
}
|
||||
|
||||
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
|
|||
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
** class TemperatureCompensation
|
||||
* Applies temperature compensation to sensor data. Loads the parameters from PX4 param storage.
|
||||
*/
|
||||
int initialize_parameter_handles(ParameterHandles ¶meter_handles);
|
||||
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);
|
||||
|
||||
|
||||
/**
|
||||
* Read out the parameters using the handles into the parameters struct.
|
||||
* @return 0 on succes, <0 on error
|
||||
*/
|
||||
int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters);
|
||||
/**
|
||||
* 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);
|
||||
|
||||
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.
|
||||
int apply_corrections_baro(int topic_instance, float &sensor_data, float temperature,
|
||||
float *offsets, float *scales);
|
||||
|
||||
Arguments:
|
||||
private:
|
||||
|
||||
coef : reference to struct containing calibration coefficients
|
||||
measured_temp : temperature measured at the sensor (deg C)
|
||||
offset : reference to sensor offset
|
||||
/* Struct containing parameters used by the single axis 5th order temperature compensation algorithm
|
||||
|
||||
Returns:
|
||||
Input:
|
||||
|
||||
Boolean true if the measured temperature is inside the valid range for the compensation
|
||||
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
|
||||
|
||||
*/
|
||||
bool calc_thermal_offsets_1D(SENSOR_CAL_DATA_1D &coef, const float &measured_temp, float &offset);
|
||||
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
|
||||
|
||||
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.
|
||||
*/
|
||||
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;
|
||||
};
|
||||
|
||||
Arguments:
|
||||
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;
|
||||
};
|
||||
|
||||
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:
|
||||
/* Struct containing parameters used by the 3-axis 3rd order temperature compensation algorithm
|
||||
|
||||
Boolean true if the measured temperature is inside the valid range for the compensation
|
||||
Input:
|
||||
|
||||
*/
|
||||
bool calc_thermal_offsets_3D(SENSOR_CAL_DATA_3D &coef, const float &measured_temp, float offset[]);
|
||||
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 ¶meter_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(SensorCalData3D &coef, float measured_temp, float offset[]);
|
||||
|
||||
|
||||
Parameters _parameters;
|
||||
|
||||
|
||||
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;
|
||||
|
||||
|
||||
template<typename T>
|
||||
static inline int set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data,
|
||||
const T *sensor_cal_data);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -63,6 +63,26 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters)
|
|||
memset(&_accel_diff, 0, sizeof(_accel_diff));
|
||||
memset(&_gyro_diff, 0, sizeof(_gyro_diff));
|
||||
|
||||
// initialise the corrections
|
||||
memset(&_corrections, 0, sizeof(_corrections));
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_corrections.gyro_scale[i] = 1.0f;
|
||||
_corrections.accel_scale[i] = 1.0f;
|
||||
|
||||
for (unsigned j = 0; j < SENSOR_COUNT_MAX; j++) {
|
||||
_accel_scale[j][i] = 1.0f;
|
||||
_gyro_scale[j][i] = 1.0f;
|
||||
_baro_scale[j] = 1.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);
|
||||
|
|
@ -77,31 +97,7 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw)
|
|||
|
||||
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;
|
||||
_gyro_scale[j][i] = 1.0f;
|
||||
_baro_scale[j] = 1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
_msl_pressure = 101325.0f;
|
||||
_corrections_changed = true; //make sure to initially publish the corrections topic
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -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()
|
|||
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()
|
|||
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()
|
|||
}
|
||||
|
||||
/* 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()
|
|||
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()
|
|||
}
|
||||
|
||||
/* 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()
|
|||
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()
|
|||
// 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()
|
|||
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)
|
|||
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)
|
|||
// 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)
|
|||
// 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)
|
|||
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)
|
|||
// 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)
|
|||
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)
|
|||
// 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)
|
|||
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)
|
|||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue