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

AP_Compass: updates to support multiple compasses

Browse Source
This commit is contained in:
Andrew Tridgell 2013-12-09 16:05:57 +11:00
parent f37a2c979d
commit 8a97042fb1
6 changed files with 105 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -63,7 +63,7 @@ bool AP_Compass_HIL::read()
}
// return last values provided by setHIL function
_field = _hil_mag + ofs + _motor_offset;
_field[0] = _hil_mag + ofs + _motor_offset;
// values set by setHIL function
last_update = hal.scheduler->micros(); // record time of update

22
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -316,47 +316,43 @@ bool AP_Compass_HMC5843::read()
}
}
_field.x = _mag_x_accum * calibration[0] / _accum_count;
_field.y = _mag_y_accum * calibration[1] / _accum_count;
_field.z = _mag_z_accum * calibration[2] / _accum_count;
_field[0].x = _mag_x_accum * calibration[0] / _accum_count;
_field[0].y = _mag_y_accum * calibration[1] / _accum_count;
_field[0].z = _mag_z_accum * calibration[2] / _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
last_update = hal.scheduler->micros(); // record time of update
// rotate to the desired orientation
Vector3f rot_mag = Vector3f(_field.x,_field.y,_field.z);
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rot_mag.rotate(ROTATION_YAW_90);
_field[0].rotate(ROTATION_YAW_90);
}
// apply default board orientation for this compass type. This is
// a noop on most boards
rot_mag.rotate(MAG_BOARD_ORIENTATION);
_field[0].rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
rot_mag.rotate((enum Rotation)_orientation.get());
_field[0].rotate((enum Rotation)_orientation.get());
if (!_external) {
// and add in AHRS_ORIENTATION setting if not an external compass
rot_mag.rotate(_board_orientation);
_field[0].rotate(_board_orientation);
}
rot_mag += _offset.get();
_field[0] += _offset.get();
// apply motor compensation
if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset = _motor_compensation.get() * _thr_or_curr;
rot_mag += _motor_offset;
_field[0] += _motor_offset;
}else{
_motor_offset.x = 0;
_motor_offset.y = 0;
_motor_offset.z = 0;
}
_field.x = rot_mag.x;
_field.y = rot_mag.y;
_field.z = rot_mag.z;
healthy = true;
return true;

119
libraries/AP_Compass/AP_Compass_PX4.cpp
View File

