Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: use new accumulate functions for AK8963

This commit is contained in:
Andrew Tridgell 2018-10-19 11:25:32 +11:00
parent 55e4b64755
commit 579a84b53e
2 changed files with 2 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -180,19 +180,7 @@ void AP_Compass_AK8963::read()
return;
}
if (_accum_count == 0) {
return;
}
Vector3f field;
{
WITH_SEMAPHORE(_sem);
field = Vector3f(_mag_x_accum, _mag_y_accum, _mag_z_accum) / _accum_count;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_accum_count = 0;
}
publish_filtered_field(field, _compass_instance);
drain_accumulated_samples(_compass_instance);
}
void AP_Compass_AK8963::_make_adc_sensitivity_adjustment(Vector3f& field) const
@ -234,27 +222,7 @@ void AP_Compass_AK8963::_update()
_make_adc_sensitivity_adjustment(raw_field);
raw_field *= AK8963_MILLIGAUSS_SCALE;
// rotate raw_field from sensor frame to body frame
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);
WITH_SEMAPHORE(_sem);
_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;
}
accumulate_sample(raw_field, _compass_instance, 10);
}
bool AP_Compass_AK8963::_check_id()

4
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -53,10 +53,6 @@ private:
AP_AK8963_BusDriver *_bus;
float _magnetometer_ASA[3] {0, 0, 0};
float _mag_x_accum;
float _mag_y_accum;
float _mag_z_accum;
uint32_t _accum_count;
uint8_t _compass_instance;
bool _initialized;