yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

voted_sensors_update: cleanup & remove some attributes

Browse Source 
since the correction topic now contains data from all sensors, we don't
need additional fields in voted_sensors_update
This commit is contained in:
Beat Küng
2017-01-26 14:10:07 +01:00
committed by Lorenz Meier
parent 4763a119bc
commit 4a8d29800c
2 changed files with 15 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
src/modules/sensors/voted_sensors_update.cpp
View File

@@ -73,22 +73,12 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters)
_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 */
/*

6
src/modules/sensors/voted_sensors_update.h
View File

@@ -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;