int ITG3200::init(byte address, byte _SRateDiv, byte _Range, byte _filterBW, byte _ClockSrc, bool _ITGReady, bool _INTRawDataReady) { int a=0; // _dev_address = address; // //this needs to be debugged // a=readFrom(_dev_address,WHO_AM_I,1,_buff); // MYASSERT(a,"Failed to write to register\n\r") // usart_printfm(USARTx,(const int *)"Succesfully read from register\n\r"); // usart_printfm(USARTx,(const int *)"Value of the who am i register %d \n\r",_buff[0]); // // a=writeTo(_dev_address,WHO_AM_I,66); // // MYASSERT(a,"Failed to write 2 register2\n\r") // // usart_printfm(USARTx,(const int *)"Succesfully written to register2\n\r"); // a=readFrom(_dev_address,WHO_AM_I,1,_buff); // usart_printfm(USARTx,(const int *)"Value of the who am i register %d \n\r",_buff[0]); // MYASSERT(a,"Failed to write to register\n\r") // //this is needed to be debugged // usart_printfm(USARTx,(const int *)"Value of address:%2x\n\r",_dev_address); a=setSampleRateDiv(_SRateDiv); MYASSERT(a,"Failed to set sample rate div\n\r") a=setFSRange(_Range); MYASSERT(a,"Failed to set range\n\r") a=setFilterBW(_filterBW); MYASSERT(a,"Failed to set filter BW\n\r") a=setClockSource(_ClockSrc); MYASSERT(a,"Failed to set clocksrc\n\r") a=setITGReady(_ITGReady); MYASSERT(a,"Failed to set ITG ready\n\r") a=setRawDataReady(_INTRawDataReady); MYASSERT(a,"Failed to set data ready mode\n\r") ms_delay(GYROSTART_UP_DELAY); // startup return 0; }
bool MPU6050::initialize() { setClockSource(MPU6050_CLOCK_PLL_XGYRO); setFullScaleGyroRange(MPU6050_GYRO_FS_250); setFullScaleAccelRange(MPU6050_ACCEL_FS_2); setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! return 1; }
// specific configuration combinations void xpcc::atxmega::TimerInterruptE1::setMsTimer(F function, uint8_t interval) { setClockSource(TC_CLKSEL_DIV64_gc); attachOverflowInterrupt(TC_OVFINTLVL_MED_gc, function); TCE1_PER = (interval * F_CPU) / 64000l; }
// specific configuration combinations void xpcc::atxmega::TimerD0::setMsTimer(uint8_t interval) { setClockSource(TC_CLKSEL_DIV64_gc); setOverflowInterrupt(TC_OVFINTLVL_MED_gc); TCD0_PER = (interval * F_CPU) / 64000l; }
void ITG3200::init(unsigned int address, byte _SRateDiv, byte _Range, byte _filterBW, byte _ClockSrc, bool _ITGReady, bool _INTRawDataReady) { _dev_address = address; setSampleRateDiv(_SRateDiv); setFSRange(_Range); setFilterBW(_filterBW); setClockSource(_ClockSrc); setITGReady(_ITGReady); setRawDataReady(_INTRawDataReady); delay(GYROSTART_UP_DELAY); // startup }
void mpu_setup() { devAddr = MPU6050_DEFAULT_ADDRESS; //switchSPIEnabled(true); DELAY_US(1*1000); setClockSource(MPU6050_CLOCK_PLL_XGYRO); setFullScaleGyroRange(MPU6050_GYRO_FS_250); setFullScaleAccelRange(MPU6050_ACCEL_FS_2); setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! }
void MPU6XXX32::init(SPI2C_config * con) { i2cConfig = con; comP = SPI2C::setSPIC2C(i2cConfig); WR->setSPI2CConfig(i2cConfig); if (comP->currentConfg->spi2ctype == SPI2C_SPI_TRANSACTIONAL) spiMode = true; setClockSource(MPU6050_CLOCK_PLL_XGYRO); setFullScaleGyroRange(MPU6050_GYRO_FS_250); setFullScaleAccelRange(MPU6050_ACCEL_FS_2); setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! }
/** Power on and prepare for general usage. * This will activate the device and take it out of sleep mode (which must be done * after start-up). This function also sets both the accelerometer and the gyroscope * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets * the clock source to use the X Gyro for reference, which is slightly better than * the default internal clock source. */ void MPU6050::initialize() { #ifdef useDebugSerial debugSerial.printf("MPU6050::initialize start\n\r"); #endif setClockSource(MPU6050_CLOCK_PLL_XGYRO); setFullScaleGyroRange(MPU6050_GYRO_FS_500); setFullScaleAccelRange(MPU6050_ACCEL_FS_16); setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! #ifdef useDebugSerial debugSerial.printf("MPU6050::initialize end\n\r"); #endif }
/* * Put sensor in default sampling configuration * Range = +/- 2000 deg/sec * Sample rate divider = 9 -> sample rate = 100 Hz * LPF = 188 Hz bandwidth * Clock source = xGyro */ bool ITG3200::init() { bool status = false; if(test()) { gyroStatus = ON; setFullRange(); setSampleRate(0x9); setLPF(ITG3200_DLPF_188); setClockSource(ITG3200_CLK_SEL_XGYRO); status = true; } else { Serial.println("WARNING: gyro not responding"); status = false; } return status; }
/******************************** MPU6050 Functions ********************************/ void initializeIMU() { //Init Interrupt Output Pin P1REN |= BIT4; P1OUT |= BIT4; P1IFG &= ~BIT4; // P1.4 IFG cleared P1IE |= BIT4; // P1.4 interrupt enabled P4SEL |= BIT1 + BIT2; // P4.1 & P4.2 SDA/SCL Select // Delays do not need to be this long. Delays are present to keep I2C line clear. // This functions can be altered by the user. Check MPU6050.h file by Jeff Rowberg to determine the sensitivty variables. setClockSource(MPU6050_CLOCK_PLL_XGYRO); // Set MPU6050 clock msDelay(100); setFullScaleGyroRange(MPU6050_GYRO_FS_2000); // Gyroscope sensitivity set to 2000 degrees/sec msDelay(100); setFullScaleAccelRange(MPU6050_ACCEL_FS_4); // Accelerometer sensitivity set to 4g msDelay(100); setDLPFConfig(MPU6050_DLPF_BW_5); // Digital Low Pass Filter Configuration setInterruptPin(); setSleepEnabled(0); // Wake up device. msDelay(100); }
int main (void) { initI2C1 (); initUsart (); #if 0 /* * +----------------+ * | Initialization | * +----------------+ */ accelgyro.initialize(); setClockSource(MPU6050_CLOCK_PLL_XGYRO/*0x01*/); /* * Excerpt from domcumentation : Upon power up, the MPU-60X0 clock source defaults to the internal oscillator. * However, it is highly recommended that the device be configured to use one of the gyroscopes. Below is the code * which does it: */ I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1/*0x6B*/, MPU6050_PWR1_CLKSEL_BIT/*2*/, MPU6050_PWR1_CLKSEL_LENGTH/*3*/, source/*MPU6050_CLOCK_PLL_XGYRO 0x01*/); setFullScaleGyroRange(MPU6050_GYRO_FS_250/*0x00*/); /* * 0x1b register is used to trigger gyroscope self-test and configure the gyroscopes’ full scale range. Below * we set ful scale to be +/- 250 units (seconds?) */ I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG/*0x1B*/, MPU6050_GCONFIG_FS_SEL_BIT/*4*/, MPU6050_GCONFIG_FS_SEL_LENGTH/*2*/, range/*0x00*/); setFullScaleAccelRange(MPU6050_ACCEL_FS_2/*0x00*/); /* * Set accelerometer full scale to be +/- 2g. */ I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG/*0x1C*/, MPU6050_ACONFIG_AFS_SEL_BIT/*4*/, MPU6050_ACONFIG_AFS_SEL_LENGTH/*2*/, range/*0*/); setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! /* * By default MPU6050 is in sleep mode after powering up. Below we are waking it back on. This * is done using the same register as in first line, */ I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1/*0x6B*/, MPU6050_PWR1_SLEEP_BIT/*6*/, enabled/*false*/); accelgyro.testConnection() getDeviceID() == 0x34; /* * This register is used to verify the identity of the device. The contents of WHO_AM_I are * the upper 6 bits of the MPU-60X0’s 7-bit I C address. The Power-On-Reset value of Bit6:Bit1 is 0b110100 == 0x34. */ I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I/*0x75*/, MPU6050_WHO_AM_I_BIT/*6*/, MPU6050_WHO_AM_I_LENGTH/*6*/, buffer); return buffer[0]; /* * +----------------+ * | Main loop | * +----------------+ */ int16_t ax, ay, az; int16_t gx, gy, gz; accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); /* * In MPU-6000 and MPU-6050 Product Specification Rev 3.3 on pages 36 and 37 we read, that I²C reads and writes * can be performed with single byte or multiple bytes. In single byte mode, we issue (after sending slave * address ofcourse) a register address, and send or receive one byte of data. Multiple byte reads and writes, at the * other hand consist of slave address, regiser address and multiple consecutive bytes od data. Slave puts or gets * first byte from the register with the address we've just sent, and increases this addres by 1 after each byte. * * This is very useful in case of accelerometer and gyroscope because manufacturer has set up the apropriate registers * cnsecutively, so one can read accel, internal temp and gyro data in one read command. Below is the code which does * exactly this: */ I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H/*0x3B*/, 14, buffer); *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; *az = (((int16_t)buffer[4]) << 8) | buffer[5]; *gx = (((int16_t)buffer[8]) << 8) | buffer[9]; *gy = (((int16_t)buffer[10]) << 8) | buffer[11]; *gz = (((int16_t)buffer[12]) << 8) | buffer[13]; #endif // Configuration: I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter); // start a transmission in Master transmitter mode I2C_write_slow (I2C1, MPU6050_RA_PWR_MGMT_1); // Register address I2C_write (I2C1, MPU6050_CLOCK_PLL_XGYRO); // Register value = 0x01. Which means, that DEVICE_RESET, SLEEP, CYCLE and TEMP_DIS are all 0. I2C_stop (I2C1); I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter); I2C_write (I2C1, MPU6050_RA_GYRO_CONFIG); I2C_write (I2C1, MPU6050_GYRO_FS_250); // All bits set to zero. I2C_stop (I2C1); I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter); I2C_write (I2C1, MPU6050_RA_ACCEL_CONFIG); I2C_write (I2C1, MPU6050_ACCEL_FS_2); // All bits set to zero. I2C_stop (I2C1); // Simple test if communication is working I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter); I2C_write (I2C1, MPU6050_RA_WHO_AM_I); I2C_stop (I2C1); I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Receiver); uint8_t whoAmI = I2C_read_nack (I2C1); // read one byte and don't request another byte I2C_stop (I2C1); if (whoAmI == 0x34) { usartSendString (USART1, "Accelerometer has been found!\r\n"); } else { usartSendString (USART1, "*NO* Accelerometer has been found!\r\n"); } while (1) { I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Transmitter); I2C_write (I2C1, MPU6050_RA_ACCEL_XOUT_H); I2C_stop (I2C1); I2C_start (I2C1, MPU6050_ADDRESS_AD0_LOW, I2C_Direction_Receiver); uint16_t ax = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1); uint16_t ay = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1); uint16_t az = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1); uint16_t temp = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1); uint16_t gx = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1); uint16_t gy = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_ack (I2C1); uint16_t gz = ((uint16_t)I2C_read_ack (I2C1) << 8) | I2C_read_nack (I2C1); I2C_stop (I2C1); printf ("Accel : (%d, %d, %d), temperature : %d, gyro : (%d, %d, %d)\r\n", ax, ay, az, temp, gx, gy, gz); } }
unsigned char dmpInitialize() { // reset device mpu_reset(); DELAY_US(30*1000); // disable sleep mode setSleepEnabled(false); // get MPU hardware revision setMemoryBank(0x10, true, true); setMemoryStartAddress(0x06); unsigned char hwRevision = readMemoryByte(); setMemoryBank(0, false, false); // check OTP bank valid unsigned char otpValid = getOTPBankValid(); // get X/Y/Z gyro offsets char xgOffsetTC = getXGyroOffset(); char ygOffsetTC = getYGyroOffset(); char zgOffsetTC = getZGyroOffset(); // setup weird slave stuff (?) // setSlaveAddress(0, 0x7F); // setI2CMasterModeEnabled(false); // setSlaveAddress(0, 0x68); // resetI2CMaster(); DELAY_US(20*1000); // load DMP code into memory banks if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE,0,0,false)) { if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { setClockSource(MPU6050_CLOCK_PLL_ZGYRO); setIntEnabled(0x12); setRate(4); // 1khz / (1 + 4) = 200 Hz setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); setDLPFMode(MPU6050_DLPF_BW_42); // setFullScaleGyroRange(MPU6050_GYRO_FS_2000); // setFullScaleAccelRange(MPU6050_ACCEL_FS_16); setDMPConfig1(0x03); setDMPConfig2(0x00); setOTPBankValid(false); // setXGyroOffset(xgOffsetTC); // setYGyroOffset(ygOffsetTC); // setZGyroOffset(zgOffsetTC); unsigned char dmpUpdate[16], j; unsigned int pos = 0; for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false); resetFIFO(); unsigned int fifoCount = getFIFOCount(); if (fifoCount > 128) { fifoCount = 128; } getFIFOBytes(fifoBuffer, fifoCount); setMotionDetectionThreshold(2); setZeroMotionDetectionThreshold(156); setMotionDetectionDuration(80); setZeroMotionDetectionDuration(0); resetFIFO(); setFIFOEnabled(true); setDMPEnabled(true); resetDMP(); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false); // while ((getFIFOCount()) < 3) {DELAY_US(500);} // // fifoCount = getFIFOCount(); // // getFIFOBytes(fifoBuffer, fifoCount); unsigned char mpuIntStatus = getIntStatus(); 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]); // while ((getFIFOCount()) < 3) {DELAY_US(500);} // // fifoCount = getFIFOCount(); // getFIFOBytes(fifoBuffer, fifoCount); mpuIntStatus = getIntStatus(); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false); setDMPEnabled(true); resetFIFO(); getIntStatus(); } else { return 2; // configuration block loading failed } } else { return 1; // main binary block loading failed } return 0; // success }
/** * Initialize DMP inside MPU6050 * @return boolean */ uint8_t MPU6050::dmpInitialize() { //Resetting MPU6050... reset(); usleep(30000); // wait after reset //Disabling sleep mode... setSleepEnabled(false); // get MPU hardware revision //Selecting user bank 16... setMemoryBank(0x10, true, true); //Selecting memory byte 6... setMemoryStartAddress(0x06); //Checking hardware revision... uint8_t hwRevision __attribute__((__unused__)) = readMemoryByte(); //Revision @ user[16][6] = //hwRevision, HEX); //Resetting memory bank selection to 0... setMemoryBank(0, false, false); // check OTP bank valid //Reading OTP bank valid flag... uint8_t otpValid __attribute__((__unused__)) = getOTPBankValid(); //OTP bank is //otpValid ? F("valid!") : F("invalid! // get X/Y/Z gyro offsets //Reading gyro offset values... /* int8_t xgOffset = getXGyroOffset(); int8_t ygOffset = getYGyroOffset(); int8_t zgOffset = getZGyroOffset(); sleep(5); for(int cnt = 0; cnt < 1000; cnt = cnt + 1) { std::cout << "X: " << typeid(xgOffset).name() << "Y: " << typeid(ygOffset).name() << "Z: " << typeid(zgOffset).name() << "\n"; sleep(1); //xgOffset = (getXGyroOffset() + xgOffset)/2; //ygOffset = (getYGyroOffset() + ygOffset)/2; //zgOffset = (getZGyroOffset() + zgOffset)/2; } */ //X gyro offset = //xgOffset); //Y gyro offset = //ygOffset); //Z gyro offset = //zgOffset); // setup weird slave stuff (?) //Setting slave 0 address to 0x7F... setSlaveAddress(0, 0x7F); //Disabling I2C Master mode... setI2CMasterModeEnabled(false); //Setting slave 0 address to 0x68 (self)... setSlaveAddress(0, 0x68); /* if (addr == 104) { std::cout << "Address: 104"; setSlaveAddress(0, 0x68); } else { std::cout << "Address: 105"; setSlaveAddress(0, 0x69); } * */ //Resetting I2C Master control... resetI2CMaster(); usleep(20000); // load DMP code into memory banks //Writing DMP code to MPU memory banks ( // bytes) if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { // write DMP configuration //Writing DMP configuration to MPU memory banks ( // bytes in config def) if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { //Setting clock source to Z Gyro... setClockSource(MPU6050_CLOCK_PLL_ZGYRO); //Setting DMP and FIFO_OFLOW interrupts enabled... setIntEnabled(0x12); //Setting sample rate to 200Hz... setRate(4); // 1khz / (1 + 4) = 200 Hz //Setting external frame sync to TEMP_OUT_L[0]... setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); //Setting DLPF bandwidth to 42Hz... setDLPFMode(MPU6050_DLPF_BW_42); //Setting gyro sensitivity to +/- 2000 deg/sec... setFullScaleGyroRange(MPU6050_GYRO_FS_2000); //Setting DMP configuration bytes (function unknown)... setDMPConfig1(0x03); setDMPConfig2(0x00); //Clearing OTP Bank flag... setOTPBankValid(false); /* //Setting X/Y/Z gyro offsets to previous values... setXGyroOffset(xgOffset); setYGyroOffset(ygOffset); setZGyroOffset(zgOffset); //Setting X/Y/Z gyro user offsets to zero... setXGyroOffsetUser(0); setYGyroOffsetUser(0); setZGyroOffsetUser(0); */ //Writing final memory update 1/7 (function unknown)... uint8_t dmpUpdate[16], j; uint16_t pos = 0; for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); //Writing final memory update 2/7 (function unknown)... for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); //Resetting FIFO... resetFIFO(); //Reading FIFO count... uint8_t fifoCount = getFIFOCount(); uint8_t fifoBuffer[128]; //fifoCount); if (fifoCount > 0) getFIFOBytes(fifoBuffer, fifoCount); //Setting motion detection threshold to 2... setMotionDetectionThreshold(2); //Setting zero-motion detection threshold to 156... setZeroMotionDetectionThreshold(156); //Setting motion detection duration to 80... setMotionDetectionDuration(80); //Setting zero-motion detection duration to 0... setZeroMotionDetectionDuration(0); //Resetting FIFO... resetFIFO(); //Enabling FIFO... setFIFOEnabled(true); //Enabling DMP... setDMPEnabled(true); //Resetting DMP... resetDMP(); //Writing final memory update 3/7 (function unknown)... for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); //Writing final memory update 4/7 (function unknown)... for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); //Writing final memory update 5/7 (function unknown)... for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); while ((fifoCount = getFIFOCount()) < 3); //Reading FIFO data... getFIFOBytes(fifoBuffer, fifoCount); //Reading interrupt status... uint8_t mpuIntStatus __attribute__((__unused__)) = getIntStatus(); //Current interrupt status= mpuIntStatus, HEX //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]); //Waiting for FIFO count > 2... while ((fifoCount = getFIFOCount()) < 3); //Current FIFO count= //fifoCount); //Reading FIFO data... getFIFOBytes(fifoBuffer, fifoCount); //Reading interrupt status... mpuIntStatus = getIntStatus(); //Current interrupt status= //mpuIntStatus, HEX //Writing final memory update 7/7 (function unknown)... for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); //DMP is good to go! Finally. //Disabling DMP (you turn it on later)... setDMPEnabled(false); //Setting up internal 42-byte (default) DMP packet buffer... dmpPacketSize = 42; /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { return 3; // TODO: proper error code for no memory }*/ //Resetting FIFO and clearing INT status one last time... resetFIFO(); getIntStatus(); } else { //ERROR! DMP configuration verification failed. return 2; // configuration block loading failed } } else { //ERROR! DMP code verification failed. return 1; // main binary block loading failed } return 0; // success }
uint8_t MPU6050DMP::dmpInitialize() { // reset device DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); reset(); delay(30); // wait after reset // enable sleep mode and wake cycle /*Serial.println(F("Enabling sleep mode...")); setSleepEnabled(true); Serial.println(F("Enabling wake cycle...")); setWakeCycleEnabled(true);*/ // disable sleep mode DEBUG_PRINTLN(F("Disabling sleep mode...")); setSleepEnabled(false); // get MPU hardware revision DEBUG_PRINTLN(F("Selecting user bank 16...")); setMemoryBank(0x10, true, true); DEBUG_PRINTLN(F("Selecting memory byte 6...")); setMemoryStartAddress(0x06); DEBUG_PRINTLN(F("Checking hardware revision...")); uint8_t hwRevision = readMemoryByte(); DEBUG_PRINT(F("Revision @ user[16][6] = ")); DEBUG_PRINTLNF(hwRevision, HEX); DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); setMemoryBank(0, false, false); // check OTP bank valid DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); uint8_t otpValid = getOTPBankValid(); DEBUG_PRINT(F("OTP bank is ")); DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); // get X/Y/Z gyro offsets DEBUG_PRINTLN(F("Reading gyro offset TC values...")); int8_t xgOffsetTC = getXGyroOffsetTC(); int8_t ygOffsetTC = getYGyroOffsetTC(); int8_t zgOffsetTC = getZGyroOffsetTC(); DEBUG_PRINT(F("X gyro offset = ")); DEBUG_PRINTLN(xgOffset); DEBUG_PRINT(F("Y gyro offset = ")); DEBUG_PRINTLN(ygOffset); DEBUG_PRINT(F("Z gyro offset = ")); DEBUG_PRINTLN(zgOffset); // setup weird slave stuff (?) DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); setSlaveAddress(0, 0x7F); DEBUG_PRINTLN(F("Disabling I2C Master mode...")); setI2CMasterModeEnabled(false); DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); setSlaveAddress(0, 0x68); DEBUG_PRINTLN(F("Resetting I2C Master control...")); resetI2CMaster(); delay(20); // load DMP code into memory banks DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); DEBUG_PRINTLN(F(" bytes)")); if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { DEBUG_PRINTLN(F("Success! DMP code written and verified.")); // write DMP configuration DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); DEBUG_PRINTLN(F(" bytes in config def)")); if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); setClockSource(MPU6050_CLOCK_PLL_ZGYRO); DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); setIntEnabled(0x12); DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); setRate(4); // 1khz / (1 + 4) = 200 Hz DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); setDLPFMode(MPU6050_DLPF_BW_42); DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); setFullScaleGyroRange(MPU6050_GYRO_FS_2000); DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); setDMPConfig1(0x03); setDMPConfig2(0x00); DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); setOTPBankValid(false); DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); setXGyroOffsetTC(xgOffsetTC); setYGyroOffsetTC(ygOffsetTC); setZGyroOffsetTC(zgOffsetTC); //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); //setXGyroOffset(0); //setYGyroOffset(0); //setZGyroOffset(0); DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); uint8_t dmpUpdate[16], j; uint16_t pos = 0; for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Resetting FIFO...")); resetFIFO(); DEBUG_PRINTLN(F("Reading FIFO count...")); uint16_t fifoCount = getFIFOCount(); uint8_t fifoBuffer[128]; DEBUG_PRINT(F("Current FIFO count=")); DEBUG_PRINTLN(fifoCount); getFIFOBytes(fifoBuffer, fifoCount); DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); setMotionDetectionThreshold(2); DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); setZeroMotionDetectionThreshold(156); DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); setMotionDetectionDuration(80); DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); setZeroMotionDetectionDuration(0); DEBUG_PRINTLN(F("Resetting FIFO...")); resetFIFO(); DEBUG_PRINTLN(F("Enabling FIFO...")); setFIFOEnabled(true); DEBUG_PRINTLN(F("Enabling DMP...")); setDMPEnabled(true); DEBUG_PRINTLN(F("Resetting DMP...")); resetDMP(); DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); while ((fifoCount = getFIFOCount()) < 3); DEBUG_PRINT(F("Current FIFO count=")); DEBUG_PRINTLN(fifoCount); DEBUG_PRINTLN(F("Reading FIFO data...")); getFIFOBytes(fifoBuffer, fifoCount); DEBUG_PRINTLN(F("Reading interrupt status...")); uint8_t mpuIntStatus = getIntStatus(); DEBUG_PRINT(F("Current interrupt status=")); DEBUG_PRINTLNF(mpuIntStatus, HEX); DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); while ((fifoCount = getFIFOCount()) < 3); DEBUG_PRINT(F("Current FIFO count=")); DEBUG_PRINTLN(fifoCount); DEBUG_PRINTLN(F("Reading FIFO data...")); getFIFOBytes(fifoBuffer, fifoCount); DEBUG_PRINTLN(F("Reading interrupt status...")); mpuIntStatus = getIntStatus(); DEBUG_PRINT(F("Current interrupt status=")); DEBUG_PRINTLNF(mpuIntStatus, HEX); DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("DMP is good to go! Finally.")); DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); setDMPEnabled(false); DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); dmpPacketSize = 42; /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { return 3; // TODO: proper error code for no memory }*/ DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); resetFIFO(); getIntStatus(); } else { DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); return 2; // configuration block loading failed } } else { DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); return 1; // main binary block loading failed } return 0; // success }