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

AP_Compass: use report timestamp on PX4 for accurate timing

Browse Source
This commit is contained in:
Andrew Tridgell 2013-01-22 20:08:33 +11:00
parent ab1978ad50
commit 80eaa52ed8
2 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -74,7 +74,7 @@ bool AP_Compass_PX4::read(void)
_sum.zero(); _sum.zero();
_count = 0; _count = 0;
last_update = hal.scheduler->micros(); last_update = _last_timestamp;
return true; return true;
} }
@ -82,9 +82,11 @@ bool AP_Compass_PX4::read(void)
void AP_Compass_PX4::accumulate(void) void AP_Compass_PX4::accumulate(void)
{ {
struct mag_report mag_report; struct mag_report mag_report;
while (::read(_mag_fd, &mag_report, sizeof(mag_report)) == sizeof(mag_report)) { while (::read(_mag_fd, &mag_report, sizeof(mag_report)) == sizeof(mag_report) &&
mag_report.timestamp != _last_timestamp) {
_sum += Vector3f(mag_report.x, mag_report.y, mag_report.z); _sum += Vector3f(mag_report.x, mag_report.y, mag_report.z);
_count++; _count++;
_last_timestamp = mag_report.timestamp;
healthy = true; healthy = true;
} }
} }

1
libraries/AP_Compass/AP_Compass_PX4.h
View File

@ -19,6 +19,7 @@ private:
int _mag_fd; int _mag_fd;
Vector3f _sum; Vector3f _sum;
uint32_t _count; uint32_t _count;
uint64_t _last_timestamp;
}; };
#endif // AP_Compass_PX4_H #endif // AP_Compass_PX4_H