From f1befbe29ac0e080dbf91dfbaca2e3b31543e73c Mon Sep 17 00:00:00 2001 From: Koen <98043234+koen1711@users.noreply.github.com> Date: Mon, 18 Sep 2023 12:14:31 +0200 Subject: [PATCH 1/3] Delete src/LeaphyCompass.cpp --- src/LeaphyCompass.cpp | 179 ------------------------------------------ 1 file changed, 179 deletions(-) delete mode 100644 src/LeaphyCompass.cpp diff --git a/src/LeaphyCompass.cpp b/src/LeaphyCompass.cpp deleted file mode 100644 index 4142204..0000000 --- a/src/LeaphyCompass.cpp +++ /dev/null @@ -1,179 +0,0 @@ - - - - -#include "Arduino.h" -#include "QMC5883LCompass.h" -#include - -void QMC5883LCompass::init(){ - Wire.begin(); - _writeReg(0x0B,0x01); - setMode(0x01,0x0C,0x10,0X00); -} - -void QMC5883LCompass::_writeReg(byte r, byte v){ - Wire.beginTransmission(_ADDR); - Wire.write(r); - Wire.write(v); - Wire.endTransmission(); -} - - - - -void QMC5883LCompass::setMode(byte mode, byte odr, byte rng, byte osr){ - _writeReg(0x09,mode|odr|rng|osr); -} - - - - - -void QMC5883LCompass::setSmoothing(byte steps, bool adv){ - _smoothUse = true; - _smoothSteps = ( steps > 10) ? 10 : steps; - _smoothAdvanced = (adv == true) ? true : false; -} - - -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; -} - - - - -void QMC5883LCompass::read(){ - Wire.beginTransmission(_ADDR); - Wire.write(0x00); - int err = Wire.endTransmission(); - if (!err) { - 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(); - } - - - - } -} - - -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; -} - - - -void QMC5883LCompass::_smoothing(){ - byte max = 0; - byte min = 0; - - if ( _vScan > _smoothSteps - 1 ) { _vScan = 0; } - - for ( int i = 0; i < 3; i++ ) { - if ( _vTotals[i] != 0 ) { - _vTotals[i] = _vTotals[i] - _vHistory[_vScan][i]; - } - _vHistory[_vScan][i] = ( _calibrationUse ) ? _vCalibrated[i] : _vRaw[i]; - _vTotals[i] = _vTotals[i] + _vHistory[_vScan][i]; - - if ( _smoothAdvanced ) { - max = 0; - for (int j = 0; j < _smoothSteps - 1; j++) { - max = ( _vHistory[j][i] > _vHistory[max][i] ) ? j : max; - } - - min = 0; - for (int k = 0; k < _smoothSteps - 1; k++) { - min = ( _vHistory[k][i] < _vHistory[min][i] ) ? k : min; - } - - _vSmooth[i] = ( _vTotals[i] - (_vHistory[max][i] + _vHistory[min][i]) ) / (_smoothSteps - 2); - } else { - _vSmooth[i] = _vTotals[i] / _smoothSteps; - } - } - - _vScan++; -} - - - -int QMC5883LCompass::getX(){ - return _get(0); -} - - - -int QMC5883LCompass::getY(){ - return _get(1); -} - - - -int QMC5883LCompass::getZ(){ - return _get(2); -} - - -int QMC5883LCompass::_get(int i){ - if ( _smoothUse ) - return _vSmooth[i]; - - if ( _calibrationUse ) - return _vCalibrated[i]; - - return _vRaw[i]; -} - - - - -byte QMC5883LCompass::getBearing(int azimuth){ - unsigned long a = azimuth / 22.5; - unsigned long r = a - (int)a; - byte sexdec = 0; - sexdec = ( r >= .5 ) ? ceil(a) : floor(a); - return sexdec; -} - - - -void QMC5883LCompass::getDirection(char* myArray, int azimuth){ - int d = getBearing(azimuth); - myArray[0] = _bearings[d][0]; - myArray[1] = _bearings[d][1]; - myArray[2] = _bearings[d][2]; -} \ No newline at end of file From d221904cd62ea69e0867ebf82519ae4d3742cd37 Mon Sep 17 00:00:00 2001 From: Koen <98043234+koen1711@users.noreply.github.com> Date: Mon, 18 Sep 2023 12:16:22 +0200 Subject: [PATCH 2/3] Delete src/LeaphyCompass.h --- src/LeaphyCompass.h | 62 --------------------------------------------- 1 file changed, 62 deletions(-) delete mode 100644 src/LeaphyCompass.h diff --git a/src/LeaphyCompass.h b/src/LeaphyCompass.h deleted file mode 100644 index 4dc4b9c..0000000 --- a/src/LeaphyCompass.h +++ /dev/null @@ -1,62 +0,0 @@ -#ifndef QMC5883L_Compass -#define QMC5883L_Compass - -#include "Arduino.h" -#include "Wire.h" - - -class QMC5883LCompass{ - - public: - void init(); - 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 read(); - int getX(); - int getY(); - int getZ(); - byte getBearing(int azimuth); - void getDirection(char* myArray, int azimuth); - - private: - void _writeReg(byte reg,byte val); - int _get(int index); - 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(); - bool _calibrationUse = false; - int _vCalibration[3][2]; - 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'}, - }; - - - -}; - -#endif From 7adb00ce32903d4b65cc246549970962cad79043 Mon Sep 17 00:00:00 2001 From: Koen <98043234+koen1711@users.noreply.github.com> Date: Mon, 18 Sep 2023 12:18:50 +0200 Subject: [PATCH 3/3] Update library.properties --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index 5e4c477..0135787 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Leaphy Extra Extension -version=0.0.19 +version=0.0.20 author=Leaphy Robotics maintainer=Leaphy Robotics sentence=Provides Extra functionality to Leaphy robots