diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 443785e9c4..6e76690532 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -30,3 +30,8 @@ void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z) mag_z = _mag_z + ofs.z; healthy = true; } + +void AP_Compass_HIL::accumulate(void) +{ + // nothing to do +} diff --git a/libraries/AP_Compass/AP_Compass_HIL.h b/libraries/AP_Compass/AP_Compass_HIL.h index 638b768721..57da27d569 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.h +++ b/libraries/AP_Compass/AP_Compass_HIL.h @@ -11,6 +11,7 @@ public: product_id = AP_COMPASS_TYPE_HIL; } bool read(void); + void accumulate(void); void setHIL(float Mag_X, float Mag_Y, float Mag_Z); }; diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index 80a4c36ef8..28bf80de9f 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -95,14 +95,42 @@ bool AP_Compass_HMC5843::read_raw() return false; } - mag_x = -rx; - mag_y = ry; - mag_z = -rz; + _mag_x = -rx; + _mag_y = ry; + _mag_z = -rz; return true; } +// accumulate a reading from the magnetometer +void AP_Compass_HMC5843::accumulate(void) +{ + uint32_t tnow = micros(); + if (healthy && _accum_count != 0 && (tnow - _last_accum_time) < 13333) { + // the compass gets new data at 75Hz + return; + } + if (read_raw()) { + // the _mag_N values are in the range -2048 to 2047, so we can + // accumulate up to 15 of them in an int16_t. Let's make it 14 + // for ease of calculation. We expect to do reads at 10Hz, and + // we get new data at most 75Hz, so we don't expect to + // accumulate more than 8 before a read + _mag_x_accum += _mag_x; + _mag_y_accum += _mag_y; + _mag_z_accum += _mag_z; + _accum_count++; + if (_accum_count == 14) { + _mag_x_accum /= 2; + _mag_y_accum /= 2; + _mag_z_accum /= 2; + _accum_count = 7; + } + _last_accum_time = tnow; + } +} + /* * re-initialise after a IO error @@ -178,9 +206,9 @@ AP_Compass_HMC5843::init() float cal[3]; - cal[0] = fabs(expected_x / (float)mag_x); - cal[1] = fabs(expected_yz / (float)mag_y); - cal[2] = fabs(expected_yz / (float)mag_z); + cal[0] = fabs(expected_x / (float)_mag_x); + cal[1] = fabs(expected_yz / (float)_mag_y); + cal[2] = fabs(expected_yz / (float)_mag_z); if (cal[0] > 0.7 && cal[0] < 1.3 && cal[1] > 0.7 && cal[1] < 1.3 && @@ -194,11 +222,11 @@ AP_Compass_HMC5843::init() #if 0 /* useful for debugging */ Serial.print("mag_x: "); - Serial.print(mag_x); + Serial.print(_mag_x); Serial.print(" mag_y: "); - Serial.print(mag_y); + Serial.print(_mag_y); Serial.print(" mag_z: "); - Serial.println(mag_z); + Serial.println(_mag_z); Serial.print("CalX: "); Serial.print(calibration[0]/good_count); Serial.print(" CalY: "); @@ -227,6 +255,10 @@ AP_Compass_HMC5843::init() _initialised = true; + // perform an initial read + healthy = true; + read(); + return success; } @@ -249,16 +281,21 @@ bool AP_Compass_HMC5843::read() } } - if (!read_raw()) { - // try again in 1 second, and set I2c clock speed slower - _retry_time = millis() + 1000; - I2c.setSpeed(false); - return false; - } + if (_accum_count == 0) { + accumulate(); + if (!healthy || _accum_count == 0) { + // try again in 1 second, and set I2c clock speed slower + _retry_time = millis() + 1000; + I2c.setSpeed(false); + return false; + } + } - mag_x *= calibration[0]; - mag_y *= calibration[1]; - mag_z *= calibration[2]; + 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; + _accum_count = 0; + _mag_x_accum = _mag_y_accum = _mag_z_accum = 0; last_update = micros(); // record time of update diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index eb4d5af101..f3c17d71b2 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -56,12 +56,22 @@ private: bool write_register(uint8_t address, byte value); uint32_t _retry_time; // when unhealthy the millis() value to retry at + int16_t _mag_x; + int16_t _mag_y; + int16_t _mag_z; + int16_t _mag_x_accum; + int16_t _mag_y_accum; + int16_t _mag_z_accum; + uint8_t _accum_count; + uint32_t _last_accum_time; + public: AP_Compass_HMC5843() : Compass() { } - virtual bool init(void); - virtual bool read(void); - virtual void set_orientation(enum Rotation rotation); + bool init(void); + bool read(void); + void accumulate(void); + void set_orientation(enum Rotation rotation); }; #endif diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 7176cd0cb4..aa2b5d8e18 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -38,6 +38,10 @@ public: /// virtual bool read(void) = 0; + /// use spare CPU cycles to accumulate values from the compass if + /// possible + virtual void accumulate(void) = 0; + /// Calculate the tilt-compensated heading_ variables. /// /// @param roll The current airframe roll angle. diff --git a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde index 1fce879409..8f97a7cc34 100644 --- a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde +++ b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde @@ -26,7 +26,7 @@ void setup() I2c.begin(); I2c.timeOut(20); - // I2c.setSpeed(true); + I2c.setSpeed(true); if (!compass.init()) { Serial.println("compass initialisation failed!"); @@ -61,6 +61,8 @@ void loop() { static float min[3], max[3], offset[3]; + compass.accumulate(); + if((micros()- timer) > 100000L) { timer = micros(); @@ -73,6 +75,7 @@ void loop() return; } heading = compass.calculate_heading(0,0); // roll = 0, pitch = 0 for this example + compass.null_offsets(); // capture min if( compass.mag_x < min[0] )