From dd52f55656811b0aa22269b0b1de728b9bd025b8 Mon Sep 17 00:00:00 2001 From: LuckyBor11 <120314028+LuckyBor11@users.noreply.github.com> Date: Thu, 6 Jun 2024 15:21:45 +0300 Subject: [PATCH 1/4] Added --- examples/Calibrated_relativity_ESP32 | 195 +++++++++++++++++++++++++++ 1 file changed, 195 insertions(+) create mode 100644 examples/Calibrated_relativity_ESP32 diff --git a/examples/Calibrated_relativity_ESP32 b/examples/Calibrated_relativity_ESP32 new file mode 100644 index 0000000..3d5a0ea --- /dev/null +++ b/examples/Calibrated_relativity_ESP32 @@ -0,0 +1,195 @@ +#include "FastIMU.h" +#include "Madgwick.h" +#include "EEPROM.h" +#include +#include +#include +#include +#include + +//This example is for use with the Relativty steamvr driver. it outputs a rotation quaternion over HID that the driver can interpret as HMD rotation. + +#define IMU_ADDRESS 0x68 // Change to the address of the IMU +MPU6500 IMU; // Change to the name of any supported IMU! + +#define IMU_GEOMETRY 0 // Change to your current IMU geometry (check docs for a reference pic). + +// Currently supported IMUS: MPU9255 MPU9250 MPU6886 MPU6500 MPU6050 ICM20689 ICM20690 BMI055 BMX055 BMI160 LSM6DS3 LSM6DSL QMI8658 + +#define FILTER_MAX_BETA 0.15 +#define FILTER_MIN_BETA 0.015 +#define FILTER_DROPOFF 0.85 // filter values + +calData calib = { 0 }; // Calibration data +AccelData IMUAccel; // Sensor data +GyroData IMUGyro; +MagData IMUMag; + +GyroData GyroVel; // used for angular velocity based filter beta + +Madgwick filter; +bool flag; + +const uint8_t customHidReportDescriptor[] = { + 0x06, 0x03, 0x00, // USAGE_PAGE (vendor defined) + 0x09, 0x00, // USAGE (Undefined) + 0xa1, 0x01, // COLLECTION (Application) + 0x15, 0x00, // LOGICAL_MINIMUM (0) + 0x26, 0xff, 0x00, // LOGICAL_MAXIMUM (255) + 0x85, 0x01, // REPORT_ID (1) + 0x75, 0x08, // REPORT_SIZE (8) + 0x95, 0x3f, // REPORT_COUNT (63) + 0x09, 0x00, // USAGE (Undefined) + 0x81, 0x02, // INPUT (Data,Var,Abs) - to the host + 0xc0 +}; + +BLEHIDDevice* hid; +BLECharacteristic* inputReport; + +float quat[4]; + +void setup() { + Wire.begin(35, 36); + Wire.setClock(400000); // 400kHz clock + + int calStatus = 0; + Serial.begin(115200); + + BLEDevice::init("RelativityVR"); + BLEServer* pServer = BLEDevice::createServer(); + hid = new BLEHIDDevice(pServer); + + inputReport = hid->inputReport(1); // Report ID + + hid->manufacturer()->setValue("Relativity"); + hid->pnp(0x01, 0x02E5, 0xABCD, 0x0110); + hid->hidInfo(0x00, 0x01); + + hid->reportMap((uint8_t*)customHidReportDescriptor, sizeof(customHidReportDescriptor)); + hid->startServices(); + + BLESecurity* pSecurity = new BLESecurity(); + pSecurity->setAuthenticationMode(ESP_LE_AUTH_BOND); + + BLEAdvertising* pAdvertising = pServer->getAdvertising(); + pAdvertising->setAppearance(HID_KEYBOARD); + pAdvertising->addServiceUUID(hid->hidService()->getUUID()); + pAdvertising->start(); + + hid->setBatteryLevel(100); + + EEPROM.get(100, calStatus); + if (calStatus == 99) { + EEPROM.get(200, calib); + } + IMU.setIMUGeometry(IMU_GEOMETRY); + int err = IMU.init(calib, IMU_ADDRESS); + if (err != 0) { + Serial.print("Error initializing IMU! e:"); + Serial.println(err); + } + + filter.begin(2.f); // warm up filter before use + for (int i = 0; i < 2000; i++) { + IMU.update(); + IMU.getAccel(&IMUAccel); + IMU.getGyro(&IMUGyro); + if (IMU.hasMagnetometer()) { + IMU.getMag(&IMUMag); + filter.update(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ, IMUMag.magX, IMUMag.magY, IMUMag.magZ); + } else { + filter.updateIMU(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ); + } + } +} + +void loop() { + if (Serial) { + if (!flag) { + Serial.println("Serial monitor open, do you want to enter calibration mode? (y/n)"); + } + flag = true; + if (Serial.read() == 'y') { + calib = { 0 }; //this looks important + IMU.init(calib, IMU_ADDRESS); + Serial.println("Calibrating IMU... Keep headset still on a flat and level surface..."); + delay(10000); + IMU.calibrateAccelGyro(&calib); + IMU.init(calib, IMU_ADDRESS); + Serial.println("Accelerometer and Gyroscope calibrated!"); + if (IMU.hasMagnetometer()) { + delay(1000); + Serial.println("Magnetometer calibration: move IMU in figure 8 pattern until done."); + delay(5000); + IMU.calibrateMag(&calib); + Serial.println("Magnetic calibration done!"); + } + Serial.println("IMU Calibration complete!"); + Serial.println("Accel biases X/Y/Z: "); + Serial.print(calib.accelBias[0]); + Serial.print(", "); + Serial.print(calib.accelBias[1]); + Serial.print(", "); + Serial.println(calib.accelBias[2]); + Serial.println("Gyro biases X/Y/Z: "); + Serial.print(calib.gyroBias[0]); + Serial.print(", "); + Serial.print(calib.gyroBias[1]); + Serial.print(", "); + Serial.println(calib.gyroBias[2]); + if (IMU.hasMagnetometer()) { + Serial.println("Mag biases X/Y/Z: "); + Serial.print(calib.magBias[0]); + Serial.print(", "); + Serial.print(calib.magBias[1]); + Serial.print(", "); + Serial.println(calib.magBias[2]); + Serial.println("Mag Scale X/Y/Z: "); + Serial.print(calib.magScale[0]); + Serial.print(", "); + Serial.print(calib.magScale[1]); + Serial.print(", "); + Serial.println(calib.magScale[2]); + } + + Serial.println("Saving Calibration values to EEPROM!"); + EEPROM.put(200, calib); + EEPROM.put(100, 99); + delay(1000); + Serial.println("Please remember to set hmdIMUdmpPackets to false in the driver settings."); + Serial.println("You can now close the Serial monitor."); + delay(5000); + } + } + IMU.update(); + IMU.getAccel(&IMUAccel); + IMU.getGyro(&IMUGyro); + + float Av = GyroVel.gyroX * GyroVel.gyroX + GyroVel.gyroY * GyroVel.gyroY + GyroVel.gyroZ * GyroVel.gyroZ; //sqr magnitude + if (Av > 100.f) Av = 100.f; + filter.changeBeta(Av * (FILTER_MAX_BETA - FILTER_MIN_BETA) / 100 + FILTER_MIN_BETA); //some stuff + + if (IMU.hasMagnetometer()) { + IMU.getMag(&IMUMag); + filter.update(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ, IMUMag.magX, IMUMag.magY, IMUMag.magZ); + } + else { + filter.updateIMU(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ); + } + + GyroVel.gyroX += IMUGyro.gyroX * filter.delta_t; + GyroVel.gyroY += IMUGyro.gyroY * filter.delta_t; + GyroVel.gyroZ += IMUGyro.gyroZ * filter.delta_t; + GyroVel.gyroX *= FILTER_DROPOFF; + GyroVel.gyroY *= FILTER_DROPOFF; + GyroVel.gyroZ *= FILTER_DROPOFF; //velocity calculations and dropoff... + + quat[0] = filter.getQuatW(); + quat[1] = filter.getQuatY(); + quat[2] = filter.getQuatZ(); + quat[3] = filter.getQuatX(); + +inputReport->setValue((uint8_t*)quat, 63); +inputReport->notify(); +} From 3fef5a76aeb18d1bdebe6fe01214c8ccea5d25a1 Mon Sep 17 00:00:00 2001 From: LuckyBor11 <120314028+LuckyBor11@users.noreply.github.com> Date: Thu, 6 Jun 2024 15:35:24 +0300 Subject: [PATCH 2/4] added bluetooth hid support for the esp32 and the esp32 s3 it'll probably work with an esp8266 idk --- .../Calibrated_relativty.ino} | 0 .../Calibrated_relativty_ESP32/Madgwick.cpp | 249 ++++++++++++++++++ .../Calibrated_relativty_ESP32/Madgwick.h | 64 +++++ 3 files changed, 313 insertions(+) rename examples/{Calibrated_relativity_ESP32 => Calibrated_relativty_ESP32/Calibrated_relativty.ino} (100%) create mode 100644 examples/Calibrated_relativty_ESP32/Madgwick.cpp create mode 100644 examples/Calibrated_relativty_ESP32/Madgwick.h diff --git a/examples/Calibrated_relativity_ESP32 b/examples/Calibrated_relativty_ESP32/Calibrated_relativty.ino similarity index 100% rename from examples/Calibrated_relativity_ESP32 rename to examples/Calibrated_relativty_ESP32/Calibrated_relativty.ino diff --git a/examples/Calibrated_relativty_ESP32/Madgwick.cpp b/examples/Calibrated_relativty_ESP32/Madgwick.cpp new file mode 100644 index 0000000..96dfdab --- /dev/null +++ b/examples/Calibrated_relativty_ESP32/Madgwick.cpp @@ -0,0 +1,249 @@ +//============================================================================================= +// Madgwick.c +//============================================================================================= +// +// Implementation of Madgwick's IMU and AHRS algorithms. +// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ +// +// From the x-io website "Open-source resources available on this website are +// provided under the GNU General Public Licence unless an alternative licence +// is provided in source." +// +// Date Author Notes +// 29/09/2011 SOH Madgwick Initial release +// 02/10/2011 SOH Madgwick Optimised for reduced CPU load +// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised +// +//============================================================================================= + +//------------------------------------------------------------------------------------------- +// Header files + +#include "Madgwick.h" +#include +#include +#include + +//------------------------------------------------------------------------------------------- +// Definitions + +#define sampleFreqDef 512.0f // sample frequency in Hz + + +//============================================================================================ +// Functions + +//------------------------------------------------------------------------------------------- +// AHRS algorithm update + +Madgwick::Madgwick() { + q0 = 1.0f; + q1 = 0.0f; + q2 = 0.0f; + q3 = 0.0f; + now = micros(); +} + +void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float hx, hy; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + + // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) + if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + updateIMU(gx, gy, gz, ax, ay, az); + return; + } + + now = micros(); + // Set integration time by time elapsed since last filter update + delta_t = ((now - last_update) / (float)1000000.0f); + last_update = now; + + // Convert gyroscope degrees/sec to radians/sec + gx *= (float)0.0174533f; + gy *= (float)0.0174533f; + gz *= (float)0.0174533f; + + // Rate of change of quaternion from gyroscope + qDot1 = (float)0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = (float)0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = (float)0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = (float)0.5f * (q0 * gz + q1 * gy - q2 * gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == (float)0.0f) && (ay == (float)0.0f) && (az == (float)0.0f))) { + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Normalise magnetometer measurement + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0mx = (float)2.0f * q0 * mx; + _2q0my = (float)2.0f * q0 * my; + _2q0mz = (float)2.0f * q0 * mz; + _2q1mx = (float)2.0f * q1 * mx; + _2q0 = (float)2.0f * q0; + _2q1 = (float)2.0f * q1; + _2q2 = (float)2.0f * q2; + _2q3 = (float)2.0f * q3; + _2q0q2 = (float)2.0f * q0 * q2; + _2q2q3 = (float)2.0f * q2 * q3; + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; + + // Reference direction of Earth's magnetic field + hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; + hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; + _2bx = sqrtf(hx * hx + hy * hy); + _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; + _4bx = (float)2.0f * _2bx; + _4bz = (float)2.0f * _2bz; + + // Gradient decent algorithm corrective step + s0 = -_2q2 * ((float)2.0f * q1q3 - _2q0q2 - ax) + _2q1 * ((float)2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * ((float)0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * ((float)0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * ((float)2.0f * q1q3 - _2q0q2 - ax) + _2q0 * ((float)2.0f * q0q1 + _2q2q3 - ay) - (float)4.0f * q1 * (1 - (float)2.0f * q1q1 - (float)2.0f * q2q2 - az) + _2bz * q3 * (_2bx * ((float)0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * ((float)0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * ((float)2.0f * q1q3 - _2q0q2 - ax) + _2q3 * ((float)2.0f * q0q1 + _2q2q3 - ay) - (float)4.0f * q2 * (1 - (float)2.0f * q1q1 - (float)2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * ((float)0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * ((float)0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * ((float)2.0f * q1q3 - _2q0q2 - ax) + _2q2 * ((float)2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * ((float)0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * ((float)0.5f - q1q1 - q2q2) - mz); + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * delta_t; + q1 += qDot2 * delta_t; + q2 += qDot3 * delta_t; + q3 += qDot4 * delta_t; + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; +} + +//------------------------------------------------------------------------------------------- +// IMU algorithm update + +void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) { + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + + // Convert gyroscope degrees/sec to radians/sec + gx *= (float)0.0174533f; + gy *= (float)0.0174533f; + gz *= (float)0.0174533f; + + now = micros(); + // Set integration time by time elapsed since last filter update + delta_t = ((now - last_update) / (float)1000000.0f); + last_update = now; + + // Rate of change of quaternion from gyroscope + qDot1 = (float)0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = (float)0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = (float)0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = (float)0.5f * (q0 * gz + q1 * gy - q2 * gx); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + _2q0 = (float)2.0f * q0; + _2q1 = (float)2.0f * q1; + _2q2 = (float)2.0f * q2; + _2q3 = (float)2.0f * q3; + _4q0 = (float)4.0f * q0; + _4q1 = (float)4.0f * q1; + _4q2 = (float)4.0f * q2; + _8q1 = (float)8.0f * q1; + _8q2 = (float)8.0f * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + // Gradient decent algorithm corrective step + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = (float)4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = (float)4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + // Apply feedback step + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + q0 += qDot1 * delta_t; + q1 += qDot2 * delta_t; + q2 += qDot3 * delta_t; + q3 += qDot4 * delta_t; + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; +} + +//------------------------------------------------------------------------------------------- +// Fast inverse square-root +// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root + +float Madgwick::invSqrt(float x) { + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i>>1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + y = y * (1.5f - (halfx * y * y)); + return y; +} + +//------------------------------------------------------------------------------------------- diff --git a/examples/Calibrated_relativty_ESP32/Madgwick.h b/examples/Calibrated_relativty_ESP32/Madgwick.h new file mode 100644 index 0000000..32b7149 --- /dev/null +++ b/examples/Calibrated_relativty_ESP32/Madgwick.h @@ -0,0 +1,64 @@ +//============================================================================================= +// Madgwick.h +//============================================================================================= +// +// Implementation of Madgwick's IMU and AHRS algorithms. +// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ +// +// From the x-io website "Open-source resources available on this website are +// provided under the GNU General Public Licence unless an alternative licence +// is provided in source." +// +// Date Author Notes +// 29/09/2011 SOH Madgwick Initial release +// 02/10/2011 SOH Madgwick Optimised for reduced CPU load +// +//============================================================================================= +#ifndef __Madgwick_h__ +#define __Madgwick_h__ +#include +#include + +//-------------------------------------------------------------------------------------------- +// Variable declaration +class Madgwick { + private: + + uint32_t now = 0; // used to calculate integration interval + uint32_t last_update = 0; // used to calculate integration interval + static float invSqrt(float x); + float beta; // algorithm gain + float q0; + float q1; + float q2; + float q3; // quaternion of sensor frame relative to auxiliary frame + + + //------------------------------------------------------------------------------------------- + // Function declarations + public: + float delta_t = 0; // Used to control display output rate + Madgwick(void); + void changeBeta(float newBeta) { beta = newBeta; } + void begin(float confBeta) { beta = confBeta; } + void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); + void updateIMU(float gx, float gy, float gz, float ax, float ay, float az); + + float getQuatW() { + return q0; + } + + float getQuatX() { + return q1; + } + + float getQuatY() { + return q2; + } + + float getQuatZ() { + return q3; + } + +}; +#endif From 8b91313404af3a97def1fa39085ab05fc03de8cf Mon Sep 17 00:00:00 2001 From: LuckyBor11 <120314028+LuckyBor11@users.noreply.github.com> Date: Thu, 6 Jun 2024 15:38:49 +0300 Subject: [PATCH 3/4] changed name from relativity to relativity esp32 --- .../{Calibrated_relativty.ino => Calibrated_relativty_ESP32.ino} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename examples/Calibrated_relativty_ESP32/{Calibrated_relativty.ino => Calibrated_relativty_ESP32.ino} (100%) diff --git a/examples/Calibrated_relativty_ESP32/Calibrated_relativty.ino b/examples/Calibrated_relativty_ESP32/Calibrated_relativty_ESP32.ino similarity index 100% rename from examples/Calibrated_relativty_ESP32/Calibrated_relativty.ino rename to examples/Calibrated_relativty_ESP32/Calibrated_relativty_ESP32.ino From 5705f3d34567ceb29447722e06e65d8da6b0f20e Mon Sep 17 00:00:00 2001 From: LuckyBor11 <120314028+LuckyBor11@users.noreply.github.com> Date: Thu, 6 Jun 2024 15:48:08 +0300 Subject: [PATCH 4/4] Added support for ESP32 i2c pins --- .../Calibrated_relativty_ESP32/Calibrated_relativty_ESP32.ino | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/examples/Calibrated_relativty_ESP32/Calibrated_relativty_ESP32.ino b/examples/Calibrated_relativty_ESP32/Calibrated_relativty_ESP32.ino index 3d5a0ea..0b079bc 100644 --- a/examples/Calibrated_relativty_ESP32/Calibrated_relativty_ESP32.ino +++ b/examples/Calibrated_relativty_ESP32/Calibrated_relativty_ESP32.ino @@ -50,7 +50,11 @@ BLECharacteristic* inputReport; float quat[4]; void setup() { + #if defined(ARDUINO_ARCH_ESP32) + Wire.begin(21, 22); + #else Wire.begin(35, 36); + #endif Wire.setClock(400000); // 400kHz clock int calStatus = 0;