From 117826a31f0682d152fdd2d3c35fd065f6e722ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 17 Jan 2017 19:07:20 +0100 Subject: [PATCH] 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) --- .../sensors/temperature_compensation.cpp | 209 ++++++++--- .../sensors/temperature_compensation.h | 346 ++++++++++-------- src/modules/sensors/voted_sensors_update.cpp | 242 +++++------- src/modules/sensors/voted_sensors_update.h | 9 +- 4 files changed, 466 insertions(+), 340 deletions(-) diff --git a/src/modules/sensors/temperature_compensation.cpp b/src/modules/sensors/temperature_compensation.cpp index 6bac1edcef..7cebe035ad 100644 --- a/src/modules/sensors/temperature_compensation.cpp +++ b/src/modules/sensors/temperature_compensation.cpp @@ -44,10 +44,10 @@ #include #include -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 +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; +} + } diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/sensors/temperature_compensation.h index 48ab652f3a..46d019f78c 100644 --- a/src/modules/sensors/temperature_compensation.h +++ b/src/modules/sensors/temperature_compensation.h @@ -37,165 +37,225 @@ * Sensor correction methods * * @author Paul Riseborough + * @author Beat Küng */ #include #include -/* 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 ¶meter_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 ¶meter_handles, Parameters ¶meters); - -/* - -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 ¶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. -*/ -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 + static inline int set_sensor_id(uint32_t device_id, int topic_instance, PerSensorData &sensor_data, + const T *sensor_cal_data); +}; } diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index a1ec637493..3f809094b1 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -63,36 +63,12 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters) 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) } } - _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() _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; } } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 29872a19ef..c45fd14927 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -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 */ };