AP_Compass: use new accumulate functions for UAVCAN

libraries/AP_Compass/AP_Compass_UAVCAN.cpp

@@ -131,9 +131,6 @@ void AP_Compass_UAVCAN::init()
     set_dev_id(_instance, d.devid);
     set_external(_instance, true);
 
-    _sum.zero();
-    _count = 0;
-
     debug_mag_uavcan(2, _ap_uavcan->get_driver_index(),  "AP_Compass_UAVCAN loaded\n\r");
 }
 
@@ -179,19 +176,7 @@ void AP_Compass_UAVCAN::handle_mag_msg(const Vector3f &mag)
 {
     Vector3f raw_field = mag * 1000.0;
 
-    // rotate raw_field from sensor frame to body frame
+    accumulate_sample(raw_field, _instance);
-    rotate_field(raw_field, _instance);
-
-    // publish raw_field (uncorrected point sample) for calibration use
-    publish_raw_field(raw_field, _instance);
-
-    // correct raw_field for known errors
-    correct_field(raw_field, _instance);
-
-    WITH_SEMAPHORE(_sem);
-    // accumulate into averaging filter
-    _sum += raw_field;
-    _count++;
 }
 
 void AP_Compass_UAVCAN::handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MagCb &cb)
@@ -227,18 +212,7 @@ void AP_Compass_UAVCAN::handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t no
 
 void AP_Compass_UAVCAN::read(void)
 {
-    // avoid division by zero if we haven't received any mag reports
+    drain_accumulated_samples(_instance);
-    if (_count == 0) {
-        return;
-    }
-
-    WITH_SEMAPHORE(_sem);
-    _sum /= _count;
-
-    publish_filtered_field(_sum, _instance);
-
-    _sum.zero();
-    _count = 0;
 }
 
 #endif

libraries/AP_Compass/AP_Compass_UAVCAN.h

@@ -33,9 +33,6 @@ private:
     uint8_t  _instance;
     bool _initialized;
 
-    Vector3f _sum;
-    uint32_t _count;
-
     AP_UAVCAN* _ap_uavcan;
     uint8_t _node_id;
     uint8_t _sensor_id;