roboforum.ru

Народ, нужна ваша помощь.

Собрал свой первый квадрокоптер на APM 2.6+телеметрия+Power Module+GPS модуль
http://www.banggood.com/APM-Flight-Cont ... 20659.html
Полетал и осознал: APM - идеальное решения для робота. Arduino based (2560 MEGA), открытая среда, малые размеры,
куча модулей.
Конечно, можно Ardurover прошить и использовать, но это решение больше для тележки, чем для автономного робота.
Моя идея прошить свой скетч на APM и использовать все плюшки платформы Ardupilot(встроенный гироскоп+акселерометр+
компас++телеметрия+Power Module+GPS модуль). Всё это уже готово. Легко заказать и именно то, что надо для робота.
Сейчас я использую в своем роботе Arduino 2560+ плата собственной разводки, а тут такое решение шикарное.
Моя прошивка стала как родная на APM 2.6, но хотелось бы считать данные с встроенных датчиков: MPU6000, PMC5883L.
Начал с MPU6000 и уперся в него ((
Использовал вот эту прогу http://www.robogaia.com/6-axis-accelero ... hield.html
Не работает.
И вот эту: http://ardupilot.com/forum/viewtopic.php?t=6439&p=12013
Не работает.
И вот это - точная копия того, что стоит на Ardupilot: https://code.google.com/p/ardu-imu/downloads/list
Не работает.
Ковырял и выуживал куски из Ardupilot: http://dev.ardupilot.com/
Тот же результат.
Уверен, что всё просто и я уперся в какую-то мелочь.
Помогите разобраться!
Уверен, что найдутся еще люди, которые разделят мое мнение в отношении использования APM 2.6 в качестве контроллера для роботов.

Я поищу скетч для ардуины, как домой вернусь, который читает данные с mpu6000. Там, в целом, ничего сложного - надо просто даташит найти или готовые библиотеки на нее и правильно пины инициализировать.
В целом, там проблема с использованием их родной прошивки в том, что она не собирается в arudino. Надо ставить нормальный gcc и весь тулчейн, и тогда можно будет использовать их библиотеки (которые, кстати, довольно удобные). А так - пилить все с нуля =(

1. проверьте точно ли используются ожидаемые чипы
2. посмотрите куда подключены датчики к МК, может у вас просто порты не совпадают

Вот работающий код для APM 2.5 Подозреваю что 2.6 имеет аналогичную распиновки и заработает без изменений, но хз. Код делался как proof of concept, накопипащено из пачки источников, так что вопросы отвечать про него не буду )

Код: Выделить всё • Развернуть

#include <SPI.h>
#include <math.h>

#define RED_LED_PIN        27 
#define YELLOW_LED_PIN     26
#define GREEN_LED_PIN      25 

byte blinkState = 0x00, blink_divider = 0; // controls (red) LED on or off and at what blink speed
//#define DEBUG 1

#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 MPU6000_CS_PIN       53        // APM pin connected to mpu6000's chip select pin

#define MPU6050_DMP_CODE_SIZE         1929    // the number of values for writing the dmpMemory[]
#define MPU6050_DMP_CONFIG_SIZE        192    // the number of values for writing the dmpConfig[]
#define MPU6050_DMP_UPDATES_SIZE        47    // the number of values for writing the dmpUpdates[]



boolean dmpReady = false;     // set true if DMP initialization was successful
unsigned int packetSize = 42; // number of unique bytes of data written by the DMP each time (FIFO can hold multiples of 42-bytes)
unsigned int fifoCount;       // count of all bytes currently in FIFO
byte fifoBuffer[64];          // FIFO storage buffer (in fact only 42 used...)

// INTERRUPT FROM MPU-6000 DETECTION ROUTINE
volatile boolean mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
  mpuInterrupt = true;
}

void init_spi() {
  Serial.println("Initializing SPI Protocol...");
  SPI.begin();  // start the SPI library
  SPI.setClockDivider(SPI_CLOCK_DIV64); // ArduIMU+ V3 board runs on 16 MHz: 16 MHz / SPI_CLOCK_DIV16 = 1 MHz
  // 1 MHz is the maximum SPI clock frequency according to the MPU-6000 Product Specification
  SPI.setBitOrder(MSBFIRST);  // data delivered MSB first as in MPU-6000 Product Specification
  SPI.setDataMode(SPI_MODE0); // latched on rising edge, transitioned on falling edge, active low
  Serial.println("...SPI Protocol initializing done.");
  Serial.println(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
  delay(100);
  
  pinMode(40, OUTPUT);  
  digitalWrite(40, HIGH); 

  pinMode(A3, OUTPUT);  
  digitalWrite(A3, HIGH); 

  pinMode(28, OUTPUT);  
  digitalWrite(28, HIGH); 
}


void init_mpu6k(){
  DEBUG_PRINTLN(F("Disabling sleep mode..."));
  SPIwrite(0x6B, 0x80, MPU6000_CS_PIN); // RESET
  delay(100);
  SPIwrite(0x6B, 0x03, MPU6000_CS_PIN); // RESET
  delay(100);
  SPIread(0x6B, MPU6000_CS_PIN);

}

// the setup routine runs once when you press reset:
void setup() {
  pinMode(RED_LED_PIN, OUTPUT);      // Red LED
  pinMode(YELLOW_LED_PIN, OUTPUT);      // Red LED
  pinMode(GREEN_LED_PIN, OUTPUT);     // Blue LED

  // initialize serial communication at 9600 bits per second:
  Serial.begin(115200);

  while(Serial.available() > 0) Serial.read(); // Flush serial buffer to clean up remnants from previous run
  Serial.println();
  Serial.println("############# MPU-6000 Data Acquisition #############");
  init_spi();  
  delay(400);

  //--- Configure the chip select pin as output ---//
  pinMode(MPU6000_CS_PIN, OUTPUT);

  // write & verify dmpMemory, dmpConfig and dmpUpdates into the DMP, and make all kinds of other settings
  // !!! this is the main routine to make the DMP work, and get the quaternion out of the FIFO !!!
  Serial.println("Initializing Digital Motion Processor (DMP)...");
  byte devStatus = 99; // return status after each device operation (0 = success, !0 = error)
  
  devStatus = dmpInitialize();
  // make sure it worked: dmpInitialize() returns a 0 in devStatus if so
  if (devStatus == 0)
  {
    // now that it's ready, turn on the DMP 
    Serial.print("Enabling DMP... ");
    SPIwriteBit(0x6A, 7, true, MPU6000_CS_PIN); // USER_CTRL_DMP_EN
    Serial.println("done.");

    // enable Arduino interrupt detection, this will execute dmpDataReady whenever there is an interrupt,
    // independing on what this sketch is doing at that moment
    // http://arduino.cc/en/Reference/AttachInterrupt
    Serial.print("Enabling interrupt detection... ");
    // attachInterrupt(interrupt, function, mode) specifies a function to call when an external interrupt occurs
    // ArduIMU+ V3 has ATMEGA328 INT0 / D2 pin 32 (input) connected to MPU-6000 INT pin 12 (output)

    //    attachInterrupt(0, dmpDataReady, RISING); // the 0 points correctly to INT0 / D2

    // -> if there is an interrupt from MPU-6000 to ATMEGA328, boolean mpuInterrupt will be made true
    byte mpuIntStatus = SPIread(0x3A, MPU6000_CS_PIN); // by reading INT_STATUS register, all interrupts are cleared
    Serial.println("done.");

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    dmpReady = true;
    Serial.println("DMP ready! Waiting for first data from MPU-6000...");
    Serial.println(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");    
  }
  else // have to check if this is still functional
  {
    // 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("DMP Initialization failed (code ");
    Serial.print(devStatus);
    Serial.println(")");
  }

}

// the loop routine runs over and over again forever:
void loop() {
#ifdef DEBUG
  Serial.println(">>> Main loop start (must be seen over and over again)...");
#endif

  // if DMP initialization during setup failed, don't try to do anything
  if (!dmpReady)
  {
    return;
  }

  // wait for MPU interrupt or extra packet(s) available
  // INFO: if there is an interrupt send from the MPU-6000 to the ATmega328P (the "Arduino" processor),
  //       boolean variable "mpuInterrupt" will be made "true" (see explanation in void setup() )
//  while ((mpuInterrupt == false) && (fifoCount < packetSize))
//  {
//    // do nothing until mpuInterrupt = true or fifoCount >= 42
//  }

  // there has just been an interrupt, so reset the interrupt flag, then get INT_STATUS byte
  mpuInterrupt = false;
  byte mpuIntStatus = SPIread(0x3A, MPU6000_CS_PIN); // by reading INT_STATUS register, all interrupts are cleared

  // get current FIFO count
  fifoCount = getFIFOCount(MPU6000_CS_PIN);

  // check for FIFO overflow (this should never happen unless our code is too inefficient or DEBUG output delays code too much)
  if ((mpuIntStatus & 0x10) || fifoCount == 1008)
    // mpuIntStatus & 0x10 checks register 0x3A for FIFO_OFLOW_INT
    // the size of the FIFO buffer is 1024 bytes, but max. set to 1008 so after 24 packets of 42 bytes
    // the FIFO is reset, otherwise the last FIFO reading before reaching 1024 contains only 16 bytes
    // and can/will produces output value miscalculations 
  {
    // reset so we can continue cleanly
    //SPIwriteBit(0x6A, 6, false, ChipSelPin1); // FIFO_EN = 0 = disable
    SPIwriteBit(0x6A, 2, true, MPU6000_CS_PIN); // FIFO_RESET = 1 = reset (ok) only when FIFO_EN = 0
    //SPIwriteBit(0x6A, 6, true, ChipSelPin1); // FIFO_EN = 1 = enable

    digitalWrite(RED_LED_PIN, HIGH); // shows FIFO overflow without disturbing output with message
    DEBUG_PRINTLN("FIFO overflow! FIFO resetted to continue cleanly.");
  }

  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  else if (mpuIntStatus & 0x02)
    // mpuIntStatus & 0x02 checks register 0x3A for (undocumented) DMP_INT
  {

    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = getFIFOCount(MPU6000_CS_PIN);

    digitalWrite(RED_LED_PIN, LOW); // LED off again now that FIFO overflow is resolved

    // read a packet from FIFO
    SPIreadBytes(0x74, packetSize, fifoBuffer, MPU6000_CS_PIN);

    // verify contents of fifoBuffer before use:
# ifdef DEBUG
    for (byte n = 0 ; n < packetSize; n ++)
    {
      Serial.print("\tfifoBuffer["); 
      Serial.print(n); 
      Serial.print("]\t: "); 
      Serial.println(fifoBuffer[n], HEX);
    }
# endif

    // track FIFO count here in case there is more than one packet (each of 42 bytes) available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount = fifoCount - packetSize;

    // ============================================================================================== //
    // >>>>>>>>> - from here the 42 FIFO bytes from the MPU-6000 can be used to generate output >>>>>>>>
    // >>>>>>>>> - this would be the place to add your own code into                            >>>>>>>>
    // >>>>>>>>> - of course all the normal MPU-6000 registers can be used here too             >>>>>>>>
    // ============================================================================================== //

    // get the quaternion values from the FIFO - needed for Euler and roll/pitch/yaw angle calculations
    int raw_q_w = ((fifoBuffer[0] << 8)  + fifoBuffer[1]);  // W
    int raw_q_x = ((fifoBuffer[4] << 8)  + fifoBuffer[5]);  // X
    int raw_q_y = ((fifoBuffer[8] << 8)  + fifoBuffer[9]);  // Y
    int raw_q_z = ((fifoBuffer[12] << 8) + fifoBuffer[13]); // Z
    float q_w = raw_q_w / 16384.0f;
    float q_x = raw_q_x / 16384.0f;
    float q_y = raw_q_y / 16384.0f;
    float q_z = raw_q_z / 16384.0f;

    // print accelerometer values from fifoBuffer
    int AcceX = ((fifoBuffer[28] << 8) + fifoBuffer[29]);
    int AcceY = ((fifoBuffer[32] << 8) + fifoBuffer[33]);
    int AcceZ = ((fifoBuffer[36] << 8) + fifoBuffer[37]);
    Serial.print("Raw acceleration ax, ay, az [8192 = 1 g]: "); 
    Serial.print("\t\t");
    Serial.print  (AcceX); 
    Serial.print("\t");
    Serial.print  (AcceY); 
    Serial.print("\t");
    Serial.println(AcceZ);
  }

//  digitalWrite(RED_LED_PIN, HIGH);   // turn the LED on (HIGH is the voltage level)
//  delay(1000);               // wait for a second
//  digitalWrite(RED_LED_PIN, LOW);    // turn the LED off by making the voltage LOW
//  delay(1000);               // wait for a second

}


// ############################################################################################## //
// ################################ SPI read/write functions #################################### //
// ############################################################################################## //

// --- Function for SPI reading one byte from sensor
// reg        : MPU-6000 register number to read from
// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by MPU6000_CS_PIN)
// return     > register contents
byte SPIread(byte reg, int ChipSelPin)
{
  DEBUG_PRINT("SPI (/CS"); 
  DEBUG_PRINT(ChipSelPin); 
  DEBUG_PRINT(") ");
  DEBUG_PRINT("reading 1 byte from register 0x");
  if (reg < 0x10) DEBUG_PRINT("0"); // add leading zero - this is an Arduino bug
  DEBUG_PRINTF(reg, HEX); 
  DEBUG_PRINT("... ");
  digitalWrite(ChipSelPin, LOW);     // select MPU-6000 for SPI transfer (low active)
  SPI.transfer(reg | 0x80);          // reg | 0x80 causes a "1" added as MSB to reg to denote reading from reg i.s.o. writing to it
  byte read_value = SPI.transfer(0x00); // write 8-bits zero to MPU-6000, read the 8-bits coming back from reg at the same time
  digitalWrite(ChipSelPin, HIGH);    // deselect MPU-6000 for SPI transfer
  DEBUG_PRINT("0x");
  if (read_value < 0x10) DEBUG_PRINT("0"); // add leading zero - this is an Arduino bug
  DEBUG_PRINTF(read_value, HEX); 
  DEBUG_PRINTLN(" (done)");
  return read_value;
}

// --- Function for SPI writing one byte to sensor
// reg        : MPU-6000 register number to write to
// data       : data to be written into reg
// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by MPU6000_CS_PIN)
// return     > nothing
void SPIwrite(byte reg, byte data, int ChipSelPin)
{
  DEBUG_PRINT("SPI (/CS"); 
  DEBUG_PRINT(ChipSelPin); 
  DEBUG_PRINT(") ");
  DEBUG_PRINT("writing 1 byte to   register 0x"); 
  DEBUG_PRINTF(reg, HEX); 
  DEBUG_PRINT("... ");
  digitalWrite(ChipSelPin, LOW);
  SPI.transfer(reg);
  SPI.transfer(data);
  digitalWrite(ChipSelPin, HIGH);
  DEBUG_PRINT("0x");
  if (data < 0x10) DEBUG_PRINT("0"); // add leading zero - this is an Arduino bug
  DEBUG_PRINTF(data, HEX); 
  DEBUG_PRINTLN(" (done)");
}

// --- Function for SPI reading one bit from sensor
// reg        : MPU-6000 register number to read from
// bitNum     : bit number in the register to read - 7 (MSB) to 0 (LSB)
// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by MPU6000_CS_PIN)
// return     > byte 0x00 if bit is 0, otherwise byte with a 1 at bitNum (rest 0's)
byte SPIreadBit(byte reg, byte bitNum, int ChipSelPin)
{
  byte byte_value = SPIread(reg, ChipSelPin);
  byte bit_value  = byte_value & (1 << bitNum); // AND result from register byte value and byte with only one "1" at place of bit to return (rest "0"'s)

#ifdef DEBUG
  Serial.print(" bit_"); 
  Serial.print(bitNum); 
  Serial.print(" = "); 
  if (bit_value == 0x00)
  {
    Serial.println("0");
  }
  else
  {
    Serial.println("1");
  }
#endif

  return bit_value;
}


// This array contains the default DMP memory bank binary that gets loaded during dmpInitialize().
// It was reconstructed from observed I2C traffic generated by the UC3-A3 demo code, and not extracted
// directly from that code. That is true of all transmissions in this sketch, and any documentation has
// been added after the fact by referencing the Invensense code.
// It gets written to volatile memory, so it has to be done at each start (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, 137 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
};

// This array contains the DMP configurations that gets loaded during dmpInitialize().
// 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, 0x01                          // 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 100 Hz. Going faster than 100 Hz (0x00 = 200 Hz) tends to result in very noisy data.
  // DMP output frequency is calculated easily using this equation: (200 Hz / (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.
};

// This array contains the DMP updates that get loaded during dmpInitialize().
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
};


