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: simplify PX4 compass driver

Browse Source 
we can now rely on queueing in the NuttX driver
This commit is contained in:
Andrew Tridgell 2013-08-28 19:18:05 +10:00
parent 4c752e4a94
commit 1ff669eb97
2 changed files with 10 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -26,12 +26,6 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
int AP_Compass_PX4::_mag_fd = -1;
Vector3f AP_Compass_PX4::_sum;
uint32_t AP_Compass_PX4::_count = 0;
uint32_t AP_Compass_PX4::_last_timer = 0;
uint64_t AP_Compass_PX4::_last_timestamp = 0;
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
@ -49,32 +43,28 @@ 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 // average over up to 20 samples
ioctl(_mag_fd, SENSORIOCSQUEUEDEPTH, 10); ioctl(_mag_fd, SENSORIOCSQUEUEDEPTH, 20);
healthy = false; healthy = false;
_count = 0; _count = 0;
_sum.zero(); _sum.zero();
hal.scheduler->register_timer_process(_compass_timer); // give the driver a chance to run, and gather one sample
// give the timer a chance to run, and gather one sample
hal.scheduler->delay(40); hal.scheduler->delay(40);
accumulate();
return true; return true;
} }
bool AP_Compass_PX4::read(void) bool AP_Compass_PX4::read(void)
{ {
hal.scheduler->suspend_timer_procs();
// try to accumulate one more sample, so we have the latest data // try to accumulate one more sample, so we have the latest data
_accumulate(); accumulate();
// consider the compass healthy if we got a reading in the last 0.2s // consider the compass healthy if we got a reading in the last 0.2s
healthy = (hrt_absolute_time() - _last_timestamp < 200000); healthy = (hrt_absolute_time() - _last_timestamp < 200000);
if (!healthy || _count == 0) { if (!healthy || _count == 0) {
hal.scheduler->resume_timer_procs();
return healthy; return healthy;
} }
@ -109,14 +99,12 @@ bool AP_Compass_PX4::read(void)
_sum.zero(); _sum.zero();
_count = 0; _count = 0;
hal.scheduler->resume_timer_procs();
last_update = _last_timestamp; last_update = _last_timestamp;
return true; return true;
} }
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) &&
@ -127,19 +115,4 @@ void AP_Compass_PX4::_accumulate(void)
} }
} }
void AP_Compass_PX4::accumulate(void)
{
// let the timer do the work
}
void AP_Compass_PX4::_compass_timer(uint32_t now)
{
// try to accumulate samples at 100Hz
if (now - _last_timer < 10000) {
return;
}
_last_timer = hal.scheduler->micros();
_accumulate();
}
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

11
libraries/AP_Compass/AP_Compass_PX4.h
View File

@ -16,13 +16,10 @@ public:
void accumulate(void); void accumulate(void);
private: private:
static int _mag_fd; int _mag_fd;
static Vector3f _sum; Vector3f _sum;
static uint32_t _count; uint32_t _count;
static uint32_t _last_timer; uint64_t _last_timestamp;
static uint64_t _last_timestamp;
static void _accumulate(void);
static void _compass_timer(uint32_t now);
}; };
#endif // AP_Compass_PX4_H #endif // AP_Compass_PX4_H