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

AP_Compass: QMC5883L: use common method to accumulate samples

This commit is contained in:
Lucas De Marchi 2018-09-28 10:47:10 -07:00
parent c0bccdaed6
commit e769c6932f
2 changed files with 2 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Compass/AP_Compass_QMC5883L.cpp
View File

@ -195,30 +195,7 @@ void AP_Compass_QMC5883L::timer()
field.rotate(ROTATION_YAW_90);
}
/* rotate raw_field from sensor frame to body frame */
rotate_field(field, _instance);
/* publish raw_field (uncorrected point sample) for calibration use */
publish_raw_field(field, _instance);
/* correct raw_field for known errors */
correct_field(field, _instance);
if (!field_ok(field)) {
return;
}
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_accum += field;
_accum_count++;
if(_accum_count == 20){
_accum.x /= 2;
_accum.y /= 2;
_accum.z /= 2;
_accum_count = 10;
}
_sem->give();
}
accumulate_sample(field, _instance, 20);
}
void AP_Compass_QMC5883L::read()
@ -227,20 +204,7 @@ void AP_Compass_QMC5883L::read()
return;
}
if (_accum_count == 0) {
_sem->give();
return;
}
Vector3f field(_accum);
field /= _accum_count;
publish_filtered_field(field, _instance);
_accum.zero();
_accum_count = 0;
_sem->give();
drain_accumulated_samples(_instance);
}
void AP_Compass_QMC5883L::_dump_registers()

3
libraries/AP_Compass/AP_Compass_QMC5883L.h
View File

@ -62,9 +62,6 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
Vector3f _accum = Vector3f();
uint32_t _accum_count = 0;
enum Rotation _rotation;
uint8_t _instance;
bool _force_external:1;