Professional Documents
Culture Documents
#include <avr/pgmspace.h>
//general variables
byte i2cbyte; //byte to send/receive via i2c
byte i2cinchar[20]; //20 char buffer for i2cRB()
byte i2coutchar[16]; //16 char buffer for i2cSB()
byte i2cnumber; //number of actual chars in/out of buffer
byte i2cRA; //register to begin the reading or writing
byte i2ccount; //general use
byte i2cackbit; //usually ignored
void setup() {
//release i2c bus lines and allow GY-521 2.2K to pull up.
pinMode (12, INPUT); //SCL clocks going high
pinMode (11, INPUT); //SDA
i2cstart (); //grab and release bus to ensure clear
delay(1);
i2cstop ();
//load 3kb firmware from nano flash checking each 16 byte block
for (mpubank = 0; mpubank < 12; mpubank++) { //firmware loading now
Serial.println(" ");
Serial.print(" bank # ");
Serial.println(mpubank);
for (mpumemadd = 0; mpumemadd < 241; mpumemadd = mpumemadd + 16) {
//Regs 112 and 113 (70 & 71) have DMP program start address $0400
i2cRA = 0x70; //unlisted Regs
i2cnumber = 2;
i2coutchar[0] = 0x04;
i2coutchar[1] = 0x00;
i2cmultiSB (); //send 2 bytes
Serial.println(" ");
Serial.println("DMP loaded");
} //end of "if not loaded"
} //end of setup
void loop() {
ffread:
digitalWrite (13, LOW); //diagnostic for fifo rate
i2cRA = 0x72; //Reg 114,115 (72,73) FIFO count
i2cnumber = 2;
i2cmultiRB ();
mpufifo = i2cinchar[0]; //high order byte
mpufifo = mpufifo << 8;
mpufifo = mpufifo | i2cinchar[1]; //low order byte
if (mpufifo>=20) {
//unload fifo 20 bytes into i2cinchar[]
i2cRA = 0x74; //Reg 116 (74) FIFO data port
i2cnumber = 20;
i2cmultiRB ();
digitalWrite (13, HIGH); //oscilloscope analysis of rate
quatload (); //put quat data (16) into quat[] long values
quats[0] = quat[0] >> 16; //and scale down quaternion values into quats[]
quats[1] = quat[1] >> 16; //to keep fifo integrity test within long arithmetic
quats[2] = quat[2] >> 16;
quats[3] = quat[3] >> 16;
long normsq = quats[0] * quats[0] + quats[1] * quats[1] + quats[2] * quats[2] + quats[3] * quats[3];
if (normsq > 285212672 | normsq < 251658240) {
//errors seen in quaternion - reset the fifo and bin this input
i2cRA = 0x6A; //Reg 106 (6A) user control
i2cnumber = 1;
i2coutchar[0] = B10000000; //enable DMP (reserved bit) and disable FIFO
i2cmultiSB ();
i2coutchar[0] = B10000100; //enable DMP (reserved bit) and reset FIFO
i2cmultiSB ();
i2coutchar[0] = B11000000; //enable DMP and FIFO
i2cmultiSB ();
Serial.print(" reset fifo ");
goto ffread;
} //end of quaternion error handling
long w=quat[0];
long x=quat[1];
long y=quat[2];
long z=quat[3];
//function to put fifo data into quaternion form 16 bytes become 4 long values
long quatload () {
quat[0] = ((long)i2cinchar[0] << 24) | ((long)i2cinchar[1] << 16) |
((long)i2cinchar[2] << 8) | i2cinchar[3];
quat[1] = ((long)i2cinchar[4] << 24) | ((long)i2cinchar[5] << 16) |
((long)i2cinchar[6] << 8) | i2cinchar[7];
quat[2] = ((long)i2cinchar[8] << 24) | ((long)i2cinchar[9] << 16) |
((long)i2cinchar[10] << 8) | i2cinchar[11];
quat[3] = ((long)i2cinchar[12] << 24) | ((long)i2cinchar[13] << 16) |
((long)i2cinchar[14] << 8) | i2cinchar[15];
} //end of quatload ()
//function to send bank and mem start address for each block
void mpusendbank () {
i2cstart ();
i2cbyte = 0xD0; //device address + write
i2cSB ();
i2cbyte = 0x6D; //unlisted Regs for firmware load
i2cSB ();
i2cbyte = mpubank;
i2cSB ();
i2cbyte = mpumemadd;
i2cSB ();
i2cstop ();
} //end of mpusendbank ()