//--- Function for SPI writing one bit to sensor
// reg        : MPU-6000 register number to write to
// bitNum     : bit number in the register to write to - 7 (MSB) to 0 (LSB)
// databit    : bit value to be written into reg - false or 0 | true or non-zero (1 will be logical)
// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by MPU6000_CS_PIN)
// return     > nothing
//
// first read byte, then insert bit value, then write byte:
// otherwise all other bits will be written 0, this may trigger unexpected behaviour
void SPIwriteBit(byte reg, byte bitNum, byte databit, int ChipSelPin)
{
  byte byte_value = SPIread(reg, ChipSelPin);
  if (databit == 0)
  {
    byte_value = byte_value & ~(1 << bitNum); // AND result from register byte value and byte with only one "0" at place of bit to write (rest "1"'s)
  }
  else // databit is intended to be a "1"
  {
    byte_value = byte_value |  (1 << bitNum); // OR  result from register byte value and byte with only one "1" at place of bit to write (rest "0"'s)
  }
  SPIwrite(reg, byte_value, ChipSelPin);

#ifdef DEBUG
  Serial.print(" bit_"); 
  Serial.print(bitNum); 
  Serial.print(" set to "); 
  if (databit == 0)
  {
    Serial.println("0");
  }
  else
  {
    Serial.println("1");
  }
#endif

}