@ -42,38 +42,41 @@ extern const AP_HAL::HAL& hal;
bool AP_Compass_PX4::init(void)
{
_mag_fd = open(MAG_DEVICE_PATH, O_RDONLY);
if (_mag_fd < 0) {
_mag_fd[0] = open(MAG_DEVICE_PATH, O_RDONLY);
if (_mag_fd[0] < 0) {
hal.console->printf("Unable to open " MAG_DEVICE_PATH "\n");
return false;
}
/* set the driver to poll at 100Hz */
if (0 != ioctl(_mag_fd, SENSORIOCSPOLLRATE, 100)) {
hal.console->printf("Failed to setup compass poll rate\n");
return false;
_mag_fd[1] = open(MAG_DEVICE_PATH "1", O_RDONLY);
if (_mag_fd[1] >= 0) {
_num_instances = 2;
} else {
_num_instances = 1;
}
// average over up to 20 samples
if (ioctl(_mag_fd, SENSORIOCSQUEUEDEPTH, 20) != 0) {
hal.console->printf("Failed to setup compass queue\n");
return false;
}
for (uint8_t i=0; i<_num_instances; i++) {
// average over up to 20 samples
if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) {
hal.console->printf("Failed to setup compass queue\n");
return false;
}
// remember if the compass is external
_is_external = (ioctl(_mag_fd, MAGIOCGEXTERNAL, 0) > 0);
if (_is_external) {
hal.console->printf("Using external compass\n");
// remember if the compass is external
_is_external[i] = (ioctl(_mag_fd[i], MAGIOCGEXTERNAL, 0) > 0);
if (_is_external[0]) {
hal.console->printf("Using external compass[%u]\n", (unsigned)i);
}
_count[0] = 0;
_sum[i].zero();
}
healthy = false;
_count = 0;
_sum.zero();
// give the driver a chance to run, and gather one sample
hal.scheduler->delay(40);
accumulate();
if (_count == 0) {
if (_count[0] == 0) {
hal.console->printf("Failed initial compass accumulate\n");
}
return true;
@ -86,50 +89,52 @@ bool AP_Compass_PX4::read(void)
accumulate();
// 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) {
healthy = (hrt_absolute_time() - _last_timestamp[0] < 200000);
if (!healthy || _count[0] == 0) {
if (was_healthy) {
hal.console->printf("Compass unhealthy deltat=%u _count=%u\n",
(unsigned)(hrt_absolute_time() - _last_timestamp),
(unsigned)_count);
(unsigned)(hrt_absolute_time() - _last_timestamp[0]),
(unsigned)_count[0]);
}
return healthy;
}
_sum /= _count;
_sum *= 1000;
for (uint8_t i=0; i<_num_instances; i++) {
_sum[i] /= _count[i];
_sum[i] *= 1000;
// apply default board orientation for this compass type. This is
// a noop on most boards
_sum.rotate(MAG_BOARD_ORIENTATION);
// apply default board orientation for this compass type. This is
// a noop on most boards
_sum[i].rotate(MAG_BOARD_ORIENTATION);
// override any user setting of COMPASS_EXTERNAL
_external.set(_is_external);
// override any user setting of COMPASS_EXTERNAL
_external.set(_is_external[0]);
if (_external) {
// add user selectable orientation
_sum.rotate((enum Rotation)_orientation.get());
} else {
// add in board orientation from AHRS
_sum.rotate(_board_orientation);
if (_is_external[i]) {
// add user selectable orientation
_sum[i].rotate((enum Rotation)_orientation.get());
} else {
// add in board orientation from AHRS
_sum[i].rotate(_board_orientation);
}
_sum[i] += _offset.get();
// apply motor compensation
if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset = _motor_compensation.get() * _thr_or_curr;
_sum[i] += _motor_offset;
} else {
_motor_offset.zero();
}
_field[i] = _sum[i];
_sum[i].zero();
_count[i] = 0;
}
_sum += _offset.get();
// apply motor compensation
if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset = _motor_compensation.get() * _thr_or_curr;
_sum += _motor_offset;
} else {
_motor_offset.zero();
}
_field = _sum;
_sum.zero();
_count = 0;
last_update = _last_timestamp;
last_update = _last_timestamp[0];
return true;
}
@ -137,11 +142,13 @@ bool AP_Compass_PX4::read(void)
void AP_Compass_PX4::accumulate(void)
{
struct mag_report 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);
_count++;
_last_timestamp = mag_report.timestamp;
for (uint8_t i=0; i<_num_instances; i++) {
while (::read(_mag_fd[i], &mag_report, sizeof(mag_report)) == sizeof(mag_report) &&
mag_report.timestamp != _last_timestamp[i]) {
_sum[i] += Vector3f(mag_report.x, mag_report.y, mag_report.z);
_count[i]++;
_last_timestamp[i] = mag_report.timestamp;
}
}
}

15
libraries/AP_Compass/AP_Compass_PX4.h
View File

@ -10,17 +10,22 @@ class AP_Compass_PX4 : public Compass
public:
AP_Compass_PX4() : Compass() {
product_id = AP_COMPASS_TYPE_PX4;
_num_instances = 0;
}
bool init(void);
bool read(void);
void accumulate(void);
// return the number of compass instances
uint8_t get_count(void) const { return _num_instances; }
private:
int _mag_fd;
Vector3f _sum;
uint32_t _count;
uint64_t _last_timestamp;
bool _is_external;
uint8_t _num_instances;
int _mag_fd[COMPASS_MAX_INSTANCES];
Vector3f _sum[COMPASS_MAX_INSTANCES];
uint32_t _count[COMPASS_MAX_INSTANCES];
uint64_t _last_timestamp[COMPASS_MAX_INSTANCES];
bool _is_external[COMPASS_MAX_INSTANCES];
};
#endif // AP_Compass_PX4_H

