From 8a97042fb154e35de144c7e41e7bb78112c63eff Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Dec 2013 16:05:57 +1100 Subject: [PATCH] AP_Compass: updates to support multiple compasses --- libraries/AP_Compass/AP_Compass_HIL.cpp | 2 +- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 22 ++-- libraries/AP_Compass/AP_Compass_PX4.cpp | 119 +++++++++++--------- libraries/AP_Compass/AP_Compass_PX4.h | 15 ++- libraries/AP_Compass/Compass.cpp | 10 +- libraries/AP_Compass/Compass.h | 20 +++- 6 files changed, 105 insertions(+), 83 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index caa4b7994a..69673ab1fa 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -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 diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 2fb65111b3..93c77f98d7 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -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; diff --git a/libraries/AP_Compass/AP_Compass_PX4.cpp b/libraries/AP_Compass/AP_Compass_PX4.cpp index b71a13361b..d7b56d8451 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.cpp +++ b/libraries/AP_Compass/AP_Compass_PX4.cpp @@ -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; + } } } diff --git a/libraries/AP_Compass/AP_Compass_PX4.h b/libraries/AP_Compass/AP_Compass_PX4.h index 7469ab6a2e..397e8cf85b 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.h +++ b/libraries/AP_Compass/AP_Compass_PX4.h @@ -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 diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index abef6330f5..3b76a4fa97 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -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 diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 1216d3b71a..4677c35370 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -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; ///