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

AP_Compass: convert SITL to new accumulate approach

Browse Source
This commit is contained in:
Andrew Tridgell 2018-10-16 19:26:29 +11:00
parent 1978a0cb94
commit 83065d5e88
2 changed files with 2 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Compass/AP_Compass_SITL.cpp
View File

@ -8,7 +8,6 @@ extern const AP_HAL::HAL& hal;
AP_Compass_SITL::AP_Compass_SITL()
: _sitl(AP::sitl())
, _has_sample(false)
{
if (_sitl != nullptr) {
_compass._setup_earth_field();
@ -121,37 +120,14 @@ void AP_Compass_SITL::_timer()
publish_raw_field(f, _compass_instance[i]);
correct_field(f, _compass_instance[i]);
_mag_accum[i] += f;
}
WITH_SEMAPHORE(_sem);
_accum_count++;
if (_accum_count == 10) {
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
_mag_accum[i] /= 2;
}
_accum_count = 5;
_has_sample = true;
accumulate_sample(f, _compass_instance[i], 10);
}
}
void AP_Compass_SITL::read()
{
WITH_SEMAPHORE(_sem);
if (!_has_sample) {
return;
}
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
Vector3f field(_mag_accum[i]);
field /= _accum_count;
_mag_accum[i].zero();
publish_filtered_field(field, _compass_instance[i]);
drain_accumulated_samples(_compass_instance[i], nullptr);
}
_accum_count = 0;
_has_sample = false;
}
#endif

4
libraries/AP_Compass/AP_Compass_SITL.h
View File

@ -32,12 +32,8 @@ private:
VectorN<readings_compass,buffer_length> buffer;
void _timer();
bool _has_sample;
uint32_t _last_sample_time;
Vector3f _mag_accum[SITL_NUM_COMPASSES];
uint32_t _accum_count;
void _setup_eliptical_correcion();
Matrix3f _eliptical_corr;