Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 06:58:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: HMC5843: use common method to accumulate samples

Browse Source
This commit is contained in:
Lucas De Marchi 2018-09-28 10:41:48 -07:00
parent 329516373c
commit c0bccdaed6
2 changed files with 7 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -233,47 +233,19 @@ void AP_Compass_HMC5843::_timer()
return;
}
// the _mag_N values are in the range -2048 to 2047, so we can
// accumulate up to 15 of them in an int16_t. Let's make it 14
// for ease of calculation. We expect to do reads at 10Hz, and
// we get new data at most 75Hz, so we don't expect to
// accumulate more than 8 before a read
// get raw_field - sensor frame, uncorrected
Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z);
raw_field *= _gain_scale;
// rotate to the desired orientation
if (is_external(_compass_instance)) {
raw_field.rotate(ROTATION_YAW_90);
}
// 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);
if (!field_ok(raw_field)) {
return;
}
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
}
_mag_x_accum += raw_field.x;
_mag_y_accum += raw_field.y;
_mag_z_accum += raw_field.z;
_accum_count++;
if (_accum_count == 14) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 7;
}
_sem->give();
// We expect to do reads at 10Hz, and we get new data at most 75Hz, so we
// don't expect to accumulate more than 8 before a read; let's make it
// 14 to give more room for the initialization phase
accumulate_sample(raw_field, _compass_instance, 14);
}
/*
@ -291,26 +263,7 @@ void AP_Compass_HMC5843::read()
return;
}
if (!_sem->take_nonblocking()) {
return;
}
if (_accum_count == 0) {
_sem->give();
return;
}
Vector3f field(_mag_x_accum * _scaling[0],
_mag_y_accum * _scaling[1],
_mag_z_accum * _scaling[2]);
field /= _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
_sem->give();
publish_filtered_field(field, _compass_instance);
drain_accumulated_samples(_compass_instance, &_scaling);
}
bool AP_Compass_HMC5843::_setup_sampling_mode()

6
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -47,16 +47,12 @@ private:
AP_HMC5843_BusDriver *_bus;
float _scaling[3];
Vector3f _scaling;
float _gain_scale;
int16_t _mag_x;
int16_t _mag_y;
int16_t _mag_z;
int16_t _mag_x_accum;
int16_t _mag_y_accum;
int16_t _mag_z_accum;
uint8_t _accum_count;
uint8_t _compass_instance;