aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: LSM303D: use common method to accumulate samples

Browse Source
This commit is contained in:
Lucas De Marchi 2018-09-28 11:10:22 -07:00
parent 38d1f8705f
commit 2324a28e39
2 changed files with 2 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_Compass/AP_Compass_LSM303D.cpp
View File

@ -341,28 +341,7 @@ void AP_Compass_LSM303D::_update()
Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale; Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale;
// rotate raw_field from sensor frame to body frame accumulate_sample(raw_field, _compass_instance, 10);
rotate_field(raw_field, _compass_instance);
// publish raw_field (uncorrected point sample) for calibration use
publish_raw_field(raw_field, _compass_instance);
// correct raw_field for known errors
correct_field(raw_field, _compass_instance);
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 5;
}
_sem->give();
}
} }
// Read Sensor data // Read Sensor data
@ -372,25 +351,7 @@ void AP_Compass_LSM303D::read()
return; return;
} }
if (!_sem->take_nonblocking()) { drain_accumulated_samples(_compass_instance);
return;
}
if (_accum_count == 0) {
/* We're not ready to publish*/
_sem->give();
return;
}
Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
field /= _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_sem->give();
publish_filtered_field(field, _compass_instance);
} }
void AP_Compass_LSM303D::_disable_i2c() void AP_Compass_LSM303D::_disable_i2c()

4
libraries/AP_Compass/AP_Compass_LSM303D.h
View File

@ -42,13 +42,9 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev; AP_HAL::OwnPtr<AP_HAL::Device> _dev;
float _mag_range_scale; float _mag_range_scale;
float _mag_x_accum;
float _mag_y_accum;
float _mag_z_accum;
int16_t _mag_x; int16_t _mag_x;
int16_t _mag_y; int16_t _mag_y;
int16_t _mag_z; int16_t _mag_z;
uint8_t _accum_count;
uint8_t _compass_instance; uint8_t _compass_instance;
bool _initialised; bool _initialised;