Skip to content
Snippets Groups Projects
Commit 0fce0eb4 authored by Andrew Tridgell's avatar Andrew Tridgell
Browse files

AP_Compass_PX4: fixed handling of a saturated compass

a saturated compass is now marked unhealthy, rather than causing the
code to spin waiting for a valid sample. This fixes a problem with
strong magnets causing the main flight loop to stop
parent d7ec9850
No related branches found
No related tags found
No related merge requests found
...@@ -20,11 +20,19 @@ ...@@ -20,11 +20,19 @@
#include <unistd.h> #include <unistd.h>
#include <drivers/drv_mag.h> #include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <stdio.h> #include <stdio.h>
#include <errno.h> #include <errno.h>
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 //////////////////////////////////////////////////////////////
bool AP_Compass_PX4::init(void) bool AP_Compass_PX4::init(void)
...@@ -48,17 +56,26 @@ bool AP_Compass_PX4::init(void) ...@@ -48,17 +56,26 @@ bool AP_Compass_PX4::init(void)
_count = 0; _count = 0;
_sum.zero(); _sum.zero();
hal.scheduler->register_timer_process(_compass_timer);
// give the timer a chance to run, and gather one sample
hal.scheduler->delay(40);
return true; return true;
} }
bool AP_Compass_PX4::read(void) bool AP_Compass_PX4::read(void)
{ {
accumulate(); hal.scheduler->suspend_timer_procs();
while (_count == 0) {
accumulate(); // try to accumulate one more sample, so we have the latest data
if (_count == 0) { _accumulate();
hal.scheduler->delay(1);
} // consider the compass healthy if we got a reading in the last 0.2s
healthy = (hrt_absolute_time() - _last_timestamp < 200000);
if (!healthy || _count == 0) {
hal.scheduler->resume_timer_procs();
return healthy;
} }
_sum /= _count; _sum /= _count;
...@@ -68,28 +85,30 @@ bool AP_Compass_PX4::read(void) ...@@ -68,28 +85,30 @@ bool AP_Compass_PX4::read(void)
_sum += _offset.get(); _sum += _offset.get();
// apply motor compensation // apply motor compensation
if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) { if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset = _motor_compensation.get() * _thr_or_curr; _motor_offset = _motor_compensation.get() * _thr_or_curr;
_sum += _motor_offset; _sum += _motor_offset;
}else{ } else {
_motor_offset.x = 0; _motor_offset.x = 0;
_motor_offset.y = 0; _motor_offset.y = 0;
_motor_offset.z = 0; _motor_offset.z = 0;
} }
mag_x = _sum.x; mag_x = _sum.x;
mag_y = _sum.y; mag_y = _sum.y;
mag_z = _sum.z; mag_z = _sum.z;
_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) &&
...@@ -97,8 +116,22 @@ void AP_Compass_PX4::accumulate(void) ...@@ -97,8 +116,22 @@ void AP_Compass_PX4::accumulate(void)
_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; _last_timestamp = mag_report.timestamp;
healthy = true;
} }
} }
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
...@@ -16,10 +16,13 @@ public: ...@@ -16,10 +16,13 @@ public:
void accumulate(void); void accumulate(void);
private: private:
int _mag_fd; static int _mag_fd;
Vector3f _sum; static Vector3f _sum;
uint32_t _count; static uint32_t _count;
uint64_t _last_timestamp; static uint32_t _last_timer;
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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment