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

AP_Compass: MMC3416: use common method to accumulate samples

Browse Source
This commit is contained in:
Lucas De Marchi 2018-09-28 11:11:42 -07:00
parent e83b345cbb
commit 1a9c386df2
2 changed files with 5 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
libraries/AP_Compass/AP_Compass_MMC3416.cpp
View File

@ -237,7 +237,8 @@ void AP_Compass_MMC3416::timer()
offset.x, offset.y, offset.z); offset.x, offset.y, offset.z);
#endif #endif
accumulate_field(field); last_sample_ms = AP_HAL::millis();
accumulate_sample(field, compass_instance);
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) {
state = STATE_REFILL1; state = STATE_REFILL1;
@ -263,7 +264,8 @@ void AP_Compass_MMC3416::timer()
field *= -counts_to_milliGauss; field *= -counts_to_milliGauss;
field += offset; field += offset;
accumulate_field(field); last_sample_ms = AP_HAL::millis();
accumulate_sample(field, compass_instance);
// we stay in STATE_MEASURE_WAIT3 for measure_count_limit cycles // we stay in STATE_MEASURE_WAIT3 for measure_count_limit cycles
if (measure_count++ >= measure_count_limit) { if (measure_count++ >= measure_count_limit) {
@ -279,52 +281,7 @@ void AP_Compass_MMC3416::timer()
} }
} }
/*
accumulate a field
*/
void AP_Compass_MMC3416::accumulate_field(Vector3f &field)
{
#if 0
DataFlash_Class::instance()->Log_Write("MMC", "TimeUS,X,Y,Z", "Qfff",
AP_HAL::micros64(),
(double)field.x,
(double)field.y,
(double)field.z);
#endif
/* 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(0)) {
accum += field;
accum_count++;
_sem->give();
}
last_sample_ms = AP_HAL::millis();
}
void AP_Compass_MMC3416::read() void AP_Compass_MMC3416::read()
{ {
if (!_sem->take(0)) { drain_accumulated_samples(compass_instance);
return;
}
if (accum_count == 0) {
_sem->give();
return;
}
accum /= accum_count;
publish_filtered_field(accum, compass_instance);
accum.zero();
accum_count = 0;
_sem->give();
} }

2
libraries/AP_Compass/AP_Compass_MMC3416.h
View File

@ -61,8 +61,6 @@ private:
void accumulate_field(Vector3f &field); void accumulate_field(Vector3f &field);
uint8_t compass_instance; uint8_t compass_instance;
Vector3f accum;
uint16_t accum_count;
bool force_external; bool force_external;
Vector3f offset; Vector3f offset;
uint16_t measure_count; uint16_t measure_count;