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
}
