diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp b/libraries/AP_Compass/AP_Compass_UAVCAN.cpp index 5dfa1e3a1e..dbb3819ac7 100644 --- a/libraries/AP_Compass/AP_Compass_UAVCAN.cpp +++ b/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 - 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++; + accumulate_sample(raw_field, _instance); } 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 - if (_count == 0) { - return; - } - - WITH_SEMAPHORE(_sem); - _sum /= _count; - - publish_filtered_field(_sum, _instance); - - _sum.zero(); - _count = 0; + drain_accumulated_samples(_instance); } #endif diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.h b/libraries/AP_Compass/AP_Compass_UAVCAN.h index 1a5e4fb5d6..0e18230643 100644 --- a/libraries/AP_Compass/AP_Compass_UAVCAN.h +++ b/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;