From cb16733918b0228bfa33e9b527c56f7473565efb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Dec 2013 14:09:34 +1100 Subject: [PATCH] AP_Compass: switched to a vector based interface this is more consistent with other APIs and makes multi-device support easier --- libraries/AP_Compass/AP_Compass_HIL.cpp | 4 +-- libraries/AP_Compass/AP_Compass_HMC5843.cpp | 14 ++++----- libraries/AP_Compass/AP_Compass_PX4.cpp | 8 ++--- libraries/AP_Compass/Compass.cpp | 10 +++--- libraries/AP_Compass/Compass.h | 13 ++++++-- .../AP_Compass_test/AP_Compass_test.pde | 31 ++++++++++--------- 6 files changed, 41 insertions(+), 39 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 843f54c5f9..caa4b7994a 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -63,9 +63,7 @@ bool AP_Compass_HIL::read() } // return last values provided by setHIL function - mag_x = _hil_mag.x + ofs.x + _motor_offset.x; - mag_y = _hil_mag.y + ofs.y + _motor_offset.y; - mag_z = _hil_mag.z + ofs.z + _motor_offset.z; + _field = _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 4d56bd1a8c..2fb65111b3 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -316,16 +316,16 @@ bool AP_Compass_HMC5843::read() } } - mag_x = _mag_x_accum * calibration[0] / _accum_count; - mag_y = _mag_y_accum * calibration[1] / _accum_count; - mag_z = _mag_z_accum * calibration[2] / _accum_count; + _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; _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(mag_x,mag_y,mag_z); + Vector3f rot_mag = Vector3f(_field.x,_field.y,_field.z); if (product_id == AP_COMPASS_TYPE_HMC5883L) { rot_mag.rotate(ROTATION_YAW_90); } @@ -354,9 +354,9 @@ bool AP_Compass_HMC5843::read() _motor_offset.z = 0; } - mag_x = rot_mag.x; - mag_y = rot_mag.y; - mag_z = rot_mag.z; + _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 6a0611a5a4..b71a13361b 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.cpp +++ b/libraries/AP_Compass/AP_Compass_PX4.cpp @@ -121,14 +121,10 @@ bool AP_Compass_PX4::read(void) _motor_offset = _motor_compensation.get() * _thr_or_curr; _sum += _motor_offset; } else { - _motor_offset.x = 0; - _motor_offset.y = 0; - _motor_offset.z = 0; + _motor_offset.zero(); } - mag_x = _sum.x; - mag_y = _sum.y; - mag_z = _sum.z; + _field = _sum; _sum.zero(); _count = 0; diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index ea631ca918..abef6330f5 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 = mag_y * dcm_matrix.c.z - mag_z * dcm_matrix.c.y; + float headY = _field.y * dcm_matrix.c.z - _field.z * dcm_matrix.c.y; // Tilt compensated magnetic field X component: - float headX = mag_x * cos_pitch_sq - dcm_matrix.c.x * (mag_y * dcm_matrix.c.y + mag_z * dcm_matrix.c.z); + float headX = _field.x * cos_pitch_sq - dcm_matrix.c.x * (_field.y * dcm_matrix.c.y + _field.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((mag_x+0.5f) - ofs.x, (mag_y+0.5f) - ofs.y, (mag_z+0.5f) - ofs.z); + _mag_history[i] = Vector3i((_field.x+0.5f) - ofs.x, (_field.y+0.5f) - ofs.y, (_field.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(mag_x, mag_y, mag_z); + b2 = Vector3f(_field.x, _field.y, _field.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((mag_x+0.5f) - ofs.x, (mag_y+0.5f) - ofs.y, (mag_z+0.5f) - ofs.z); + _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_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 f106c5da00..1216d3b71a 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -41,9 +41,6 @@ class Compass { public: int16_t product_id; /// product id - int16_t mag_x; ///< magnetic field strength along the X axis - int16_t mag_y; ///< magnetic field strength along the Y axis - int16_t mag_z; ///< magnetic field strength along the Z axis uint32_t last_update; ///< micros() time of last update bool healthy; ///< true if last read OK @@ -62,6 +59,8 @@ public: /// virtual bool read(void) = 0; + + /// use spare CPU cycles to accumulate values from the compass if /// possible virtual void accumulate(void) = 0; @@ -87,6 +86,12 @@ public: /// void save_offsets(); + /// Return the current field as a Vector3f + const Vector3f &get_field(void) const { return _field; } + + /// set the current field as a Vector3f + void set_field(const Vector3f &field) { _field = field; } + /// Returns the current offset values /// /// @returns The current compass offsets. @@ -194,6 +199,8 @@ public: AP_Int8 _learn; /// max[0] ) - max[0] = compass.mag_x; - if( compass.mag_y > max[1] ) - max[1] = compass.mag_y; - if( compass.mag_z > max[2] ) - max[2] = compass.mag_z; + if( mag.x > max[0] ) + max[0] = mag.x; + if( mag.y > max[1] ) + max[1] = mag.y; + if( mag.z > max[2] ) + max[2] = mag.z; // calculate offsets offset[0] = -(max[0]+min[0])/2; @@ -107,9 +108,9 @@ void loop() // display all to user hal.console->printf("Heading: %.2f (%3d,%3d,%3d) i2c error: %u", ToDeg(heading), - (int)compass.mag_x, - (int)compass.mag_y, - (int)compass.mag_z, + (int)mag.x, + (int)mag.y, + (int)mag.z, (unsigned)hal.i2c->lockup_count()); // display offsets