mirror of
https://gitee.com/peng_zhihui/HoloCubic
synced 2025-09-27 02:19:11 +08:00
[Fw] Fixed firmware compile error in Arduino IDE.
This commit is contained in:
parent
9a703c3ade
commit
0847129aca
@ -18,7 +18,6 @@ extern "C" {
|
||||
*********************/
|
||||
#include "lvgl.h"
|
||||
#include "ff.h"
|
||||
#include "lv_bmp.h"
|
||||
/*********************
|
||||
* DEFINES
|
||||
*********************/
|
||||
|
21
2.Firmware/Libraries/MPU6050/LICENSE
Normal file
21
2.Firmware/Libraries/MPU6050/LICENSE
Normal file
@ -0,0 +1,21 @@
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2019 ElectronicCats
|
||||
|
||||
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.
|
21
2.Firmware/Libraries/MPU6050/README.md
Normal file
21
2.Firmware/Libraries/MPU6050/README.md
Normal file
@ -0,0 +1,21 @@
|
||||
# MPU6050 by Electronic Cats
|
||||
|
||||
MPU-6050 6-axis accelerometer/gyroscope Arduino Library
|
||||
adapted for Arduino Library Manager by Electronic Cats, Feb 2019
|
||||
|
||||
The MPU6050 combines a 3-axis gyroscope and a 3-axis accelerometer on the same silicon die together with
|
||||
an onboard Digital Motion Processor(DMP) which processes complex 6-axis MotionFusion algorithms.
|
||||
|
||||
### Original Library
|
||||
|
||||
Based in the work of [https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050](https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050)
|
||||
|
||||
### Maintainer
|
||||
|
||||
Electronic Cats invests time and resources providing this open source design, please support Electronic Cats and open-source hardware by purchasing products from Electronic Cats!
|
||||
|
||||
### License
|
||||
|
||||
MIT
|
||||
|
||||
|
318
2.Firmware/Libraries/MPU6050/examples/IMU_Zero/IMU_Zero.ino
Normal file
318
2.Firmware/Libraries/MPU6050/examples/IMU_Zero/IMU_Zero.ino
Normal file
@ -0,0 +1,318 @@
|
||||
// MPU6050 offset-finder, based on Jeff Rowberg's MPU6050_RAW
|
||||
// 2016-10-19 by Robert R. Fenichel (bob@fenichel.net)
|
||||
|
||||
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
|
||||
// 10/7/2011 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// 2016-11-25 - added delays to reduce sampling rate to ~200 Hz
|
||||
// added temporizing printing during long computations
|
||||
// 2016-10-25 - requires inequality (Low < Target, High > Target) during expansion
|
||||
// dynamic speed change when closing in
|
||||
// 2016-10-22 - cosmetic changes
|
||||
// 2016-10-19 - initial release of IMU_Zero
|
||||
// 2013-05-08 - added multiple output formats
|
||||
// - added seamless Fastwire support
|
||||
// 2011-10-07 - initial release of MPU6050_RAW
|
||||
|
||||
/* ============================================
|
||||
I2Cdev device library code is placed under the MIT license
|
||||
Copyright (c) 2011 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.
|
||||
|
||||
If an MPU6050
|
||||
* is an ideal member of its tribe,
|
||||
* is properly warmed up,
|
||||
* is at rest in a neutral position,
|
||||
* is in a location where the pull of gravity is exactly 1g, and
|
||||
* has been loaded with the best possible offsets,
|
||||
then it will report 0 for all accelerations and displacements, except for
|
||||
Z acceleration, for which it will report 16384 (that is, 2^14). Your device
|
||||
probably won't do quite this well, but good offsets will all get the baseline
|
||||
outputs close to these target values.
|
||||
|
||||
Put the MPU6050 on a flat and horizontal surface, and leave it operating for
|
||||
5-10 minutes so its temperature gets stabilized.
|
||||
|
||||
Run this program. A "----- done -----" line will indicate that it has done its best.
|
||||
With the current accuracy-related constants (NFast = 1000, NSlow = 10000), it will take
|
||||
a few minutes to get there.
|
||||
|
||||
Along the way, it will generate a dozen or so lines of output, showing that for each
|
||||
of the 6 desired offsets, it is
|
||||
* first, trying to find two estimates, one too low and one too high, and
|
||||
* then, closing in until the bracket can't be made smaller.
|
||||
|
||||
The line just above the "done" line will look something like
|
||||
[567,567] --> [-1,2] [-2223,-2223] --> [0,1] [1131,1132] --> [16374,16404] [155,156] --> [-1,1] [-25,-24] --> [0,3] [5,6] --> [0,4]
|
||||
As will have been shown in interspersed header lines, the six groups making up this
|
||||
line describe the optimum offsets for the X acceleration, Y acceleration, Z acceleration,
|
||||
X gyro, Y gyro, and Z gyro, respectively. In the sample shown just above, the trial showed
|
||||
that +567 was the best offset for the X acceleration, -2223 was best for Y acceleration,
|
||||
and so on.
|
||||
|
||||
The need for the delay between readings (usDelay) was brought to my attention by Nikolaus Doppelhammer.
|
||||
===============================================
|
||||
*/
|
||||
|
||||
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
|
||||
// for both classes must be in the include path of your project
|
||||
#include "I2Cdev.h"
|
||||
#include "MPU6050.h"
|
||||
|
||||
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
|
||||
// is used in I2Cdev.h
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
#include "Wire.h"
|
||||
#endif
|
||||
|
||||
// class default I2C address is 0x68
|
||||
// specific I2C addresses may be passed as a parameter here
|
||||
// AD0 low = 0x68 (default for InvenSense evaluation board)
|
||||
// AD0 high = 0x69
|
||||
MPU6050 accelgyro;
|
||||
//MPU6050 accelgyro(0x69); // <-- use for AD0 high
|
||||
|
||||
|
||||
const char LBRACKET = '[';
|
||||
const char RBRACKET = ']';
|
||||
const char COMMA = ',';
|
||||
const char BLANK = ' ';
|
||||
const char PERIOD = '.';
|
||||
|
||||
const int iAx = 0;
|
||||
const int iAy = 1;
|
||||
const int iAz = 2;
|
||||
const int iGx = 3;
|
||||
const int iGy = 4;
|
||||
const int iGz = 5;
|
||||
|
||||
const int usDelay = 3150; // empirical, to hold sampling to 200 Hz
|
||||
const int NFast = 1000; // the bigger, the better (but slower)
|
||||
const int NSlow = 10000; // ..
|
||||
const int LinesBetweenHeaders = 5;
|
||||
int LowValue[6];
|
||||
int HighValue[6];
|
||||
int Smoothed[6];
|
||||
int LowOffset[6];
|
||||
int HighOffset[6];
|
||||
int Target[6];
|
||||
int LinesOut;
|
||||
int N;
|
||||
|
||||
void ForceHeader()
|
||||
{ LinesOut = 99; }
|
||||
|
||||
void GetSmoothed()
|
||||
{ int16_t RawValue[6];
|
||||
int i;
|
||||
long Sums[6];
|
||||
for (i = iAx; i <= iGz; i++)
|
||||
{ Sums[i] = 0; }
|
||||
// unsigned long Start = micros();
|
||||
|
||||
for (i = 1; i <= N; i++)
|
||||
{ // get sums
|
||||
accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz],
|
||||
&RawValue[iGx], &RawValue[iGy], &RawValue[iGz]);
|
||||
if ((i % 500) == 0)
|
||||
Serial.print(PERIOD);
|
||||
delayMicroseconds(usDelay);
|
||||
for (int j = iAx; j <= iGz; j++)
|
||||
Sums[j] = Sums[j] + RawValue[j];
|
||||
} // get sums
|
||||
// unsigned long usForN = micros() - Start;
|
||||
// Serial.print(" reading at ");
|
||||
// Serial.print(1000000/((usForN+N/2)/N));
|
||||
// Serial.println(" Hz");
|
||||
for (i = iAx; i <= iGz; i++)
|
||||
{ Smoothed[i] = (Sums[i] + N/2) / N ; }
|
||||
} // GetSmoothed
|
||||
|
||||
void Initialize()
|
||||
{
|
||||
// join I2C bus (I2Cdev library doesn't do this automatically)
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
Wire.begin();
|
||||
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
||||
Fastwire::setup(400, true);
|
||||
#endif
|
||||
|
||||
Serial.begin(9600);
|
||||
|
||||
// initialize device
|
||||
Serial.println("Initializing I2C devices...");
|
||||
accelgyro.initialize();
|
||||
|
||||
// verify connection
|
||||
Serial.println("Testing device connections...");
|
||||
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
|
||||
} // Initialize
|
||||
|
||||
void SetOffsets(int TheOffsets[6])
|
||||
{ accelgyro.setXAccelOffset(TheOffsets [iAx]);
|
||||
accelgyro.setYAccelOffset(TheOffsets [iAy]);
|
||||
accelgyro.setZAccelOffset(TheOffsets [iAz]);
|
||||
accelgyro.setXGyroOffset (TheOffsets [iGx]);
|
||||
accelgyro.setYGyroOffset (TheOffsets [iGy]);
|
||||
accelgyro.setZGyroOffset (TheOffsets [iGz]);
|
||||
} // SetOffsets
|
||||
|
||||
void ShowProgress()
|
||||
{ if (LinesOut >= LinesBetweenHeaders)
|
||||
{ // show header
|
||||
Serial.println("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro");
|
||||
LinesOut = 0;
|
||||
} // show header
|
||||
Serial.print(BLANK);
|
||||
for (int i = iAx; i <= iGz; i++)
|
||||
{ Serial.print(LBRACKET);
|
||||
Serial.print(LowOffset[i]),
|
||||
Serial.print(COMMA);
|
||||
Serial.print(HighOffset[i]);
|
||||
Serial.print("] --> [");
|
||||
Serial.print(LowValue[i]);
|
||||
Serial.print(COMMA);
|
||||
Serial.print(HighValue[i]);
|
||||
if (i == iGz)
|
||||
{ Serial.println(RBRACKET); }
|
||||
else
|
||||
{ Serial.print("]\t"); }
|
||||
}
|
||||
LinesOut++;
|
||||
} // ShowProgress
|
||||
|
||||
void PullBracketsIn()
|
||||
{ boolean AllBracketsNarrow;
|
||||
boolean StillWorking;
|
||||
int NewOffset[6];
|
||||
|
||||
Serial.println("\nclosing in:");
|
||||
AllBracketsNarrow = false;
|
||||
ForceHeader();
|
||||
StillWorking = true;
|
||||
while (StillWorking)
|
||||
{ StillWorking = false;
|
||||
if (AllBracketsNarrow && (N == NFast))
|
||||
{ SetAveraging(NSlow); }
|
||||
else
|
||||
{ AllBracketsNarrow = true; }// tentative
|
||||
for (int i = iAx; i <= iGz; i++)
|
||||
{ if (HighOffset[i] <= (LowOffset[i]+1))
|
||||
{ NewOffset[i] = LowOffset[i]; }
|
||||
else
|
||||
{ // binary search
|
||||
StillWorking = true;
|
||||
NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2;
|
||||
if (HighOffset[i] > (LowOffset[i] + 10))
|
||||
{ AllBracketsNarrow = false; }
|
||||
} // binary search
|
||||
}
|
||||
SetOffsets(NewOffset);
|
||||
GetSmoothed();
|
||||
for (int i = iAx; i <= iGz; i++)
|
||||
{ // closing in
|
||||
if (Smoothed[i] > Target[i])
|
||||
{ // use lower half
|
||||
HighOffset[i] = NewOffset[i];
|
||||
HighValue[i] = Smoothed[i];
|
||||
} // use lower half
|
||||
else
|
||||
{ // use upper half
|
||||
LowOffset[i] = NewOffset[i];
|
||||
LowValue[i] = Smoothed[i];
|
||||
} // use upper half
|
||||
} // closing in
|
||||
ShowProgress();
|
||||
} // still working
|
||||
|
||||
} // PullBracketsIn
|
||||
|
||||
void PullBracketsOut()
|
||||
{ boolean Done = false;
|
||||
int NextLowOffset[6];
|
||||
int NextHighOffset[6];
|
||||
|
||||
Serial.println("expanding:");
|
||||
ForceHeader();
|
||||
|
||||
while (!Done)
|
||||
{ Done = true;
|
||||
SetOffsets(LowOffset);
|
||||
GetSmoothed();
|
||||
for (int i = iAx; i <= iGz; i++)
|
||||
{ // got low values
|
||||
LowValue[i] = Smoothed[i];
|
||||
if (LowValue[i] >= Target[i])
|
||||
{ Done = false;
|
||||
NextLowOffset[i] = LowOffset[i] - 1000;
|
||||
}
|
||||
else
|
||||
{ NextLowOffset[i] = LowOffset[i]; }
|
||||
} // got low values
|
||||
|
||||
SetOffsets(HighOffset);
|
||||
GetSmoothed();
|
||||
for (int i = iAx; i <= iGz; i++)
|
||||
{ // got high values
|
||||
HighValue[i] = Smoothed[i];
|
||||
if (HighValue[i] <= Target[i])
|
||||
{ Done = false;
|
||||
NextHighOffset[i] = HighOffset[i] + 1000;
|
||||
}
|
||||
else
|
||||
{ NextHighOffset[i] = HighOffset[i]; }
|
||||
} // got high values
|
||||
ShowProgress();
|
||||
for (int i = iAx; i <= iGz; i++)
|
||||
{ LowOffset[i] = NextLowOffset[i]; // had to wait until ShowProgress done
|
||||
HighOffset[i] = NextHighOffset[i]; // ..
|
||||
}
|
||||
} // keep going
|
||||
} // PullBracketsOut
|
||||
|
||||
void SetAveraging(int NewN)
|
||||
{ N = NewN;
|
||||
Serial.print("averaging ");
|
||||
Serial.print(N);
|
||||
Serial.println(" readings each time");
|
||||
} // SetAveraging
|
||||
|
||||
void setup()
|
||||
{ Initialize();
|
||||
for (int i = iAx; i <= iGz; i++)
|
||||
{ // set targets and initial guesses
|
||||
Target[i] = 0; // must fix for ZAccel
|
||||
HighOffset[i] = 0;
|
||||
LowOffset[i] = 0;
|
||||
} // set targets and initial guesses
|
||||
Target[iAz] = 16384;
|
||||
SetAveraging(NFast);
|
||||
|
||||
PullBracketsOut();
|
||||
PullBracketsIn();
|
||||
|
||||
Serial.println("-------------- done --------------");
|
||||
} // setup
|
||||
|
||||
void loop()
|
||||
{
|
||||
} // loop
|
@ -0,0 +1,382 @@
|
||||
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
|
||||
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// 2016-04-18 - Eliminated a potential infinite loop
|
||||
// 2013-05-08 - added seamless Fastwire support
|
||||
// - added note about gyro calibration
|
||||
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
|
||||
// 2012-06-20 - improved FIFO overflow handling and simplified read process
|
||||
// 2012-06-19 - completely rearranged DMP initialization code and simplification
|
||||
// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
|
||||
// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
|
||||
// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
|
||||
// - add 3D math helper file to DMP6 example sketch
|
||||
// - add Euler output and Yaw/Pitch/Roll output formats
|
||||
// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
|
||||
// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
|
||||
// 2012-05-30 - basic DMP initialization working
|
||||
|
||||
/* ============================================
|
||||
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.
|
||||
===============================================
|
||||
*/
|
||||
|
||||
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
|
||||
// for both classes must be in the include path of your project
|
||||
#include "I2Cdev.h"
|
||||
|
||||
#include "MPU6050_6Axis_MotionApps20.h"
|
||||
//#include "MPU6050.h" // not necessary if using MotionApps include file
|
||||
|
||||
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
|
||||
// is used in I2Cdev.h
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
#include "Wire.h"
|
||||
#endif
|
||||
|
||||
// class default I2C address is 0x68
|
||||
// specific I2C addresses may be passed as a parameter here
|
||||
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
|
||||
// AD0 high = 0x69
|
||||
MPU6050 mpu;
|
||||
//MPU6050 mpu(0x69); // <-- use for AD0 high
|
||||
|
||||
/* =========================================================================
|
||||
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
|
||||
depends on the MPU-6050's INT pin being connected to the Arduino's
|
||||
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
|
||||
digital I/O pin 2.
|
||||
* ========================================================================= */
|
||||
|
||||
/* =========================================================================
|
||||
NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
|
||||
when using Serial.write(buf, len). The Teapot output uses this method.
|
||||
The solution requires a modification to the Arduino USBAPI.h file, which
|
||||
is fortunately simple, but annoying. This will be fixed in the next IDE
|
||||
release. For more info, see these links:
|
||||
|
||||
http://arduino.cc/forum/index.php/topic,109987.0.html
|
||||
http://code.google.com/p/arduino/issues/detail?id=958
|
||||
* ========================================================================= */
|
||||
|
||||
|
||||
|
||||
// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
|
||||
// quaternion components in a [w, x, y, z] format (not best for parsing
|
||||
// on a remote host such as Processing or something though)
|
||||
//#define OUTPUT_READABLE_QUATERNION
|
||||
|
||||
// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
|
||||
// (in degrees) calculated from the quaternions coming from the FIFO.
|
||||
// Note that Euler angles suffer from gimbal lock (for more info, see
|
||||
// http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
//#define OUTPUT_READABLE_EULER
|
||||
|
||||
// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
|
||||
// pitch/roll angles (in degrees) calculated from the quaternions coming
|
||||
// from the FIFO. Note this also requires gravity vector calculations.
|
||||
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
|
||||
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
#define OUTPUT_READABLE_YAWPITCHROLL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
|
||||
// components with gravity removed. This acceleration reference frame is
|
||||
// not compensated for orientation, so +X is always +X according to the
|
||||
// sensor, just without the effects of gravity. If you want acceleration
|
||||
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
|
||||
//#define OUTPUT_READABLE_REALACCEL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
|
||||
// components with gravity removed and adjusted for the world frame of
|
||||
// reference (yaw is relative to initial orientation, since no magnetometer
|
||||
// is present in this case). Could be quite handy in some cases.
|
||||
//#define OUTPUT_READABLE_WORLDACCEL
|
||||
|
||||
// uncomment "OUTPUT_TEAPOT" if you want output that matches the
|
||||
// format used for the InvenSense teapot demo
|
||||
//#define OUTPUT_TEAPOT
|
||||
|
||||
|
||||
|
||||
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
|
||||
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
|
||||
bool blinkState = false;
|
||||
|
||||
// MPU control/status vars
|
||||
bool dmpReady = false; // set true if DMP init was successful
|
||||
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
|
||||
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
|
||||
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
|
||||
uint16_t fifoCount; // count of all bytes currently in FIFO
|
||||
uint8_t fifoBuffer[64]; // FIFO storage buffer
|
||||
|
||||
// orientation/motion vars
|
||||
Quaternion q; // [w, x, y, z] quaternion container
|
||||
VectorInt16 aa; // [x, y, z] accel sensor measurements
|
||||
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
|
||||
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
|
||||
VectorFloat gravity; // [x, y, z] gravity vector
|
||||
float euler[3]; // [psi, theta, phi] Euler angle container
|
||||
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
||||
|
||||
// packet structure for InvenSense teapot demo
|
||||
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === INTERRUPT DETECTION ROUTINE ===
|
||||
// ================================================================
|
||||
|
||||
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
|
||||
void dmpDataReady() {
|
||||
mpuInterrupt = true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === INITIAL SETUP ===
|
||||
// ================================================================
|
||||
|
||||
void setup() {
|
||||
// join I2C bus (I2Cdev library doesn't do this automatically)
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
Wire.begin();
|
||||
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
|
||||
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
||||
Fastwire::setup(400, true);
|
||||
#endif
|
||||
|
||||
// initialize serial communication
|
||||
// (115200 chosen because it is required for Teapot Demo output, but it's
|
||||
// really up to you depending on your project)
|
||||
Serial.begin(115200);
|
||||
while (!Serial); // wait for Leonardo enumeration, others continue immediately
|
||||
|
||||
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
|
||||
// Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
|
||||
// the baud timing being too misaligned with processor ticks. You must use
|
||||
// 38400 or slower in these cases, or use some kind of external separate
|
||||
// crystal solution for the UART timer.
|
||||
|
||||
// initialize device
|
||||
Serial.println(F("Initializing I2C devices..."));
|
||||
mpu.initialize();
|
||||
pinMode(INTERRUPT_PIN, INPUT);
|
||||
|
||||
// verify connection
|
||||
Serial.println(F("Testing device connections..."));
|
||||
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
|
||||
|
||||
// wait for ready
|
||||
Serial.println(F("\nSend any character to begin DMP programming and demo: "));
|
||||
while (Serial.available() && Serial.read()); // empty buffer
|
||||
while (!Serial.available()); // wait for data
|
||||
while (Serial.available() && Serial.read()); // empty buffer again
|
||||
|
||||
// load and configure the DMP
|
||||
Serial.println(F("Initializing DMP..."));
|
||||
devStatus = mpu.dmpInitialize();
|
||||
|
||||
// supply your own gyro offsets here, scaled for min sensitivity
|
||||
mpu.setXGyroOffset(220);
|
||||
mpu.setYGyroOffset(76);
|
||||
mpu.setZGyroOffset(-85);
|
||||
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
|
||||
|
||||
// make sure it worked (returns 0 if so)
|
||||
if (devStatus == 0) {
|
||||
// turn on the DMP, now that it's ready
|
||||
Serial.println(F("Enabling DMP..."));
|
||||
mpu.setDMPEnabled(true);
|
||||
|
||||
// enable Arduino interrupt detection
|
||||
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
|
||||
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
|
||||
Serial.println(F(")..."));
|
||||
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
|
||||
mpuIntStatus = mpu.getIntStatus();
|
||||
|
||||
// set our DMP Ready flag so the main loop() function knows it's okay to use it
|
||||
Serial.println(F("DMP ready! Waiting for first interrupt..."));
|
||||
dmpReady = true;
|
||||
|
||||
// get expected DMP packet size for later comparison
|
||||
packetSize = mpu.dmpGetFIFOPacketSize();
|
||||
} else {
|
||||
// ERROR!
|
||||
// 1 = initial memory load failed
|
||||
// 2 = DMP configuration updates failed
|
||||
// (if it's going to break, usually the code will be 1)
|
||||
Serial.print(F("DMP Initialization failed (code "));
|
||||
Serial.print(devStatus);
|
||||
Serial.println(F(")"));
|
||||
}
|
||||
|
||||
// configure LED for output
|
||||
pinMode(LED_PIN, OUTPUT);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === MAIN PROGRAM LOOP ===
|
||||
// ================================================================
|
||||
|
||||
void loop() {
|
||||
// if programming failed, don't try to do anything
|
||||
if (!dmpReady) return;
|
||||
|
||||
// wait for MPU interrupt or extra packet(s) available
|
||||
while (!mpuInterrupt && fifoCount < packetSize) {
|
||||
if (mpuInterrupt && fifoCount < packetSize) {
|
||||
// try to get out of the infinite loop
|
||||
fifoCount = mpu.getFIFOCount();
|
||||
}
|
||||
// other program behavior stuff here
|
||||
// .
|
||||
// .
|
||||
// .
|
||||
// if you are really paranoid you can frequently test in between other
|
||||
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
|
||||
// while() loop to immediately process the MPU data
|
||||
// .
|
||||
// .
|
||||
// .
|
||||
}
|
||||
|
||||
// reset interrupt flag and get INT_STATUS byte
|
||||
mpuInterrupt = false;
|
||||
mpuIntStatus = mpu.getIntStatus();
|
||||
|
||||
// get current FIFO count
|
||||
fifoCount = mpu.getFIFOCount();
|
||||
|
||||
// check for overflow (this should never happen unless our code is too inefficient)
|
||||
if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
|
||||
// reset so we can continue cleanly
|
||||
mpu.resetFIFO();
|
||||
fifoCount = mpu.getFIFOCount();
|
||||
Serial.println(F("FIFO overflow!"));
|
||||
|
||||
// otherwise, check for DMP data ready interrupt (this should happen frequently)
|
||||
} else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
|
||||
// wait for correct available data length, should be a VERY short wait
|
||||
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
|
||||
|
||||
// read a packet from FIFO
|
||||
mpu.getFIFOBytes(fifoBuffer, packetSize);
|
||||
|
||||
// track FIFO count here in case there is > 1 packet available
|
||||
// (this lets us immediately read more without waiting for an interrupt)
|
||||
fifoCount -= packetSize;
|
||||
|
||||
#ifdef OUTPUT_READABLE_QUATERNION
|
||||
// display quaternion values in easy matrix form: w x y z
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
Serial.print("quat\t");
|
||||
Serial.print(q.w);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(q.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_EULER
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetEuler(euler, &q);
|
||||
Serial.print("euler\t");
|
||||
Serial.print(euler[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(euler[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(euler[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_YAWPITCHROLL
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
||||
Serial.print("ypr\t");
|
||||
Serial.print(ypr[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(ypr[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(ypr[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_REALACCEL
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
Serial.print("areal\t");
|
||||
Serial.print(aaReal.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaReal.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaReal.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_WORLDACCEL
|
||||
// display initial world-frame acceleration, adjusted to remove gravity
|
||||
// and rotated based on known orientation from quaternion
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
|
||||
Serial.print("aworld\t");
|
||||
Serial.print(aaWorld.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaWorld.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaWorld.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_TEAPOT
|
||||
// display quaternion values in InvenSense Teapot demo format:
|
||||
teapotPacket[2] = fifoBuffer[0];
|
||||
teapotPacket[3] = fifoBuffer[1];
|
||||
teapotPacket[4] = fifoBuffer[4];
|
||||
teapotPacket[5] = fifoBuffer[5];
|
||||
teapotPacket[6] = fifoBuffer[8];
|
||||
teapotPacket[7] = fifoBuffer[9];
|
||||
teapotPacket[8] = fifoBuffer[12];
|
||||
teapotPacket[9] = fifoBuffer[13];
|
||||
Serial.write(teapotPacket, 14);
|
||||
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
|
||||
#endif
|
||||
|
||||
// blink LED to indicate activity
|
||||
blinkState = !blinkState;
|
||||
digitalWrite(LED_PIN, blinkState);
|
||||
}
|
||||
}
|
@ -0,0 +1,242 @@
|
||||
// I2C device class (I2Cdev) demonstration Processing sketch for MPU6050 DMP output
|
||||
// 6/20/2012 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// 2012-06-20 - initial release
|
||||
|
||||
/* ============================================
|
||||
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.
|
||||
===============================================
|
||||
*/
|
||||
|
||||
import processing.serial.*;
|
||||
import processing.opengl.*;
|
||||
import toxi.geom.*;
|
||||
import toxi.processing.*;
|
||||
|
||||
// NOTE: requires ToxicLibs to be installed in order to run properly.
|
||||
// 1. Download from http://toxiclibs.org/downloads
|
||||
// 2. Extract into [userdir]/Processing/libraries
|
||||
// (location may be different on Mac/Linux)
|
||||
// 3. Run and bask in awesomeness
|
||||
|
||||
ToxiclibsSupport gfx;
|
||||
|
||||
Serial port; // The serial port
|
||||
char[] teapotPacket = new char[14]; // InvenSense Teapot packet
|
||||
int serialCount = 0; // current packet byte position
|
||||
int synced = 0;
|
||||
int interval = 0;
|
||||
|
||||
float[] q = new float[4];
|
||||
Quaternion quat = new Quaternion(1, 0, 0, 0);
|
||||
|
||||
float[] gravity = new float[3];
|
||||
float[] euler = new float[3];
|
||||
float[] ypr = new float[3];
|
||||
|
||||
void setup() {
|
||||
// 300px square viewport using OpenGL rendering
|
||||
size(300, 300, OPENGL);
|
||||
gfx = new ToxiclibsSupport(this);
|
||||
|
||||
// setup lights and antialiasing
|
||||
lights();
|
||||
smooth();
|
||||
|
||||
// display serial port list for debugging/clarity
|
||||
println(Serial.list());
|
||||
|
||||
// get the first available port (use EITHER this OR the specific port code below)
|
||||
String portName = Serial.list()[0];
|
||||
|
||||
// get a specific serial port (use EITHER this OR the first-available code above)
|
||||
//String portName = "COM4";
|
||||
|
||||
// open the serial port
|
||||
port = new Serial(this, portName, 115200);
|
||||
|
||||
// send single character to trigger DMP init/start
|
||||
// (expected by MPU6050_DMP6 example Arduino sketch)
|
||||
port.write('r');
|
||||
}
|
||||
|
||||
void draw() {
|
||||
if (millis() - interval > 1000) {
|
||||
// resend single character to trigger DMP init/start
|
||||
// in case the MPU is halted/reset while applet is running
|
||||
port.write('r');
|
||||
interval = millis();
|
||||
}
|
||||
|
||||
// black background
|
||||
background(0);
|
||||
|
||||
// translate everything to the middle of the viewport
|
||||
pushMatrix();
|
||||
translate(width / 2, height / 2);
|
||||
|
||||
// 3-step rotation from yaw/pitch/roll angles (gimbal lock!)
|
||||
// ...and other weirdness I haven't figured out yet
|
||||
//rotateY(-ypr[0]);
|
||||
//rotateZ(-ypr[1]);
|
||||
//rotateX(-ypr[2]);
|
||||
|
||||
// toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!)
|
||||
// (axis order [1, 3, 2] and inversion [-1, +1, +1] is a consequence of
|
||||
// different coordinate system orientation assumptions between Processing
|
||||
// and InvenSense DMP)
|
||||
float[] axis = quat.toAxisAngle();
|
||||
rotate(axis[0], -axis[1], axis[3], axis[2]);
|
||||
|
||||
// draw main body in red
|
||||
fill(255, 0, 0, 200);
|
||||
box(10, 10, 200);
|
||||
|
||||
// draw front-facing tip in blue
|
||||
fill(0, 0, 255, 200);
|
||||
pushMatrix();
|
||||
translate(0, 0, -120);
|
||||
rotateX(PI/2);
|
||||
drawCylinder(0, 20, 20, 8);
|
||||
popMatrix();
|
||||
|
||||
// draw wings and tail fin in green
|
||||
fill(0, 255, 0, 200);
|
||||
beginShape(TRIANGLES);
|
||||
vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); // wing top layer
|
||||
vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); // wing bottom layer
|
||||
vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); // tail left layer
|
||||
vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); // tail right layer
|
||||
endShape();
|
||||
beginShape(QUADS);
|
||||
vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80);
|
||||
vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80);
|
||||
vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30);
|
||||
vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98);
|
||||
vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70);
|
||||
vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70);
|
||||
endShape();
|
||||
|
||||
popMatrix();
|
||||
}
|
||||
|
||||
void serialEvent(Serial port) {
|
||||
interval = millis();
|
||||
while (port.available() > 0) {
|
||||
int ch = port.read();
|
||||
|
||||
if (synced == 0 && ch != '$') return; // initial synchronization - also used to resync/realign if needed
|
||||
synced = 1;
|
||||
print ((char)ch);
|
||||
|
||||
if ((serialCount == 1 && ch != 2)
|
||||
|| (serialCount == 12 && ch != '\r')
|
||||
|| (serialCount == 13 && ch != '\n')) {
|
||||
serialCount = 0;
|
||||
synced = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
if (serialCount > 0 || ch == '$') {
|
||||
teapotPacket[serialCount++] = (char)ch;
|
||||
if (serialCount == 14) {
|
||||
serialCount = 0; // restart packet byte position
|
||||
|
||||
// get quaternion from data packet
|
||||
q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f;
|
||||
q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f;
|
||||
q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f;
|
||||
q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f;
|
||||
for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i];
|
||||
|
||||
// set our toxilibs quaternion to new data
|
||||
quat.set(q[0], q[1], q[2], q[3]);
|
||||
|
||||
/*
|
||||
// below calculations unnecessary for orientation only using toxilibs
|
||||
|
||||
// calculate gravity vector
|
||||
gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]);
|
||||
gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]);
|
||||
gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
|
||||
|
||||
// calculate Euler angles
|
||||
euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
|
||||
euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]);
|
||||
euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1);
|
||||
|
||||
// calculate yaw/pitch/roll angles
|
||||
ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
|
||||
ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
|
||||
ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));
|
||||
|
||||
// output various components for debugging
|
||||
//println("q:\t" + round(q[0]*100.0f)/100.0f + "\t" + round(q[1]*100.0f)/100.0f + "\t" + round(q[2]*100.0f)/100.0f + "\t" + round(q[3]*100.0f)/100.0f);
|
||||
//println("euler:\t" + euler[0]*180.0f/PI + "\t" + euler[1]*180.0f/PI + "\t" + euler[2]*180.0f/PI);
|
||||
//println("ypr:\t" + ypr[0]*180.0f/PI + "\t" + ypr[1]*180.0f/PI + "\t" + ypr[2]*180.0f/PI);
|
||||
*/
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) {
|
||||
float angle = 0;
|
||||
float angleIncrement = TWO_PI / sides;
|
||||
beginShape(QUAD_STRIP);
|
||||
for (int i = 0; i < sides + 1; ++i) {
|
||||
vertex(topRadius*cos(angle), 0, topRadius*sin(angle));
|
||||
vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle));
|
||||
angle += angleIncrement;
|
||||
}
|
||||
endShape();
|
||||
|
||||
// If it is not a cone, draw the circular top cap
|
||||
if (topRadius != 0) {
|
||||
angle = 0;
|
||||
beginShape(TRIANGLE_FAN);
|
||||
|
||||
// Center point
|
||||
vertex(0, 0, 0);
|
||||
for (int i = 0; i < sides + 1; i++) {
|
||||
vertex(topRadius * cos(angle), 0, topRadius * sin(angle));
|
||||
angle += angleIncrement;
|
||||
}
|
||||
endShape();
|
||||
}
|
||||
|
||||
// If it is not a cone, draw the circular bottom cap
|
||||
if (bottomRadius != 0) {
|
||||
angle = 0;
|
||||
beginShape(TRIANGLE_FAN);
|
||||
|
||||
// Center point
|
||||
vertex(0, tall, 0);
|
||||
for (int i = 0; i < sides + 1; i++) {
|
||||
vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));
|
||||
angle += angleIncrement;
|
||||
}
|
||||
endShape();
|
||||
}
|
||||
}
|
@ -0,0 +1,370 @@
|
||||
/* ============================================
|
||||
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.
|
||||
===============================================
|
||||
*/
|
||||
|
||||
/* This driver reads quaternion data from the MPU6060 and sends
|
||||
Open Sound Control messages.
|
||||
|
||||
GY-521 NodeMCU
|
||||
MPU6050 devkit 1.0
|
||||
board Lolin Description
|
||||
======= ========== ====================================================
|
||||
VCC VU (5V USB) Not available on all boards so use 3.3V if needed.
|
||||
GND G Ground
|
||||
SCL D1 (GPIO05) I2C clock
|
||||
SDA D2 (GPIO04) I2C data
|
||||
XDA not connected
|
||||
XCL not connected
|
||||
AD0 not connected
|
||||
INT D8 (GPIO15) Interrupt pin
|
||||
|
||||
*/
|
||||
|
||||
#if defined(ESP8266)
|
||||
#include <ESP8266WiFi.h>
|
||||
#else
|
||||
#include <WiFi.h>
|
||||
#endif
|
||||
#include <DNSServer.h>
|
||||
#include <WiFiClient.h>
|
||||
#include <WiFiUdp.h>
|
||||
#include <OSCMessage.h>
|
||||
#include <WiFiManager.h> //https://github.com/tzapu/WiFiManager
|
||||
|
||||
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
|
||||
// for both classes must be in the include path of your project
|
||||
#include "I2Cdev.h"
|
||||
|
||||
#include "MPU6050_6Axis_MotionApps20.h"
|
||||
//#include "MPU6050.h" // not necessary if using MotionApps include file
|
||||
|
||||
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
|
||||
// is used in I2Cdev.h
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
#include "Wire.h"
|
||||
#endif
|
||||
|
||||
// class default I2C address is 0x68
|
||||
// specific I2C addresses may be passed as a parameter here
|
||||
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
|
||||
// AD0 high = 0x69
|
||||
MPU6050 mpu;
|
||||
//MPU6050 mpu(0x69); // <-- use for AD0 high
|
||||
|
||||
/* =========================================================================
|
||||
NOTE: In addition to connection 5/3.3v, GND, SDA, and SCL, this sketch
|
||||
depends on the MPU-6050's INT pin being connected to the ESP8266 GPIO15
|
||||
pin.
|
||||
* ========================================================================= */
|
||||
|
||||
// MPU control/status vars
|
||||
bool dmpReady = false; // set true if DMP init was successful
|
||||
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
|
||||
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
|
||||
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
|
||||
uint16_t fifoCount; // count of all bytes currently in FIFO
|
||||
uint8_t fifoBuffer[64]; // FIFO storage buffer
|
||||
|
||||
// orientation/motion vars
|
||||
Quaternion q; // [w, x, y, z] quaternion container
|
||||
VectorInt16 aa; // [x, y, z] accel sensor measurements
|
||||
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
|
||||
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
|
||||
VectorFloat gravity; // [x, y, z] gravity vector
|
||||
#ifdef OUTPUT_READABLE_EULER
|
||||
float euler[3]; // [psi, theta, phi] Euler angle container
|
||||
#endif
|
||||
#ifdef OUTPUT_READABLE_YAWPITCHROLL
|
||||
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
||||
#endif
|
||||
|
||||
// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
|
||||
// quaternion components in a [w, x, y, z] format (not best for parsing
|
||||
// on a remote host such as Processing or something though)
|
||||
//#define OUTPUT_READABLE_QUATERNION
|
||||
|
||||
// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
|
||||
// (in degrees) calculated from the quaternions coming from the FIFO.
|
||||
// Note that Euler angles suffer from gimbal lock (for more info, see
|
||||
// http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
//#define OUTPUT_READABLE_EULER
|
||||
|
||||
// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
|
||||
// pitch/roll angles (in degrees) calculated from the quaternions coming
|
||||
// from the FIFO. Note this also requires gravity vector calculations.
|
||||
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
|
||||
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
//#define OUTPUT_READABLE_YAWPITCHROLL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
|
||||
// components with gravity removed. This acceleration reference frame is
|
||||
// not compensated for orientation, so +X is always +X according to the
|
||||
// sensor, just without the effects of gravity. If you want acceleration
|
||||
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
|
||||
//#define OUTPUT_READABLE_REALACCEL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
|
||||
// components with gravity removed and adjusted for the world frame of
|
||||
// reference (yaw is relative to initial orientation, since no magnetometer
|
||||
// is present in this case). Could be quite handy in some cases.
|
||||
//#define OUTPUT_READABLE_WORLDACCEL
|
||||
|
||||
// uncomment "OUTPUT_TEAPOT_OSC" if you want output that matches the
|
||||
// format used for the InvenSense teapot demo
|
||||
#define OUTPUT_TEAPOT_OSC
|
||||
|
||||
#define INTERRUPT_PIN 15 // use pin 15 on ESP8266
|
||||
|
||||
const char DEVICE_NAME[] = "mpu6050";
|
||||
|
||||
WiFiUDP Udp; // A UDP instance to let us send and receive packets over UDP
|
||||
const IPAddress outIp(192, 168, 1, 11); // remote IP to receive OSC
|
||||
const unsigned int outPort = 9999; // remote port to receive OSC
|
||||
|
||||
// ================================================================
|
||||
// === INTERRUPT DETECTION ROUTINE ===
|
||||
// ================================================================
|
||||
|
||||
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
|
||||
void dmpDataReady() {
|
||||
mpuInterrupt = true;
|
||||
}
|
||||
|
||||
void mpu_setup()
|
||||
{
|
||||
// join I2C bus (I2Cdev library doesn't do this automatically)
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
Wire.begin();
|
||||
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
|
||||
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
||||
Fastwire::setup(400, true);
|
||||
#endif
|
||||
|
||||
// initialize device
|
||||
Serial.println(F("Initializing I2C devices..."));
|
||||
mpu.initialize();
|
||||
pinMode(INTERRUPT_PIN, INPUT);
|
||||
|
||||
// verify connection
|
||||
Serial.println(F("Testing device connections..."));
|
||||
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
|
||||
|
||||
// load and configure the DMP
|
||||
Serial.println(F("Initializing DMP..."));
|
||||
devStatus = mpu.dmpInitialize();
|
||||
|
||||
// supply your own gyro offsets here, scaled for min sensitivity
|
||||
mpu.setXGyroOffset(220);
|
||||
mpu.setYGyroOffset(76);
|
||||
mpu.setZGyroOffset(-85);
|
||||
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
|
||||
|
||||
// make sure it worked (returns 0 if so)
|
||||
if (devStatus == 0) {
|
||||
// turn on the DMP, now that it's ready
|
||||
Serial.println(F("Enabling DMP..."));
|
||||
mpu.setDMPEnabled(true);
|
||||
|
||||
// enable Arduino interrupt detection
|
||||
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
|
||||
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
|
||||
mpuIntStatus = mpu.getIntStatus();
|
||||
|
||||
// set our DMP Ready flag so the main loop() function knows it's okay to use it
|
||||
Serial.println(F("DMP ready! Waiting for first interrupt..."));
|
||||
dmpReady = true;
|
||||
|
||||
// get expected DMP packet size for later comparison
|
||||
packetSize = mpu.dmpGetFIFOPacketSize();
|
||||
} else {
|
||||
// ERROR!
|
||||
// 1 = initial memory load failed
|
||||
// 2 = DMP configuration updates failed
|
||||
// (if it's going to break, usually the code will be 1)
|
||||
Serial.print(F("DMP Initialization failed (code "));
|
||||
Serial.print(devStatus);
|
||||
Serial.println(F(")"));
|
||||
}
|
||||
}
|
||||
|
||||
void setup(void)
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial.println(F("\nOrientation Sensor OSC output")); Serial.println();
|
||||
|
||||
//WiFiManager
|
||||
//Local intialization. Once its business is done, there is no need to keep it around
|
||||
WiFiManager wifiManager;
|
||||
//reset saved settings
|
||||
//wifiManager.resetSettings();
|
||||
|
||||
//fetches ssid and pass from eeprom and tries to connect
|
||||
//if it does not connect it starts an access point with the specified name
|
||||
//and goes into a blocking loop awaiting configuration
|
||||
wifiManager.autoConnect(DEVICE_NAME);
|
||||
|
||||
Serial.print(F("WiFi connected! IP address: "));
|
||||
Serial.println(WiFi.localIP());
|
||||
|
||||
mpu_setup();
|
||||
}
|
||||
|
||||
void mpu_loop()
|
||||
{
|
||||
// if programming failed, don't try to do anything
|
||||
if (!dmpReady) return;
|
||||
|
||||
// wait for MPU interrupt or extra packet(s) available
|
||||
if (!mpuInterrupt && fifoCount < packetSize) return;
|
||||
|
||||
// reset interrupt flag and get INT_STATUS byte
|
||||
mpuInterrupt = false;
|
||||
mpuIntStatus = mpu.getIntStatus();
|
||||
|
||||
// get current FIFO count
|
||||
fifoCount = mpu.getFIFOCount();
|
||||
|
||||
// check for overflow (this should never happen unless our code is too inefficient)
|
||||
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
|
||||
// reset so we can continue cleanly
|
||||
mpu.resetFIFO();
|
||||
Serial.println(F("FIFO overflow!"));
|
||||
|
||||
// otherwise, check for DMP data ready interrupt (this should happen frequently)
|
||||
} else if (mpuIntStatus & 0x02) {
|
||||
// wait for correct available data length, should be a VERY short wait
|
||||
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
|
||||
|
||||
// read a packet from FIFO
|
||||
mpu.getFIFOBytes(fifoBuffer, packetSize);
|
||||
|
||||
// track FIFO count here in case there is > 1 packet available
|
||||
// (this lets us immediately read more without waiting for an interrupt)
|
||||
fifoCount -= packetSize;
|
||||
|
||||
#ifdef OUTPUT_READABLE_QUATERNION
|
||||
// display quaternion values in easy matrix form: w x y z
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
Serial.print("quat\t");
|
||||
Serial.print(q.w);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(q.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_TEAPOT_OSC
|
||||
#ifndef OUTPUT_READABLE_QUATERNION
|
||||
// display quaternion values in easy matrix form: w x y z
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
#endif
|
||||
// Send OSC message
|
||||
OSCMessage msg("/imuquat");
|
||||
msg.add((float)q.w);
|
||||
msg.add((float)q.x);
|
||||
msg.add((float)q.y);
|
||||
msg.add((float)q.z);
|
||||
|
||||
Udp.beginPacket(outIp, outPort);
|
||||
msg.send(Udp);
|
||||
Udp.endPacket();
|
||||
|
||||
msg.empty();
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_EULER
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetEuler(euler, &q);
|
||||
Serial.print("euler\t");
|
||||
Serial.print(euler[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(euler[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(euler[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_YAWPITCHROLL
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
||||
Serial.print("ypr\t");
|
||||
Serial.print(ypr[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(ypr[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(ypr[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_REALACCEL
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
Serial.print("areal\t");
|
||||
Serial.print(aaReal.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaReal.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaReal.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_WORLDACCEL
|
||||
// display initial world-frame acceleration, adjusted to remove gravity
|
||||
// and rotated based on known orientation from quaternion
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
|
||||
Serial.print("aworld\t");
|
||||
Serial.print(aaWorld.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaWorld.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaWorld.z);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
/**************************************************************************/
|
||||
/*
|
||||
Arduino loop function, called once 'setup' is complete (your own code
|
||||
should go here)
|
||||
*/
|
||||
/**************************************************************************/
|
||||
void loop(void)
|
||||
{
|
||||
if (WiFi.status() != WL_CONNECTED) {
|
||||
Serial.println();
|
||||
Serial.println("*** Disconnected from AP so rebooting ***");
|
||||
Serial.println();
|
||||
ESP.reset();
|
||||
}
|
||||
|
||||
mpu_loop();
|
||||
}
|
@ -0,0 +1,188 @@
|
||||
// I2C device class (I2Cdev) demonstration Processing sketch for MPU6050 DMP output
|
||||
// 6/20/2012 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// 2012-06-20 - initial release
|
||||
|
||||
/* ============================================
|
||||
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.
|
||||
===============================================
|
||||
*/
|
||||
|
||||
/**
|
||||
* MPUOSCTeapot Processing demo for MPU6050 DMP modified for OSC
|
||||
* https://gitub.com/jrowberg/i2cdevlib
|
||||
* The original demo uses serial port I/O which has been replaced with
|
||||
* OSC UDP messages in this sketch.
|
||||
*
|
||||
* The MPU6050 is connected to an ESP8266 with battery so it is completely
|
||||
* wire free.
|
||||
*
|
||||
* Tested on Processing 3.3.5 running on Ubuntu Linux 14.04
|
||||
*
|
||||
* Dependencies installed using Library Manager
|
||||
*
|
||||
* Open Sound Control library
|
||||
* oscP5 website at http://www.sojamo.de/oscP5
|
||||
* ToxicLibs
|
||||
* quaternion functions http://toxiclibs.org/
|
||||
*
|
||||
*/
|
||||
|
||||
// Install oscP5 using the IDE library manager.
|
||||
// From the IDE menu bar, Sketch | Import Library | Add library.
|
||||
// In the search box type "osc".
|
||||
import oscP5.*;
|
||||
import netP5.*;
|
||||
// Install ToxicLibs using the IDE library manager
|
||||
// From the IDE menu bar, Sketch | Import Library | Add library.
|
||||
// In the search box type "toxic".
|
||||
import toxi.geom.*;
|
||||
import toxi.processing.*;
|
||||
|
||||
ToxiclibsSupport gfx;
|
||||
|
||||
Quaternion quat = new Quaternion(1, 0, 0, 0);
|
||||
|
||||
OscP5 oscP5;
|
||||
|
||||
void setup() {
|
||||
// 300px square viewport using OpenGL rendering
|
||||
size(300, 300, P3D);
|
||||
gfx = new ToxiclibsSupport(this);
|
||||
|
||||
// setup lights and antialiasing
|
||||
lights();
|
||||
smooth();
|
||||
|
||||
/* start oscP5, listening for incoming messages at port 9999 */
|
||||
oscP5 = new OscP5(this, 9999);
|
||||
|
||||
oscP5.plug(this, "imu", "/imuquat");
|
||||
}
|
||||
|
||||
/* incoming osc message are forwarded to the oscEvent method. */
|
||||
void oscEvent(OscMessage theOscMessage) {
|
||||
/* print the address pattern and the typetag of the received OscMessage */
|
||||
//print("### received an osc message.");
|
||||
//print(" addrpattern: "+theOscMessage.addrPattern());
|
||||
//println(" typetag: "+theOscMessage.typetag());
|
||||
}
|
||||
|
||||
public void imu(float quant_w, float quant_x, float quant_y, float quant_z) {
|
||||
//println(quant_w, quant_x, quant_y, quant_z);
|
||||
quat.set(quant_w, quant_x, quant_y, quant_z);
|
||||
}
|
||||
|
||||
void draw() {
|
||||
// black background
|
||||
background(0);
|
||||
|
||||
// translate everything to the middle of the viewport
|
||||
pushMatrix();
|
||||
translate(width / 2, height / 2);
|
||||
|
||||
// 3-step rotation from yaw/pitch/roll angles (gimbal lock!)
|
||||
// ...and other weirdness I haven't figured out yet
|
||||
//rotateY(-ypr[0]);
|
||||
//rotateZ(-ypr[1]);
|
||||
//rotateX(-ypr[2]);
|
||||
|
||||
// toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!)
|
||||
// (axis order [1, 3, 2] and inversion [-1, +1, +1] is a consequence of
|
||||
// different coordinate system orientation assumptions between Processing
|
||||
// and InvenSense DMP)
|
||||
float[] axis = quat.toAxisAngle();
|
||||
rotate(axis[0], -axis[1], axis[3], axis[2]);
|
||||
|
||||
// draw main body in red
|
||||
fill(255, 0, 0, 200);
|
||||
box(10, 10, 200);
|
||||
|
||||
// draw front-facing tip in blue
|
||||
fill(0, 0, 255, 200);
|
||||
pushMatrix();
|
||||
translate(0, 0, -120);
|
||||
rotateX(PI/2);
|
||||
drawCylinder(0, 20, 20, 8);
|
||||
popMatrix();
|
||||
|
||||
// draw wings and tail fin in green
|
||||
fill(0, 255, 0, 200);
|
||||
beginShape(TRIANGLES);
|
||||
vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); // wing top layer
|
||||
vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); // wing bottom layer
|
||||
vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); // tail left layer
|
||||
vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); // tail right layer
|
||||
endShape();
|
||||
beginShape(QUADS);
|
||||
vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80);
|
||||
vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80);
|
||||
vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30);
|
||||
vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98);
|
||||
vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70);
|
||||
vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70);
|
||||
endShape();
|
||||
|
||||
popMatrix();
|
||||
}
|
||||
|
||||
void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) {
|
||||
float angle = 0;
|
||||
float angleIncrement = TWO_PI / sides;
|
||||
beginShape(QUAD_STRIP);
|
||||
for (int i = 0; i < sides + 1; ++i) {
|
||||
vertex(topRadius*cos(angle), 0, topRadius*sin(angle));
|
||||
vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle));
|
||||
angle += angleIncrement;
|
||||
}
|
||||
endShape();
|
||||
|
||||
// If it is not a cone, draw the circular top cap
|
||||
if (topRadius != 0) {
|
||||
angle = 0;
|
||||
beginShape(TRIANGLE_FAN);
|
||||
|
||||
// Center point
|
||||
vertex(0, 0, 0);
|
||||
for (int i = 0; i < sides + 1; i++) {
|
||||
vertex(topRadius * cos(angle), 0, topRadius * sin(angle));
|
||||
angle += angleIncrement;
|
||||
}
|
||||
endShape();
|
||||
}
|
||||
|
||||
// If it is not a cone, draw the circular bottom cap
|
||||
if (bottomRadius != 0) {
|
||||
angle = 0;
|
||||
beginShape(TRIANGLE_FAN);
|
||||
|
||||
// Center point
|
||||
vertex(0, tall, 0);
|
||||
for (int i = 0; i < sides + 1; i++) {
|
||||
vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));
|
||||
angle += angleIncrement;
|
||||
}
|
||||
endShape();
|
||||
}
|
||||
}
|
@ -0,0 +1,545 @@
|
||||
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) over Ethernet
|
||||
// 2/27/2016 by hellphoenix
|
||||
// Based on I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) (6/21/2012 by Jeff Rowberg <jeff@rowberg.net>)
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// 2016-04-18 - Eliminated a potential infinite loop
|
||||
// 2016-02-28 - Cleaned up code to be in line with other example codes
|
||||
// - Added Ethernet outputs for Quaternion, Euler, RealAccel, WorldAccel
|
||||
// 2016-02-27 - Initial working code compiled
|
||||
// Bugs:
|
||||
// - There is still a hangup after some time, though it only occurs when you are reading data from the website.
|
||||
// If you only read the data from the serial port, there are no hangups.
|
||||
/* ============================================
|
||||
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.
|
||||
===============================================
|
||||
*/
|
||||
#include <Ethernet.h>
|
||||
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
|
||||
// for both classes must be in the include path of your project
|
||||
#include "I2Cdev.h"
|
||||
|
||||
#include "MPU6050_6Axis_MotionApps20.h"
|
||||
//#include "MPU6050.h" // not necessary if using MotionApps include file
|
||||
|
||||
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
|
||||
// is used in I2Cdev.h
|
||||
#include "Wire.h"
|
||||
#include "avr/wdt.h"// Watchdog library
|
||||
|
||||
// class default I2C address is 0x68
|
||||
// specific I2C addresses may be passed as a parameter here
|
||||
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
|
||||
// AD0 high = 0x69
|
||||
MPU6050 mpu;
|
||||
//MPU6050 mpu(0x69); // <-- use for AD0 high
|
||||
|
||||
// MAC address from Ethernet shield sticker under board
|
||||
byte mac[] = {
|
||||
0x90, 0xA2, 0xDA, 0x10, 0x26, 0x82
|
||||
};
|
||||
// assign an IP address for the controller:
|
||||
IPAddress ip(192,168,1,50);
|
||||
// the router's gateway address:
|
||||
byte gateway[] = { 192, 168, 1, 1 };
|
||||
// the subnet:
|
||||
byte subnet[] = { 255, 255, 0, 0 };
|
||||
|
||||
// Initialize the Ethernet server library
|
||||
// with the IP address and port you want to use
|
||||
// (port 80 is default for HTTP):
|
||||
EthernetServer server(80);
|
||||
|
||||
String HTTP_req; // stores the HTTP request
|
||||
|
||||
/* =========================================================================
|
||||
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
|
||||
depends on the MPU-6050's INT pin being connected to the Arduino's
|
||||
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
|
||||
digital I/O pin 2.
|
||||
* ========================================================================= */
|
||||
|
||||
/* =========================================================================
|
||||
NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
|
||||
when using Serial.write(buf, len). The Teapot output uses this method.
|
||||
The solution requires a modification to the Arduino USBAPI.h file, which
|
||||
is fortunately simple, but annoying. This will be fixed in the next IDE
|
||||
release. For more info, see these links:
|
||||
|
||||
http://arduino.cc/forum/index.php/topic,109987.0.html
|
||||
http://code.google.com/p/arduino/issues/detail?id=958
|
||||
* ========================================================================= */
|
||||
|
||||
|
||||
// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
|
||||
// quaternion components in a [w, x, y, z] format (not best for parsing
|
||||
// on a remote host such as Processing or something though)
|
||||
//#define OUTPUT_READABLE_QUATERNION
|
||||
|
||||
// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
|
||||
// (in degrees) calculated from the quaternions coming from the FIFO.
|
||||
// Note that Euler angles suffer from gimbal lock (for more info, see
|
||||
// http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
//#define OUTPUT_READABLE_EULER
|
||||
|
||||
// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
|
||||
// pitch/roll angles (in degrees) calculated from the quaternions coming
|
||||
// from the FIFO. Note this also requires gravity vector calculations.
|
||||
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
|
||||
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
#define OUTPUT_READABLE_YAWPITCHROLL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
|
||||
// components with gravity removed. This acceleration reference frame is
|
||||
// not compensated for orientation, so +X is always +X according to the
|
||||
// sensor, just without the effects of gravity. If you want acceleration
|
||||
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
|
||||
//#define OUTPUT_READABLE_REALACCEL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
|
||||
// components with gravity removed and adjusted for the world frame of
|
||||
// reference (yaw is relative to initial orientation, since no magnetometer
|
||||
// is present in this case). Could be quite handy in some cases.
|
||||
//#define OUTPUT_READABLE_WORLDACCEL
|
||||
|
||||
// uncomment "OUTPUT_TEAPOT" if you want output that matches the
|
||||
// format used for the InvenSense teapot demo
|
||||
//#define OUTPUT_TEAPOT
|
||||
|
||||
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
|
||||
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
|
||||
bool blinkState = false;
|
||||
|
||||
// MPU control/status vars
|
||||
bool dmpReady = false; // set true if DMP init was successful
|
||||
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
|
||||
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
|
||||
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
|
||||
uint16_t fifoCount; // count of all bytes currently in FIFO
|
||||
uint8_t fifoBuffer[64]; // FIFO storage buffer
|
||||
|
||||
// orientation/motion vars
|
||||
Quaternion q; // [w, x, y, z] quaternion container
|
||||
VectorInt16 aa; // [x, y, z] accel sensor measurements
|
||||
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
|
||||
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
|
||||
VectorFloat gravity; // [x, y, z] gravity vector
|
||||
float euler[3]; // [psi, theta, phi] Euler angle container
|
||||
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
||||
|
||||
// packet structure for InvenSense teapot demo
|
||||
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === INTERRUPT DETECTION ROUTINE ===
|
||||
// ================================================================
|
||||
|
||||
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
|
||||
void dmpDataReady() {
|
||||
mpuInterrupt = true;
|
||||
}
|
||||
// ================================================================
|
||||
// === INITIAL SETUP ===
|
||||
// ================================================================
|
||||
|
||||
void setup() {
|
||||
wdt_enable(WDTO_1S); //Watchdog enable.
|
||||
//WDTO_1S sets the watchdog timer to 1 second. The time set here is approximate.
|
||||
// You can find more time settings at http://www.nongnu.org/avr-libc/user-manual/group__avr__watchdog.html .
|
||||
|
||||
// join I2C bus (I2Cdev library doesn't do this automatically)
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
Wire.begin();
|
||||
Wire.setClock(400000); // 400kHz I2C clock (200kHz if CPU is 8MHz). Comment this line if having compilation difficulties
|
||||
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
||||
Fastwire::setup(400, true);
|
||||
#endif
|
||||
|
||||
// initialize serial communication
|
||||
// (115200 chosen because it is required for Teapot Demo output, but it's
|
||||
// really up to you depending on your project)
|
||||
Serial.begin(115200);
|
||||
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
|
||||
// Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
|
||||
// the baud timing being too misaligned with processor ticks. You must use
|
||||
// 38400 or slower in these cases, or use some kind of external separate
|
||||
// crystal solution for the UART timer.
|
||||
|
||||
Ethernet.begin(mac, ip, gateway, subnet);
|
||||
server.begin();
|
||||
Serial.print("server is at ");
|
||||
Serial.println(Ethernet.localIP());
|
||||
while (!Serial); // wait for Leonardo enumeration, others continue immediately
|
||||
|
||||
// initialize device
|
||||
Serial.println(F("Initializing I2C devices..."));
|
||||
mpu.initialize();
|
||||
pinMode(INTERRUPT_PIN, INPUT);
|
||||
|
||||
// verify connection
|
||||
Serial.println(F("Testing device connections..."));
|
||||
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
|
||||
|
||||
// load and configure the DMP
|
||||
Serial.println(F("Initializing DMP..."));
|
||||
devStatus = mpu.dmpInitialize();
|
||||
|
||||
// supply your own gyro offsets here, scaled for min sensitivity
|
||||
mpu.setXGyroOffset(220);
|
||||
mpu.setYGyroOffset(76);
|
||||
mpu.setZGyroOffset(-85);
|
||||
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
|
||||
|
||||
// make sure it worked (returns 0 if so)
|
||||
if (devStatus == 0) {
|
||||
// turn on the DMP, now that it's ready
|
||||
Serial.println(F("Enabling DMP..."));
|
||||
mpu.setDMPEnabled(true);
|
||||
|
||||
// enable Arduino interrupt detection
|
||||
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
|
||||
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
|
||||
Serial.println(F(")..."));
|
||||
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
|
||||
mpuIntStatus = mpu.getIntStatus();
|
||||
|
||||
// set our DMP Ready flag so the main loop() function knows it's okay to use it
|
||||
Serial.println(F("DMP ready! Waiting for first interrupt..."));
|
||||
dmpReady = true;
|
||||
|
||||
// get expected DMP packet size for later comparison
|
||||
packetSize = mpu.dmpGetFIFOPacketSize();
|
||||
} else {
|
||||
// ERROR!
|
||||
// 1 = initial memory load failed
|
||||
// 2 = DMP configuration updates failed
|
||||
// (if it's going to break, usually the code will be 1)
|
||||
Serial.print(F("DMP Initialization failed (code "));
|
||||
Serial.print(devStatus);
|
||||
Serial.println(F(")"));
|
||||
}
|
||||
|
||||
// configure LED for output
|
||||
pinMode(LED_PIN, OUTPUT);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === MAIN PROGRAM LOOP ===
|
||||
// ================================================================
|
||||
|
||||
void loop() {
|
||||
// if programming failed, don't try to do anything
|
||||
if (!dmpReady) return;
|
||||
|
||||
wdt_reset();//Resets the watchdog timer. If the timer is not reset, and the timer expires, a watchdog-initiated device reset will occur.
|
||||
// wait for MPU interrupt or extra packet(s) available
|
||||
while (!mpuInterrupt && fifoCount < packetSize) {
|
||||
if (mpuInterrupt && fifoCount < packetSize) {
|
||||
// try to get out of the infinite loop
|
||||
fifoCount = mpu.getFIFOCount();
|
||||
}
|
||||
// other program behavior stuff here
|
||||
// .
|
||||
// .
|
||||
// if you are really paranoid you can frequently test in between other
|
||||
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
|
||||
// while() loop to immediately process the MPU data
|
||||
// .
|
||||
// .
|
||||
// .
|
||||
}
|
||||
|
||||
// reset interrupt flag and get INT_STATUS byte
|
||||
mpuInterrupt = false;
|
||||
mpuIntStatus = mpu.getIntStatus();
|
||||
|
||||
// get current FIFO count
|
||||
fifoCount = mpu.getFIFOCount();
|
||||
|
||||
// check for overflow (this should never happen unless our code is too inefficient)
|
||||
if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
|
||||
// reset so we can continue cleanly
|
||||
mpu.resetFIFO();
|
||||
fifoCount = mpu.getFIFOCount();
|
||||
Serial.println(F("FIFO overflow!"));
|
||||
|
||||
// otherwise, check for DMP data ready interrupt (this should happen frequently)
|
||||
} else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
|
||||
// wait for correct available data length, should be a VERY short wait
|
||||
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
|
||||
|
||||
// read a packet from FIFO, then clear the buffer
|
||||
mpu.getFIFOBytes(fifoBuffer, packetSize);
|
||||
//mpu.resetFIFO();
|
||||
|
||||
// track FIFO count here in case there is > 1 packet available
|
||||
// (this lets us immediately read more without waiting for an interrupt)
|
||||
fifoCount -= packetSize;
|
||||
|
||||
#ifdef OUTPUT_READABLE_QUATERNION
|
||||
// display quaternion values in easy matrix form: w x y z
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
Serial.print("quat\t");
|
||||
Serial.print(q.w);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(q.z);
|
||||
#endif
|
||||
#ifdef OUTPUT_READABLE_EULER
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetEuler(euler, &q);
|
||||
Serial.print("euler\t");
|
||||
Serial.print(euler[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(euler[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(euler[2] * 180/M_PI);
|
||||
#endif
|
||||
#ifdef OUTPUT_READABLE_YAWPITCHROLL
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
||||
Serial.print("ypr\t");
|
||||
Serial.print(ypr[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(ypr[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(ypr[2] * 180/M_PI);
|
||||
#endif
|
||||
#ifdef OUTPUT_READABLE_REALACCEL
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
Serial.print("areal\t");
|
||||
Serial.print(aaReal.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaReal.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaReal.z);
|
||||
#endif
|
||||
#ifdef OUTPUT_READABLE_WORLDACCEL
|
||||
// display initial world-frame acceleration, adjusted to remove gravity
|
||||
// and rotated based on known orientation from quaternion
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
|
||||
Serial.print("aworld\t");
|
||||
Serial.print(aaWorld.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaWorld.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaWorld.z);
|
||||
#endif
|
||||
#ifdef OUTPUT_TEAPOT
|
||||
// display quaternion values in InvenSense Teapot demo format:
|
||||
teapotPacket[2] = fifoBuffer[0];
|
||||
teapotPacket[3] = fifoBuffer[1];
|
||||
teapotPacket[4] = fifoBuffer[4];
|
||||
teapotPacket[5] = fifoBuffer[5];
|
||||
teapotPacket[6] = fifoBuffer[8];
|
||||
teapotPacket[7] = fifoBuffer[9];
|
||||
teapotPacket[8] = fifoBuffer[12];
|
||||
teapotPacket[9] = fifoBuffer[13];
|
||||
Serial.write(teapotPacket, 14);
|
||||
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
|
||||
#endif
|
||||
serversend();
|
||||
// blink LED to indicate activity
|
||||
blinkState = !blinkState;
|
||||
digitalWrite(LED_PIN, blinkState);
|
||||
}
|
||||
}
|
||||
|
||||
void serversend(){
|
||||
|
||||
EthernetClient client = server.available(); // try to get client
|
||||
|
||||
if (client) { // got client?
|
||||
//boolean currentLineIsBlank = true;
|
||||
while (client.connected()) {
|
||||
if (client.available()) { // client data available to read
|
||||
char c = client.read(); // read 1 byte (character) from client
|
||||
HTTP_req += c; // save the HTTP request 1 char at a time
|
||||
// last line of client request is blank and ends with \n
|
||||
// respond to client only after last line received
|
||||
if (c == '\n') {
|
||||
// send a standard http response header
|
||||
client.println("HTTP/1.1 200 OK");
|
||||
client.println("Content-Type: text/html");
|
||||
//client.println("Connection: keep-alive");
|
||||
client.println();
|
||||
// AJAX request for switch state
|
||||
if (HTTP_req.indexOf("ajax_switch") > -1) {
|
||||
// read switch state and analog input
|
||||
GetAjaxData(client);
|
||||
}
|
||||
else { // HTTP request for web page
|
||||
// send web page - contains JavaScript with AJAX calls
|
||||
client.println("<!DOCTYPE html>");
|
||||
client.println("<html>");
|
||||
client.println("<head>");
|
||||
client.println("<title>Arduino Web Page</title>");
|
||||
client.println("<script>");
|
||||
client.println("function GetAjaxData() {");
|
||||
client.println(
|
||||
"nocache = \"&nocache=\" + Math.random() * 1000000;");
|
||||
client.println("var request = new XMLHttpRequest();");
|
||||
client.println("request.onreadystatechange = function() {");
|
||||
client.println("if (this.readyState == 4) {");
|
||||
client.println("if (this.status == 200) {");
|
||||
client.println("if (this.responseText != null) {");
|
||||
client.println("document.getElementById(\"sw_an_data\")\
|
||||
.innerHTML = this.responseText;");
|
||||
client.println("}}}}");
|
||||
client.println(
|
||||
"request.open(\"GET\", \"ajax_switch\" + nocache, true);");
|
||||
client.println("request.send(null);");
|
||||
client.println("setTimeout('GetAjaxData()', 10);");
|
||||
client.println("}");
|
||||
client.println("</script>");
|
||||
client.println("</head>");
|
||||
client.println("<body onload=\"GetAjaxData()\">");
|
||||
client.println("<h1>MPU6050 Output</h1>");
|
||||
client.println("<div id=\"sw_an_data\">");
|
||||
client.println("</div>");
|
||||
client.println("</body>");
|
||||
client.println("</html>");
|
||||
}
|
||||
// display received HTTP request on serial port
|
||||
Serial.print(HTTP_req);
|
||||
HTTP_req = ""; // finished with request, empty string
|
||||
client.stop(); // close the connection
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void GetAjaxData(EthernetClient cl)
|
||||
{
|
||||
#ifdef OUTPUT_READABLE_QUATERNION
|
||||
// display quaternion values in easy matrix form: w x y z
|
||||
cl.print("Quaternion Values:\t");
|
||||
cl.print("<p>w:");
|
||||
cl.print(q.w);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
cl.print("<p>x:");
|
||||
cl.print(q.x);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
cl.print("<p>y:");
|
||||
cl.print(q.y);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
cl.print("<p>z:");
|
||||
cl.print(q.z);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
#endif
|
||||
#ifdef OUTPUT_READABLE_EULER
|
||||
// display Euler angles in degrees
|
||||
cl.print("Euler Angles:\t");
|
||||
cl.print("<p>Yaw:");
|
||||
cl.print(euler[0] * 180/M_PI);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
cl.print("<p>Pitch:");
|
||||
cl.print(euler[2] * 180/M_PI);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
cl.print("<p>Roll:");
|
||||
cl.print(euler[1] * 180/M_PI);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
#endif
|
||||
#ifdef OUTPUT_READABLE_YAWPITCHROLL
|
||||
// display Yaw/Pitch/Roll values in degrees
|
||||
cl.print("Yaw, Pitch, and Roll:\t");
|
||||
cl.print("<p>Yaw:");
|
||||
cl.print(ypr[0] * 180/M_PI);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
cl.print("<p>Pitch:");
|
||||
cl.print(ypr[2] * 180/M_PI);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
cl.print("<p>Roll:");
|
||||
cl.print(ypr[1] * 180/M_PI);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
#endif
|
||||
#ifdef OUTPUT_READABLE_REALACCEL
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
cl.print("Real Accel:\t");
|
||||
cl.print("<p>Yaw:");
|
||||
cl.print(aaReal.x);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
cl.print("<p>Pitch:");
|
||||
cl.print(aaReal.z);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
cl.print("<p>Roll:");
|
||||
cl.print(aaReal.y);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
#endif
|
||||
#ifdef OUTPUT_READABLE_WORLDACCEL
|
||||
// display initial world-frame acceleration, adjusted to remove gravity
|
||||
// and rotated based on known orientation from quaternion
|
||||
cl.print("World Accel:\t");
|
||||
cl.print("<p>Yaw:");
|
||||
cl.print(aaWorld.x);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
cl.print("<p>Pitch:");
|
||||
cl.print(aaWorld.z);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
cl.print("<p>Roll:");
|
||||
cl.print(aaWorld.y);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
#endif
|
||||
#ifdef OUTPUT_TEAPOT
|
||||
cl.print("<p>teapotpacket:");
|
||||
cl.write(teapotPacket, 14);
|
||||
cl.print("\t");
|
||||
cl.println("</p>");
|
||||
#endif
|
||||
}
|
@ -0,0 +1,152 @@
|
||||
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
|
||||
// 10/7/2011 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// 2013-05-08 - added multiple output formats
|
||||
// - added seamless Fastwire support
|
||||
// 2011-10-07 - initial release
|
||||
|
||||
/* ============================================
|
||||
I2Cdev device library code is placed under the MIT license
|
||||
Copyright (c) 2011 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.
|
||||
===============================================
|
||||
*/
|
||||
|
||||
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
|
||||
// for both classes must be in the include path of your project
|
||||
#include "I2Cdev.h"
|
||||
#include "MPU6050.h"
|
||||
|
||||
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
|
||||
// is used in I2Cdev.h
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
#include "Wire.h"
|
||||
#endif
|
||||
|
||||
// class default I2C address is 0x68
|
||||
// specific I2C addresses may be passed as a parameter here
|
||||
// AD0 low = 0x68 (default for InvenSense evaluation board)
|
||||
// AD0 high = 0x69
|
||||
MPU6050 accelgyro;
|
||||
//MPU6050 accelgyro(0x69); // <-- use for AD0 high
|
||||
|
||||
int16_t ax, ay, az;
|
||||
int16_t gx, gy, gz;
|
||||
|
||||
|
||||
|
||||
// uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated
|
||||
// list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read,
|
||||
// not so easy to parse, and slow(er) over UART.
|
||||
#define OUTPUT_READABLE_ACCELGYRO
|
||||
|
||||
// uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit
|
||||
// binary, one right after the other. This is very fast (as fast as possible
|
||||
// without compression or data loss), and easy to parse, but impossible to read
|
||||
// for a human.
|
||||
//#define OUTPUT_BINARY_ACCELGYRO
|
||||
|
||||
|
||||
#define LED_PIN 13
|
||||
bool blinkState = false;
|
||||
|
||||
void setup() {
|
||||
// join I2C bus (I2Cdev library doesn't do this automatically)
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
Wire.begin();
|
||||
//Wire.begin(32,33);
|
||||
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
||||
Fastwire::setup(400, true);
|
||||
#endif
|
||||
|
||||
// initialize serial communication
|
||||
// (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
|
||||
// it's really up to you depending on your project)
|
||||
Serial.begin(115200);
|
||||
|
||||
// initialize device
|
||||
Serial.println("Initializing I2C devices...");
|
||||
accelgyro.initialize();
|
||||
|
||||
// verify connection
|
||||
Serial.println("Testing device connections...");
|
||||
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
|
||||
|
||||
// use the code below to change accel/gyro offset values
|
||||
/*
|
||||
Serial.println("Updating internal sensor offsets...");
|
||||
// -76 -2359 1688 0 0 0
|
||||
Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76
|
||||
Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359
|
||||
Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688
|
||||
Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0
|
||||
Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0
|
||||
Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0
|
||||
Serial.print("\n");
|
||||
accelgyro.setXGyroOffset(220);
|
||||
accelgyro.setYGyroOffset(76);
|
||||
accelgyro.setZGyroOffset(-85);
|
||||
Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76
|
||||
Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359
|
||||
Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688
|
||||
Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0
|
||||
Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0
|
||||
Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0
|
||||
Serial.print("\n");
|
||||
*/
|
||||
|
||||
// configure Arduino LED pin for output
|
||||
pinMode(LED_PIN, OUTPUT);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// read raw accel/gyro measurements from device
|
||||
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
|
||||
|
||||
// these methods (and a few others) are also available
|
||||
//accelgyro.getAcceleration(&ax, &ay, &az);
|
||||
//accelgyro.getRotation(&gx, &gy, &gz);
|
||||
|
||||
#ifdef OUTPUT_READABLE_ACCELGYRO
|
||||
// display tab-separated accel/gyro x/y/z values
|
||||
Serial.print("a/g:\t");
|
||||
Serial.print(ax); Serial.print("\t");
|
||||
Serial.print(ay); Serial.print("\t");
|
||||
Serial.print(az); Serial.print("\t");
|
||||
Serial.print(gx); Serial.print("\t");
|
||||
Serial.print(gy); Serial.print("\t");
|
||||
Serial.println(gz);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_BINARY_ACCELGYRO
|
||||
Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF));
|
||||
Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF));
|
||||
Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF));
|
||||
Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF));
|
||||
Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF));
|
||||
Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF));
|
||||
#endif
|
||||
|
||||
// blink LED to indicate activity
|
||||
blinkState = !blinkState;
|
||||
digitalWrite(LED_PIN, blinkState);
|
||||
}
|
46
2.Firmware/Libraries/MPU6050/keywords.txt
Normal file
46
2.Firmware/Libraries/MPU6050/keywords.txt
Normal file
@ -0,0 +1,46 @@
|
||||
#######################################
|
||||
# Syntax Coloring Map For MPU6050
|
||||
#######################################
|
||||
|
||||
#######################################
|
||||
# Class (KEYWORD1)
|
||||
#######################################
|
||||
MPU6050 KEYWORD1
|
||||
|
||||
#######################################
|
||||
# Methods and Functions (KEYWORD2)
|
||||
#######################################
|
||||
|
||||
initialize KEYWORD2
|
||||
testConnection KEYWORD2
|
||||
|
||||
getMotion6 KEYWORD2
|
||||
getMotion9 KEYWORD2
|
||||
getAccelerationX KEYWORD2
|
||||
getAccelerationY KEYWORD2
|
||||
getAccelerationZ KEYWORD2
|
||||
|
||||
getTemperature KEYWORD2
|
||||
|
||||
getRotation KEYWORD2
|
||||
getRotationX KEYWORD2
|
||||
getRotationY KEYWORD2
|
||||
getRotationZ KEYWORD2
|
||||
|
||||
setI2CMasterModeEnabled KEYWORD2
|
||||
setI2CBypassEnabled KEYWORD2
|
||||
setSleepEnabled KEYWORD2
|
||||
getFreefallDetectionThreshold KEYWORD2
|
||||
setFreefallDetectionThreshold KEYWORD2
|
||||
getFreefallDetectionDuration KEYWORD2
|
||||
setFreefallDetectionDuration KEYWORD2
|
||||
getMotionDetectionThreshold KEYWORD2
|
||||
setMotionDetectionThreshold KEYWORD2
|
||||
getMotionDetectionDuration KEYWORD2
|
||||
setMotionDetectionDuration KEYWORD2
|
||||
|
||||
|
||||
#######################################
|
||||
# Constants (LITERAL1)
|
||||
#######################################
|
||||
|
10
2.Firmware/Libraries/MPU6050/library.properties
Normal file
10
2.Firmware/Libraries/MPU6050/library.properties
Normal file
@ -0,0 +1,10 @@
|
||||
name=MPU6050
|
||||
version=0.0.1
|
||||
author=Electronic Cats
|
||||
maintainer=Electronic Cats <hola@electroniccats.com>
|
||||
sentence=MPU6050 Arduino Library.
|
||||
paragraph=MPU-6050 6-axis accelerometer/gyroscope Arduino Library.
|
||||
category=Sensors
|
||||
url=https://github.com/electroniccats/mpu6050
|
||||
architectures=avr,samd,esp32
|
||||
includes=MPU6050.h
|
1468
2.Firmware/Libraries/MPU6050/src/I2Cdev.cpp
Normal file
1468
2.Firmware/Libraries/MPU6050/src/I2Cdev.cpp
Normal file
File diff suppressed because it is too large
Load Diff
283
2.Firmware/Libraries/MPU6050/src/I2Cdev.h
Normal file
283
2.Firmware/Libraries/MPU6050/src/I2Cdev.h
Normal file
@ -0,0 +1,283 @@
|
||||
// I2Cdev library collection - Main I2C device class header file
|
||||
// Abstracts bit and byte I2C R/W functions into a convenient class
|
||||
// 2013-06-05 by Jeff Rowberg <jeff@rowberg.net>
|
||||
//
|
||||
// Changelog:
|
||||
// 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1
|
||||
// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications
|
||||
// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
|
||||
// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
|
||||
// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
|
||||
// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
|
||||
// 2011-10-03 - added automatic Arduino version detection for ease of use
|
||||
// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
|
||||
// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
|
||||
// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default
|
||||
// 2011-08-02 - added support for 16-bit registers
|
||||
// - fixed incorrect Doxygen comments on some methods
|
||||
// - added timeout value for read operations (thanks mem @ Arduino forums)
|
||||
// 2011-07-30 - changed read/write function structures to return success or byte counts
|
||||
// - made all methods static for multi-device memory savings
|
||||
// 2011-07-28 - initial release
|
||||
|
||||
/* ============================================
|
||||
I2Cdev device library code is placed under the MIT license
|
||||
Copyright (c) 2013 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 _I2CDEV_H_
|
||||
#define _I2CDEV_H_
|
||||
|
||||
// -----------------------------------------------------------------------------
|
||||
// I2C interface implementation setting
|
||||
// -----------------------------------------------------------------------------
|
||||
#ifndef I2CDEV_IMPLEMENTATION
|
||||
#define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE
|
||||
//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE
|
||||
//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE
|
||||
#endif // I2CDEV_IMPLEMENTATION
|
||||
|
||||
// comment this out if you are using a non-optimal IDE/implementation setting
|
||||
// but want the compiler to shut up about it
|
||||
#define I2CDEV_IMPLEMENTATION_WARNINGS
|
||||
|
||||
// -----------------------------------------------------------------------------
|
||||
// I2C interface implementation options
|
||||
// -----------------------------------------------------------------------------
|
||||
#define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino
|
||||
#define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project
|
||||
// ^^^ NBWire implementation is still buggy w/some interrupts!
|
||||
#define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project
|
||||
#define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library
|
||||
#define I2CDEV_BUILTIN_SBWIRE 5 // I2C object from Shuning (Steve) Bian's SBWire Library at https://github.com/freespace/SBWire
|
||||
|
||||
// -----------------------------------------------------------------------------
|
||||
// Arduino-style "Serial.print" debug constant (uncomment to enable)
|
||||
// -----------------------------------------------------------------------------
|
||||
//#define I2CDEV_SERIAL_DEBUG
|
||||
|
||||
#ifdef ARDUINO
|
||||
#if ARDUINO < 100
|
||||
#include "WProgram.h"
|
||||
#else
|
||||
#include "Arduino.h"
|
||||
#endif
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
#include <Wire.h>
|
||||
#endif
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY
|
||||
#include <I2C.h>
|
||||
#endif
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE
|
||||
#include "SBWire.h"
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef SPARK
|
||||
#include <spark_wiring_i2c.h>
|
||||
#define ARDUINO 101
|
||||
#endif
|
||||
|
||||
|
||||
// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];")
|
||||
#define I2CDEV_DEFAULT_READ_TIMEOUT 1000
|
||||
|
||||
class I2Cdev {
|
||||
public:
|
||||
I2Cdev();
|
||||
|
||||
static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
|
||||
static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
|
||||
static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
|
||||
static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
|
||||
static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
|
||||
static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
|
||||
static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
|
||||
static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
|
||||
|
||||
static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
|
||||
static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
|
||||
static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
|
||||
static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
|
||||
static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
|
||||
static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
|
||||
static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
|
||||
static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
|
||||
|
||||
static uint16_t readTimeout;
|
||||
};
|
||||
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
||||
//////////////////////
|
||||
// FastWire 0.24
|
||||
// This is a library to help faster programs to read I2C devices.
|
||||
// Copyright(C) 2012
|
||||
// Francesco Ferrara
|
||||
//////////////////////
|
||||
|
||||
/* Master */
|
||||
#define TW_START 0x08
|
||||
#define TW_REP_START 0x10
|
||||
|
||||
/* Master Transmitter */
|
||||
#define TW_MT_SLA_ACK 0x18
|
||||
#define TW_MT_SLA_NACK 0x20
|
||||
#define TW_MT_DATA_ACK 0x28
|
||||
#define TW_MT_DATA_NACK 0x30
|
||||
#define TW_MT_ARB_LOST 0x38
|
||||
|
||||
/* Master Receiver */
|
||||
#define TW_MR_ARB_LOST 0x38
|
||||
#define TW_MR_SLA_ACK 0x40
|
||||
#define TW_MR_SLA_NACK 0x48
|
||||
#define TW_MR_DATA_ACK 0x50
|
||||
#define TW_MR_DATA_NACK 0x58
|
||||
|
||||
#define TW_OK 0
|
||||
#define TW_ERROR 1
|
||||
|
||||
class Fastwire {
|
||||
private:
|
||||
static boolean waitInt();
|
||||
|
||||
public:
|
||||
static void setup(int khz, boolean pullup);
|
||||
static byte beginTransmission(byte device);
|
||||
static byte write(byte value);
|
||||
static byte writeBuf(byte device, byte address, byte *data, byte num);
|
||||
static byte readBuf(byte device, byte address, byte *data, byte num);
|
||||
static void reset();
|
||||
static byte stop();
|
||||
};
|
||||
#endif
|
||||
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
|
||||
// NBWire implementation based heavily on code by Gene Knight <Gene@Telobot.com>
|
||||
// Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html
|
||||
// Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html
|
||||
|
||||
#define NBWIRE_BUFFER_LENGTH 32
|
||||
|
||||
class TwoWire {
|
||||
private:
|
||||
static uint8_t rxBuffer[];
|
||||
static uint8_t rxBufferIndex;
|
||||
static uint8_t rxBufferLength;
|
||||
|
||||
static uint8_t txAddress;
|
||||
static uint8_t txBuffer[];
|
||||
static uint8_t txBufferIndex;
|
||||
static uint8_t txBufferLength;
|
||||
|
||||
// static uint8_t transmitting;
|
||||
static void (*user_onRequest)(void);
|
||||
static void (*user_onReceive)(int);
|
||||
static void onRequestService(void);
|
||||
static void onReceiveService(uint8_t*, int);
|
||||
|
||||
public:
|
||||
TwoWire();
|
||||
void begin();
|
||||
void begin(uint8_t);
|
||||
void begin(int);
|
||||
void beginTransmission(uint8_t);
|
||||
//void beginTransmission(int);
|
||||
uint8_t endTransmission(uint16_t timeout=0);
|
||||
void nbendTransmission(void (*function)(int)) ;
|
||||
uint8_t requestFrom(uint8_t, int, uint16_t timeout=0);
|
||||
//uint8_t requestFrom(int, int);
|
||||
void nbrequestFrom(uint8_t, int, void (*function)(int));
|
||||
void send(uint8_t);
|
||||
void send(uint8_t*, uint8_t);
|
||||
//void send(int);
|
||||
void send(char*);
|
||||
uint8_t available(void);
|
||||
uint8_t receive(void);
|
||||
void onReceive(void (*)(int));
|
||||
void onRequest(void (*)(void));
|
||||
};
|
||||
|
||||
#define TWI_READY 0
|
||||
#define TWI_MRX 1
|
||||
#define TWI_MTX 2
|
||||
#define TWI_SRX 3
|
||||
#define TWI_STX 4
|
||||
|
||||
#define TW_WRITE 0
|
||||
#define TW_READ 1
|
||||
|
||||
#define TW_MT_SLA_NACK 0x20
|
||||
#define TW_MT_DATA_NACK 0x30
|
||||
|
||||
#define CPU_FREQ 16000000L
|
||||
#define TWI_FREQ 100000L
|
||||
#define TWI_BUFFER_LENGTH 32
|
||||
|
||||
/* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */
|
||||
|
||||
#define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3))
|
||||
#define TW_STATUS (TWSR & TW_STATUS_MASK)
|
||||
#define TW_START 0x08
|
||||
#define TW_REP_START 0x10
|
||||
#define TW_MT_SLA_ACK 0x18
|
||||
#define TW_MT_SLA_NACK 0x20
|
||||
#define TW_MT_DATA_ACK 0x28
|
||||
#define TW_MT_DATA_NACK 0x30
|
||||
#define TW_MT_ARB_LOST 0x38
|
||||
#define TW_MR_ARB_LOST 0x38
|
||||
#define TW_MR_SLA_ACK 0x40
|
||||
#define TW_MR_SLA_NACK 0x48
|
||||
#define TW_MR_DATA_ACK 0x50
|
||||
#define TW_MR_DATA_NACK 0x58
|
||||
#define TW_ST_SLA_ACK 0xA8
|
||||
#define TW_ST_ARB_LOST_SLA_ACK 0xB0
|
||||
#define TW_ST_DATA_ACK 0xB8
|
||||
#define TW_ST_DATA_NACK 0xC0
|
||||
#define TW_ST_LAST_DATA 0xC8
|
||||
#define TW_SR_SLA_ACK 0x60
|
||||
#define TW_SR_ARB_LOST_SLA_ACK 0x68
|
||||
#define TW_SR_GCALL_ACK 0x70
|
||||
#define TW_SR_ARB_LOST_GCALL_ACK 0x78
|
||||
#define TW_SR_DATA_ACK 0x80
|
||||
#define TW_SR_DATA_NACK 0x88
|
||||
#define TW_SR_GCALL_DATA_ACK 0x90
|
||||
#define TW_SR_GCALL_DATA_NACK 0x98
|
||||
#define TW_SR_STOP 0xA0
|
||||
#define TW_NO_INFO 0xF8
|
||||
#define TW_BUS_ERROR 0x00
|
||||
|
||||
//#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr))
|
||||
//#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr))
|
||||
|
||||
#ifndef sbi // set bit
|
||||
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
|
||||
#endif // sbi
|
||||
|
||||
#ifndef cbi // clear bit
|
||||
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
|
||||
#endif // cbi
|
||||
|
||||
extern TwoWire Wire;
|
||||
|
||||
#endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE
|
||||
|
||||
#endif /* _I2CDEV_H_ */
|
3205
2.Firmware/Libraries/MPU6050/src/MPU6050.cpp
Normal file
3205
2.Firmware/Libraries/MPU6050/src/MPU6050.cpp
Normal file
File diff suppressed because it is too large
Load Diff
1031
2.Firmware/Libraries/MPU6050/src/MPU6050.h
Normal file
1031
2.Firmware/Libraries/MPU6050/src/MPU6050.h
Normal file
File diff suppressed because it is too large
Load Diff
782
2.Firmware/Libraries/MPU6050/src/MPU6050_6Axis_MotionApps20.h
Normal file
782
2.Firmware/Libraries/MPU6050/src/MPU6050_6Axis_MotionApps20.h
Normal file
@ -0,0 +1,782 @@
|
||||
// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation
|
||||
// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
|
||||
// 5/20/2013 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// ... - ongoing debug release
|
||||
|
||||
/* ============================================
|
||||
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 _MPU6050_6AXIS_MOTIONAPPS20_H_
|
||||
#define _MPU6050_6AXIS_MOTIONAPPS20_H_
|
||||
|
||||
#include "I2Cdev.h"
|
||||
#include "helper_3dmath.h"
|
||||
|
||||
// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board
|
||||
#define MPU6050_INCLUDE_DMP_MOTIONAPPS20
|
||||
|
||||
#include "MPU6050.h"
|
||||
|
||||
// Tom Carpenter's conditional PROGMEM code
|
||||
// http://forum.arduino.cc/index.php?topic=129407.0
|
||||
#ifdef __AVR__
|
||||
#include <avr/pgmspace.h>
|
||||
#else
|
||||
// Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
|
||||
#ifndef __PGMSPACE_H_
|
||||
#define __PGMSPACE_H_ 1
|
||||
#include <inttypes.h>
|
||||
|
||||
#define PROGMEM
|
||||
#define PGM_P const char *
|
||||
#define PSTR(str) (str)
|
||||
#define F(x) x
|
||||
|
||||
//typedef void prog_void;
|
||||
//typedef char prog_char;
|
||||
//typedef unsigned char prog_uchar;
|
||||
//typedef int8_t prog_int8_t;
|
||||
//typedef uint8_t prog_uint8_t;
|
||||
//typedef int16_t prog_int16_t;
|
||||
//typedef uint16_t prog_uint16_t;
|
||||
//typedef int32_t prog_int32_t;
|
||||
//typedef uint32_t prog_uint32_t;
|
||||
|
||||
#define strcpy_P(dest, src) strcpy((dest), (src))
|
||||
#define strcat_P(dest, src) strcat((dest), (src))
|
||||
#define strcmp_P(a, b) strcmp((a), (b))
|
||||
|
||||
#define pgm_read_byte(addr) (*(const unsigned char *)(addr))
|
||||
#define pgm_read_word(addr) (*(const unsigned short *)(addr))
|
||||
#define pgm_read_dword(addr) (*(const unsigned long *)(addr))
|
||||
#define pgm_read_float(addr) (*(const float *)(addr))
|
||||
|
||||
#define pgm_read_byte_near(addr) pgm_read_byte(addr)
|
||||
#define pgm_read_word_near(addr) pgm_read_word(addr)
|
||||
#define pgm_read_dword_near(addr) pgm_read_dword(addr)
|
||||
#define pgm_read_float_near(addr) pgm_read_float(addr)
|
||||
#define pgm_read_byte_far(addr) pgm_read_byte(addr)
|
||||
#define pgm_read_word_far(addr) pgm_read_word(addr)
|
||||
#define pgm_read_dword_far(addr) pgm_read_dword(addr)
|
||||
#define pgm_read_float_far(addr) pgm_read_float(addr)
|
||||
#endif
|
||||
#endif
|
||||
|
||||
/* Source is from the InvenSense MotionApps v2 demo code. Original source is
|
||||
* unavailable, unless you happen to be amazing as decompiling binary by
|
||||
* hand (in which case, please contact me, and I'm totally serious).
|
||||
*
|
||||
* Also, I'd like to offer many, many thanks to Noah Zerkin for all of the
|
||||
* DMP reverse-engineering he did to help make this bit of wizardry
|
||||
* possible.
|
||||
*/
|
||||
|
||||
// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size.
|
||||
// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno)
|
||||
// after moving string constants to flash memory storage using the F()
|
||||
// compiler macro (Arduino IDE 1.0+ required).
|
||||
|
||||
//#define DEBUG
|
||||
#ifdef DEBUG
|
||||
#define DEBUG_PRINT(x) Serial.print(x)
|
||||
#define DEBUG_PRINTF(x, y) Serial.print(x, y)
|
||||
#define DEBUG_PRINTLN(x) Serial.println(x)
|
||||
#define DEBUG_PRINTLNF(x, y) Serial.println(x, y)
|
||||
#else
|
||||
#define DEBUG_PRINT(x)
|
||||
#define DEBUG_PRINTF(x, y)
|
||||
#define DEBUG_PRINTLN(x)
|
||||
#define DEBUG_PRINTLNF(x, y)
|
||||
#endif
|
||||
|
||||
#define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[]
|
||||
#define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[]
|
||||
#define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[]
|
||||
|
||||
/* ================================================================================================ *
|
||||
| Default MotionApps v2.0 42-byte FIFO packet structure: |
|
||||
| |
|
||||
| [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
|
||||
| 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
|
||||
| |
|
||||
| [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
|
||||
| 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 |
|
||||
* ================================================================================================ */
|
||||
|
||||
// this block of memory gets written to the MPU on start-up, and it seems
|
||||
// to be volatile memory, so it has to be done each time (it only takes ~1
|
||||
// second though)
|
||||
const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
|
||||
// bank 0, 256 bytes
|
||||
0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
|
||||
0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
|
||||
0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
|
||||
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00,
|
||||
0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
|
||||
0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
|
||||
0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
|
||||
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
|
||||
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
|
||||
0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
|
||||
0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
|
||||
|
||||
// bank 1, 256 bytes
|
||||
0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
|
||||
0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
|
||||
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
|
||||
0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
|
||||
0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
|
||||
|
||||
// bank 2, 256 bytes
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
|
||||
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
|
||||
// bank 3, 256 bytes
|
||||
0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F,
|
||||
0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2,
|
||||
0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF,
|
||||
0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C,
|
||||
0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1,
|
||||
0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01,
|
||||
0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80,
|
||||
0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C,
|
||||
0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80,
|
||||
0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E,
|
||||
0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9,
|
||||
0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24,
|
||||
0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0,
|
||||
0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86,
|
||||
0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
|
||||
0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
|
||||
|
||||
// bank 4, 256 bytes
|
||||
0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
|
||||
0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
|
||||
0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
|
||||
0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
|
||||
0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
|
||||
0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
|
||||
0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
|
||||
0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
|
||||
0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
|
||||
0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
|
||||
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
|
||||
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
|
||||
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
|
||||
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
|
||||
0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
|
||||
0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
|
||||
|
||||
// bank 5, 256 bytes
|
||||
0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
|
||||
0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
|
||||
0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
|
||||
0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
|
||||
0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
|
||||
0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
|
||||
0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
|
||||
0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
|
||||
0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A,
|
||||
0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60,
|
||||
0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97,
|
||||
0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04,
|
||||
0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78,
|
||||
0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79,
|
||||
0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68,
|
||||
0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68,
|
||||
|
||||
// bank 6, 256 bytes
|
||||
0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04,
|
||||
0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66,
|
||||
0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31,
|
||||
0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60,
|
||||
0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76,
|
||||
0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56,
|
||||
0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD,
|
||||
0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91,
|
||||
0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8,
|
||||
0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE,
|
||||
0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9,
|
||||
0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD,
|
||||
0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E,
|
||||
0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8,
|
||||
0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89,
|
||||
0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79,
|
||||
|
||||
// bank 7, 138 bytes (remainder)
|
||||
0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8,
|
||||
0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA,
|
||||
0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB,
|
||||
0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3,
|
||||
0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3,
|
||||
0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
|
||||
0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3,
|
||||
0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC,
|
||||
0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
|
||||
};
|
||||
|
||||
#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR
|
||||
#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01
|
||||
#endif
|
||||
|
||||
// thanks to Noah Zerkin for piecing this stuff together!
|
||||
const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
|
||||
// BANK OFFSET LENGTH [DATA]
|
||||
0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration
|
||||
0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration
|
||||
0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration
|
||||
0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration
|
||||
0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration
|
||||
0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration
|
||||
0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration
|
||||
0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration
|
||||
0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration
|
||||
0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01
|
||||
0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02
|
||||
0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10
|
||||
0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11
|
||||
0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12
|
||||
0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20
|
||||
0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21
|
||||
0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22
|
||||
0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel
|
||||
0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors
|
||||
0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
|
||||
0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update
|
||||
0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone
|
||||
// SPECIAL 0x01 = enable interrupts
|
||||
0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION
|
||||
0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt
|
||||
0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion
|
||||
0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer
|
||||
0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro
|
||||
0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo
|
||||
0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo
|
||||
0x02, 0x16, 0x02, 0x00, MPU6050_DMP_FIFO_RATE_DIVISOR // D_0_22 inv_set_fifo_rate
|
||||
|
||||
// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
|
||||
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
|
||||
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
|
||||
|
||||
// It is important to make sure the host processor can keep up with reading and processing
|
||||
// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
|
||||
};
|
||||
|
||||
const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = {
|
||||
0x01, 0xB2, 0x02, 0xFF, 0xFF,
|
||||
0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35,
|
||||
0x01, 0x6A, 0x02, 0x06, 0x00,
|
||||
0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
|
||||
0x01, 0x62, 0x02, 0x00, 0x00,
|
||||
0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00
|
||||
};
|
||||
|
||||
uint8_t MPU6050::dmpInitialize() {
|
||||
// reset device
|
||||
DEBUG_PRINTLN(F("\n\nResetting MPU6050..."));
|
||||
reset();
|
||||
delay(30); // wait after reset
|
||||
|
||||
// enable sleep mode and wake cycle
|
||||
/*Serial.println(F("Enabling sleep mode..."));
|
||||
setSleepEnabled(true);
|
||||
Serial.println(F("Enabling wake cycle..."));
|
||||
setWakeCycleEnabled(true);*/
|
||||
|
||||
// disable sleep mode
|
||||
DEBUG_PRINTLN(F("Disabling sleep mode..."));
|
||||
setSleepEnabled(false);
|
||||
|
||||
// get MPU hardware revision
|
||||
DEBUG_PRINTLN(F("Selecting user bank 16..."));
|
||||
setMemoryBank(0x10, true, true);
|
||||
DEBUG_PRINTLN(F("Selecting memory byte 6..."));
|
||||
setMemoryStartAddress(0x06);
|
||||
DEBUG_PRINTLN(F("Checking hardware revision..."));
|
||||
DEBUG_PRINT(F("Revision @ user[16][6] = "));
|
||||
DEBUG_PRINTLNF(readMemoryByte(), HEX);
|
||||
DEBUG_PRINTLN(F("Resetting memory bank selection to 0..."));
|
||||
setMemoryBank(0, false, false);
|
||||
|
||||
// check OTP bank valid
|
||||
DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));
|
||||
DEBUG_PRINT(F("OTP bank is "));
|
||||
DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!"));
|
||||
|
||||
// get X/Y/Z gyro offsets
|
||||
DEBUG_PRINTLN(F("Reading gyro offset TC values..."));
|
||||
int8_t xgOffsetTC = getXGyroOffsetTC();
|
||||
int8_t ygOffsetTC = getYGyroOffsetTC();
|
||||
int8_t zgOffsetTC = getZGyroOffsetTC();
|
||||
DEBUG_PRINT(F("X gyro offset = "));
|
||||
DEBUG_PRINTLN(xgOffsetTC);
|
||||
DEBUG_PRINT(F("Y gyro offset = "));
|
||||
DEBUG_PRINTLN(ygOffsetTC);
|
||||
DEBUG_PRINT(F("Z gyro offset = "));
|
||||
DEBUG_PRINTLN(zgOffsetTC);
|
||||
|
||||
// setup weird slave stuff (?)
|
||||
DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F..."));
|
||||
setSlaveAddress(0, 0x7F);
|
||||
DEBUG_PRINTLN(F("Disabling I2C Master mode..."));
|
||||
setI2CMasterModeEnabled(false);
|
||||
DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)..."));
|
||||
setSlaveAddress(0, 0x68);
|
||||
DEBUG_PRINTLN(F("Resetting I2C Master control..."));
|
||||
resetI2CMaster();
|
||||
delay(20);
|
||||
|
||||
// load DMP code into memory banks
|
||||
DEBUG_PRINT(F("Writing DMP code to MPU memory banks ("));
|
||||
DEBUG_PRINT(MPU6050_DMP_CODE_SIZE);
|
||||
DEBUG_PRINTLN(F(" bytes)"));
|
||||
if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
|
||||
DEBUG_PRINTLN(F("Success! DMP code written and verified."));
|
||||
|
||||
// write DMP configuration
|
||||
DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks ("));
|
||||
DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE);
|
||||
DEBUG_PRINTLN(F(" bytes in config def)"));
|
||||
if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
|
||||
DEBUG_PRINTLN(F("Success! DMP configuration written and verified."));
|
||||
|
||||
DEBUG_PRINTLN(F("Setting clock source to Z Gyro..."));
|
||||
setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
|
||||
setIntEnabled(1<<MPU6050_INTERRUPT_FIFO_OFLOW_BIT|1<<MPU6050_INTERRUPT_DMP_INT_BIT);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
|
||||
setRate(4); // 1khz / (1 + 4) = 200 Hz
|
||||
|
||||
DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
|
||||
setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
|
||||
setDLPFMode(MPU6050_DLPF_BW_42);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
|
||||
setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting DMP programm start address"));
|
||||
//write start address MSB into register
|
||||
setDMPConfig1(0x03);
|
||||
//write start address LSB into register
|
||||
setDMPConfig2(0x00);
|
||||
|
||||
DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
|
||||
setOTPBankValid(false);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values..."));
|
||||
setXGyroOffsetTC(xgOffsetTC);
|
||||
setYGyroOffsetTC(ygOffsetTC);
|
||||
setZGyroOffsetTC(zgOffsetTC);
|
||||
|
||||
//DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
|
||||
//setXGyroOffset(0);
|
||||
//setYGyroOffset(0);
|
||||
//setZGyroOffset(0);
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)..."));
|
||||
uint8_t dmpUpdate[16], j;
|
||||
uint16_t pos = 0;
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Resetting FIFO..."));
|
||||
resetFIFO();
|
||||
|
||||
DEBUG_PRINTLN(F("Reading FIFO count..."));
|
||||
uint16_t fifoCount = getFIFOCount();
|
||||
uint8_t fifoBuffer[128];
|
||||
|
||||
DEBUG_PRINT(F("Current FIFO count="));
|
||||
DEBUG_PRINTLN(fifoCount);
|
||||
getFIFOBytes(fifoBuffer, fifoCount);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
|
||||
setMotionDetectionThreshold(2);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
|
||||
setZeroMotionDetectionThreshold(156);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
|
||||
setMotionDetectionDuration(80);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
|
||||
setZeroMotionDetectionDuration(0);
|
||||
|
||||
DEBUG_PRINTLN(F("Resetting FIFO..."));
|
||||
resetFIFO();
|
||||
|
||||
DEBUG_PRINTLN(F("Enabling FIFO..."));
|
||||
setFIFOEnabled(true);
|
||||
|
||||
DEBUG_PRINTLN(F("Enabling DMP..."));
|
||||
setDMPEnabled(true);
|
||||
|
||||
DEBUG_PRINTLN(F("Resetting DMP..."));
|
||||
resetDMP();
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
|
||||
while ((fifoCount = getFIFOCount()) < 3);
|
||||
|
||||
DEBUG_PRINT(F("Current FIFO count="));
|
||||
DEBUG_PRINTLN(fifoCount);
|
||||
DEBUG_PRINTLN(F("Reading FIFO data..."));
|
||||
getFIFOBytes(fifoBuffer, fifoCount);
|
||||
|
||||
DEBUG_PRINTLN(F("Reading interrupt status..."));
|
||||
|
||||
DEBUG_PRINT(F("Current interrupt status="));
|
||||
DEBUG_PRINTLNF(getIntStatus(), HEX);
|
||||
|
||||
DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
|
||||
while ((fifoCount = getFIFOCount()) < 3);
|
||||
|
||||
DEBUG_PRINT(F("Current FIFO count="));
|
||||
DEBUG_PRINTLN(fifoCount);
|
||||
|
||||
DEBUG_PRINTLN(F("Reading FIFO data..."));
|
||||
getFIFOBytes(fifoBuffer, fifoCount);
|
||||
|
||||
DEBUG_PRINTLN(F("Reading interrupt status..."));
|
||||
|
||||
DEBUG_PRINT(F("Current interrupt status="));
|
||||
DEBUG_PRINTLNF(getIntStatus(), HEX);
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("DMP is good to go! Finally."));
|
||||
|
||||
DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
|
||||
setDMPEnabled(false);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer..."));
|
||||
dmpPacketSize = 42;
|
||||
/*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
|
||||
return 3; // TODO: proper error code for no memory
|
||||
}*/
|
||||
|
||||
DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time..."));
|
||||
resetFIFO();
|
||||
getIntStatus();
|
||||
} else {
|
||||
DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed."));
|
||||
return 2; // configuration block loading failed
|
||||
}
|
||||
} else {
|
||||
DEBUG_PRINTLN(F("ERROR! DMP code verification failed."));
|
||||
return 1; // main binary block loading failed
|
||||
}
|
||||
return 0; // success
|
||||
}
|
||||
|
||||
bool MPU6050::dmpPacketAvailable() {
|
||||
return getFIFOCount() >= dmpGetFIFOPacketSize();
|
||||
}
|
||||
|
||||
// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate);
|
||||
// uint8_t MPU6050::dmpGetFIFORate();
|
||||
// uint8_t MPU6050::dmpGetSampleStepSizeMS();
|
||||
// uint8_t MPU6050::dmpGetSampleFrequency();
|
||||
// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg);
|
||||
|
||||
//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
|
||||
//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func);
|
||||
//uint8_t MPU6050::dmpRunFIFORateProcesses();
|
||||
|
||||
// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
|
||||
uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]);
|
||||
data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]);
|
||||
data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]);
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = (packet[28] << 8) | packet[29];
|
||||
data[1] = (packet[32] << 8) | packet[33];
|
||||
data[2] = (packet[36] << 8) | packet[37];
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
v -> x = (packet[28] << 8) | packet[29];
|
||||
v -> y = (packet[32] << 8) | packet[33];
|
||||
v -> z = (packet[36] << 8) | packet[37];
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]);
|
||||
data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]);
|
||||
data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]);
|
||||
data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]);
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = ((packet[0] << 8) | packet[1]);
|
||||
data[1] = ((packet[4] << 8) | packet[5]);
|
||||
data[2] = ((packet[8] << 8) | packet[9]);
|
||||
data[3] = ((packet[12] << 8) | packet[13]);
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
int16_t qI[4];
|
||||
uint8_t status = dmpGetQuaternion(qI, packet);
|
||||
if (status == 0) {
|
||||
q -> w = (float)qI[0] / 16384.0f;
|
||||
q -> x = (float)qI[1] / 16384.0f;
|
||||
q -> y = (float)qI[2] / 16384.0f;
|
||||
q -> z = (float)qI[3] / 16384.0f;
|
||||
return 0;
|
||||
}
|
||||
return status; // int16 return value, indicates error if this line is reached
|
||||
}
|
||||
// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
|
||||
uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]);
|
||||
data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]);
|
||||
data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]);
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = (packet[16] << 8) | packet[17];
|
||||
data[1] = (packet[20] << 8) | packet[21];
|
||||
data[2] = (packet[24] << 8) | packet[25];
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
v -> x = (packet[16] << 8) | packet[17];
|
||||
v -> y = (packet[20] << 8) | packet[21];
|
||||
v -> z = (packet[24] << 8) | packet[25];
|
||||
return 0;
|
||||
}
|
||||
// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
|
||||
// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
|
||||
uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
|
||||
// get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g)
|
||||
v -> x = vRaw -> x - gravity -> x*8192;
|
||||
v -> y = vRaw -> y - gravity -> y*8192;
|
||||
v -> z = vRaw -> z - gravity -> z*8192;
|
||||
return 0;
|
||||
}
|
||||
// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
|
||||
uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
|
||||
// rotate measured 3D acceleration vector into original state
|
||||
// frame of reference based on orientation quaternion
|
||||
memcpy(v, vReal, sizeof(VectorInt16));
|
||||
v -> rotate(q);
|
||||
return 0;
|
||||
}
|
||||
// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
|
||||
uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
|
||||
/* +1g corresponds to +8192, sensitivity is 2g. */
|
||||
int16_t qI[4];
|
||||
uint8_t status = dmpGetQuaternion(qI, packet);
|
||||
data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
|
||||
data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
|
||||
data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
|
||||
- (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
|
||||
return status;
|
||||
}
|
||||
|
||||
uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
|
||||
v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
|
||||
v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
|
||||
v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
|
||||
return 0;
|
||||
}
|
||||
// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet);
|
||||
|
||||
uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
|
||||
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi
|
||||
data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta
|
||||
data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi
|
||||
return 0;
|
||||
}
|
||||
|
||||
#ifdef USE_OLD_DMPGETYAWPITCHROLL
|
||||
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
|
||||
// yaw: (about Z axis)
|
||||
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
|
||||
// pitch: (nose up/down, about Y axis)
|
||||
data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
|
||||
// roll: (tilt left/right, about X axis)
|
||||
data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
|
||||
return 0;
|
||||
}
|
||||
#else
|
||||
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
|
||||
// yaw: (about Z axis)
|
||||
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
|
||||
// pitch: (nose up/down, about Y axis)
|
||||
data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
|
||||
// roll: (tilt left/right, about X axis)
|
||||
data[2] = atan2(gravity -> y , gravity -> z);
|
||||
if (gravity -> z < 0) {
|
||||
if(data[1] > 0) {
|
||||
data[1] = PI - data[1];
|
||||
} else {
|
||||
data[1] = -PI - data[1];
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
|
||||
|
||||
uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) {
|
||||
/*for (uint8_t k = 0; k < dmpPacketSize; k++) {
|
||||
if (dmpData[k] < 0x10) Serial.print("0");
|
||||
Serial.print(dmpData[k], HEX);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.print("\n");*/
|
||||
//Serial.println((uint16_t)dmpPacketBuffer);
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
|
||||
uint8_t status;
|
||||
uint8_t buf[dmpPacketSize];
|
||||
for (uint8_t i = 0; i < numPackets; i++) {
|
||||
// read packet from FIFO
|
||||
getFIFOBytes(buf, dmpPacketSize);
|
||||
|
||||
// process packet
|
||||
if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
|
||||
|
||||
// increment external process count variable, if supplied
|
||||
if (processed != 0) (*processed)++;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void));
|
||||
|
||||
// uint8_t MPU6050::dmpInitFIFOParam();
|
||||
// uint8_t MPU6050::dmpCloseFIFO();
|
||||
// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source);
|
||||
// uint8_t MPU6050::dmpDecodeQuantizedAccel();
|
||||
// uint32_t MPU6050::dmpGetGyroSumOfSquare();
|
||||
// uint32_t MPU6050::dmpGetAccelSumOfSquare();
|
||||
// void MPU6050::dmpOverrideQuaternion(long *q);
|
||||
uint16_t MPU6050::dmpGetFIFOPacketSize() {
|
||||
return dmpPacketSize;
|
||||
}
|
||||
|
||||
#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */
|
887
2.Firmware/Libraries/MPU6050/src/MPU6050_9Axis_MotionApps41.h
Normal file
887
2.Firmware/Libraries/MPU6050/src/MPU6050_9Axis_MotionApps41.h
Normal file
@ -0,0 +1,887 @@
|
||||
// I2Cdev library collection - MPU6050 I2C device class, 9-axis MotionApps 4.1 implementation
|
||||
// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
|
||||
// 6/18/2012 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// ... - ongoing debug release
|
||||
|
||||
/* ============================================
|
||||
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 _MPU6050_9AXIS_MOTIONAPPS41_H_
|
||||
#define _MPU6050_9AXIS_MOTIONAPPS41_H_
|
||||
|
||||
#include "I2Cdev.h"
|
||||
#include "helper_3dmath.h"
|
||||
|
||||
// MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board
|
||||
#define MPU6050_INCLUDE_DMP_MOTIONAPPS41
|
||||
|
||||
#include "MPU6050.h"
|
||||
|
||||
// Tom Carpenter's conditional PROGMEM code
|
||||
// http://forum.arduino.cc/index.php?topic=129407.0
|
||||
#ifdef __AVR__
|
||||
#include <avr/pgmspace.h>
|
||||
#else
|
||||
// Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
|
||||
#ifndef __PGMSPACE_H_
|
||||
#define __PGMSPACE_H_ 1
|
||||
#include <inttypes.h>
|
||||
|
||||
#define PROGMEM
|
||||
#define PGM_P const char *
|
||||
#define PSTR(str) (str)
|
||||
#define F(x) x
|
||||
|
||||
typedef void prog_void;
|
||||
typedef char prog_char;
|
||||
//typedef unsigned char prog_uchar;
|
||||
typedef int8_t prog_int8_t;
|
||||
typedef uint8_t prog_uint8_t;
|
||||
typedef int16_t prog_int16_t;
|
||||
typedef uint16_t prog_uint16_t;
|
||||
typedef int32_t prog_int32_t;
|
||||
typedef uint32_t prog_uint32_t;
|
||||
|
||||
#define strcpy_P(dest, src) strcpy((dest), (src))
|
||||
#define strcat_P(dest, src) strcat((dest), (src))
|
||||
#define strcmp_P(a, b) strcmp((a), (b))
|
||||
|
||||
#define pgm_read_byte(addr) (*(const unsigned char *)(addr))
|
||||
#define pgm_read_word(addr) (*(const unsigned short *)(addr))
|
||||
#define pgm_read_dword(addr) (*(const unsigned long *)(addr))
|
||||
#define pgm_read_float(addr) (*(const float *)(addr))
|
||||
|
||||
#define pgm_read_byte_near(addr) pgm_read_byte(addr)
|
||||
#define pgm_read_word_near(addr) pgm_read_word(addr)
|
||||
#define pgm_read_dword_near(addr) pgm_read_dword(addr)
|
||||
#define pgm_read_float_near(addr) pgm_read_float(addr)
|
||||
#define pgm_read_byte_far(addr) pgm_read_byte(addr)
|
||||
#define pgm_read_word_far(addr) pgm_read_word(addr)
|
||||
#define pgm_read_dword_far(addr) pgm_read_dword(addr)
|
||||
#define pgm_read_float_far(addr) pgm_read_float(addr)
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size.
|
||||
// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno)
|
||||
// after moving string constants to flash memory storage using the F()
|
||||
// compiler macro (Arduino IDE 1.0+ required).
|
||||
|
||||
//#define DEBUG
|
||||
#ifdef DEBUG
|
||||
#define DEBUG_PRINT(x) Serial.print(x)
|
||||
#define DEBUG_PRINTF(x, y) Serial.print(x, y)
|
||||
#define DEBUG_PRINTLN(x) Serial.println(x)
|
||||
#define DEBUG_PRINTLNF(x, y) Serial.println(x, y)
|
||||
#else
|
||||
#define DEBUG_PRINT(x)
|
||||
#define DEBUG_PRINTF(x, y)
|
||||
#define DEBUG_PRINTLN(x)
|
||||
#define DEBUG_PRINTLNF(x, y)
|
||||
#endif
|
||||
|
||||
#define MPU6050_DMP_CODE_SIZE 1962 // dmpMemory[]
|
||||
#define MPU6050_DMP_CONFIG_SIZE 232 // dmpConfig[]
|
||||
#define MPU6050_DMP_UPDATES_SIZE 140 // dmpUpdates[]
|
||||
|
||||
/* ================================================================================================ *
|
||||
| Default MotionApps v4.1 48-byte FIFO packet structure: |
|
||||
| |
|
||||
| [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
|
||||
| 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
|
||||
| |
|
||||
| [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
|
||||
| 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 |
|
||||
* ================================================================================================ */
|
||||
|
||||
// this block of memory gets written to the MPU on start-up, and it seems
|
||||
// to be volatile memory, so it has to be done each time (it only takes ~1
|
||||
// second though)
|
||||
const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
|
||||
// bank 0, 256 bytes
|
||||
0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
|
||||
0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
|
||||
0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
|
||||
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00,
|
||||
0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
|
||||
0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
|
||||
0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
|
||||
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
|
||||
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
|
||||
0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
|
||||
0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
|
||||
|
||||
// bank 1, 256 bytes
|
||||
0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
|
||||
0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
|
||||
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
|
||||
0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
|
||||
0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
|
||||
|
||||
// bank 2, 256 bytes
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
|
||||
0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
|
||||
// bank 3, 256 bytes
|
||||
0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4,
|
||||
0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA,
|
||||
0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80,
|
||||
0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0,
|
||||
0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1,
|
||||
0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3,
|
||||
0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88,
|
||||
0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF,
|
||||
0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89,
|
||||
0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9,
|
||||
0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A,
|
||||
0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11,
|
||||
0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55,
|
||||
0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54,
|
||||
0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D,
|
||||
0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86,
|
||||
|
||||
// bank 4, 256 bytes
|
||||
0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
|
||||
0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
|
||||
0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
|
||||
0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
|
||||
0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
|
||||
0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
|
||||
0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
|
||||
0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
|
||||
0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
|
||||
0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
|
||||
0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
|
||||
0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
|
||||
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
|
||||
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
|
||||
0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
|
||||
0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
|
||||
|
||||
// bank 5, 256 bytes
|
||||
0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
|
||||
0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
|
||||
0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
|
||||
0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
|
||||
0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
|
||||
0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
|
||||
0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
|
||||
0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
|
||||
0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
|
||||
0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
|
||||
0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86,
|
||||
0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40,
|
||||
0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9,
|
||||
0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30,
|
||||
0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0,
|
||||
0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24,
|
||||
|
||||
// bank 6, 256 bytes
|
||||
0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40,
|
||||
0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71,
|
||||
0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0,
|
||||
0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02,
|
||||
0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38,
|
||||
0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19,
|
||||
0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86,
|
||||
0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A,
|
||||
0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E,
|
||||
0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55,
|
||||
0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D,
|
||||
0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51,
|
||||
0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19,
|
||||
0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9,
|
||||
0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76,
|
||||
0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC,
|
||||
|
||||
// bank 7, 170 bytes (remainder)
|
||||
0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24,
|
||||
0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9,
|
||||
0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D,
|
||||
0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D,
|
||||
0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34,
|
||||
0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9,
|
||||
0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3,
|
||||
0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
|
||||
0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3,
|
||||
0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3,
|
||||
0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
|
||||
};
|
||||
|
||||
#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR
|
||||
#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x03
|
||||
#endif
|
||||
|
||||
const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
|
||||
// BANK OFFSET LENGTH [DATA]
|
||||
0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ?
|
||||
0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration
|
||||
0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration
|
||||
0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration
|
||||
0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration
|
||||
0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration
|
||||
0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration
|
||||
0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration
|
||||
|
||||
0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration
|
||||
0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01
|
||||
0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02
|
||||
0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10
|
||||
0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11
|
||||
0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12
|
||||
0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20
|
||||
0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21
|
||||
0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22
|
||||
|
||||
0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel
|
||||
0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors
|
||||
0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
|
||||
0x00, 0xA3, 0x01, 0x00, // ?
|
||||
0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update
|
||||
0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion
|
||||
0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer
|
||||
0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro
|
||||
0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo
|
||||
0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ?
|
||||
0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ?
|
||||
0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ?
|
||||
// SPECIAL 0x01 = enable interrupts
|
||||
0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION
|
||||
0x07, 0xA7, 0x01, 0xFE, // ?
|
||||
0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ?
|
||||
0x07, 0x67, 0x01, 0x9A, // ?
|
||||
0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo
|
||||
0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo
|
||||
0x02, 0x16, 0x02, 0x00, MPU6050_DMP_FIFO_RATE_DIVISOR // D_0_22 inv_set_fifo_rate
|
||||
|
||||
// This very last 0x03 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
|
||||
// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
|
||||
// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
|
||||
|
||||
// It is important to make sure the host processor can keep up with reading and processing
|
||||
// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
|
||||
};
|
||||
|
||||
const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = {
|
||||
0x01, 0xB2, 0x02, 0xFF, 0xF5,
|
||||
0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0,
|
||||
0x00, 0xA3, 0x01, 0x00,
|
||||
0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D,
|
||||
0x01, 0x6A, 0x02, 0x06, 0x00,
|
||||
0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
|
||||
0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x01, 0x08, 0x02, 0x01, 0x20,
|
||||
0x01, 0x0A, 0x02, 0x00, 0x4E,
|
||||
0x01, 0x02, 0x02, 0xFE, 0xB3,
|
||||
0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ
|
||||
0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00,
|
||||
0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C,
|
||||
0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00,
|
||||
0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00,
|
||||
0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
|
||||
0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00
|
||||
};
|
||||
|
||||
uint8_t MPU6050::dmpInitialize() {
|
||||
// reset device
|
||||
DEBUG_PRINTLN(F("\n\nResetting MPU6050..."));
|
||||
reset();
|
||||
delay(30); // wait after reset
|
||||
|
||||
// disable sleep mode
|
||||
DEBUG_PRINTLN(F("Disabling sleep mode..."));
|
||||
setSleepEnabled(false);
|
||||
|
||||
// get MPU product ID
|
||||
DEBUG_PRINTLN(F("Getting product ID..."));
|
||||
//uint8_t productID = 0; //getProductID();
|
||||
DEBUG_PRINT(F("Product ID = "));
|
||||
DEBUG_PRINT(productID);
|
||||
|
||||
// get MPU hardware revision
|
||||
DEBUG_PRINTLN(F("Selecting user bank 16..."));
|
||||
setMemoryBank(0x10, true, true);
|
||||
DEBUG_PRINTLN(F("Selecting memory byte 6..."));
|
||||
setMemoryStartAddress(0x06);
|
||||
DEBUG_PRINTLN(F("Checking hardware revision..."));
|
||||
uint8_t hwRevision = readMemoryByte();
|
||||
DEBUG_PRINT(F("Revision @ user[16][6] = "));
|
||||
DEBUG_PRINTLNF(hwRevision, HEX);
|
||||
DEBUG_PRINTLN(F("Resetting memory bank selection to 0..."));
|
||||
setMemoryBank(0, false, false);
|
||||
|
||||
// check OTP bank valid
|
||||
DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));
|
||||
uint8_t otpValid = getOTPBankValid();
|
||||
DEBUG_PRINT(F("OTP bank is "));
|
||||
DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!"));
|
||||
|
||||
// get X/Y/Z gyro offsets
|
||||
DEBUG_PRINTLN(F("Reading gyro offset values..."));
|
||||
int8_t xgOffset = getXGyroOffset();
|
||||
int8_t ygOffset = getYGyroOffset();
|
||||
int8_t zgOffset = getZGyroOffset();
|
||||
DEBUG_PRINT(F("X gyro offset = "));
|
||||
DEBUG_PRINTLN(xgOffset);
|
||||
DEBUG_PRINT(F("Y gyro offset = "));
|
||||
DEBUG_PRINTLN(ygOffset);
|
||||
DEBUG_PRINT(F("Z gyro offset = "));
|
||||
DEBUG_PRINTLN(zgOffset);
|
||||
|
||||
I2Cdev::readByte(devAddr, MPU6050_RA_USER_CTRL, buffer); // ?
|
||||
|
||||
DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled"));
|
||||
I2Cdev::writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32);
|
||||
|
||||
// enable MPU AUX I2C bypass mode
|
||||
//DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode..."));
|
||||
//setI2CBypassEnabled(true);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting magnetometer mode to power-down..."));
|
||||
//mag -> setMode(0);
|
||||
I2Cdev::writeByte(0x0E, 0x0A, 0x00);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access..."));
|
||||
//mag -> setMode(0x0F);
|
||||
I2Cdev::writeByte(0x0E, 0x0A, 0x0F);
|
||||
|
||||
DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration..."));
|
||||
int8_t asax, asay, asaz;
|
||||
//mag -> getAdjustment(&asax, &asay, &asaz);
|
||||
I2Cdev::readBytes(0x0E, 0x10, 3, buffer);
|
||||
asax = (int8_t)buffer[0];
|
||||
asay = (int8_t)buffer[1];
|
||||
asaz = (int8_t)buffer[2];
|
||||
DEBUG_PRINT(F("Adjustment X/Y/Z = "));
|
||||
DEBUG_PRINT(asax);
|
||||
DEBUG_PRINT(F(" / "));
|
||||
DEBUG_PRINT(asay);
|
||||
DEBUG_PRINT(F(" / "));
|
||||
DEBUG_PRINTLN(asaz);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting magnetometer mode to power-down..."));
|
||||
//mag -> setMode(0);
|
||||
I2Cdev::writeByte(0x0E, 0x0A, 0x00);
|
||||
|
||||
// load DMP code into memory banks
|
||||
DEBUG_PRINT(F("Writing DMP code to MPU memory banks ("));
|
||||
DEBUG_PRINT(MPU6050_DMP_CODE_SIZE);
|
||||
DEBUG_PRINTLN(F(" bytes)"));
|
||||
if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
|
||||
DEBUG_PRINTLN(F("Success! DMP code written and verified."));
|
||||
|
||||
DEBUG_PRINTLN(F("Configuring DMP and related settings..."));
|
||||
|
||||
// write DMP configuration
|
||||
DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks ("));
|
||||
DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE);
|
||||
DEBUG_PRINTLN(F(" bytes in config def)"));
|
||||
if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
|
||||
DEBUG_PRINTLN(F("Success! DMP configuration written and verified."));
|
||||
|
||||
DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
|
||||
setIntEnabled(1<<MPU6050_INTERRUPT_FIFO_OFLOW_BIT|1<<MPU6050_INTERRUPT_DMP_INT_BIT);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
|
||||
setRate(4); // 1khz / (1 + 4) = 200 Hz
|
||||
|
||||
DEBUG_PRINTLN(F("Setting clock source to Z Gyro..."));
|
||||
setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
|
||||
setDLPFMode(MPU6050_DLPF_BW_42);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
|
||||
setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
|
||||
setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)..."));
|
||||
setDMPConfig1(0x03);
|
||||
setDMPConfig2(0x00);
|
||||
|
||||
DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
|
||||
setOTPBankValid(false);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting X/Y/Z gyro offsets to previous values..."));
|
||||
setXGyroOffsetTC(xgOffset);
|
||||
setYGyroOffsetTC(ygOffset);
|
||||
setZGyroOffsetTC(zgOffset);
|
||||
|
||||
//DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
|
||||
//setXGyroOffset(0);
|
||||
//setYGyroOffset(0);
|
||||
//setZGyroOffset(0);
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 1/19 (function unknown)..."));
|
||||
uint8_t dmpUpdate[16], j;
|
||||
uint16_t pos = 0;
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 2/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Resetting FIFO..."));
|
||||
resetFIFO();
|
||||
|
||||
DEBUG_PRINTLN(F("Reading FIFO count..."));
|
||||
uint8_t fifoCount = getFIFOCount();
|
||||
|
||||
DEBUG_PRINT(F("Current FIFO count="));
|
||||
DEBUG_PRINTLN(fifoCount);
|
||||
uint8_t fifoBuffer[128];
|
||||
//getFIFOBytes(fifoBuffer, fifoCount);
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 3/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 4/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Disabling all standby flags..."));
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_PWR_MGMT_2, 0x00);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting accelerometer sensitivity to +/- 2g..."));
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_ACCEL_CONFIG, 0x00);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
|
||||
setMotionDetectionThreshold(2);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
|
||||
setZeroMotionDetectionThreshold(156);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
|
||||
setMotionDetectionDuration(80);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
|
||||
setZeroMotionDetectionDuration(0);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting AK8975 to single measurement mode..."));
|
||||
//mag -> setMode(1);
|
||||
I2Cdev::writeByte(0x0E, 0x0A, 0x01);
|
||||
|
||||
// setup AK8975 (0x0E) as Slave 0 in read mode
|
||||
DEBUG_PRINTLN(F("Setting up AK8975 read slave 0..."));
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x8E);
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_REG, 0x01);
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_CTRL, 0xDA);
|
||||
|
||||
// setup AK8975 (0x0E) as Slave 2 in write mode
|
||||
DEBUG_PRINTLN(F("Setting up AK8975 write slave 2..."));
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_ADDR, 0x0E);
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_REG, 0x0A);
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_CTRL, 0x81);
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_DO, 0x01);
|
||||
|
||||
// setup I2C timing/delay control
|
||||
DEBUG_PRINTLN(F("Setting up slave access delay..."));
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18);
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05);
|
||||
|
||||
// enable interrupts
|
||||
DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass..."));
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00);
|
||||
|
||||
// enable I2C master mode and reset DMP/FIFO
|
||||
DEBUG_PRINTLN(F("Enabling I2C master mode..."));
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20);
|
||||
DEBUG_PRINTLN(F("Resetting FIFO..."));
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24);
|
||||
DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know"));
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20);
|
||||
DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO..."));
|
||||
I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8);
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
#ifdef DEBUG
|
||||
DEBUG_PRINT(F("Read bytes: "));
|
||||
for (j = 0; j < 4; j++) {
|
||||
DEBUG_PRINTF(dmpUpdate[3 + j], HEX);
|
||||
DEBUG_PRINT(" ");
|
||||
}
|
||||
DEBUG_PRINTLN("");
|
||||
#endif
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Waiting for FIRO count >= 46..."));
|
||||
while ((fifoCount = getFIFOCount()) < 46);
|
||||
DEBUG_PRINTLN(F("Reading FIFO..."));
|
||||
getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
|
||||
DEBUG_PRINTLN(F("Reading interrupt status..."));
|
||||
getIntStatus();
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Waiting for FIRO count >= 48..."));
|
||||
while ((fifoCount = getFIFOCount()) < 48);
|
||||
DEBUG_PRINTLN(F("Reading FIFO..."));
|
||||
getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
|
||||
DEBUG_PRINTLN(F("Reading interrupt status..."));
|
||||
getIntStatus();
|
||||
DEBUG_PRINTLN(F("Waiting for FIRO count >= 48..."));
|
||||
while ((fifoCount = getFIFOCount()) < 48);
|
||||
DEBUG_PRINTLN(F("Reading FIFO..."));
|
||||
getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
|
||||
DEBUG_PRINTLN(F("Reading interrupt status..."));
|
||||
getIntStatus();
|
||||
|
||||
DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)..."));
|
||||
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
|
||||
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
|
||||
|
||||
DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
|
||||
setDMPEnabled(false);
|
||||
|
||||
DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer..."));
|
||||
dmpPacketSize = 48;
|
||||
/*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
|
||||
return 3; // TODO: proper error code for no memory
|
||||
}*/
|
||||
|
||||
DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time..."));
|
||||
resetFIFO();
|
||||
getIntStatus();
|
||||
} else {
|
||||
DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed."));
|
||||
return 2; // configuration block loading failed
|
||||
}
|
||||
} else {
|
||||
DEBUG_PRINTLN(F("ERROR! DMP code verification failed."));
|
||||
return 1; // main binary block loading failed
|
||||
}
|
||||
return 0; // success
|
||||
}
|
||||
|
||||
bool MPU6050::dmpPacketAvailable() {
|
||||
return getFIFOCount() >= dmpGetFIFOPacketSize();
|
||||
}
|
||||
|
||||
// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate);
|
||||
// uint8_t MPU6050::dmpGetFIFORate();
|
||||
// uint8_t MPU6050::dmpGetSampleStepSizeMS();
|
||||
// uint8_t MPU6050::dmpGetSampleFrequency();
|
||||
// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg);
|
||||
|
||||
//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
|
||||
//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func);
|
||||
//uint8_t MPU6050::dmpRunFIFORateProcesses();
|
||||
|
||||
// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
|
||||
|
||||
uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = (((uint32_t)packet[34] << 24) | ((uint32_t)packet[35] << 16) | ((uint32_t)packet[36] << 8) | packet[37]);
|
||||
data[1] = (((uint32_t)packet[38] << 24) | ((uint32_t)packet[39] << 16) | ((uint32_t)packet[40] << 8) | packet[41]);
|
||||
data[2] = (((uint32_t)packet[42] << 24) | ((uint32_t)packet[43] << 16) | ((uint32_t)packet[44] << 8) | packet[45]);
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = (packet[34] << 8) | packet[35];
|
||||
data[1] = (packet[38] << 8) | packet[39];
|
||||
data[2] = (packet[42] << 8) | packet[43];
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
v -> x = (packet[34] << 8) | packet[35];
|
||||
v -> y = (packet[38] << 8) | packet[39];
|
||||
v -> z = (packet[42] << 8) | packet[43];
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]);
|
||||
data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]);
|
||||
data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]);
|
||||
data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]);
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = ((packet[0] << 8) | packet[1]);
|
||||
data[1] = ((packet[4] << 8) | packet[5]);
|
||||
data[2] = ((packet[8] << 8) | packet[9]);
|
||||
data[3] = ((packet[12] << 8) | packet[13]);
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
int16_t qI[4];
|
||||
uint8_t status = dmpGetQuaternion(qI, packet);
|
||||
if (status == 0) {
|
||||
q -> w = (float)qI[0] / 16384.0f;
|
||||
q -> x = (float)qI[1] / 16384.0f;
|
||||
q -> y = (float)qI[2] / 16384.0f;
|
||||
q -> z = (float)qI[3] / 16384.0f;
|
||||
return 0;
|
||||
}
|
||||
return status; // int16 return value, indicates error if this line is reached
|
||||
}
|
||||
// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
|
||||
uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]);
|
||||
data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]);
|
||||
data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]);
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = (packet[16] << 8) | packet[17];
|
||||
data[1] = (packet[20] << 8) | packet[21];
|
||||
data[2] = (packet[24] << 8) | packet[25];
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpGetMag(int16_t *data, const uint8_t* packet) {
|
||||
// TODO: accommodate different arrangements of sent data (ONLY default supported now)
|
||||
if (packet == 0) packet = dmpPacketBuffer;
|
||||
data[0] = (packet[28] << 8) | packet[29];
|
||||
data[1] = (packet[30] << 8) | packet[31];
|
||||
data[2] = (packet[32] << 8) | packet[33];
|
||||
return 0;
|
||||
}
|
||||
// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
|
||||
// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
|
||||
uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
|
||||
// get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet)
|
||||
v -> x = vRaw -> x - gravity -> x*4096;
|
||||
v -> y = vRaw -> y - gravity -> y*4096;
|
||||
v -> z = vRaw -> z - gravity -> z*4096;
|
||||
return 0;
|
||||
}
|
||||
// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
|
||||
uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
|
||||
// rotate measured 3D acceleration vector into original state
|
||||
// frame of reference based on orientation quaternion
|
||||
memcpy(v, vReal, sizeof(VectorInt16));
|
||||
v -> rotate(q);
|
||||
return 0;
|
||||
}
|
||||
// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
|
||||
uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
|
||||
/* +1g corresponds to +8192, sensitivity is 2g. */
|
||||
int16_t qI[4];
|
||||
uint8_t status = dmpGetQuaternion(qI, packet);
|
||||
data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
|
||||
data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
|
||||
data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
|
||||
- (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
|
||||
return status;
|
||||
}
|
||||
|
||||
uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
|
||||
v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
|
||||
v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
|
||||
v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
|
||||
return 0;
|
||||
}
|
||||
// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet);
|
||||
|
||||
uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
|
||||
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi
|
||||
data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta
|
||||
data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi
|
||||
return 0;
|
||||
}
|
||||
|
||||
#ifdef USE_OLD_DMPGETYAWPITCHROLL
|
||||
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
|
||||
// yaw: (about Z axis)
|
||||
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
|
||||
// pitch: (nose up/down, about Y axis)
|
||||
data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
|
||||
// roll: (tilt left/right, about X axis)
|
||||
data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
|
||||
return 0;
|
||||
}
|
||||
#else
|
||||
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
|
||||
// yaw: (about Z axis)
|
||||
data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
|
||||
// pitch: (nose up/down, about Y axis)
|
||||
data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
|
||||
// roll: (tilt left/right, about X axis)
|
||||
data[2] = atan2(gravity -> y , gravity -> z);
|
||||
if(gravity->z<0) {
|
||||
if(data[1]>0) {
|
||||
data[1] = PI - data[1];
|
||||
} else {
|
||||
data[1] = -PI - data[1];
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
|
||||
// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
|
||||
|
||||
uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) {
|
||||
/*for (uint8_t k = 0; k < dmpPacketSize; k++) {
|
||||
if (dmpData[k] < 0x10) Serial.print("0");
|
||||
Serial.print(dmpData[k], HEX);
|
||||
Serial.print(" ");
|
||||
}
|
||||
Serial.print("\n");*/
|
||||
//Serial.println((uint16_t)dmpPacketBuffer);
|
||||
return 0;
|
||||
}
|
||||
uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
|
||||
uint8_t status;
|
||||
uint8_t buf[dmpPacketSize];
|
||||
for (uint8_t i = 0; i < numPackets; i++) {
|
||||
// read packet from FIFO
|
||||
getFIFOBytes(buf, dmpPacketSize);
|
||||
|
||||
// process packet
|
||||
if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
|
||||
|
||||
// increment external process count variable, if supplied
|
||||
if (processed != 0) *processed++;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void));
|
||||
|
||||
// uint8_t MPU6050::dmpInitFIFOParam();
|
||||
// uint8_t MPU6050::dmpCloseFIFO();
|
||||
// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source);
|
||||
// uint8_t MPU6050::dmpDecodeQuantizedAccel();
|
||||
// uint32_t MPU6050::dmpGetGyroSumOfSquare();
|
||||
// uint32_t MPU6050::dmpGetAccelSumOfSquare();
|
||||
// void MPU6050::dmpOverrideQuaternion(long *q);
|
||||
uint16_t MPU6050::dmpGetFIFOPacketSize() {
|
||||
return dmpPacketSize;
|
||||
}
|
||||
|
||||
#endif /* _MPU6050_9AXIS_MOTIONAPPS41_H_ */
|
216
2.Firmware/Libraries/MPU6050/src/helper_3dmath.h
Normal file
216
2.Firmware/Libraries/MPU6050/src/helper_3dmath.h
Normal file
@ -0,0 +1,216 @@
|
||||
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
|
||||
// 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// 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_
|
||||
|
||||
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_ */
|
Loading…
Reference in New Issue
Block a user