Merge pull request mprograms#2 from TheNitek/master

Add calibration functions

examples/calibration/calibration.ino

@@ -0,0 +1,83 @@
+/*
+===============================================================================================================
+QMC5883LCompass.h Library XYZ Example Sketch
+Learn more at [https://github.com/mprograms/QMC5883LCompass]
+
+Generate calibration values for your QMC5883L
+
+===============================================================================================================
+Release under the GNU General Public License v3
+[https://www.gnu.org/licenses/gpl-3.0.en.html]
+===============================================================================================================
+*/
+#include <QMC5883LCompass.h>
+
+QMC5883LCompass compass;
+
+int calibrationData[3][2];
+bool changed = false;
+
+void setup() {
+  Serial.begin(115200);
+  compass.init();
+
+  Serial.println("Slowly move your magnetometer in all directions until the output does not change anymore");
+  Serial.println("Afterwards just copy the most recent output line into your sketch to use calibration data");
+}
+
+void loop() {
+  int x, y, z;
+
+  // Read compass values
+  compass.read();
+
+  // Return XYZ readings
+  x = compass.getX();
+  y = compass.getY();
+  z = compass.getZ();
+
+  changed = false;
+
+  if(x < calibrationData[0][0]) {
+    calibrationData[0][0] = x;
+    changed = true;
+  }
+  if(x > calibrationData[0][1]) {
+    calibrationData[0][1] = x;
+    changed = true;
+  }
+
+  if(y < calibrationData[1][0]) {
+    calibrationData[1][0] = y;
+    changed = true;
+  }
+  if(y > calibrationData[1][1]) {
+    calibrationData[1][1] = y;
+    changed = true;
+  }
+
+  if(z < calibrationData[2][0]) {
+    calibrationData[2][0] = z;
+    changed = true;
+  }
+  if(z > calibrationData[2][1]) {
+    calibrationData[2][1] = z;
+    changed = true;
+  }
+
+  if(changed) {  
+    Serial.print("compass.setCalibration(");
+    Serial.print(calibrationData[0][0]);
+    Serial.print(", ");
+    Serial.print(calibrationData[0][1]);
+    Serial.print(", ");
+    Serial.print(calibrationData[1][0]);
+    Serial.print(", ");
+    Serial.print(calibrationData[1][1]);
+    Serial.print(", ");
+    Serial.print(calibrationData[2][0]);
+    Serial.print(", ");
+    Serial.print(calibrationData[2][1]);
+    Serial.println(");");
+  }
+}

examples/xyz/xyz.ino

