/*
* get quaternion and wait
*/
uint8_t mpu6050_getQuaternionWait(double *qw, double *qx, double *qy, double *qz) {
while (!mpu6050_mpuInterrupt && mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize);
//reset interrupt
mpu6050_mpuInterrupt = 0;
//check for overflow
mpu6050_mpuIntStatus = mpu6050_getIntStatus();
mpu6050_fifoCount = mpu6050_getFIFOCount();
if ((mpu6050_mpuIntStatus & 0x10) || mpu6050_fifoCount == 1024) {
//reset
mpu6050_resetFIFO();
} else if (mpu6050_mpuIntStatus & 0x02) {
//wait for correct available data length, should be a VERY short wait
while (mpu6050_fifoCount < MPU6050_DMP_dmpPacketSize)
mpu6050_fifoCount = mpu6050_getFIFOCount();
//read a packet from FIFO
mpu6050_getFIFOBytes(mpu6050_fifoBuffer, MPU6050_DMP_dmpPacketSize);
mpu6050_fifoCount -= MPU6050_DMP_dmpPacketSize;
//get quaternion
mpu6050_getQuaternion(mpu6050_fifoBuffer, qw, qx, qy, qz);
return 1;
}
return 0;
}
uint8_t mpu6050_dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
uint8_t status, i;
uint8_t buf[dmpPacketSize];
for (i=0; i < numPackets; i++) {
// read packet from FIFO
mpu6050_getFIFOBytes(buf, dmpPacketSize);
// process packet
if ((status = mpu6050_dmpProcessFIFOPacket(buf)) > 0) return status;
// increment external process count variable, if supplied
if (processed != 0) *processed++;
}
return 0;
}
/*
* initialize mpu6050 dmp
*/
uint8_t mpu6050_dmpInitialize() {
//setup interrupt
MPU6050_DMP_INT0SETUP;
//reset
mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1);
_delay_ms(30);//wait after reset
//disable sleep mode
mpu6050_setSleepDisabled();
//set memorybank to 0
mpu6050_setMemoryBank(0, 0, 0);
//get X/Y/Z gyro offsets
int8_t xgOffset = mpu6050_getXGyroOffset();
int8_t ygOffset = mpu6050_getYGyroOffset();
int8_t zgOffset = mpu6050_getZGyroOffset();
//setting slave 0 address to 0x7F
mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x7F);
//disabling I2C Master mode
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 0);
//setting slave 0 address to 0x68 (self)
mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x68);
//resetting I2C Master control
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1);
_delay_ms(20);
//load DMP code into memory banks
if (mpu6050_writeMemoryBlock(mpu6050_dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 1, 1) == 1) {
if (mpu6050_writeDMPConfigurationSet(mpu6050_dmpConfig, MPU6050_DMP_CONFIG_SIZE, 1)) {
//set clock source
mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO);
//set DMP and FIFO_OFLOW interrupts enabled
mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x12);
//set sample rate
mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); // 1khz / (1 + 4) = 200 Hz
//set external frame sync to TEMP_OUT_L[0]
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L);
//set DLPF bandwidth to 42Hz
mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42);
//set gyro sensitivity to +/- 2000 deg/sec
mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000);
//set DMP configuration bytes (function unknown)
mpu6050_writeByte(MPU6050_RA_DMP_CFG_1, 0x03);
mpu6050_writeByte(MPU6050_RA_DMP_CFG_2, 0x00);
//clear OTP Bank flag
mpu6050_writeBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 0);
//set X/Y/Z gyro offsets to previous values
//xgOffset = 0;
//ygOffset = 0;
zgOffset = 90;
mpu6050_setXGyroOffset(xgOffset);
mpu6050_setYGyroOffset(ygOffset);
mpu6050_setZGyroOffset(zgOffset);
//set X/Y/Z gyro user offsets to zero
mpu6050_writeWords(MPU6050_RA_XG_OFFS_USRH, 1, 0);
mpu6050_writeWords(MPU6050_RA_YG_OFFS_USRH, 1, 0);
mpu6050_writeWords(MPU6050_RA_ZG_OFFS_USRH, 1, 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(&mpu6050_dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);
//writing final memory update 2/7 (function unknown)
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);
//reset FIFO
mpu6050_writeBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1, 1);
//reading FIFO count
uint8_t fifoCount = mpu6050_getFIFOCount();
uint8_t fifoBuffer[128];
//current FIFO count
mpu6050_readBytes(MPU6050_RA_FIFO_R_W, fifoCount, fifoBuffer);
//setting motion detection threshold to 2
mpu6050_writeByte(MPU6050_RA_MOT_THR, 2);
//setting zero-motion detection threshold to 156
mpu6050_writeByte(MPU6050_RA_ZRMOT_THR, 156);
//setting motion detection duration to 80
mpu6050_writeByte(MPU6050_RA_MOT_DUR, 80);
//setting zero-motion detection duration to 0
mpu6050_writeByte(MPU6050_RA_ZRMOT_DUR, 0);
//reset FIFO
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1);
//enabling FIFO
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, 1);
//enabling DMP
mpu6050_dmpEnable();
//resetting DMP
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1);
//waiting for FIFO count > 2
while ((fifoCount = mpu6050_getFIFOCount()) < 3);
//writing final memory update 3/7 (function unknown)
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);
//writing final memory update 4/7 (function unknown)
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);
//writing final memory update 5/7 (function unknown)
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);
//reading FIFO data..."));
mpu6050_getFIFOBytes(fifoBuffer, fifoCount);
//reading final memory update 6/7 (function unknown)
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
mpu6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
//waiting for FIFO count > 2
while ((fifoCount = mpu6050_getFIFOCount()) < 3);
//reading FIFO data
mpu6050_getFIFOBytes(fifoBuffer, fifoCount);
//writing final memory update 7/7 (function unknown)
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);
//disabling DMP (you turn it on later)
mpu6050_dmpDisable();
//resetting FIFO and clearing INT status one last time
mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1);
} else {
return 2; // configuration block loading failed
}
} else {
return 1; // main binary block loading failed
}
return 0; // success
}
int main(void) {
/* Configure Watch dog timer as an interval timer. WDTIFG is
* set upon expiration of selected time interval, and PUC is not
* generated, so there is no reset of the device. Also, WDTIE bit
* remains unchanged, so you don't have to reset the WDT interrupt.
*
* WDTCTL is 16 bits and always needs to be accessed with
* the upper 8 bits as the WDT password, WDTPW (0x5A).
* Use ACLK for WDTCNT - selected with the WDTSSEL bit
* Set WDTTMSEL bit to 1 for interval timer mode.
* WDTIS0 and WDTIS1 set the interval.
* 00 = WDT clock source/32768 **This is the PUC value
* 01 = WDT clock source/8192
* 10 = WDT clock source/512
* 11 = WDT clock source/64
* With ACLK = 1.5kHz and dividing it by 32768, we get ~21.8 seconds
* With ACLK = 1.5kHz and dividing it by 64, we get 42.6mS
* between WDT interrupts. */
WDTCTL = WDTPW + WDTHOLD;
initClocks();
/* Configure Bluetooth module */
hc05_init(__baud_to_uca0br(9600));
hc05_transmit("HC05 init\r\n",11);
/* Initialize mpu6050 */
mpu6050_init();
hc05_transmit("mpu6050 init\r\n",14);
/* Now we are ready for application code to run. Enable interrupts */
_BIS_SR(GIE);
while(1) {
if(data_received != 0) {
switch(data_received) {
case 'T':
data_received = 0;
mpu6050_temp();
break;
case 'A':
data_received = 0;
mpu6050_accel();
break;
case 'G':
data_received = 0;
mpu6050_gyro();
break;
case 'g':
data_received = 0;
mpu6050_calibrate_gyros();
break;
case 'M':
data_received = 0;
mpu6050_getAddress();
break;
case 'W':
data_received = 0;
mpu6050_wakeup();
break;
case 'S':
data_received = 0;
mpu6050_sleep();
break;
case 'R':
data_received = 0;
dmp_mode = 0;
motion_detect_mode = 0;
mpu6050_reset();
break;
case 'd':
data_received = 0;
mpu6050_dmpinit();
break;
case 'E':
data_received = 0;
motion_detect_mode = 0;
dmp_mode = 1;
mpu6050_setDMPEnabled(true);
P2DIR &= ~MPU6050_INT; // Input
P2SEL &= ~MPU6050_INT; // Digital IO Psel and psel2 are 0
P2SEL2 &= ~MPU6050_INT;
P2IES &= ~MPU6050_INT; // Edge select 0 = low to high
P2IFG &= ~MPU6050_INT; // Clear the interrupt flag before enabling interrupt
P2IE |= MPU6050_INT; // Interrupt enable
//mpu6050_resetFIFO();
break;
case 'e':
dmp_mode = 0;
data_received = 0;
mpu6050_setDMPEnabled(false);
break;
case 'm': /* Itseems that I can have motion detect interrupts if I first call the dmpinit() function, then this code is run. I can probably narrow it down
to a certain function call in the dmpinit() it will just take time */
sendAck();
motion_detect_mode = 1;
dmp_mode = 0;
mpu6050_setDMPEnabled(false);
i2c_write_reg(MPU6050_RA_INT_PIN_CFG,0x10);//interrupt status cleared on any read
//i2c_write_reg(MPU6050_RA_MOT_DETECT_CTRL,0x30); // add the 3 ms delay to accel put
mpu6050_setMotionDetectionThreshold(threshold);//not sure... but I'm told it's 2mg per LSB so 0xFF would only be about 0.512g
mpu6050_setMotionDetectionDuration(threshold_duration); // This duration will really change the snappiness and responsiveness of the motion detect (duh) so
// it should be set to as low as possible, then set detection threshold to the appropriate value for punches or whatever
mpu6050_setIntEnabled(0x40);//motion detect... based on the product specification document, I don't think motion detect can generate an interrupt on INT pin,
// so we also set the data ready interrupt.
mpu6050_configAccel(MPU6050_ACCEL_FS_16<<(MPU6050_ACONFIG_AFS_SEL_BIT-1));
data_received = 0;
P2DIR &= ~MPU6050_INT; // Input
P2SEL &= ~MPU6050_INT; // Digital IO Psel and psel2 are 0
P2SEL2 &= ~MPU6050_INT;
P2IES &= ~MPU6050_INT; // Edge select 0 = low to high
P2IFG &= ~MPU6050_INT; // Clear the interrupt flag before enabling interrupt
P2IE |= MPU6050_INT; // Interrupt enable
/*while(1) {
if(mpu6050_getIntStatus() & 0x40) {
//motion interrupt
hc05_transmit("Motion\r\n",8);
}
}*/
break;
case 'l':
threshold = threshold - 1;
mpu6050_setMotionDetectionThreshold(threshold);
break;
case 'p':
threshold = threshold + 1;
mpu6050_setMotionDetectionThreshold(threshold);
break;
case 'L':
threshold_duration = threshold_duration - 5;
mpu6050_setMotionDetectionDuration(threshold_duration);
break;
case 'P':
threshold_duration = threshold_duration + 5;
mpu6050_setMotionDetectionDuration(threshold_duration);
break;
// case 'h':
// hc05_setspeed(115200);
// data_received = 0;
// break;
//case 'k':
// hc05_key();
// data_received = 0;
// break;
default:
sendAck();
data_received = 0;
break;
}
}
if(mpu6050_interrupt) {
if(dmp_mode) {
mpuIntStatus = mpu6050_getIntStatus();
fifoCount = mpu6050_getFIFOCount();
if(fifoCount > 16) {
fifoCount =16;
}
mpu6050_getFIFOBytes(mpu6050_buffer,fifoCount);
/* This seems to keep the fifo operating. I probably need to read the fifo faster so it doesn't 'die' on me */
mpu6050_resetFIFO();
/* From J.Rowberg's library, the dmp packet output is:
bytes 0-15 quaternion (32 bits) (w,x,y,z) but just use the first two bytes as 16 bit number
bytes 16-27 gyro (32 bits) (gx,gy,gz) but just use the first two bytes as 16 bit number
bytes 28-39 acceleration (32 bits) (ax,ay,az) but just use the first two bytes as 16 bit number
*/
teapotPacket[2] = mpu6050_buffer[0];
teapotPacket[3] = mpu6050_buffer[1];
teapotPacket[4] = mpu6050_buffer[4];
teapotPacket[5] = mpu6050_buffer[5];
teapotPacket[6] = mpu6050_buffer[8];
teapotPacket[7] = mpu6050_buffer[9];
teapotPacket[8] = mpu6050_buffer[12];
teapotPacket[9] = mpu6050_buffer[13];
teapotPacket[10] = mpuIntStatus; // I modified the packet to sent the interrupt status in this byte
hc05_transmit(teapotPacket,14);
teapotPacket[11]++;
mpu6050_interrupt = 0;
} else if (motion_detect_mode) {
if(mpu6050_getIntStatus() & 0x40) {
// Disable motion interrupts, get accel and gyro values until they aren't interesting
// anymore, then quit and enable interrupt.
mpu6050_setIntEnabled(0x00);
//motion interrupt
// hc05_transmit("Motion\r\n",8);
for(uint8_t j = 0; j<100; j++) {
mpu6050_accel();
delay_ms(2);
// i2c_tx_buffer[0] = 0x3B;
// i2c_tx_buffer_counter = 1;
// i2c_transmit_to_receive();
// i2c_transmit();
// i2c_multireceive(6);
// for(j = 0; j< 6; j++) {
// TXData = i2c_rx_buffer[j];
/* These are div 16384 if +/-2g, 8192 if +/-4g, 4096 if +/-8g and 2048 if +/-16g*/
//accelX[j] = (i2c_rx_buffer[0]<<8 | i2c_rx_buffer[1]);
//accelY[j] = (i2c_rx_buffer[2]<<8 | i2c_rx_buffer[3]);
//accelZ[j] = (i2c_rx_buffer[4]<<8 | i2c_rx_buffer[5]);
//sprintf(tempbuf,"%d %d %d\r\n",ax,ay,az);
//sprintf(tempbuf,"E%d,%d,%d\r\n",ax,ay,az);
// hc05_transmit(tempbuf,strlen(tempbuf));
// hc05_transmit((char*)ax,1);
//hc05_transmit((char*)ay,1);
//hc05_transmit((char*)az,1);
//}
//__delay_us(1000);
//delay_ms(2);
}
// hc05_transmit((char*)accelX,2*ACCELBUFSIZE);
// hc05_transmit((char*)accelY,2*ACCELBUFSIZE);
// hc05_transmit((char*)accelZ,2*ACCELBUFSIZE);
// mpu6050_getIntStatus();
mpu6050_setIntEnabled(0x40);
} else {
// sprintf(tempbuf,"Unknown interrupt\r\n");
// hc05_transmit(tempbuf,strlen(tempbuf));
}
} else {
// mpu6050_getIntStatus(); // Clear any other interrupts
// sprintf(tempbuf,"Unknown mode & interrupt\r\n");
// hc05_transmit(tempbuf,strlen(tempbuf));
}
}
__bis_SR_register(LPM0_bits + GIE);
}
}
uint8_t mpu6050_dmpInitialize() {
// reset device
DEBUG_PRINTLN("\r\nResetting MPU6050...\r\n");
mpu6050_reset();
Delay(30); // wait after reset
// disable sleep mode
DEBUG_PRINTLN("Disabling sleep mode...\r\n");
mpu6050_setSleepEnabled(false);
// get MPU hardware revision
DEBUG_PRINTLN("Selecting user bank 16...\r\n");
mpu6050_setMemoryBank(0x10, true, true);
DEBUG_PRINTLN("Selecting memory byte 6...\r\n");
mpu6050_setMemoryStartAddress(0x06);
DEBUG_PRINTLN("Checking hardware revision...\r\n");
uint8_t hwRevision = mpu6050_readMemoryByte();
DEBUG_PRINTLN("Revision @ user[16][6] = 0x%0x\r\n", hwRevision);
DEBUG_PRINTLN("Resetting memory bank selection to 0...\r\n");
mpu6050_setMemoryBank(0, false, false);
// check OTP bank valid
DEBUG_PRINTLN("Reading OTP bank valid flag...\r\n");
uint8_t otpValid = mpu6050_getOTPBankValid();
DEBUG_PRINTLN("OTP bank is %s\r\n", otpValid ? "valid!" : "invalid!");
// get X/Y/Z gyro offsets
DEBUG_PRINTLN("Reading gyro offset values...");
int8_t xgOffset = mpu6050_getXGyroOffsetTC();
int8_t ygOffset = mpu6050_getYGyroOffsetTC();
int8_t zgOffset = mpu6050_getZGyroOffsetTC();
DEBUG_PRINTLN("X gyro offset = %d\r\n", xgOffset);
DEBUG_PRINTLN("Y gyro offset = %d\r\n", ygOffset);
DEBUG_PRINTLN("Z gyro offset = %d\r\n", zgOffset);
// i2c_read_byte(&mpu6050, MPU6050_RA_USER_CTRL, buffer); // ?
DEBUG_PRINTLN("Enabling interrupt latch, clear on any read, AUX bypass enabled\r\n");
i2c_write_byte(&mpu6050, MPU6050_RA_INT_PIN_CFG, 0x32);
/* hmc5883l setting */
// i2c_write_byte(&hmc5883l, HMC5883L_RA_CONFIG_A, 0x70);
hmc5883l_initialize();
// load DMP code into memory banks
DEBUG_PRINTLN("Writing DMP code to MPU memory banks\r\n");
if (mpu6050_writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, false)) {
DEBUG_PRINTLN("Success! DMP code written and verified.\r\n");
DEBUG_PRINTLN("Configuring DMP and related settings...\r\n");
// write DMP configuration
DEBUG_PRINT("Writing DMP configuration to MPU memory banks\r\n");
if (mpu6050_writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
DEBUG_PRINTLN("Success! DMP configuration written and verified.\r\n");
DEBUG_PRINTLN("Setting DMP and FIFO_OFLOW interrupts enabled...\r\n");
mpu6050_setIntEnabled(0x12);
DEBUG_PRINTLN("Setting sample rate to 200Hz...\r\n");
mpu6050_setRate(4); // 1khz / (1 + 4) = 200 Hz
DEBUG_PRINTLN("Setting clock source to Z Gyro...\r\n");
mpu6050_setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
DEBUG_PRINTLN("Setting DLPF bandwidth to 42Hz...\r\n");
mpu6050_setDLPFMode(MPU6050_DLPF_BW_42);
DEBUG_PRINTLN("Setting external frame sync to TEMP_OUT_L[0]...\r\n");
mpu6050_setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
DEBUG_PRINTLN("Setting gyro sensitivity to +/- 2000 deg/sec...\r\n");
mpu6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
DEBUG_PRINTLN("Setting DMP configuration bytes (function unknown)...\r\n");
mpu6050_setDMPConfig1(0x03);
mpu6050_setDMPConfig2(0x00);
DEBUG_PRINTLN("Clearing OTP Bank flag...\r\n");
mpu6050_setOTPBankValid(false);
DEBUG_PRINTLN("Setting X/Y/Z gyro offsets to previous values...\r\n");
mpu6050_setXGyroOffsetTC(xgOffset);
mpu6050_setYGyroOffsetTC(ygOffset);
mpu6050_setZGyroOffsetTC(zgOffset);
DEBUG_PRINTLN("Setting X/Y/Z gyro user offsets to zero...\r\n");
/*
mpu6050_setXGyroOffset(0);
mpu6050_setYGyroOffset(0);
mpu6050_setZGyroOffset(0);
*/
// mpu6050_setXGyroOffset(220);
// mpu6050_setYGyroOffset(76);
// mpu6050_setZGyroOffset(-85);
// mpu6050_setZAccelOffset(700);
DEBUG_PRINTLN("Writing final memory update 1/19 (function unknown)...\r\n");
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]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Writing final memory update 2/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Resetting FIFO...\r\n");
mpu6050_resetFIFO();
DEBUG_PRINTLN("Reading FIFO count...\r\n");
uint8_t fifoCount = mpu6050_getFIFOCount();
DEBUG_PRINT("Current FIFO count=\r\n");
DEBUG_PRINTLN(fifoCount);
uint8_t fifoBuffer[128];
// mpu6050_getFIFOBytes(fifoBuffer, fifoCount);
DEBUG_PRINTLN("Writing final memory update 3/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Writing final memory update 4/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Disabling all standby flags...\r\n");
i2c_write_byte(&mpu6050, MPU6050_RA_PWR_MGMT_2, 0x00);
DEBUG_PRINTLN("Setting accelerometer sensitivity to +/- 2g...\r\n");
i2c_write_byte(&mpu6050, MPU6050_RA_ACCEL_CONFIG, 0x00);
DEBUG_PRINTLN("Setting motion detection threshold to 2...\r\n");
mpu6050_setMotionDetectionThreshold(2);
DEBUG_PRINTLN("Setting zero-motion detection threshold to 156...\r\n");
mpu6050_setZeroMotionDetectionThreshold(156);
DEBUG_PRINTLN("Setting motion detection duration to 80...\r\n");
mpu6050_setMotionDetectionDuration(80);
DEBUG_PRINTLN("Setting zero-motion detection duration to 0...\r\n");
mpu6050_setZeroMotionDetectionDuration(0);
DEBUG_PRINTLN("Setting AK8975 to single measurement mode...\r\n");
// to do
//mag -> setMode(1);
#if 1
/* set hmc5883l */
// i2c_write_byte(&mpu6050, MPU6050_RA_I2C_MST_CTRL, 0x40);
/* slave1 for read */
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_ADDR, HMC5883L_ADDRESS | I2C_READ);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_REG, HMC5883L_RA_DATAX_H);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_CTRL, 0x86);
/* slave1 for write */
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_ADDR, HMC5883L_ADDRESS);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_REG, HMC5883L_RA_MODE);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_DO, HMC5883L_MODE_SINGLE);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_CTRL, 0x81);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV4_CTRL, 0x18);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05);
#endif
#if 0
// setup AK8975 (0x0E) as Slave 0 in read mode
DEBUG_PRINTLN("Setting up AK8975 read slave 0...\r\n");
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_ADDR, 0x1E);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_REG, 0x01);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_CTRL, 0xDA);
// setup AK8975 (0x0E) as Slave 2 in write mode
DEBUG_PRINTLN("Setting up AK8975 write slave 2...\r\n");
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_ADDR, 0x0E);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_REG, 0x0A);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_CTRL, 0x81);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_DO, 0x01);
// setup I2C timing/delay control
DEBUG_PRINTLN("Setting up slave access delay...\r\n");
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV4_CTRL, 0x18);
i2c_write_byte(&mpu6050, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05);
#endif
// enable interrupts
DEBUG_PRINTLN("Enabling default interrupt behavior/no bypass...\r\n");
i2c_write_byte(&mpu6050, MPU6050_RA_INT_PIN_CFG, 0x00);
// enable I2C master mode and reset DMP/FIFO
DEBUG_PRINTLN("Enabling I2C master mode...\r\n");
i2c_write_byte(&mpu6050, MPU6050_RA_USER_CTRL, 0x20);
DEBUG_PRINTLN("Resetting FIFO...\r\n");
i2c_write_byte(&mpu6050, MPU6050_RA_USER_CTRL, 0x24);
DEBUG_PRINTLN("Rewriting I2C master mode enabled because...I don't know\r\n");
i2c_write_byte(&mpu6050, MPU6050_RA_USER_CTRL, 0x20);
DEBUG_PRINTLN("Enabling and resetting DMP/FIFO...\r\n");
i2c_write_byte(&mpu6050, MPU6050_RA_USER_CTRL, 0xE8);
DEBUG_PRINTLN("Writing final memory update 5/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Writing final memory update 6/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Writing final memory update 7/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Writing final memory update 8/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Writing final memory update 9/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Writing final memory update 10/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Writing final memory update 11/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Reading final memory update 12/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
#ifdef DEBUG
DEBUG_PRINT("Read bytes: ");
for (j = 0; j < 4; j++) {
DEBUG_PRINTF("0x%02x ", dmpUpdate[3 + j]);
}
DEBUG_PRINTLN("\r\n");
#endif
DEBUG_PRINTLN("Writing final memory update 13/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Writing final memory update 14/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Writing final memory update 15/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Writing final memory update 16/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Writing final memory update 17/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Waiting for FIFO count >= 46...\r\n");
while ((fifoCount = mpu6050_getFIFOCount()) < 46);
DEBUG_PRINTLN("Reading FIFO...\r\n");
mpu6050_getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
DEBUG_PRINTLN("Reading interrupt status...\r\n");
mpu6050_getIntStatus();
DEBUG_PRINTLN("Writing final memory update 18/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Waiting for FIFO count >= 48...\r\n");
while ((fifoCount = mpu6050_getFIFOCount()) < 48);
DEBUG_PRINTLN("Reading FIFO...");
mpu6050_getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
DEBUG_PRINTLN("Reading interrupt status...\r\n");
mpu6050_getIntStatus();
DEBUG_PRINTLN("Waiting for FIFO count >= 48...\r\n");
while ((fifoCount = mpu6050_getFIFOCount()) < 48);
DEBUG_PRINTLN("Reading FIFO...\r\n");
mpu6050_getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
DEBUG_PRINTLN("Reading interrupt status...\r\n");
mpu6050_getIntStatus();
DEBUG_PRINTLN("Writing final memory update 19/19 (function unknown)...\r\n");
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true);
DEBUG_PRINTLN("Disabling DMP (you turn it on later)...\r\n");
mpu6050_setDMPEnabled(false);
DEBUG_PRINTLN("Setting up internal 48-byte (default) DMP packet buffer...\r\n");
dmpPacketSize = 48;
/*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
return 3; // TODO: proper error code for no memory
}*/
DEBUG_PRINTLN("Resetting FIFO and clearing INT status one last time...\r\n");
mpu6050_resetFIFO();
mpu6050_getIntStatus();
} else {
DEBUG_PRINTLN("ERROR! DMP configuration verification failed.\r\n");
return 2; // configuration block loading failed
}
} else {
DEBUG_PRINTLN("ERROR! DMP code verification failed.\r\n");
return 1; // main binary block loading failed
}
return 0; // success
}
