From 4d76185eb8663a44da152dd5498bc12343442bf8 Mon Sep 17 00:00:00 2001 From: aburt2 <32888184+aburt2@users.noreply.github.com> Date: Wed, 29 Nov 2023 10:19:57 -0500 Subject: [PATCH 1/4] added calibration parameters to puara gestures --- .vscode/settings.json | 6 +++ puara_gestures.cpp | 90 +++++++++++++++++++++++++++++++++++-------- puara_gestures.h | 35 ++++++++++++++++- 3 files changed, 115 insertions(+), 16 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..71b891d --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "files.associations": { + "*.rmd": "markdown", + "deque": "cpp" + } +} \ No newline at end of file diff --git a/puara_gestures.cpp b/puara_gestures.cpp index d14fe23..e1199ea 100644 --- a/puara_gestures.cpp +++ b/puara_gestures.cpp @@ -151,11 +151,29 @@ void PuaraGestures::updateOrientation() { orientation.update(0.01); // Weight of 0.01 towards previous orientation } +void PuaraGestures::setCalibrationParameters(calibrationParameters calParams) { + // Save calibration parameters to class + // Magnetometer Cal + sx = calParams.sx; + sy = calParams.sy; + sz = calParams.sz; + h = calParams.h; + // Accel Cal + accel_zerog = calParams.accel_zerog; + // Gyro Cal + gyro_zerorate = calParams.gyro_zerorate; +} + void PuaraGestures::setAccelerometerValues(float accelX, float accelY, float accelZ) { - orientation.setAccelerometerValues(accelX, accelY, accelZ); - this->accelX = accelX; - this->accelY = accelY; - this->accelZ = accelZ; + // Calibrate accelerometer + float accelCal[3]; + accelCal = calibrateAccelerometer(float accelX, float accelY, float accelZ); + + // Save calibrate values for sensor fusion and puara gestures + orientation.setAccelerometerValues(accelCal[0], accelCal[1], accelCal[2]); + this->accelX = accelCal[0]; + this->accelY = accelCal[1]; + this->accelZ = accelCal[2]; // Add accl data acclBuffers[0].push_back(accelX); @@ -173,28 +191,70 @@ void PuaraGestures::setAccelerometerValues(float accelX, float accelY, float acc void PuaraGestures::setGyroscopeValues(float gyroX, float gyroY, float gyroZ) { static long then = esp_timer_get_time(); long now = esp_timer_get_time(); - orientation.setGyroscopeDegreeValues(gyroX, gyroY, gyroZ, (now - then) * 0.000001); + // Calibrate Gyroscope + float gyroCal[3]; + gyroCal = calibrateGyroscope(float gyroX, float gyroY, float gyroZ); + + orientation.setGyroscopeDegreeValues(gyroCal[0], gyroCal[1], gyroCal[2], (now - then) * 0.000001); then = now; - gyroBuffers[0].push_back(gyroX); - gyroBuffers[1].push_back(gyroY); - gyroBuffers[2].push_back(gyroZ); + gyroBuffers[0].push_back(gyroCal[0]); + gyroBuffers[1].push_back(gyroCal[1]); + gyroBuffers[2].push_back(gyroCal[2]); if (gyroBuffers[0].size() > PuaraGestures::BUFFER_SIZE) { gyroBuffers[0].pop_front(); gyroBuffers[1].pop_front(); gyroBuffers[2].pop_front(); } - this->gyroX = gyroX; - this->gyroY = gyroY; - this->gyroZ = gyroZ; + this->gyroX = gyroCal[0]; + this->gyroY = gyroCal[1]; + this->gyroZ = gyroCal[2]; } void PuaraGestures::setMagnetometerValues(float magX, float magY, float magZ) { - orientation.setMagnetometerValues(magX, magY, magZ); - this->magX = magX; - this->magY = magY; - this->magZ = magZ; + // Calibrate magnetometer, sensor fusion code already assumes calibrate magnetometer, hence calibration occurs here + float magCal[3]; + magCal = calibrateMagnetometer(float magX, float magY, float magZ); + + // Set magnetometer values for sensor fusion + orientation.setMagnetometerValues(magCal[0], magCal[1], magCal[2]); + + // Save to puara gestures + this->magX = magCal[0]; + this->magY = magCal[1]; + this->magZ = magCal[2]; +} + +float PuaraGestures::calibrateMagnetometer(float magX, float magY, float magZ) { + float magCal[3] = {magX,magY,magZ}; + + // Calibrate magnetometer + magCal[0] = sx[0]*(magX-h[0]) + sx[1]*(magX-h[0]) + sx[2]*(magX-h[0]); + magCal[1] = sy[0]*(magY-h[1]) + sy[1]*(magY-h[1]) + sy[2]*(magY-h[1]); + magCal[2] = sz[0]*(magZ-h[2]) + sz[1]*(magZ-h[2]) + sz[2]*(magZ-h[2]); + return magCal; +} + +float PuaraGestures::calibrateAccelerometer(float accelX, float accelY, float accelZ) { + float accelCal[3] = {accelX,accelY,accelZ}; + + // Calibrate accelerometer + accelCal[0] -= accel_zerog[0]; + accelCal[1] -= accel_zerog[1]; + accelCal[2] -= accel_zerog[2]; + return accelCal; } +float PuaraGestures::calibrateGyroscope(float gyroX, float gyroY, float gyroZ) { + float gyroCal[3] = {gyroX,gyroY,gyroZ}; + + // Calibrate magnetometer + gyroCal[0]-= gyro_zerorate[0]; + gyroCal[1]-= gyro_zerorate[1]; + gyroCal[2]-= gyro_zerorate[2]; + return gyroCal; +} + + // Simple leaky integrator implementation // Create a unsigned long global variable for time counter for each leak implementation (timer) float PuaraGestures::leakyIntegrator (float reading, float old_value, float leak, int frequency, unsigned long& timer) { diff --git a/puara_gestures.h b/puara_gestures.h index 82db490..5f357fc 100644 --- a/puara_gestures.h +++ b/puara_gestures.h @@ -15,6 +15,21 @@ #include "esp_timer.h" #include "IMU_Sensor_Fusion/imu_orientation.h" +// Calibration parameter input structure +struct calibrationParameters { + // Accelerometer Parameters + float accel_zerog[3], + + // Gyroscope Parameters + float gyro_zerorate[3], + + // Magnetometer Parameters + float sx[3], + float sy[3], + float sz[3], + float h[3] +} + class PuaraGestures { private: @@ -133,7 +148,25 @@ class PuaraGestures { // Orientation quaternion and euler values IMU_Orientation::Quaternion getOrientationQuaternion(); IMU_Orientation::Euler getOrientationEuler(); - + + // Calibration Methods + float calibrateMagnetometer(float magX, float magY, float magZ); + float calibrateAccelerometer(float accelX, float accelY, float accelZ); + float calibrateGyroscope(float gyroX, float gyroY, float gyroZ); + void setCalibrationParameters(calibrationParameters calParams); + + // Magnetometer Calibration Variables + float sx[3] = {0.333, 0.333, 0.333}; + float sy[3] = {0.333, 0.333, 0.333}; + float sz[3] = {0.333, 0.333, 0.333}; + float h[3] = {0,0,0}; + + // Accelerometer Calibration variables + float accel_zerog[3] = {0,0,0}; + + /// Gyroscope Calibration variables + float gyro_zerorate[3] = {0,0,0}; + // touch array void updateTouchArray (int *discrete_touch, int touchSize); float touchAll; // f, 0--1 From a77f00eed61f96e7e303f9160da88e19b611a685 Mon Sep 17 00:00:00 2001 From: aburt2 <32888184+aburt2@users.noreply.github.com> Date: Wed, 29 Nov 2023 12:02:26 -0500 Subject: [PATCH 2/4] fixed syntax errors --- puara_gestures.cpp | 36 ++++++++++++------------------------ puara_gestures.h | 23 +++++++++++++---------- 2 files changed, 25 insertions(+), 34 deletions(-) diff --git a/puara_gestures.cpp b/puara_gestures.cpp index e1199ea..a9245db 100644 --- a/puara_gestures.cpp +++ b/puara_gestures.cpp @@ -154,20 +154,19 @@ void PuaraGestures::updateOrientation() { void PuaraGestures::setCalibrationParameters(calibrationParameters calParams) { // Save calibration parameters to class // Magnetometer Cal - sx = calParams.sx; - sy = calParams.sy; - sz = calParams.sz; - h = calParams.h; + std::copy(std::begin(sx), std::end(sx), std::begin(calParams.sx)); + std::copy(std::begin(sy), std::end(sy), std::begin(calParams.sy)); + std::copy(std::begin(sz), std::end(sz), std::begin(calParams.sz)); + std::copy(std::begin(h), std::end(h), std::begin(calParams.h)); // Accel Cal - accel_zerog = calParams.accel_zerog; + std::copy(std::begin(accel_zerog), std::end(accel_zerog), std::begin(calParams.accel_zerog)); // Gyro Cal - gyro_zerorate = calParams.gyro_zerorate; + std::copy(std::begin(gyro_zerorate), std::end(gyro_zerorate), std::begin(calParams.gyro_zerorate)); } void PuaraGestures::setAccelerometerValues(float accelX, float accelY, float accelZ) { // Calibrate accelerometer - float accelCal[3]; - accelCal = calibrateAccelerometer(float accelX, float accelY, float accelZ); + calibrateAccelerometer(accelX, accelY, accelZ); // Save calibrate values for sensor fusion and puara gestures orientation.setAccelerometerValues(accelCal[0], accelCal[1], accelCal[2]); @@ -192,8 +191,7 @@ void PuaraGestures::setGyroscopeValues(float gyroX, float gyroY, float gyroZ) { static long then = esp_timer_get_time(); long now = esp_timer_get_time(); // Calibrate Gyroscope - float gyroCal[3]; - gyroCal = calibrateGyroscope(float gyroX, float gyroY, float gyroZ); + calibrateGyroscope(gyroX, gyroY, gyroZ); orientation.setGyroscopeDegreeValues(gyroCal[0], gyroCal[1], gyroCal[2], (now - then) * 0.000001); then = now; @@ -212,8 +210,7 @@ void PuaraGestures::setGyroscopeValues(float gyroX, float gyroY, float gyroZ) { void PuaraGestures::setMagnetometerValues(float magX, float magY, float magZ) { // Calibrate magnetometer, sensor fusion code already assumes calibrate magnetometer, hence calibration occurs here - float magCal[3]; - magCal = calibrateMagnetometer(float magX, float magY, float magZ); + calibrateMagnetometer(magX, magY, magZ); // Set magnetometer values for sensor fusion orientation.setMagnetometerValues(magCal[0], magCal[1], magCal[2]); @@ -224,34 +221,25 @@ void PuaraGestures::setMagnetometerValues(float magX, float magY, float magZ) { this->magZ = magCal[2]; } -float PuaraGestures::calibrateMagnetometer(float magX, float magY, float magZ) { - float magCal[3] = {magX,magY,magZ}; - +void PuaraGestures::calibrateMagnetometer(float magX, float magY, float magZ) { // Calibrate magnetometer magCal[0] = sx[0]*(magX-h[0]) + sx[1]*(magX-h[0]) + sx[2]*(magX-h[0]); magCal[1] = sy[0]*(magY-h[1]) + sy[1]*(magY-h[1]) + sy[2]*(magY-h[1]); magCal[2] = sz[0]*(magZ-h[2]) + sz[1]*(magZ-h[2]) + sz[2]*(magZ-h[2]); - return magCal; } -float PuaraGestures::calibrateAccelerometer(float accelX, float accelY, float accelZ) { - float accelCal[3] = {accelX,accelY,accelZ}; - +void PuaraGestures::calibrateAccelerometer(float accelX, float accelY, float accelZ) { // Calibrate accelerometer accelCal[0] -= accel_zerog[0]; accelCal[1] -= accel_zerog[1]; accelCal[2] -= accel_zerog[2]; - return accelCal; } -float PuaraGestures::calibrateGyroscope(float gyroX, float gyroY, float gyroZ) { - float gyroCal[3] = {gyroX,gyroY,gyroZ}; - +void PuaraGestures::calibrateGyroscope(float gyroX, float gyroY, float gyroZ) { // Calibrate magnetometer gyroCal[0]-= gyro_zerorate[0]; gyroCal[1]-= gyro_zerorate[1]; gyroCal[2]-= gyro_zerorate[2]; - return gyroCal; } diff --git a/puara_gestures.h b/puara_gestures.h index 5f357fc..e4374ac 100644 --- a/puara_gestures.h +++ b/puara_gestures.h @@ -18,17 +18,17 @@ // Calibration parameter input structure struct calibrationParameters { // Accelerometer Parameters - float accel_zerog[3], + float accel_zerog[3]; // Gyroscope Parameters - float gyro_zerorate[3], + float gyro_zerorate[3]; // Magnetometer Parameters - float sx[3], - float sy[3], - float sz[3], - float h[3] -} + float sx[3]; + float sy[3]; + float sz[3]; + float h[3]; +}; class PuaraGestures { @@ -150,9 +150,9 @@ class PuaraGestures { IMU_Orientation::Euler getOrientationEuler(); // Calibration Methods - float calibrateMagnetometer(float magX, float magY, float magZ); - float calibrateAccelerometer(float accelX, float accelY, float accelZ); - float calibrateGyroscope(float gyroX, float gyroY, float gyroZ); + void calibrateMagnetometer(float magX, float magY, float magZ); + void calibrateAccelerometer(float accelX, float accelY, float accelZ); + void calibrateGyroscope(float gyroX, float gyroY, float gyroZ); void setCalibrationParameters(calibrationParameters calParams); // Magnetometer Calibration Variables @@ -160,12 +160,15 @@ class PuaraGestures { float sy[3] = {0.333, 0.333, 0.333}; float sz[3] = {0.333, 0.333, 0.333}; float h[3] = {0,0,0}; + float magCal[3]; // Accelerometer Calibration variables float accel_zerog[3] = {0,0,0}; + float accelCal[3]; /// Gyroscope Calibration variables float gyro_zerorate[3] = {0,0,0}; + float gyroCal[3]; // touch array void updateTouchArray (int *discrete_touch, int touchSize); From 6c31cd7d3b8f3ade264fc534139472327e377126 Mon Sep 17 00:00:00 2001 From: aburt2 <32888184+aburt2@users.noreply.github.com> Date: Wed, 29 Nov 2023 14:33:25 -0500 Subject: [PATCH 3/4] Fixed calibration calculation --- puara_gestures.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/puara_gestures.cpp b/puara_gestures.cpp index a9245db..84b35d3 100644 --- a/puara_gestures.cpp +++ b/puara_gestures.cpp @@ -230,16 +230,16 @@ void PuaraGestures::calibrateMagnetometer(float magX, float magY, float magZ) { void PuaraGestures::calibrateAccelerometer(float accelX, float accelY, float accelZ) { // Calibrate accelerometer - accelCal[0] -= accel_zerog[0]; - accelCal[1] -= accel_zerog[1]; - accelCal[2] -= accel_zerog[2]; + accelCal[0] = accelX - accel_zerog[0]; + accelCal[1] = accelY - accel_zerog[1]; + accelCal[2] = accelZ - accel_zerog[2]; } void PuaraGestures::calibrateGyroscope(float gyroX, float gyroY, float gyroZ) { // Calibrate magnetometer - gyroCal[0]-= gyro_zerorate[0]; - gyroCal[1]-= gyro_zerorate[1]; - gyroCal[2]-= gyro_zerorate[2]; + gyroCal[0] = gyroX - gyro_zerorate[0]; + gyroCal[1] = gyroY - gyro_zerorate[1]; + gyroCal[2] = gyroZ - gyro_zerorate[2]; } From f961d0d6737c6538cc7a6d84765a4f647151912e Mon Sep 17 00:00:00 2001 From: aburt2 <32888184+aburt2@users.noreply.github.com> Date: Wed, 29 Nov 2023 16:30:42 -0500 Subject: [PATCH 4/4] removed vscode file --- .vscode/settings.json | 6 ------ 1 file changed, 6 deletions(-) delete mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 71b891d..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "files.associations": { - "*.rmd": "markdown", - "deque": "cpp" - } -} \ No newline at end of file