@@ -1,7 +1,7 @@
 /*
 ===============================================================================================================
 QMC5883LCompass.h Library XYZ Example Sketch
-Learn more at [https://github.com/mprograms/QMC5883Compas]
+Learn more at [https://github.com/mprograms/QMC5883LCompass]
 
 This example shows how to get the XYZ values from the sensor.
 

readme.md

@@ -208,7 +208,7 @@ void setup(){
 ```
 #### Change Mode, Data Rate, Scale, Sample Ratio
 
-You can also change the mode, sensitivity, sample rate and output rate of the QMC5583L chip. To do this, simply call `compass.setMode(MODE, ODR, RNG, OSR)l` after you have called `compass.init()`. Note that each value must be a byte.
+You can also change the mode, sensitivity, sample rate and output rate of the QMC5583L chip. To do this, simply call `compass.setMode(MODE, ODR, RNG, OSR);` after you have called `compass.init()`. Note that each value must be a byte.
 
 The values to set each mode are in the table below and were taken from the [QST QMC5583L datasheet](https://nettigo.pl/attachments/440).
 
@@ -238,6 +238,11 @@ The values to set each mode are in the table below and were taken from the [QST
 | 512			          | 0x00  |
 
 ---
+
+## Calibrating The Sensor
+
+It is possible to set calibration values specific for your QMC5883L to improve the quality of the readings. Just call `compass.setCalibration(X_MIN, X_MAX, Y_MIN, Y_MAX, Z_MIN, Z_MAX);` once. To find the values specific for your sensor flash the calibration example and watch the output on the serial monitor.
+
 ## Smoothing Sensor Output
 
 Smoothing can help in cases where sensor readings seem to bounce around. QMC5883L Compass Library uses a rolling average function to store (n) sensor readings and return the average of each axis. This averaging also places smoothing on azimuth and directional output as well.
@@ -255,7 +260,7 @@ To enable smoothing call `compass.setSmoothing(STEPS, ADVANCED);` before the loo
 ```
 void setup(){
   compass.init();
-  setSmoothing(10, true);
+  compass.setSmoothing(10, true);
 }
 ```
 

src/QMC5883LCompass.cpp

@@ -132,6 +132,21 @@ void QMC5883LCompass::setSmoothing(byte steps, bool adv){
 	_smoothAdvanced = (adv == true) ? true : false;
 }
 
+/**
+    SET CALIBRATION
+	Set calibration values for more accurate readings
+**/
+void QMC5883LCompass::setCalibration(int x_min, int x_max, int y_min, int y_max, int z_min, int z_max){
+	_calibrationUse = true;
+
+	_vCalibration[0][0] = x_min;
+	_vCalibration[0][1] = x_max;
+	_vCalibration[1][0] = y_min;
+	_vCalibration[1][1] = y_max;
+	_vCalibration[2][0] = z_min;
+	_vCalibration[2][1] = z_max;
+}
+
 
 /**
 	READ
@@ -144,20 +159,52 @@ void QMC5883LCompass::read(){
 	Wire.write(0x00);
 	int err = Wire.endTransmission();
 	if (!err) {
-		Wire.requestFrom(_ADDR, (byte)7);
+		Wire.requestFrom(_ADDR, (byte)6);
 		_vRaw[0] = (int)(int16_t)(Wire.read() | Wire.read() << 8);
 		_vRaw[1] = (int)(int16_t)(Wire.read() | Wire.read() << 8);
 		_vRaw[2] = (int)(int16_t)(Wire.read() | Wire.read() << 8);
+
+		if ( _calibrationUse ) {
+			_applyCalibration();
+		}
 
 		if ( _smoothUse ) {
 			_smoothing();
 		}
 
-		byte overflow = Wire.read() & 0x02;
+		//byte overflow = Wire.read() & 0x02;
 		//return overflow << 2;
 	}
 }
 
+/**
+    APPLY CALIBRATION
+	This function uses the calibration data provided via @see setCalibration() to calculate more
+	accurate readings
+	
+
+	Based on this awesome article:
+	https://appelsiini.net/2018/calibrate-magnetometer/
+**/
+void QMC5883LCompass::_applyCalibration(){
+	int x_offset = (_vCalibration[0][0] + _vCalibration[0][1])/2;
+	int y_offset = (_vCalibration[1][0] + _vCalibration[1][1])/2;
+	int z_offset = (_vCalibration[2][0] + _vCalibration[2][1])/2;
+	int x_avg_delta = (_vCalibration[0][1] - _vCalibration[0][0])/2;
+	int y_avg_delta = (_vCalibration[1][1] - _vCalibration[1][0])/2;
+	int z_avg_delta = (_vCalibration[2][1] - _vCalibration[2][0])/2;
+
+	int avg_delta = (x_avg_delta + y_avg_delta + z_avg_delta) / 3;
+
+	float x_scale = (float)avg_delta / x_avg_delta;
+	float y_scale = (float)avg_delta / y_avg_delta;
+	float z_scale = (float)avg_delta / z_avg_delta;
+
+	_vCalibrated[0] = (_vRaw[0] - x_offset) * x_scale;
+	_vCalibrated[1] = (_vRaw[1] - y_offset) * y_scale;
+	_vCalibrated[2] = (_vRaw[2] - z_offset) * z_scale;
+}
+
 
 /**
 	SMOOTH OUTPUT
@@ -187,7 +234,7 @@ void QMC5883LCompass::_smoothing(){
 		if ( _vTotals[i] != 0 ) {
 			_vTotals[i] = _vTotals[i] - _vHistory[_vScan][i];
 		}
-		_vHistory[_vScan][i] = _vRaw[i];
+		_vHistory[_vScan][i] = ( _calibrationUse ) ? _vCalibrated[i] : _vRaw[i];
 		_vTotals[i] = _vTotals[i] + _vHistory[_vScan][i];
 
 		if ( _smoothAdvanced ) {
@@ -219,7 +266,7 @@ void QMC5883LCompass::_smoothing(){
 	@return int x axis
 **/
 int QMC5883LCompass::getX(){
-	return ( _smoothUse ) ? _vSmooth[0] : _vRaw[0];
+	return _get(0);
 }
 
 
@@ -231,7 +278,7 @@ int QMC5883LCompass::getX(){
 	@return int y axis
 **/
 int QMC5883LCompass::getY(){
-	return ( _smoothUse ) ? _vSmooth[1] : _vRaw[1];
+	return _get(1);
 }
 
 
@@ -243,10 +290,21 @@ int QMC5883LCompass::getY(){
 	@return int z axis
 **/
 int QMC5883LCompass::getZ(){
-	return ( _smoothUse ) ? _vSmooth[2] : _vRaw[2];
+	return _get(2);
+}
+
+int QMC5883LCompass::_get(int index){
+	if ( _smoothUse ) 
+		return _vSmooth[index];
+
+	if ( _calibrationUse )
+		return _vCalibrated[index];
+
+	return _vRaw[index];
 }
 
 
+
 /**
 	GET AZIMUTH
 	Calculate the azimuth (in degrees);

src/QMC5883LCompass.h

@@ -13,6 +13,7 @@ class QMC5883LCompass{
     void setADDR(byte b);
     void setMode(byte mode, byte odr, byte rng, byte osr);
 	void setSmoothing(byte steps, bool adv);
+	void setCalibration(int x_min, int x_max, int y_min, int y_max, int z_min, int z_max);
     void setReset();
     void read();
 	int getX();
@@ -24,6 +25,7 @@ class QMC5883LCompass{
 
   private:
     void _writeReg(byte reg,byte val);
+	int _get(int index);
 	bool _smoothUse = false;
 	byte _smoothSteps = 5;
 	bool _smoothAdvanced = false;
@@ -34,6 +36,10 @@ class QMC5883LCompass{
 	long _vTotals[3] = {0,0,0};
 	int _vSmooth[3] = {0,0,0};
 	void _smoothing();
+	bool _calibrationUse = false;
+	int _vCalibration[3][2];
+	int _vCalibrated[3];
+	void _applyCalibration();
 	const char _bearings[16][3] =  {
 		{' ', ' ', 'N'},
 		{'N', 'N', 'E'},