AP_Compass: convert SITL to new accumulate approach

libraries/AP_Compass/AP_Compass_SITL.cpp

@@ -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

libraries/AP_Compass/AP_Compass_SITL.h

@@ -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;