mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: QMC5883L: use common method to accumulate samples
This commit is contained in:
parent
c0bccdaed6
commit
e769c6932f
|
@ -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()
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue