From 6fd135a2b8e0310b46f594df80047c7ea157a1e6 Mon Sep 17 00:00:00 2001 From: xeonqq Date: Fri, 28 Jul 2023 15:09:38 +0200 Subject: [PATCH] Delete quaternion related code The code is not used, since MPU6050_6Axis_MotionApps20.h and MPU6050_9Axis_MotionApps41.h due to deps to freeRTOS are not included --- Sming/Libraries/MPU6050/MPU6050.cpp | 1 - Sming/Libraries/MPU6050/MPU6050.h | 205 -------------------- Sming/Libraries/MPU6050/helper_3dmath.h | 243 ------------------------ 3 files changed, 449 deletions(-) delete mode 100644 Sming/Libraries/MPU6050/helper_3dmath.h diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp index 5d174ab7a1..d6cf680d01 100644 --- a/Sming/Libraries/MPU6050/MPU6050.cpp +++ b/Sming/Libraries/MPU6050/MPU6050.cpp @@ -3221,7 +3221,6 @@ void MPU6050::setIntDMPEnabled(bool enabled) } // DMP_INT_STATUS - bool MPU6050::getDMPInt5Status() { I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer); diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h index 0e0bda8f62..bba931d1d3 100644 --- a/Sming/Libraries/MPU6050/MPU6050.h +++ b/Sming/Libraries/MPU6050/MPU6050.h @@ -38,7 +38,6 @@ THE SOFTWARE. #ifndef _MPU6050_H_ #define _MPU6050_H_ -#include "helper_3dmath.h" #include // supporting link: @@ -54,7 +53,6 @@ THE SOFTWARE. //#define pgm_read_float(x) (*(x)) //#define PSTR(STR) STR -#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 #define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) #define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW @@ -827,209 +825,6 @@ class MPU6050 uint8_t getDMPConfig2(); void setDMPConfig2(uint8_t config); -// special methods for MotionApps 2.0 implementation -#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20 - uint8_t* dmpPacketBuffer; - uint16_t dmpPacketSize; - - uint8_t dmpInitialize(); - bool dmpPacketAvailable(); - - uint8_t dmpSetFIFORate(uint8_t fifoRate); - uint8_t dmpGetFIFORate(); - uint8_t dmpGetSampleStepSizeMS(); - uint8_t dmpGetSampleFrequency(); - int32_t dmpDecodeTemperature(int8_t tempReg); - - // Register callbacks after a packet of FIFO data is processed - // uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); - // uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); - uint8_t dmpRunFIFORateProcesses(); - - // Setup FIFO for various output - uint8_t dmpSendQuaternion(uint_fast16_t accuracy); - uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); - uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - - // Get Fixed Point data from FIFO - uint8_t dmpGetAccel(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetAccel(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetAccel(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetQuaternion(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet = 0); - uint8_t dmpGet6AxisQuaternion(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGet6AxisQuaternion(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGet6AxisQuaternion(Quaternion* q, const uint8_t* packet = 0); - uint8_t dmpGetRelativeQuaternion(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetRelativeQuaternion(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetRelativeQuaternion(Quaternion* data, const uint8_t* packet = 0); - uint8_t dmpGetGyro(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGyro(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGyro(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpSetLinearAccelFilterCoefficient(float coef); - uint8_t dmpGetLinearAccel(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetLinearAccel(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetLinearAccel(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetLinearAccel(VectorInt16* v, VectorInt16* vRaw, VectorFloat* gravity); - uint8_t dmpGetLinearAccelInWorld(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetLinearAccelInWorld(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16* v, VectorInt16* vReal, Quaternion* q); - uint8_t dmpGetGyroAndAccelSensor(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGyroAndAccelSensor(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGyroAndAccelSensor(VectorInt16* g, VectorInt16* a, const uint8_t* packet = 0); - uint8_t dmpGetGyroSensor(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGyroSensor(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGyroSensor(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetControlData(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetTemperature(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGravity(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGravity(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGravity(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q); - uint8_t dmpGetUnquantizedAccel(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetUnquantizedAccel(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetUnquantizedAccel(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetQuantizedAccel(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetQuantizedAccel(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetQuantizedAccel(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetExternalSensorData(int32_t* data, uint16_t size, const uint8_t* packet = 0); - uint8_t dmpGetEIS(int32_t* data, const uint8_t* packet = 0); - - uint8_t dmpGetEuler(float* data, Quaternion* q); - uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity); - - // Get Floating Point data from FIFO - uint8_t dmpGetAccelFloat(float* data, const uint8_t* packet = 0); - uint8_t dmpGetQuaternionFloat(float* data, const uint8_t* packet = 0); - - uint8_t dmpProcessFIFOPacket(const unsigned char* dmpData); - uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t* processed = NULL); - - uint8_t dmpSetFIFOProcessedCallback(void (*func)(void)); - - uint8_t dmpInitFIFOParam(); - uint8_t dmpCloseFIFO(); - uint8_t dmpSetGyroDataSource(uint8_t source); - uint8_t dmpDecodeQuantizedAccel(); - uint32_t dmpGetGyroSumOfSquare(); - uint32_t dmpGetAccelSumOfSquare(); - void dmpOverrideQuaternion(long* q); - uint16_t dmpGetFIFOPacketSize(); -#endif - -// special methods for MotionApps 4.1 implementation -#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41 - uint8_t* dmpPacketBuffer; - uint16_t dmpPacketSize; - - uint8_t dmpInitialize(); - bool dmpPacketAvailable(); - - uint8_t dmpSetFIFORate(uint8_t fifoRate); - uint8_t dmpGetFIFORate(); - uint8_t dmpGetSampleStepSizeMS(); - uint8_t dmpGetSampleFrequency(); - int32_t dmpDecodeTemperature(int8_t tempReg); - - // Register callbacks after a packet of FIFO data is processed - // uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); - // uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); - uint8_t dmpRunFIFORateProcesses(); - - // Setup FIFO for various output - uint8_t dmpSendQuaternion(uint_fast16_t accuracy); - uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); - uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); - uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - - // Get Fixed Point data from FIFO - uint8_t dmpGetAccel(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetAccel(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetAccel(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetQuaternion(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet = 0); - uint8_t dmpGet6AxisQuaternion(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGet6AxisQuaternion(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGet6AxisQuaternion(Quaternion* q, const uint8_t* packet = 0); - uint8_t dmpGetRelativeQuaternion(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetRelativeQuaternion(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetRelativeQuaternion(Quaternion* data, const uint8_t* packet = 0); - uint8_t dmpGetGyro(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGyro(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGyro(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetMag(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpSetLinearAccelFilterCoefficient(float coef); - uint8_t dmpGetLinearAccel(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetLinearAccel(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetLinearAccel(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetLinearAccel(VectorInt16* v, VectorInt16* vRaw, VectorFloat* gravity); - uint8_t dmpGetLinearAccelInWorld(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetLinearAccelInWorld(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetLinearAccelInWorld(VectorInt16* v, VectorInt16* vReal, Quaternion* q); - uint8_t dmpGetGyroAndAccelSensor(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGyroAndAccelSensor(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGyroAndAccelSensor(VectorInt16* g, VectorInt16* a, const uint8_t* packet = 0); - uint8_t dmpGetGyroSensor(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGyroSensor(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGyroSensor(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetControlData(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetTemperature(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGravity(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGravity(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetGravity(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q); - uint8_t dmpGetUnquantizedAccel(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetUnquantizedAccel(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetUnquantizedAccel(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetQuantizedAccel(int32_t* data, const uint8_t* packet = 0); - uint8_t dmpGetQuantizedAccel(int16_t* data, const uint8_t* packet = 0); - uint8_t dmpGetQuantizedAccel(VectorInt16* v, const uint8_t* packet = 0); - uint8_t dmpGetExternalSensorData(int32_t* data, uint16_t size, const uint8_t* packet = 0); - uint8_t dmpGetEIS(int32_t* data, const uint8_t* packet = 0); - - uint8_t dmpGetEuler(float* data, Quaternion* q); - uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity); - - // Get Floating Point data from FIFO - uint8_t dmpGetAccelFloat(float* data, const uint8_t* packet = 0); - uint8_t dmpGetQuaternionFloat(float* data, const uint8_t* packet = 0); - - uint8_t dmpProcessFIFOPacket(const unsigned char* dmpData); - uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t* processed = NULL); - - uint8_t dmpSetFIFOProcessedCallback(void (*func)(void)); - - uint8_t dmpInitFIFOParam(); - uint8_t dmpCloseFIFO(); - uint8_t dmpSetGyroDataSource(uint8_t source); - uint8_t dmpDecodeQuantizedAccel(); - uint32_t dmpGetGyroSumOfSquare(); - uint32_t dmpGetAccelSumOfSquare(); - void dmpOverrideQuaternion(long* q); - uint16_t dmpGetFIFOPacketSize(); -#endif - void CalibrateGyro(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops. void CalibrateAccel(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops. void PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops); // Does the diff --git a/Sming/Libraries/MPU6050/helper_3dmath.h b/Sming/Libraries/MPU6050/helper_3dmath.h deleted file mode 100644 index 4e8ac2d51b..0000000000 --- a/Sming/Libraries/MPU6050/helper_3dmath.h +++ /dev/null @@ -1,243 +0,0 @@ -// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D -// math helper 6/5/2012 by Jeff Rowberg Updates should -// (hopefully) always be available at https://github.com/jrowberg/i2cdevlib -// -// Changelog: -// 2012-06-05 - add 3D math helper file to DMP6 example sketch - -/* ============================================ -I2Cdev device library code is placed under the MIT license -Copyright (c) 2012 Jeff Rowberg - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -=============================================== -*/ - -#ifndef _HELPER_3DMATH_H_ -#define _HELPER_3DMATH_H_ -#include -#include - -class Quaternion -{ -public: - float w; - float x; - float y; - float z; - - Quaternion() - { - w = 1.0f; - x = 0.0f; - y = 0.0f; - z = 0.0f; - } - - Quaternion(float nw, float nx, float ny, float nz) - { - w = nw; - x = nx; - y = ny; - z = nz; - } - - Quaternion getProduct(Quaternion q) - { - // Quaternion multiplication is defined by: - // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) - // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) - // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) - // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 - return Quaternion(w * q.w - x * q.x - y * q.y - z * q.z, // new w - w * q.x + x * q.w + y * q.z - z * q.y, // new x - w * q.y - x * q.z + y * q.w + z * q.x, // new y - w * q.z + x * q.y - y * q.x + z * q.w); // new z - } - - Quaternion getConjugate() - { - return Quaternion(w, -x, -y, -z); - } - - float getMagnitude() - { - return sqrt(w * w + x * x + y * y + z * z); - } - - void normalize() - { - float m = getMagnitude(); - w /= m; - x /= m; - y /= m; - z /= m; - } - - Quaternion getNormalized() - { - Quaternion r(w, x, y, z); - r.normalize(); - return r; - } -}; - -class VectorInt16 -{ -public: - int16_t x; - int16_t y; - int16_t z; - - VectorInt16() - { - x = 0; - y = 0; - z = 0; - } - - VectorInt16(int16_t nx, int16_t ny, int16_t nz) - { - x = nx; - y = ny; - z = nz; - } - - float getMagnitude() - { - return sqrt(x * x + y * y + z * z); - } - - void normalize() - { - float m = getMagnitude(); - x /= m; - y /= m; - z /= m; - } - - VectorInt16 getNormalized() - { - VectorInt16 r(x, y, z); - r.normalize(); - return r; - } - - void rotate(Quaternion* q) - { - // http://www.cprogramming.com/tutorial/3d/quaternions.html - // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm - // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation - // ^ or: - // http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 - - // P_out = q * P_in * conj(q) - // - P_out is the output vector - // - q is the orientation quaternion - // - P_in is the input vector (a*aReal) - // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], - // q*=[w,-x,-y,-z]) - Quaternion p(0, x, y, z); - - // quaternion multiplication: q * p, stored back in p - p = q->getProduct(p); - - // quaternion multiplication: p * conj(q), stored back in p - p = p.getProduct(q->getConjugate()); - - // p quaternion is now [0, x', y', z'] - x = p.x; - y = p.y; - z = p.z; - } - - VectorInt16 getRotated(Quaternion* q) - { - VectorInt16 r(x, y, z); - r.rotate(q); - return r; - } -}; - -class VectorFloat -{ -public: - float x; - float y; - float z; - - VectorFloat() - { - x = 0; - y = 0; - z = 0; - } - - VectorFloat(float nx, float ny, float nz) - { - x = nx; - y = ny; - z = nz; - } - - float getMagnitude() - { - return sqrt(x * x + y * y + z * z); - } - - void normalize() - { - float m = getMagnitude(); - x /= m; - y /= m; - z /= m; - } - - VectorFloat getNormalized() - { - VectorFloat r(x, y, z); - r.normalize(); - return r; - } - - void rotate(Quaternion* q) - { - Quaternion p(0, x, y, z); - - // quaternion multiplication: q * p, stored back in p - p = q->getProduct(p); - - // quaternion multiplication: p * conj(q), stored back in p - p = p.getProduct(q->getConjugate()); - - // p quaternion is now [0, x', y', z'] - x = p.x; - y = p.y; - z = p.z; - } - - VectorFloat getRotated(Quaternion* q) - { - VectorFloat r(x, y, z); - r.rotate(q); - return r; - } -}; - -#endif /* _HELPER_3DMATH_H_ */