Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: use queue length 10 in PX4 driver

Browse Source 
and remove unnecessary poll() call
This commit is contained in:
Andrew Tridgell 2013-01-04 20:12:03 +11:00
parent dbcaa4cf3c
commit 26bc278181
1 changed files with 10 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -20,7 +20,6 @@
#include <unistd.h> #include <unistd.h>
#include <drivers/drv_mag.h> #include <drivers/drv_mag.h>
#include <poll.h>
#include <stdio.h> #include <stdio.h>
#include <errno.h> #include <errno.h>
@ -30,7 +29,6 @@ extern const AP_HAL::HAL& hal;
bool AP_Compass_PX4::init(void) bool AP_Compass_PX4::init(void)
{ {
hal.console->printf("PX4 compass init started\n");
_mag_fd = open(MAG_DEVICE_PATH, O_RDONLY); _mag_fd = open(MAG_DEVICE_PATH, O_RDONLY);
if (_mag_fd < 0) { if (_mag_fd < 0) {
hal.console->printf("Unable to open " MAG_DEVICE_PATH); hal.console->printf("Unable to open " MAG_DEVICE_PATH);
@ -43,17 +41,19 @@ bool AP_Compass_PX4::init(void)
/* set the driver to poll at 150Hz */ /* set the driver to poll at 150Hz */
ioctl(_mag_fd, SENSORIOCSPOLLRATE, 150); ioctl(_mag_fd, SENSORIOCSPOLLRATE, 150);
// average over up to 10 samples
ioctl(_mag_fd, SENSORIOCSQUEUEDEPTH, 10);
healthy = false; healthy = false;
_count = 0; _count = 0;
_sum.zero(); _sum.zero();
hal.console->printf("PX4 compass init done\n");
return true; return true;
} }
bool AP_Compass_PX4::read(void) bool AP_Compass_PX4::read(void)
{ {
accumulate();
while (_count == 0) { while (_count == 0) {
accumulate(); accumulate();
if (_count == 0) { if (_count == 0) {
@ -81,22 +81,12 @@ bool AP_Compass_PX4::read(void)
void AP_Compass_PX4::accumulate(void) void AP_Compass_PX4::accumulate(void)
{ {
struct pollfd fds; struct mag_report mag_report;
int ret; while (::read(_mag_fd, &mag_report, sizeof(mag_report)) == sizeof(mag_report)) {
_sum += Vector3f(mag_report.x, mag_report.y, mag_report.z);
fds.fd = _mag_fd; _count++;
fds.events = POLLIN; healthy = true;
ret = poll(&fds, 1, 0); }
if (ret == 1) {
// data is available
struct mag_report mag_report;
if (::read(_mag_fd, &mag_report, sizeof(mag_report)) == sizeof(mag_report)) {
_sum += Vector3f(mag_report.x, mag_report.y, mag_report.z);
_count++;
healthy = true;
}
}
} }
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD