GY-521 Code

//GY-521 Module Test Nano v3 CH380G
//pin D11 to GY-521 SDA

//pin D12 to GY-521 SCL
#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
byte mpubank; //firmware bank (256 bytes) (Reg 6D)

int mpumemadd; //firmware memory addr in bank (Reg 6E)
int mpufifo; //fifo count 16 bit unsigned, full at 1024
int mpuprsw=0; //print full loading information if set 1
long quat[4]; //quaternion w,x,y,z 32 bits long

long quats[4]; //quaternion w,x,y,z scaled down >>16
//array for DMP firmware to be kept in nano flash memory

PROGMEM const byte DMPfirm[] = {
/* bank # 0 */
0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01,
0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e,
0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83,
0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1,
0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2,
0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00,
/* gyro scale 4 bytes @104 */
0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00,
0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf,
0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa,
0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00,
0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c,
0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65,
0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00,
0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
/* bank # 1 */
0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f,
0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00,
0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00,
0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00,
0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00,
0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83,
0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28,
0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00,
/* bank # 2 */
0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x44,
/* fifo rate 2 bytes @534 */
0x00, 0x29, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2,
0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0,
0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00,
/* accel bias 12 bytes @ 660 */
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, 0x0e, 0x00, 0x0e,
0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c,
0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64,
0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
/* bank # 3 */
0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c,
0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19,
0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88,
/* bank # 4 */
0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e,
0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf,
0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0xf1, 0xa9,
0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0xb0, 0x8a, 0xa8, 0x96,
0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83,
0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1,
0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2,
0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xf1, 0xb9, 0xa3, 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda,
0xde, 0xdf, 0xdb, 0x81, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf,
0xd9, 0xba, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, 0xa2, 0xd9, 0xba, 0xa2, 0xf8,
0xdf, 0x85, 0xa4, 0xd0, 0xc1, 0xbb, 0xad, 0x83, 0xc2, 0xc5, 0xc7, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf,
0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1,
/* gyro cal 9 bytes @1208 */
0xb8, 0xaa, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35,
0x5d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a,
0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xba, 0xa4, 0xb0, 0x8a, 0xb6, 0x91, 0x32, 0x56, 0x76, 0xb2,
0x84, 0x94, 0xa4, 0xc8, 0x08, 0xcd, 0xd8, 0xb8, 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55,
0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, 0x54, 0x7c, 0x92, 0xa4, 0xf0, 0x2c, 0x50, 0x78,
/* bank # 5 */
0xf1, 0x84, 0xa8, 0x98, 0xc4, 0xcd, 0xfc, 0xd8, 0x0d, 0xdb, 0xa8, 0xfc, 0x2d, 0xf3, 0xd9, 0xba,
0xa6, 0xf8, 0xda, 0xba, 0xa6, 0xde, 0xd8, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xf3, 0xc8,
0x41, 0xda, 0xa6, 0xc8, 0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0x82, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88,
0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7,
0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1,
0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1,
0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29,
0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a,
0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7,
0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e,
0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c,
0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8,
0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88,
0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, 0xa8, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94,
0x4a, 0x6e, 0x98, 0xdb, 0x69, 0x31, 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0x87, 0x95, 0xa8, 0xf2,
0x21, 0xd1, 0xda, 0xa5, 0xf9, 0xf4, 0x17, 0xd9, 0xf1, 0xae, 0x8e, 0xd0, 0xc0, 0xc3, 0xae, 0x82,
/* bank # 6 */
0xc6, 0x84, 0xc3, 0xa8, 0x85, 0x95, 0xc8, 0xa5, 0x88, 0xf2, 0xc0, 0xf1, 0xf4, 0x01, 0x0e, 0xf1,
0x8e, 0x9e, 0xa8, 0xc6, 0x3e, 0x56, 0xf5, 0x54, 0xf1, 0x88, 0x72, 0xf4, 0x01, 0x15, 0xf1, 0x98,
0x45, 0x85, 0x6e, 0xf5, 0x8e, 0x9e, 0x04, 0x88, 0xf1, 0x42, 0x98, 0x5a, 0x8e, 0x9e, 0x06, 0x88,
0x69, 0xf4, 0x01, 0x1c, 0xf1, 0x98, 0x1e, 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02,
0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, 0x85, 0xa5, 0xf3, 0xc1, 0xda, 0x85, 0xa5, 0xf3, 0xdf,
0xd8, 0x85, 0x95, 0xa8, 0xf3, 0x09, 0xda, 0xa5, 0xfa, 0xd8, 0x82, 0x92, 0xa8, 0xf5, 0x78, 0xf1,
0x88, 0x1a, 0x84, 0x9f, 0x26, 0x88, 0x98, 0x21, 0xda, 0xf4, 0x1d, 0xf3, 0xd8, 0x87, 0x9f, 0x39,
0xd1, 0xaf, 0xd9, 0xdf, 0xdf, 0xfb, 0xf9, 0xf4, 0x0c, 0xf3, 0xd8, 0xfa, 0xd0, 0xf8, 0xda, 0xf9,
0xf9, 0xd0, 0xdf, 0xd9, 0xf9, 0xd8, 0xf4, 0x0b, 0xd8, 0xf3, 0x87, 0x9f, 0x39, 0xd1, 0xaf, 0xd9,
0xdf, 0xdf, 0xf4, 0x1d, 0xf3, 0xd8, 0xfa, 0xfc, 0xa8, 0x69, 0xf9, 0xf9, 0xaf, 0xd0, 0xda, 0xde,
0xfa, 0xd9, 0xf8, 0x8f, 0x9f, 0xa8, 0xf1, 0xcc, 0xf3, 0x98, 0xdb, 0x45, 0xd9, 0xaf, 0xdf, 0xd0,
0xf8, 0xd8, 0xf1, 0x8f, 0x9f, 0xa8, 0xca, 0xf3, 0x88, 0x09, 0xda, 0xaf, 0x8f, 0xcb, 0xf8, 0xd8,
0xf2, 0xad, 0x97, 0x8d, 0x0c, 0xd9, 0xa5, 0xdf, 0xf9, 0xba, 0xa6, 0xf3, 0xfa, 0xf4, 0x12, 0xf2,
0xd8, 0x95, 0x0d, 0xd1, 0xd9, 0xba, 0xa6, 0xf3, 0xfa, 0xda, 0xa5, 0xf2, 0xc1, 0xba, 0xa6, 0xf3,
0xdf, 0xd8, 0xf1, 0xba, 0xb2, 0xb6, 0x86, 0x96, 0xa6, 0xd0, 0xca, 0xf3, 0x49, 0xda, 0xa6, 0xcb,
0xf8, 0xd8, 0xb0, 0xb4, 0xb8, 0xd8, 0xad, 0x84, 0xf2, 0xc0, 0xdf, 0xf1, 0x8f, 0xcb, 0xc3, 0xa8,
/* bank # 7 */
0xb2, 0xb6, 0x86, 0x96, 0xc8, 0xc1, 0xcb, 0xc3, 0xf3, 0xb0, 0xb4, 0x88, 0x98, 0xa8, 0x21, 0xdb,
0x71, 0x8d, 0x9d, 0x71, 0x85, 0x95, 0x21, 0xd9, 0xad, 0xf2, 0xfa, 0xd8, 0x85, 0x97, 0xa8, 0x28,
0xd9, 0xf4, 0x08, 0xd8, 0xf2, 0x8d, 0x29, 0xda, 0xf4, 0x05, 0xd9, 0xf2, 0x85, 0xa4, 0xc2, 0xf2,
0xd8, 0xa8, 0x8d, 0x94, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xf2, 0xd8, 0x87, 0x21,
/* Android 1 byte @1853 */
0xd8, 0xf4, 0x0a,
0xd8, 0xf2, 0x84, 0x98, 0xa8, 0xc8, 0x01, 0xd1, 0xd9, 0xf4, 0x11, 0xd8, 0xf3, 0xa4, 0xc8, 0xbb,
0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xd8, 0xf1, 0xb8, 0xf6,
0xb5, 0xb9, 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8,
0xd8, 0x7c, 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1,
0x85, 0x30, 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99,
0xa3, 0x2d, 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb,
0xd8, 0xa0, 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9,
0xf9, 0xd8, 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9,
0xf9, 0xf9, 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac,
0xde, 0xf8, 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6,
0x9d, 0x2c, 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0,
0xde, 0xd9, 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf,
/* bank # 8 */
0xd9, 0xd0, 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30,
0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9,
0xdf, 0xd0, 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0,
0xdf, 0xdf, 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1,
0xda, 0xf2, 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6,
0xf9, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8,
0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda,
0xf2, 0xae, 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde,
0xfa, 0xf9, 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde,
0xdf, 0xa4, 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88,
0xc6, 0xa3, 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf,
/* tap enable 1 byte @2224 */
0xf8, 0xd8, 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8,
0xf3, 0x84, 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5,
0xc7, 0xa3, 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8,
0xd8, 0xa3, 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7,
0xd8, 0xa1, 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8,
/* bank # 9 */
0xb4, 0xb0, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0,
0x0c, 0x20, 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b,
0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab,
0x88, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0,
0x88, 0xb4, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0,
0xb8, 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, 0x88, 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, 0xd8, 0xb1, 0xb5, 0xb9, 0xa3, 0xdf, 0xdf, 0xdf, 0xae, 0xd0,
0xdf, 0xaa, 0xd0, 0xde, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf,
0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, 0xdf, 0xb0, 0x84, 0xf2,
0xc8, 0xf8, 0xf9, 0xd9, 0xde, 0xd8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83,
/* bank # 10 */
0x9a, 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6,
0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad,
0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb,
0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c,
0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8,
0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9,
0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2,
0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xf1, 0xb0, 0x80, 0xba, 0xab, 0xc0, 0xc3, 0xb2, 0x84,
0xc1, 0xc3,
/* fifo on event 11 bytes @2690 */
0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xb0,
0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b,
/* LP quat 4 bytes @2712 */
0x8b, 0x8b, 0x8b, 0x8b, 0xb0, 0x87,
/* 6X quat 4 bytes @2718 */
0x20, 0x28,
0x30, 0x38,
/* send gyro cal 4 bytes @2722 */
0xb2, 0x8b, 0xb6, 0x9b, 0xf2,
/* sensor data 10 bytes @2727 */
0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3,
0xa3, 0xf1, 0xb0, 0x87, 0xb5, 0x9a,
/* gesture 1 byte @2742 */
0x20, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, 0xac, 0xdf, 0xb9,
/*rate 12 bytes @2753 */
0xfe, 0xf2, 0xab, 0xc4, 0xaa, 0xf1, 0xdf, 0xdf, 0xbb, 0xaf, 0xdf, 0xdf, 0xa3, 0xa3, 0xa3, 0xa3,
0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, 0x9a, 0xaa, 0x28,
0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, 0x51, 0xf1, 0x90,
0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29,
/* bank # 11 */
0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, 0x78, 0xa3, 0x83,
0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, 0xa8, 0x92, 0x19,
0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, 0x96, 0xa8, 0x39,
0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, 0xda, 0x87, 0xa7,
0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, 0x39, 0xa9, 0x80,
0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, 0x31, 0x98, 0xd9,
0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, 0xa9, 0xda, 0x26,
0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, 0x2e, 0xd8, 0x89,
0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8,
0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, 0xd9, 0x2e, 0xd8,
0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, 0xa2, 0x80, 0x22,
0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, 0xff, 0xd8, 0xa2,
0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, 0xf1, 0xd9, 0x00,
0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, 0xac, 0xd0, 0x10,
0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, 0xf1, 0x96, 0x88,
0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
void setup() {
Serial.begin(19200); //diagnostic use

delay(500);
//set PORTB outputs to LOW so they can never drive high

digitalWrite (12, LOW);
digitalWrite (11, LOW);
//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 ();
//test Reg 107 to see if not asleep, skip firmware load if so

i2cRA = 0x6B;
i2cnumber = 1;
i2cmultiRB ();
if (i2cinchar[0] != B00000001) {
Serial.println(" ");
Serial.println("load DMP firmware");
//set clock source and leave sleep mode Reg 107 (6B)
i2coutchar[0] = B00000001; //clk source from X gyro
i2cRA = 0x6B;
i2cnumber = 1;
i2cmultiSB ();
delay(100);
//load 3kb firmware from nano flash checking each 16 byte block
for (mpubank = 0; mpubank < 12; mpubank++) { //firmware loading now
Serial.print(" bank # ");
Serial.println(mpubank);
for (mpumemadd = 0; mpumemadd < 241; mpumemadd = mpumemadd + 16) {
loaderror: //go round again if read check fails

mpusendbank(); //send bank and mem start
//load send buffer with 16 bytes from flash and send
for (int i = 0; i < 16; i++) {
int k = mpubank * 256;
k = k + mpumemadd;
k = k + i;
i2coutchar[i] = pgm_read_byte_near (DMPfirm + k);
} //next i
i2cRA = 0x6F; //unlisted Reg DMP firmware portal
i2cnumber = 16;
i2cmultiSB (); //send 16 bytes via portal
mpusendbank(); //send same bank and mem start

i2cmultiRB (); //read back the 16 bytes into i2cinchar[]
//now compare in and out bytes to confirm correct firmware load

int k = 0;
for (int i = 0; i < 16; i++) {
if (i2cinchar[i] != i2coutchar[i])
k++; //mismatch
} //next i
if (k != 0) { //was there an errror?
Serial.println(" error");
delay(200);
goto loaderror;
}
//block ok, print out read back values on one line
if(mpuprsw==1) { //only print if switch is 1
for (int i = 0; i < 16; i++) {
Serial.print(" ");
Serial.print(i2cinchar[i], HEX);
} //next i
Serial.print(" block ");
Serial.println(mpumemadd);
}
} //next block mpumemadd
} //next mpubank
//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("DMP loaded");
} //end of "if not loaded"
i2cRA = 0x1B; //Reg 27 (1B) gyro config

i2cnumber = 1;
i2coutchar[0] = B00011000; //FSR 2000dps mandated for DMP fusion
i2cmultiSB ();
i2cRA = 0x6A; //Reg 106 (6A) user control

i2coutchar[0] = B11000000; //enable DMP and FIFO (reserved bit)
i2cmultiSB ();
} //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];
//apply magic scale factor of 32k ???

w = w/32768.0f;
x = x/32768.0f;
y = y/32768.0f;
z = z/32768.0f;
Serial.print(" w "); //and print out reduced quaternion

Serial.print(w);
Serial.print(" x ");
Serial.print(x);
Serial.print(" y ");
Serial.print(y);
Serial.print(" z ");
Serial.print(z);
float gx = 2 * (x*z - w*y);

float gy = 2 * (w*x + y*z);
float gz = w*w - x*x - y*y + z*z;
float yaw = atan2(2*x*y - 2*w*z, 2*w*w + 2*x*x - 1); // about Z axis
float pitch = atan(gx / sqrt(gy*gy + gz*gz)); // about Y axis
float roll = atan(gy/gz); // about X axis
Serial.print(" yaw ");

Serial.print(yaw * 180/M_PI,0);
Serial.print(" pitch ");
Serial.print(pitch * 180/M_PI,0);
Serial.print(" roll ");
Serial.println(roll * 180/M_PI,0);
/*
//show accel offset regs on second print line
i2cRA = 0x06; //Reg 6 (06) accel bias
i2cnumber = 6;
i2cmultiRB ();
for (int i=0;i<6;i++) {

Serial.print(" ");
Serial.print(i2cinchar[i]);
}
*/
} //end of "if fifo >=20"

} //end of loop
//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];
} //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 ()
//function to create a small delay between pin changes

void i2cdly () {
// delayMicroseconds(5);
} //end of i2cdly ()
//function to send (i2cnumber) multiple bytes starting at i2cRA

void i2cmultiSB () {
i2cstart ();
i2cSB ();
i2cbyte = i2cRA; //register address
i2cSB ();
//sending bits now and receiving ack/nack
for (i2ccount = 0; i2ccount < i2cnumber; i2ccount++) {
i2cbyte = i2coutchar[i2ccount]; //copy from buffer
i2cSB (); //send i2cbyte and ignore ackbit
} //next
i2cstop ();
} //end of i2cmultiSB ()
//function to read (i2cnumber) multiple bytes starting at i2cRA

void i2cmultiRB () {
i2cstart ();
i2cSB ();
i2cbyte = i2cRA; //register address
i2cSB (); //SDA should end tristate for ack/nak
DDRB = DDRB & B11101111; //SCL 0->1 ready for restart
i2cdly ();
i2cstart (); //restart to read from chosen register
i2cbyte = 0xD1; //device address + read
i2cSB ();
//reading bits now and sending back ack/nack
for (i2ccount = 0; i2ccount < i2cnumber; i2ccount++) {
i2cRB (); //read char to i2cbyte
i2cinchar[i2ccount] = i2cbyte; //copy to buffer
if (i2ccount == i2cnumber - 1)
i2cS1 (); //send nack on last char
else
i2cS0 (); //send ack
} //next
i2cstop ();
} //end of i2cmultiRB ()
//function to send bit 0. enter & exit clock low

void i2cS0 () {
i2cdly ();
DDRB = DDRB | B00001000; //SDA =0
i2cdly ();
DDRB = DDRB & B11101111; //SCL 0->1
while (!(PINB & B00010000)); //test for clock stretching
i2cdly ();
DDRB = DDRB | B00010000; //SCL=0
}
//function to send bit 1. enter & exit clock low

void i2cS1 () {
i2cdly ();
DDRB = DDRB & B11110111; //SDA=1
i2cdly ();
DDRB = DDRB & B11101111; //SCL 0->1
i2cdly ();
DDRB = DDRB | B00010000; //SCL=0
}
//function to set start condition from bus released

void i2cstart () {
DDRB = DDRB | B00001000; //SDA 1->0 signal start
i2cdly ();
DDRB = DDRB | B00010000; //SCL 1->0
i2cdly ();
}
//function to set stop condition enter clock low

void i2cstop () {
DDRB = DDRB | B00001000; //SDA=0
i2cdly ();
DDRB = DDRB & B11101111; //SCL=1
i2cdly ();
DDRB = DDRB & B11110111; //SDA 0->1 signal stop
i2cdly ();
}
//function to send byte held in i2cbyte

//enter & exit clock low, exit tristate SDA
void i2cSB () {
for (int i = 0; i < 8; i++) {
i2cdly ();
if (i2cbyte < 0x80)
DDRB = DDRB | B00001000; //SDA=0
else
DDRB = DDRB & B11110111; //SDA=1
i2cdly ();
DDRB = DDRB & B11101111; //SCL=0->1 to clock data
i2cbyte = i2cbyte * 2; //shift byte left
DDRB = DDRB | B00010000; //SCL=0, data changes when low
} //next
i2cdly (); //now clock ack bit in
DDRB = DDRB & B11110111; //Tri-State SDA
i2cdly ();
DDRB = DDRB & B11101111; //SCL 0->1 to clock ack
i2cackbit = (PINB & B00001000); //read ack bit but ignore
i2cdly ();
DDRB = DDRB | B00010000; //SCL=0 so slave can release SDA
} //end of i2cSB ()
//function to receive byte into i2cbyte

//enter & exit with clock low, exit tristate SDA
void i2cRB () {
DDRB = DDRB & B11110111; //Tri-State SDA
i2cbyte = 0;
for (int i = 0; i < 8; i++) {
i2cdly (); //clock byte in
DDRB = DDRB & B11101111; //SCL 0->1 to clock data in
i2cbyte = i2cbyte * 2; //shift byte left
if (PINB & B00001000) //read bit
i2cbyte++; //set LSB to 1
i2cdly ();
DDRB = DDRB | B00010000; //SCL=0, data changes when low
} //next
} //end of i2cRB ()

GY-521 Code

Enviado por

Dados do documento

Título original

Direitos autorais

Formatos disponíveis

Compartilhar este documento

Compartilhar ou incorporar documento

Opções de compartilhamento

Você considera este documento útil?

Este conteúdo é inapropriado?

Direitos autorais:

Formatos disponíveis

GY-521 Code

Enviado por

Direitos autorais:

Formatos disponíveis

//GY-521 Module Test Nano v3 CH380G

//pin D11 to GY-521 SDA

byte mpubank; //firmware bank (256 bytes) (Reg 6D)

long quat[4]; //quaternion w,x,y,z 32 bits long

//array for DMP firmware to be kept in nano flash memory

Serial.begin(19200); //diagnostic use

//set PORTB outputs to LOW so they can never drive high

//test Reg 107 to see if not asleep, skip firmware load if so

loaderror: //go round again if read check fails

mpusendbank(); //send same bank and mem start

//now compare in and out bytes to confirm correct firmware load

i2cRA = 0x1B; //Reg 27 (1B) gyro config

i2cRA = 0x6A; //Reg 106 (6A) user control

//apply magic scale factor of 32k ???

Serial.print(" w "); //and print out reduced quaternion

float gx = 2 * (x*z - w*y);

Serial.print(" yaw ");

for (int i=0;i<6;i++) {

} //end of "if fifo >=20"

//function to create a small delay between pin changes

//function to send (i2cnumber) multiple bytes starting at i2cRA

//function to read (i2cnumber) multiple bytes starting at i2cRA

//function to send bit 0. enter & exit clock low

//function to send bit 1. enter & exit clock low

//function to set start condition from bus released

//function to set stop condition enter clock low

//function to send byte held in i2cbyte

//function to receive byte into i2cbyte

Você também pode gostar

float gx = 2 * (xz - wy);