From 4a8d29800ca3639b0c6b0d5f0220f16cb89f9b31 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 26 Jan 2017 14:10:07 +0100 Subject: [PATCH] voted_sensors_update: cleanup & remove some attributes since the correction topic now contains data from all sensors, we don't need additional fields in voted_sensors_update --- src/modules/sensors/voted_sensors_update.cpp | 47 +++++++------------- src/modules/sensors/voted_sensors_update.h | 6 --- 2 files changed, 15 insertions(+), 38 deletions(-) diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 04560cceb7..58990119d5 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -73,22 +73,12 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters) _corrections.accel_scale_1[i] = 1.0f; _corrections.gyro_scale_2[i] = 1.0f; _corrections.accel_scale_2[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_0 = 1.0f; _corrections.baro_scale_1 = 1.0f; _corrections.baro_scale_2 = 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); @@ -162,6 +152,7 @@ void VotedSensorsUpdate::parameters_update() if (orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[topic_instance], &report) == 0) { int temp = _temperature_compensation.set_sensor_id_gyro(report.device_id, topic_instance); + if (temp < 0) { PX4_ERR("gyro temp compensation init: failed to find device ID %u for instance %i", report.device_id, topic_instance); @@ -181,6 +172,7 @@ void VotedSensorsUpdate::parameters_update() if (orb_copy(ORB_ID(sensor_accel), _accel.subscription[topic_instance], &report) == 0) { int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, topic_instance); + if (temp < 0) { PX4_ERR("accel temp compensation init: failed to find device ID %u for instance %i", report.device_id, topic_instance); @@ -192,6 +184,7 @@ void VotedSensorsUpdate::parameters_update() } } } + /* baro */ if (topic_instance < _baro.subscription_count) { // valid subscription, so get the driver id by getting the published sensor data @@ -199,6 +192,7 @@ void VotedSensorsUpdate::parameters_update() if (orb_copy(ORB_ID(sensor_baro), _baro.subscription[topic_instance], &report) == 0) { int temp = _temperature_compensation.set_sensor_id_baro(report.device_id, topic_instance); + if (temp < 0) { PX4_ERR("baro temp compensation init: failed to find device ID %u for instance %i", report.device_id, topic_instance); @@ -527,6 +521,9 @@ void VotedSensorsUpdate::parameters_update() void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) { + float *offsets[] = {_corrections.accel_offset_0, _corrections.accel_offset_1, _corrections.accel_offset_2 }; + float *scales[] = {_corrections.accel_scale_0, _corrections.accel_scale_1, _corrections.accel_scale_2 }; + for (unsigned uorb_index = 0; uorb_index < _accel.subscription_count; uorb_index++) { bool accel_updated; orb_check(_accel.subscription[uorb_index], &accel_updated); @@ -583,7 +580,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) // 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) { + offsets[uorb_index], scales[uorb_index]) == 2) { _corrections_changed = true; } @@ -616,18 +613,15 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) 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]; - _corrections.accel_offset_0[axis_index] = _accel_offset[0][axis_index]; - _corrections.accel_scale_0[axis_index] = _accel_scale[0][axis_index]; - _corrections.accel_offset_1[axis_index] = _accel_offset[1][axis_index]; - _corrections.accel_scale_1[axis_index] = _accel_scale[1][axis_index]; - _corrections.accel_offset_2[axis_index] = _accel_offset[2][axis_index]; - _corrections.accel_scale_2[axis_index] = _accel_scale[2][axis_index]; } } } void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) { + float *offsets[] = {_corrections.gyro_offset_0, _corrections.gyro_offset_1, _corrections.gyro_offset_2 }; + float *scales[] = {_corrections.gyro_scale_0, _corrections.gyro_scale_1, _corrections.gyro_scale_2 }; + for (unsigned uorb_index = 0; uorb_index < _gyro.subscription_count; uorb_index++) { bool gyro_updated; orb_check(_gyro.subscription[uorb_index], &gyro_updated); @@ -684,7 +678,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) // 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) { + offsets[uorb_index], scales[uorb_index]) == 2) { _corrections_changed = true; } @@ -718,12 +712,6 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) for (unsigned axis_index = 0; axis_index < 3; axis_index++) { raw.gyro_rad[axis_index] = _last_sensor_data[best_index].gyro_rad[axis_index]; - _corrections.gyro_offset_0[axis_index] = _gyro_offset[0][axis_index]; - _corrections.gyro_scale_0[axis_index] = _gyro_scale[0][axis_index]; - _corrections.gyro_offset_1[axis_index] = _gyro_offset[1][axis_index]; - _corrections.gyro_scale_1[axis_index] = _gyro_scale[1][axis_index]; - _corrections.gyro_offset_2[axis_index] = _gyro_offset[2][axis_index]; - _corrections.gyro_scale_2[axis_index] = _gyro_scale[2][axis_index]; } } } @@ -777,6 +765,8 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw) void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) { bool got_update = false; + float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2 }; + float *scales[] = {&_corrections.baro_scale_0, &_corrections.baro_scale_1, &_corrections.baro_scale_2 }; for (unsigned uorb_index = 0; uorb_index < _baro.subscription_count; uorb_index++) { bool baro_updated; @@ -796,7 +786,7 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) // 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) { + offsets[uorb_index], scales[uorb_index]) == 2) { _corrections_changed = true; } @@ -834,13 +824,6 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) _corrections_changed = true; } - _corrections.baro_offset_0 = _baro_offset[0]; - _corrections.baro_scale_0 = _baro_scale[0]; - _corrections.baro_offset_1 = _baro_offset[1]; - _corrections.baro_scale_1 = _baro_scale[1]; - _corrections.baro_offset_2 = _baro_offset[2]; - _corrections.baro_scale_2 = _baro_scale[2]; - /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ /* diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index b649ef9e41..0fef71ec6c 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -265,12 +265,6 @@ private: /* sensor thermal compensation */ 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;