Skip to content

Commit

Permalink
Updates to MPU6050 code, MotionApps 4.1 DMP header file
Browse files Browse the repository at this point in the history
  • Loading branch information
@jrowberg
jrowberg committed Jul 1, 2012
1 parent 8e4ff4d commit 59a8c95
Show file tree
Hide file tree
Showing 4 changed files with 929 additions and 14 deletions.
36 changes: 23 additions & 13 deletions Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino
View file
@@ -1,8 +1,10 @@
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// 6/20/2012 by Jeff Rowberg <jeff@rowberg.net>
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 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
Expand Down Expand Up @@ -54,22 +56,30 @@ THE SOFTWARE.
// AD0 high = 0x69
MPU6050 mpu;

// =============================================================
// =============================================================
// 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: 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
//#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.
Expand Down Expand Up @@ -99,7 +109,7 @@ MPU6050 mpu;

// uncomment "OUTPUT_TEAPOT" if you want output that matches the
// format used for the InvenSense teapot demo
//#define OUTPUT_TEAPOT
#define OUTPUT_TEAPOT



Expand All @@ -116,7 +126,6 @@ uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 gv; // [x, y, z] gyro sensor measurements
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
Expand Down Expand Up @@ -152,6 +161,7 @@ void setup() {
// (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 Ardunio
// Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
Expand Down
94 changes: 94 additions & 0 deletions Arduino/MPU6050/MPU6050.h
View file
Expand Up @@ -883,6 +883,100 @@ class MPU6050 {
uint16_t dmpPacketSize;

uint8_t dmpInitialize();
bool dmpPacketAvailable();

uint8_t dmpSetFIFORate(uint8_t fifoRate);
uint8_t dmpGetFIFORate();
uint8_t dmpGetSampleStepSizeMS();
uint8_t dmpGetSampleFrequency();
int32_t dmpDecodeTemperature(int8_t tempReg);

// Register callbacks after a packet of FIFO data is processed
//uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
//uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
uint8_t dmpRunFIFORateProcesses();

// Setup FIFO for various output
uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);

// Get Fixed Point data from FIFO
uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0);
uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);

uint8_t dmpGetEuler(float *data, Quaternion *q);
uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);

// Get Floating Point data from FIFO
uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);

uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);
uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);

uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));

uint8_t dmpInitFIFOParam();
uint8_t dmpCloseFIFO();
uint8_t dmpSetGyroDataSource(uint8_t source);
uint8_t dmpDecodeQuantizedAccel();
uint32_t dmpGetGyroSumOfSquare();
uint32_t dmpGetAccelSumOfSquare();
void dmpOverrideQuaternion(long *q);
uint16_t dmpGetFIFOPacketSize();
#endif

private:
Expand Down
5 changes: 4 additions & 1 deletion Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h
View file
Expand Up @@ -260,7 +260,10 @@ const prog_uchar dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo
0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate

// This very last 0x07 WAS a 0x0A, which drops the FIFO rate down to ~18 Hz. 0x07 is 25 Hz.
// 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.
};
Expand Down

0 comments on commit 59a8c95

Please sign in to comment.