//--- Function for SPI reading multiple bytes to sensor
// read multiple bytes from the same device register, most of the times this
// is the FIFO transfer register (which after each read, is automatically
// loaded with new data for the next read)
// reg        : MPU-6000 register number to write to
// length     : number of bytes to be read
// data       : buffer array (starting with [0]) to store the read data in
// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by MPU6000_CS_PIN)
// return     > array of data[0 - length]
void SPIreadBytes(byte reg, unsigned int length, byte *data, int ChipSelPin) 
{
#ifdef DEBUG
  Serial.print("SPI (/CS");
  Serial.print(ChipSelPin);
  Serial.print(") reading ");
  Serial.print(length, DEC);
  Serial.print(" byte(s) from 0x");
  if (reg < 0x10) {
    Serial.print("0");
  } // add leading zero - this is an Arduino bug
  Serial.print(reg, HEX);
  Serial.println("... ");
#endif

  digitalWrite(ChipSelPin, LOW);
  delay(10); // wait 10 ms for MPU-6000 to react on chipselect (if this is 4 ms or less, SPI.transfer fails)
  SPI.transfer(reg | 0x80); // reg | 0x80 causes a "1" added as MSB to reg to denote reading from reg i.s.o. writing to it

  unsigned int count = 0;
  byte data_bytes_printed = 0;

  for (count = 0; count < length; count ++)
  {
    data[count] = SPI.transfer(0x00);
#ifdef DEBUG
    if (data[count] < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug
    Serial.print(data[count], HEX); 
    Serial.print(" ");
    data_bytes_printed ++;
    if (data_bytes_printed == 16) // print lines of 16 bytes
    {
      Serial.println();
      data_bytes_printed = 0;
    }
#endif
  }

  digitalWrite(ChipSelPin, HIGH);
  DEBUG_PRINTLN(" (done)");
}

//--- Function for SPI reading multiple bits from sensor
// reg        : MPU-6000 register number to read from
// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by MPU6000_CS_PIN)
// return     > databits
//
// 01101001 read byte
// 76543210 bit numbers
//    xxx   bitStart = 4, length = 3
//    010   masked
//   -> 010 shifted
byte SPIreadBits(byte reg, byte bitStart, byte length, int ChipSelPin)
{
  byte b = SPIread(reg, ChipSelPin);
  byte mask = ((1 << length) - 1) << (bitStart - length + 1);
  b = b & mask;
  b = b >> (bitStart - length + 1);
  return b;
}

//--- Function for SPI writing multiple bits to sensor
// reg        : MPU-6000 register number to write to
// ChipSelPin : MPU-6000 chip select pin number (in this sketch defined by MPU6000_CS_PIN)
//
// bbbbb010 -> data (bits to write - leading 0's)
// 76543210 bit numbers
//    xxx   bitStart = 4, length = 3
// 00011100 mask byte
// 10101111 original reg value (read)
// 10100011 original reg value & ~mask
// 10101011 masked | original reg value
//
// first read byte, then insert bit values, then write byte:
// otherwise all other bits will be written 0, this may trigger unexpected behaviour
void SPIwriteBits(byte reg, byte bitStart, byte length, byte data, int ChipSelPin)
{
  byte byte_value = SPIread(reg, MPU6000_CS_PIN);
  byte mask = ((1 << length) - 1) << (bitStart - length + 1); // create mask
  data <<= (bitStart - length + 1); // shift data into correct position
  data &= mask;                     // zero all non-important bits in data (just to make sure)
  byte_value &= ~(mask);            // zero all important bits in existing byte, maintain the rest
  byte_value |= data;               // combine data with existing byte
  SPIwrite(reg, byte_value, ChipSelPin);

#ifdef DEBUG
  Serial.print(" bits set: "); 
  for (byte i = 0; i < (7 - bitStart); i ++) Serial.print("x");
  for (byte j = 0; j < length; j ++) Serial.print(bitRead(data, bitStart - j));
  for (byte k = 0; k < (bitStart - length + 1); k ++) Serial.print("x");
  Serial.println();
#endif

}

// ############################################################################################## //
// ################################ DMP functions used in dmpInitialize() ####################### //
// ############################################################################################## //
// If you like to know how it works, please read on. Otherwise, just FIRE AND FORGET ;-)

void setMemoryBank(byte bank, boolean prefetchEnabled, boolean userBank, int ChipSelPin)
{
  // - the value in 0x6D activates a specific bank in the DMP
  // - the value in 0x6E sets the read/write pointer to a specific startaddress within the specified DMP bank
  // - register 0x6F is the register from which to read or to which to write the data
  //   (after each r/w autoincrement address within the specified DMP bank starting from startaddress)
  bank = bank & 0x1F; // 0x1F = 00011111
  // bank 0: 0 & 0x1F = 00000000 $ 00011111 = 00000000
  // bank 1: 1 & 0x1F = 00000001 $ 00011111 = 00000001
  // bank 2: 2 & 0x1F = 00000010 $ 00011111 = 00000010
  // bank 3: 3 & 0x1F = 00000011 $ 00011111 = 00000011
  // bank 4: 4 & 0x1F = 00000100 $ 00011111 = 00000100
  // bank 5: 5 & 0x1F = 00000101 $ 00011111 = 00000101
  // bank 6: 6 & 0x1F = 00000110 $ 00011111 = 00000110
  // bank 7: 7 & 0x1F = 00000111 $ 00011111 = 00000111
  // is this to maximize the number of banks to 00011111 is 0x1F = 31 ?
  if (userBank) bank |= 0x20;
  if (prefetchEnabled) bank |= 0x40;
  SPIwrite(0x6D, bank, ChipSelPin);
}

//***********************************************************//
void setMemoryStartAddress(byte startaddress, int ChipSelPin)
{
  // - the value in 0x6D activates a specific bank in the DMP
  // - the value in 0x6E sets the read/write pointer to a specific startaddress within the specified DMP bank
  // - register 0x6F is the register from which to read or to which to write the data
  //   (after each r/w autoincrement address within the specified DMP bank starting from startaddress)
  SPIwrite(0x6E, startaddress, ChipSelPin);
}


//***********************************************************//
boolean writeDMPMemory()
{
  // - the value in 0x6D activates a specific bank in the DMP
  // - the value in 0x6E sets the read/write pointer to a specific startaddress within the specified DMP bank
  // - register 0x6F is the register from which to read or to which to write the data
  //   (after each r/w autoincrement address within the specified DMP bank starting from startaddress)

  Serial.print("\tWriting   DMP memory.......... ");

  unsigned int i, j;
  byte dmp_byte;

  // ### there are 8 DMP banks (numbers 0 to 7)

  // DMP banks 0 - 6 are completely filled with 256 bytes:
  for (i = 0; i < 7; i ++)
  {
    DEBUG_PRINT("@@@ write bank "); 
    DEBUG_PRINTLN(i);
    setMemoryBank(i, false, false, MPU6000_CS_PIN); // bank number  = i
    setMemoryStartAddress(0, MPU6000_CS_PIN);       // startaddress = 0 so start writing every DMP bank from the beginning
    digitalWrite(MPU6000_CS_PIN,LOW);
    SPI.transfer(0x6F);

    for (j = 0; j < 256; j ++) // max. 256 bytes of data fit into one DMP bank
    {
      dmp_byte = pgm_read_byte(dmpMemory + (i * 256) + j);
      SPI.transfer(dmp_byte);
#ifdef DEBUG
      if (dmp_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug
      Serial.println(dmp_byte, HEX);
#endif
    }
    digitalWrite(MPU6000_CS_PIN,HIGH);
    DEBUG_PRINTLN();
  }

  // DMP bank 7 gets only 137 bytes:
  DEBUG_PRINTLN("@@@ write bank 7");
  setMemoryBank(7, false, false, MPU6000_CS_PIN); // bank number  = 7
  setMemoryStartAddress(0, MPU6000_CS_PIN);       // startaddress = 0 so start writing also this DMP bank from the beginning
  digitalWrite(MPU6000_CS_PIN,LOW);
  SPI.transfer(0x6F);

  for (j = 0; j < 137; j ++) // only 137 bytes of data into DMP bank 7
  {
    dmp_byte = pgm_read_byte(dmpMemory + (7 * 256) + j);
    SPI.transfer(dmp_byte);
#ifdef DEBUG
    if (dmp_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug
    Serial.println(dmp_byte, HEX);
#endif
  }
  digitalWrite(MPU6000_CS_PIN,HIGH);
  DEBUG_PRINTLN();

  Serial.println("done.");

  return true; // end of writeDMPMemory reached
}

//***********************************************************//
boolean verifyDMPMemory()
{
  // - the value in 0x6D activates a specific bank in the DMP
  // - the value in 0x6E sets the read/write pointer to a specific startaddress within the specified DMP bank
  // - register 0x6F is the register from which to read or to which to write the data
  //   (after each r/w autoincrement address within the specified DMP bank starting from startaddress)

  Serial.print("\tVerifying DMP memory.......... ");

  unsigned int i, j;
  byte dmp_byte, check_byte;
  boolean verification = true;

  // ### there are 8 DMP banks (numbers 0 to 7)

  // DMP banks 0 - 6 are completely read, all 256 bytes:
  for (i = 0; i < 7; i ++)
  {
    DEBUG_PRINT(">>> read bank "); 
    DEBUG_PRINTLN(i);
    setMemoryBank(i, false, false, MPU6000_CS_PIN); // bank number  = i
    setMemoryStartAddress(0, MPU6000_CS_PIN);       // startaddress = 0 so start reading every DMP bank from the beginning
    digitalWrite(MPU6000_CS_PIN,LOW);
    SPI.transfer(0x6F | 0x80); // 0x6F | 0x80 causes a "1" added as MSB to 0x6F to denote reading from reg i.s.o. writing to it

    for (j = 0; j < 256; j ++) // max. 256 bytes of data fit into one DMP bank
    {
      check_byte = pgm_read_byte(dmpMemory + (i * 256) + j);
      dmp_byte = SPI.transfer(0x00);
      if (dmp_byte != check_byte)
      {
        Serial.println("$$$ dmpMemory: byte verification error");
        verification = false;
      }
#ifdef DEBUG
      if (dmp_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug
      Serial.println(dmp_byte, HEX);
#endif
    }
    digitalWrite(MPU6000_CS_PIN,HIGH);
    DEBUG_PRINTLN();
  }

  // DMP bank 7 only read first 137 bytes:
  DEBUG_PRINTLN(">>> read bank 7");
  setMemoryBank(7, false, false, MPU6000_CS_PIN); // bank number  = 7
  setMemoryStartAddress(0, MPU6000_CS_PIN);       // startaddress = 0 so start reading also this DMP bank from the beginning
  digitalWrite(MPU6000_CS_PIN,LOW);
  SPI.transfer(0x6F | 0x80); // 0x6F | 0x80 causes a "1" added as MSB to 0x6F to denote reading from reg i.s.o. writing to it

  for (j = 0; j < 137; j ++) // only 137 bytes of data into DMP bank 7
  {
    check_byte = pgm_read_byte(dmpMemory + (7 * 256) + j);
    dmp_byte = SPI.transfer(0x00);
    if (dmp_byte != check_byte)
    {
      Serial.println("$$$ dmpMemory: byte verification error");
      verification = false;
    }
#ifdef DEBUG
    if (dmp_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug
    Serial.println(dmp_byte, HEX);
#endif
  }
  digitalWrite(MPU6000_CS_PIN,HIGH);
  DEBUG_PRINTLN();

  if (verification == true)  Serial.println("success!");
  if (verification == false) Serial.println("FAILED!");

  return verification; // true if DMP correctly written, false if not
}

//***********************************************************//
boolean writeDMPConfig()
{
  byte progBuffer, success, special;
  unsigned int i, j;
  // config set dmpConfig is a long string of blocks with the following structure:
  // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
  byte bank, offset, length;

  Serial.print("\tWriting   DMP configuration... ");

  for (i = 0; i < MPU6050_DMP_CONFIG_SIZE;)
  {
    bank   = pgm_read_byte(dmpConfig + i++); // pgm_read_byte() is a macro that reads a byte of data stored in a specified address(PROGMEM area)
    offset = pgm_read_byte(dmpConfig + i++);
    length = pgm_read_byte(dmpConfig + i++);

    if (length > 0) // regular block of data to write
    {
      DEBUG_PRINT("!! bank  : "); 
      DEBUG_PRINTLNF(bank, HEX);
      setMemoryBank(bank, false, false, MPU6000_CS_PIN); // bank number  = bank
      DEBUG_PRINT("!! offset: "); 
      DEBUG_PRINTLNF(offset, HEX);
      setMemoryStartAddress(offset, MPU6000_CS_PIN);     // startaddress = offset from the beginning (0) of the bank
      DEBUG_PRINT("!! length: "); 
      DEBUG_PRINTLNF(length, HEX);

      digitalWrite(MPU6000_CS_PIN,LOW);
      SPI.transfer(0x6F);

      for (j = 0; j < length; j++)
      {
        progBuffer = pgm_read_byte(dmpConfig + i + j);
        SPI.transfer(progBuffer);
        DEBUG_PRINTLNF(progBuffer, HEX);
      }

      digitalWrite(MPU6000_CS_PIN,HIGH);
      i = i + length;
    }

    else // length = 0; special instruction to write
    {
      // NOTE: this kind of behavior (what and when to do certain things)
      // is totally undocumented. This code is in here based on observed
      // behavior only, and exactly why (or even whether) it has to be here
      // is anybody's guess for now.
      special = pgm_read_byte(dmpConfig + i++);
      DEBUG_PRINTLN("!! Special command code ");
      DEBUG_PRINTF(special, HEX);
      DEBUG_PRINTLN(" found...");
      if (special == 0x01)
      {
        // enable DMP-related interrupts (ZeroMotion, FIFOBufferOverflow, DMP)
        SPIwrite(0x38, 0x32, MPU6000_CS_PIN);  // write 00110010: ZMOT_EN, FIFO_OFLOW_EN, DMP_INT_EN true
        // by the way: this sets all other interrupt enables to false
        success = true;
      }
      else
      {
        // unknown other special command if this may be needed in the future, but for now this should not happen
        success = false;
      }
    }
  }

  Serial.println("done.");

  return true;
}

//***********************************************************//
boolean verifyDMPConfig()
{
  byte check_byte, progBuffer, success, special;
  unsigned int i, j;
  // config set dmpConfig is a long string of blocks with the following structure:
  // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
  byte bank, offset, length;
  boolean verification = true;

  Serial.print("\tVerifying DMP configuration... ");

  for (i = 0; i < MPU6050_DMP_CONFIG_SIZE;)
  {
    bank   = pgm_read_byte(dmpConfig + i++); // pgm_read_byte() is a macro that reads a byte of data stored in a specified address(PROGMEM area)
    offset = pgm_read_byte(dmpConfig + i++);
    length = pgm_read_byte(dmpConfig + i++);

    if (length > 0) // regular block of data to read
    {
      DEBUG_PRINT("!! bank  : "); 
      DEBUG_PRINTLNF(bank, HEX);
      setMemoryBank(bank, false, false, MPU6000_CS_PIN); // bank number  = bank
      DEBUG_PRINT("!! offset: "); 
      DEBUG_PRINTLNF(offset, HEX);
      setMemoryStartAddress(offset, MPU6000_CS_PIN);     // startaddress = offset from the beginning (0) of the bank
      DEBUG_PRINT("!! length: "); 
      DEBUG_PRINTLNF(length, HEX);

      digitalWrite(MPU6000_CS_PIN,LOW);
      SPI.transfer(0x6F | 0x80); // 0x6F | 0x80 causes a "1" added as MSB to 0x6F to denote reading from reg i.s.o. writing to it

      for (j = 0; j < length; j++)
      {
        progBuffer = pgm_read_byte(dmpConfig + i + j);
        check_byte = SPI.transfer(0x00);
        if (progBuffer != check_byte)
        {
          DEBUG_PRINTLN("$$$ dmpConfig: byte verification error");
          verification = false;
        }
#ifdef DEBUG
        if (check_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug
        Serial.println(check_byte, HEX);
#endif
      }

      digitalWrite(MPU6000_CS_PIN,HIGH);
      i = i + length;
    }

    else // length = 0; special instruction to write
    {
      // NOTE: this kind of behavior (what and when to do certain things)
      // is totally undocumented. This code is in here based on observed
      // behavior only, and exactly why (or even whether) it has to be here
      // is anybody's guess for now.
      special = pgm_read_byte(dmpConfig + i++);
      DEBUG_PRINT("!! Special command code ");
      DEBUG_PRINTF(special, HEX);
      DEBUG_PRINTLN(" found...");
      if (special == 0x01)
      {
        // enable DMP-related interrupts (ZeroMotion, FIFOBufferOverflow, DMP)
        check_byte = SPIread(0x38, MPU6000_CS_PIN);  // shoudl read 00110010: ZMOT_EN, FIFO_OFLOW_EN, DMP_INT_EN true

        if (check_byte != 0x32)
        {
          DEBUG_PRINTLN("$$$ dmpConfig: byte verification error");
          verification = false;
        }
#ifdef DEBUG
        if (check_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug
        Serial.println(check_byte, HEX);
#endif

        success = true;
      }
      else
      {
        // unknown special command
        success = false;
      }
    }
  }

  if (verification == true)  Serial.println("success!");
  if (verification == false) Serial.println("FAILED!");

  return verification; // true if DMP correctly written, false if not
}


//***********************************************************//
unsigned int writeDMPUpdates(unsigned int pos, byte update_number)
// process only one line from dmpUpdates each time writeDMPUpdates() is called
{
  // pos is the current reading position within dmpUpdates
  byte progBuffer, success;
  unsigned int j;
  // config set dmpUpdates is a long string of blocks with the following structure:
  // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
  byte bank, offset, length;

  Serial.print("\tWriting   DMP update "); 
  Serial.print(update_number); 
  Serial.print("/7 ..... ");


  bank   = pgm_read_byte(dmpUpdates + pos++); // pgm_read_byte() is a macro that reads a byte of data stored in a specified address(PROGMEM area)
  offset = pgm_read_byte(dmpUpdates + pos++);
  length = pgm_read_byte(dmpUpdates + pos++);

  DEBUG_PRINT("!! bank  : "); 
  DEBUG_PRINTLNF(bank, HEX);
  setMemoryBank(bank, false, false, MPU6000_CS_PIN); // bank number  = bank
  DEBUG_PRINT("!! offset: "); 
  DEBUG_PRINTLNF(offset, HEX);
  setMemoryStartAddress(offset, MPU6000_CS_PIN);     // startaddress = offset from the beginning (0) of the bank
  DEBUG_PRINT("!! length: "); 
  DEBUG_PRINTLNF(length, HEX);

  digitalWrite(MPU6000_CS_PIN,LOW);
  SPI.transfer(0x6F);

  for (j = 0; j < length; j++)
  {
    progBuffer = pgm_read_byte(dmpUpdates + pos + j);
    SPI.transfer(progBuffer);
    DEBUG_PRINTLNF(progBuffer, HEX);
  }

  digitalWrite(MPU6000_CS_PIN,HIGH);
  pos = pos + length;
  DEBUG_PRINT("!! last position written: "); 
  DEBUG_PRINTLN(pos);

  Serial.println("done.");

  return pos; // return last used position in dmpUpdates: will be starting point for next call!
}

//***********************************************************//
unsigned int verifyDMPUpdates(unsigned int pos_verify, byte update_number)
// process only one line from dmpUpdates each time writeDMPUpdates() is called
{
  // pos_verify is the current verifying position within dmpUpdates
  byte check_byte, progBuffer, success;
  unsigned int j;
  // config set dmpUpdates is a long string of blocks with the following structure:
  // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
  byte bank, offset, length;
  boolean verification = true;

  Serial.print("\tVerifying DMP update "); 
  Serial.print(update_number); 
  Serial.print("/7 ..... ");

  bank   = pgm_read_byte(dmpUpdates + pos_verify++); // pgm_read_byte() is a macro that reads a byte of data stored in a specified address(PROGMEM area)
  offset = pgm_read_byte(dmpUpdates + pos_verify++);
  length = pgm_read_byte(dmpUpdates + pos_verify++);

  DEBUG_PRINT("!! bank  : "); 
  DEBUG_PRINTLNF(bank, HEX);
  setMemoryBank(bank, false, false, MPU6000_CS_PIN); // bank number  = bank
  DEBUG_PRINT("!! offset: "); 
  DEBUG_PRINTLNF(offset, HEX);
  setMemoryStartAddress(offset, MPU6000_CS_PIN);     // startaddress = offset from the beginning (0) of the bank
  DEBUG_PRINT("!! length: "); 
  DEBUG_PRINTLNF(length, HEX);

  digitalWrite(MPU6000_CS_PIN,LOW);
  SPI.transfer(0x6F | 0x80); // 0x6F | 0x80 causes a "1" added as MSB to 0x6F to denote reading from reg i.s.o. writing to it

  for (j = 0; j < length; j++)
  {
    progBuffer = pgm_read_byte(dmpUpdates + pos_verify + j);
    check_byte = SPI.transfer(0x00);
    if (progBuffer != check_byte)
    {
      DEBUG_PRINTLN("$$$ dmpUpdates: byte verification error");
      verification = false;
    }
#ifdef DEBUG
    if (check_byte < 0x10) Serial.print("0"); // add leading zero - this is an Arduino bug
    Serial.println(check_byte, HEX);
#endif
  }

  digitalWrite(MPU6000_CS_PIN,HIGH);
  pos_verify = pos_verify + length;
  DEBUG_PRINT("!! last position verified: "); 
  DEBUG_PRINTLN(pos_verify);

  if (verification == true)  Serial.println("success!");
  if (verification == false) Serial.println("FAILED!");
  //return verification; // true if DMP correctly written, false if not

  return pos_verify; // return last used position in dmpUpdates: will be starting point for next call!
}


//***********************************************************//
/** Get current FIFO buffer size.
 * This value indicates the number of bytes stored in the FIFO buffer. This
 * number is in turn the number of bytes that can be read from the FIFO buffer
 * and it is directly proportional to the number of samples available given the
 * set of sensor data bound to be stored in the FIFO (defined by register 35 and 36).
 * - return: Current FIFO buffer size
 */
unsigned int getFIFOCount(int ChipSelPin)
{
  // FIFO_COUNT should always be read in high-low order (0x72-0x73) in order to
  // guarantee that the most current FIFO Count value is read
  byte fifo_H = SPIread(0x72, MPU6000_CS_PIN);
  byte fifo_L = SPIread(0x73, MPU6000_CS_PIN);
  unsigned int two_bytes = (fifo_H << 8) | fifo_L;
  return two_bytes;
}  

byte dmpInitialize()
{
  // Trigger a full device reset.
  // A small delay of ~50ms may be desirable after triggering a reset.
  DEBUG_PRINTLN(F("Resetting MPU6000..."));
  SPIwriteBit(0x6B, 7, true, MPU6000_CS_PIN); // DEVICE_RESET
  digitalWrite(GREEN_LED_PIN, HIGH); // shows start of DMP inititialize
  delay(30 + 170); // wait after reset + time to see the LED blink

  // Setting the SLEEP bit in the register puts the device into very low power
  // sleep mode. In this mode, only the serial interface and internal registers
  // remain active, allowing for a very low standby current. Clearing this bit
  // puts the device back into normal mode. To save power, the individual standby
  // selections for each of the gyros should be used if any gyro axis is not used
  // by the application.
  // disable sleep mode
  DEBUG_PRINTLN(F("Disabling sleep mode..."));
  SPIwriteBit(0x6B, 6, false, MPU6000_CS_PIN); // SLEEP

  // get MPU hardware revision
  DEBUG_PRINTLN(F("Selecting user bank 16..."));
  setMemoryBank(0x10, true, true, MPU6000_CS_PIN);
  DEBUG_PRINTLN(F("Selecting memory byte 6..."));
  setMemoryStartAddress(0x06, MPU6000_CS_PIN);
  DEBUG_PRINTLN(F("Checking hardware revision..."));
  digitalWrite(MPU6000_CS_PIN,LOW);
  SPI.transfer(0x6F | 0x80); // 0x6F | 0x80 causes a "1" added as MSB to 0x6F to denote reading from reg i.s.o. writing to it
  byte hwRevision = SPI.transfer(0x00);
  digitalWrite(MPU6000_CS_PIN,HIGH);
  DEBUG_PRINT(F("Revision @ user[16][6] = "));
  DEBUG_PRINTLNF(hwRevision, HEX);
  DEBUG_PRINTLN(F("Resetting memory bank selection to 0..."));
  setMemoryBank(0, false, false, MPU6000_CS_PIN);

  // check OTP bank valid
  DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));
  byte otpValid = SPIreadBit(0x00, 0, MPU6000_CS_PIN);
  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 TC values..."));
  byte xgOffsetTC = SPIreadBits(0x01, 6, 6, MPU6000_CS_PIN); // [7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
  byte ygOffsetTC = SPIreadBits(0x02, 6, 6, MPU6000_CS_PIN); // [7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
  byte zgOffsetTC = SPIreadBits(0x03, 6, 6, MPU6000_CS_PIN); // [7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
  DEBUG_PRINT("X gyro offset = ");
  DEBUG_PRINTLN(xgOffsetTC);
  DEBUG_PRINT("Y gyro offset = ");
  DEBUG_PRINTLN(ygOffsetTC);
  DEBUG_PRINT("Z gyro offset = ");
  DEBUG_PRINTLN(zgOffsetTC);

  // 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 (writeDMPMemory())
  {
    DEBUG_PRINTLN(F("########################### Success! DMP code written but not verified."));

    DEBUG_PRINTLN(F("########################### Verify DMP code..."));
    verifyDMPMemory();

    digitalWrite(GREEN_LED_PIN, LOW); // shows end of write DMP memory
    delay(200); // time to see the LED blink

    // 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 (writeDMPConfig())
    {
      DEBUG_PRINTLN(F("########################### Success! DMP configuration written but not verified."));

      DEBUG_PRINTLN(F("########################### Verify DMP configuration..."));
      verifyDMPConfig();

      digitalWrite(GREEN_LED_PIN, HIGH); // shows start of write DMP configuration
      delay(200); // time to see the LED blink

      DEBUG_PRINTLN(F("Setting clock source to Z Gyro..."));
      SPIwriteBits(0x6B, 2, 3, 0x03, MPU6000_CS_PIN); // CLKSEL[2:0] = 011 = PLL with Z axis gyroscope reference

      DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
      SPIwrite(0x38, 0x12, MPU6000_CS_PIN); // INT_ENABLE = 00010010 = FIFO_OFLOW_EN & DMP_INT_EN

        // register INT_ENABLE 0x38:
      // bit 0 DATA_RDY_EN      0x01
      // bit 1 DMP_INT_EN       0x02 (undocumented) - enabling this bit also enables DATA_RDY_EN it seems
      // bit 2 UNKNOWN_INT_EN   0x04 (undocumented)
      // bit 3 I2C_MST_INT_EN   0x08
      // bit 4 FIFO_OFLOW_EN    0x10
      // bit 5 ZMOT_EN          0x20 (undocumented)
      // bit 6 MOT_EN           0x40
      // bit 7 FF_EN            0x80 (undocumented)

      // register INT_STATUS 0x3A:
      // bit 0 DATA_RDY_INT     0x01
      // bit 1 DMP_INT          0x02 (undocumented)
      // bit 2 UNKNOWN_INT      0x04 (undocumented)
      // bit 3 I2C_MST_INT      0x08
      // bit 4 FIFO_OFLOW_INT   0x10
      // bit 5 ZMOT_INT         0x20 (undocumented)
      // bit 6 MOT_INT          0x40
      // bit 7 FF_INT           0x80 (undocumented)

      DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
      //setRate(4); // 1kHz / (1 + 4) = 200 Hz (when DLPF_CFG enabled [1 to 6] - true, see below)
      SPIwrite(0x19, 4, MPU6000_CS_PIN); // SMPLRT_DIV[7:0] = 4 (ok)

      // FSYNC input not connnected on ArduIMU+ V3
      DEBUG_PRINTLN(F("Disable external frame synchronization..."));
      SPIwriteBits(0x1A, 5, 3, 0x00, MPU6000_CS_PIN); // EXT_SYNC_SET[2:0] = 000 = input disabled

      DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
      SPIwriteBits(0x1A, 2, 3, 0x03, MPU6000_CS_PIN); // DLPF_CFG[2:0] = 011 = accel 44 Hz gyro 42 Hz

      DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
      SPIwriteBits(0x1B, 4, 2, 0x03, MPU6000_CS_PIN); // FS_SEL[1:0] = 11 = +/- 2000 deg/s

      DEBUG_PRINTLN(F("Setting accelerometer full scale range to +/- 2 g..."));
      SPIwriteBits(0x1C, 4, 2, 0x00, MPU6000_CS_PIN);

      DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)..."));
      SPIwrite(0x70, 0x03, MPU6000_CS_PIN); // DMP related register
      SPIwrite(0x71, 0x00, MPU6000_CS_PIN); // DMP related register

      DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
      SPIwriteBit(0x00, 0, false, MPU6000_CS_PIN); // [0] OTP_BNK_VLD

      // enabling this part causes misalignment and drift of x, y and z axis
      // relative to ArduIMU+ V3/MPU-6000 x, y and z axis
      /*
      DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values..."));
       SPIwriteBits(0x00, 6, 6, xgOffsetTC, MPU6000_CS_PIN);
       SPIwriteBits(0x01, 6, 6, ygOffsetTC, MPU6000_CS_PIN);
       SPIwriteBits(0x02, 6, 6, zgOffsetTC, MPU6000_CS_PIN);
       */

      /*
      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)..."));
      byte update_number = 1;      // holds update number for user information
      unsigned int pos = 0;        // pos        is the current reading position within dmpUpdates; this is the first call; set pos        = 0 only once!
      pos = writeDMPUpdates(pos, update_number); 
      unsigned int pos_verify = 0; // pos_verify is the current reading position within dmpUpdates; this is the first call; set pos_verify = 0 only once!
      pos_verify = verifyDMPUpdates(pos_verify, update_number);

      DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)..."));
      update_number ++;
      pos = writeDMPUpdates(pos, update_number); 
      pos_verify = verifyDMPUpdates(pos_verify, update_number);

      DEBUG_PRINTLN(F("Resetting FIFO..."));
      //SPIwriteBit(0x6A, 6, false, MPU6000_CS_PIN); // FIFO_EN = 0 = disable
      SPIwriteBit(0x6A, 2, true, MPU6000_CS_PIN); // FIFO_RESET = 1 = reset (ok) only when FIFO_EN = 0
      //SPIwriteBit(0x6A, 6, true, MPU6000_CS_PIN); // FIFO_EN = 1 = enable

      // Get current FIFO buffer size.
      // This value indicates the number of bytes stored in the FIFO buffer. This
      // number is in turn the number of bytes that can be read from the FIFO buffer
      // and it is directly proportional to the number of samples available given the
      // set of sensor data bound to be stored in the FIFO (register 35 and 36).
      DEBUG_PRINTLN(F("Reading FIFO count..."));
      unsigned int fifoCount = getFIFOCount(MPU6000_CS_PIN);

      // just after FIFO reset so count probably 0
      DEBUG_PRINT(F("Current FIFO count = "));
      DEBUG_PRINTLN(fifoCount);
      SPIreadBytes(0x74, fifoCount, fifoBuffer, MPU6000_CS_PIN);

      DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
      SPIwrite(0x1F, 2, MPU6000_CS_PIN); // MOT_THR[7:0] = 2

      DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
      SPIwrite(0x21, 156, MPU6000_CS_PIN); // detection threshold for Zero Motion interrupt generation

      DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
      SPIwrite(0x20, 80, MPU6000_CS_PIN); // duration counter threshold for Motion interrupt generation. The duration counter ticks at 1 kHz, therefore MOT_DUR has a unit of 1 LSB = 1 ms

      DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
      SPIwrite(0x22, 0, MPU6000_CS_PIN); // duration counter threshold for Zero Motion interrupt generation. The duration counter ticks at 16 Hz, therefore ZRMOT_DUR has a unit of 1 LSB = 64 ms

      DEBUG_PRINTLN(F("Resetting FIFO..."));
      //SPIwriteBit(0x6A, 6, false, MPU6000_CS_PIN); // FIFO_EN = 0 = disable
      SPIwriteBit(0x6A, 2, true, MPU6000_CS_PIN); // FIFO_RESET = 1 = reset (ok) only when FIFO_EN = 0
      //SPIwriteBit(0x6A, 6, true, MPU6000_CS_PIN); // FIFO_EN = 1 = enable

      DEBUG_PRINTLN(F("Enabling FIFO..."));
      SPIwriteBit(0x6A, 6, true, MPU6000_CS_PIN); // FIFO_EN = 1 = enable

      DEBUG_PRINTLN(F("Enabling DMP..."));
      SPIwriteBit(0x6A, 7, true, MPU6000_CS_PIN); // USER_CTRL_DMP_EN

        DEBUG_PRINTLN(F("Resetting DMP..."));
      SPIwriteBit(0x6A, 3, true, MPU6000_CS_PIN); // Reset DMP

      DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)..."));
      update_number ++;
      pos = writeDMPUpdates(pos, update_number); 
      pos_verify = verifyDMPUpdates(pos_verify, update_number);

      DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)..."));
      update_number ++;
      pos = writeDMPUpdates(pos, update_number); 
      pos_verify = verifyDMPUpdates(pos_verify, update_number);

      DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)..."));
      update_number ++;
      pos = writeDMPUpdates(pos, update_number); 
      pos_verify = verifyDMPUpdates(pos_verify, update_number);

      //delay(50); // may be used as test just to see if number of FIFO bytes changes

      DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
      while ((fifoCount = getFIFOCount(MPU6000_CS_PIN)) < 3);

      /* switched off, may crash the sketch (FIFO contents not used here anyway)
       // 1st read FIFO
       DEBUG_PRINT(F("Current FIFO count = "));
       DEBUG_PRINTLN(fifoCount);
       DEBUG_PRINTLN(F("Reading FIFO data..."));
       Serial.println("Reading FIFO data 1st time...");
       SPIreadBytes(0x74, fifoCount, fifoBuffer, MPU6000_CS_PIN);
       */

      DEBUG_PRINTLN(F("Reading interrupt status..."));
      byte mpuIntStatus = SPIread(0x3A, MPU6000_CS_PIN);

      DEBUG_PRINT(F("Current interrupt status = "));
      DEBUG_PRINTLNF(mpuIntStatus, HEX);

      // Jeff Rowberg's code had a read statement here... I suppose that must be a write statement!
      //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("Writing final memory update 6/7 (function unknown)..."));
      update_number ++;
      pos = writeDMPUpdates(pos, update_number); 
      pos_verify = verifyDMPUpdates(pos_verify, update_number);

      DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
      while ((fifoCount = getFIFOCount(MPU6000_CS_PIN)) < 3);

      DEBUG_PRINT(F("Current FIFO count="));
      DEBUG_PRINTLN(fifoCount);

      /* switched off, may crash the sketch (FIFO contents not used here anyway)
       // 2nd read FIFO
       //DEBUG_PRINTLN(F("Reading FIFO data..."));
       Serial.println("Reading FIFO data 2nd time...");
       SPIreadBytes(0x74, fifoCount, fifoBuffer, MPU6000_CS_PIN);
       */

      DEBUG_PRINTLN(F("Reading interrupt status..."));
      mpuIntStatus = SPIread(0x3A, MPU6000_CS_PIN);

      DEBUG_PRINT(F("Current interrupt status="));
      DEBUG_PRINTLNF(mpuIntStatus, HEX);

      DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)..."));
      update_number ++;
      pos = writeDMPUpdates(pos, update_number); 
      pos_verify = verifyDMPUpdates(pos_verify, update_number);

      DEBUG_PRINTLN(F("DMP is good to go! Finally."));

      DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
      SPIwriteBit(0x6A, 7, false, MPU6000_CS_PIN); // USER_CTRL_DMP_EN

        DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time..."));
      //SPIwriteBit(0x6A, 6, false, MPU6000_CS_PIN); // FIFO_EN = 0 = disable
      SPIwriteBit(0x6A, 2, true, MPU6000_CS_PIN); // FIFO_RESET = 1 = reset (ok) only when FIFO_EN = 0
      //SPIwriteBit(0x6A, 6, true, MPU6000_CS_PIN); // FIFO_EN = 1 = enable
      SPIread(0x3A, MPU6000_CS_PIN); // reading the register will clear all INT bits

    }
    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
  }

  digitalWrite(GREEN_LED_PIN, LOW); // shows end of write DMP configuration
  delay(200); // time to see the LED blink

  Serial.println("... Digital Motion Processor (DMP) initializing done.");
  Serial.println(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
  return 0; // success
}

hudbrog писал(а):Вот работающий код для APM 2.5

Земляк, спасибо!!! Очень выручил!!! Работает. Буду копать.

Тему не закрываю, буду выкладывать наработки на эту тему.