Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: use new accumulate functions for UAVCAN

Browse Source
This commit is contained in:
Andrew Tridgell 2018-10-19 11:25:47 +11:00
parent 579a84b53e
commit 84d7160128
2 changed files with 2 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_Compass/AP_Compass_UAVCAN.cpp
View File

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

3
libraries/AP_Compass/AP_Compass_UAVCAN.h
View File

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