diff --git a/src/QMC5883LCompass.cpp b/src/QMC5883LCompass.cpp index 489fc76..7e015d7 100644 --- a/src/QMC5883LCompass.cpp +++ b/src/QMC5883LCompass.cpp @@ -125,7 +125,7 @@ void QMC5883LCompass::setMode(byte mode, byte odr, byte rng, byte osr){ * then: setMagneticDeclination(-19, 43); */ void QMC5883LCompass::setMagneticDeclination(int degrees, uint8_t minutes) { - _magneticDeclinationDegrees = degrees + minutes / 60; + _magneticDeclinationDegrees = degrees < 0 ? 0 - (abs(degrees) + minutes / 60) : degrees + minutes / 60; } @@ -409,9 +409,18 @@ int QMC5883LCompass::_get(int i){ @return int azimuth **/ int QMC5883LCompass::getAzimuth(){ + read(); float heading = atan2( getY(), getX() ) * 180.0 / PI; + if (compassFlip){ + heading = atan2(-getY(), getX()) * 180.0 / PI;; + } heading += _magneticDeclinationDegrees; - return (int)heading % 360; + heading = heading - offsetDegrees; + // Ensure heading is in the range of 0 to 360 degrees + if (heading < 0) { + heading += 360; + } + return heading; } diff --git a/src/QMC5883LCompass.h b/src/QMC5883LCompass.h index 84c4e47..fd35363 100644 --- a/src/QMC5883LCompass.h +++ b/src/QMC5883LCompass.h @@ -9,63 +9,66 @@ class QMC5883LCompass{ public: QMC5883LCompass(); - void init(); + void init(); void setADDR(byte b); void setMode(byte mode, byte odr, byte rng, byte osr); - void setMagneticDeclination(int degrees, uint8_t minutes); - void setSmoothing(byte steps, bool adv); - void calibrate(); - void setCalibration(int x_min, int x_max, int y_min, int y_max, int z_min, int z_max); - void setCalibrationOffsets(float x_offset, float y_offset, float z_offset); - void setCalibrationScales(float x_scale, float y_scale, float z_scale); + void setMagneticDeclination(int degrees, uint8_t minutes); + void setSmoothing(byte steps, bool adv); + void calibrate(); + void setCalibration(int x_min, int x_max, int y_min, int y_max, int z_min, int z_max); + void setCalibrationOffsets(float x_offset, float y_offset, float z_offset); + void setCalibrationScales(float x_scale, float y_scale, float z_scale); float getCalibrationOffset(uint8_t index); - float getCalibrationScale(uint8_t index); - void clearCalibration(); - void setReset(); + float getCalibrationScale(uint8_t index); + void clearCalibration(); + void setReset(); void read(); - int getX(); - int getY(); - int getZ(); - int getAzimuth(); - byte getBearing(int azimuth); - void getDirection(char* myArray, int azimuth); - + int getX(); + int getY(); + int getZ(); + int getAzimuth(); + byte getBearing(int azimuth); + void getDirection(char* myArray, int azimuth); + bool compassFlip = false; + int offsetDegrees = 0; + private: void _writeReg(byte reg,byte val); - int _get(int index); - float _magneticDeclinationDegrees = 0; - bool _smoothUse = false; - byte _smoothSteps = 5; - bool _smoothAdvanced = false; - byte _ADDR = 0x0D; - int _vRaw[3] = {0,0,0}; - int _vHistory[10][3]; - int _vScan = 0; - long _vTotals[3] = {0,0,0}; - int _vSmooth[3] = {0,0,0}; - void _smoothing(); - float _offset[3] = {0.,0.,0.}; - float _scale[3] = {1.,1.,1.}; - int _vCalibrated[3]; - void _applyCalibration(); - const char _bearings[16][3] = { - {' ', ' ', 'N'}, - {'N', 'N', 'E'}, - {' ', 'N', 'E'}, - {'E', 'N', 'E'}, - {' ', ' ', 'E'}, - {'E', 'S', 'E'}, - {' ', 'S', 'E'}, - {'S', 'S', 'E'}, - {' ', ' ', 'S'}, - {'S', 'S', 'W'}, - {' ', 'S', 'W'}, - {'W', 'S', 'W'}, - {' ', ' ', 'W'}, - {'W', 'N', 'W'}, - {' ', 'N', 'W'}, - {'N', 'N', 'W'}, - }; + int _get(int index); + float _magneticDeclinationDegrees = 0; + + bool _smoothUse = false; + byte _smoothSteps = 5; + bool _smoothAdvanced = false; + byte _ADDR = 0x0D; + int _vRaw[3] = {0,0,0}; + int _vHistory[10][3]; + int _vScan = 0; + long _vTotals[3] = {0,0,0}; + int _vSmooth[3] = {0,0,0}; + void _smoothing(); + float _offset[3] = {0.,0.,0.}; + float _scale[3] = {1.,1.,1.}; + int _vCalibrated[3]; + void _applyCalibration(); + const char _bearings[16][3] = { + {' ', ' ', 'N'}, + {'N', 'N', 'E'}, + {' ', 'N', 'E'}, + {'E', 'N', 'E'}, + {' ', ' ', 'E'}, + {'E', 'S', 'E'}, + {' ', 'S', 'E'}, + {'S', 'S', 'E'}, + {' ', ' ', 'S'}, + {'S', 'S', 'W'}, + {' ', 'S', 'W'}, + {'W', 'S', 'W'}, + {' ', ' ', 'W'}, + {'W', 'N', 'W'}, + {' ', 'N', 'W'}, + {'N', 'N', 'W'}, + };