10
libraries/AP_Compass/Compass.cpp
View File

@ -192,10 +192,10 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix) const
float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x);
// Tilt compensated magnetic field Y component:
float headY = _field.y * dcm_matrix.c.z - _field.z * dcm_matrix.c.y;
float headY = _field[0].y * dcm_matrix.c.z - _field[0].z * dcm_matrix.c.y;
// Tilt compensated magnetic field X component:
float headX = _field.x * cos_pitch_sq - dcm_matrix.c.x * (_field.y * dcm_matrix.c.y + _field.z * dcm_matrix.c.z);
float headX = _field[0].x * cos_pitch_sq - dcm_matrix.c.x * (_field[0].y * dcm_matrix.c.y + _field[0].z * dcm_matrix.c.z);
// magnetic heading
// 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
@ -258,7 +258,7 @@ Compass::null_offsets(void)
for (uint8_t i=0; i<_mag_history_size; i++) {
// fill the history buffer with the current mag vector,
// with the offset removed
_mag_history[i] = Vector3i((_field.x+0.5f) - ofs.x, (_field.y+0.5f) - ofs.y, (_field.z+0.5f) - ofs.z);
_mag_history[i] = Vector3i((_field[0].x+0.5f) - ofs.x, (_field[0].y+0.5f) - ofs.y, (_field[0].z+0.5f) - ofs.z);
}
_mag_history_index = 0;
return;
@ -275,7 +275,7 @@ Compass::null_offsets(void)
b1 += ofs;
// get the current vector
b2 = Vector3f(_field.x, _field.y, _field.z);
b2 = Vector3f(_field[0].x, _field[0].y, _field[0].z);
// calculate the delta for this sample
diff = b2 - b1;
@ -293,7 +293,7 @@ Compass::null_offsets(void)
}
// put the vector in the history
_mag_history[_mag_history_index] = Vector3i((_field.x+0.5f) - ofs.x, (_field.y+0.5f) - ofs.y, (_field.z+0.5f) - ofs.z);
_mag_history[_mag_history_index] = Vector3i((_field[0].x+0.5f) - ofs.x, (_field[0].y+0.5f) - ofs.y, (_field[0].z+0.5f) - ofs.z);
_mag_history_index = (_mag_history_index + 1) % _mag_history_size;
// equation 6 of Bills paper

20
libraries/AP_Compass/Compass.h
View File

@ -37,6 +37,16 @@
# error "You must define a default compass orientation for this board"
#endif
/**
maximum number of compass instances available on this platform. If more
than 1 then redundent sensors may be available
*/
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#define COMPASS_MAX_INSTANCES 2
#else
#define COMPASS_MAX_INSTANCES 1
#endif
class Compass
{
public:
@ -86,11 +96,15 @@ public:
///
void save_offsets();
// return the number of compass instances
virtual uint8_t get_count(void) const { return 1; }
/// Return the current field as a Vector3f
const Vector3f &get_field(void) const { return _field; }
const Vector3f &get_field(uint8_t i) const { return _field[i]; }
const Vector3f &get_field(void) const { return get_field(0); }
/// set the current field as a Vector3f
void set_field(const Vector3f &field) { _field = field; }
void set_field(const Vector3f &field) { _field[0] = field; }
/// Returns the current offset values
///
@ -199,7 +213,7 @@ public:
AP_Int8 _learn; ///<enable calibration learning
protected:
Vector3f _field; ///< magnetic field strength
Vector3f _field[COMPASS_MAX_INSTANCES]; ///< magnetic field strength
AP_Int8 _orientation;
AP_Vector3f _offset;