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

AP_Compass: AK09916: use common method to accumulate samples

Browse Source
This commit is contained in:
Lucas De Marchi 2018-09-28 11:08:52 -07:00
parent e769c6932f
commit 565df5ec94
2 changed files with 2 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_Compass/AP_Compass_AK09916.cpp
View File

@ -228,20 +228,7 @@ void AP_Compass_AK09916::timer()
field(data.magx * range_scale, data.magy * range_scale, data.magz * range_scale);
/* rotate raw_field from sensor frame to body frame */
rotate_field(field, compass_instance);
/* publish raw_field (uncorrected point sample) for calibration use */
publish_raw_field(field, compass_instance);
/* correct raw_field for known errors */
correct_field(field, compass_instance);
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
accum += field;
accum_count++;
_sem->give();
}
accumulate_sample(field, compass_instance);
check_registers:
dev->check_next_register();
@ -249,20 +236,5 @@ check_registers:
void AP_Compass_AK09916::read()
{
if (!_sem->take_nonblocking()) {
return;
}
if (accum_count == 0) {
_sem->give();
return;
}
accum /= accum_count;
publish_filtered_field(accum, compass_instance);
accum.zero();
accum_count = 0;
_sem->give();
drain_accumulated_samples(compass_instance);
}

2
libraries/AP_Compass/AP_Compass_AK09916.h
View File

@ -70,8 +70,6 @@ private:
void timer();
uint8_t compass_instance;
Vector3f accum;
uint16_t accum_count;
bool force_external;
enum Rotation rotation;
};