void mpu6050_init(char address)
{
#if MPU6050_I2CINIT == 1
//init i2c
i2c_init();
delay_us(10);
#endif
//set clock source
// it is highly recommended that the device be configured to use one of the gyroscopes (or an external clock source)
// as the clock reference for improved stability
i2cWriteBits(address, MPU6050_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO);
//set DLPF bandwidth to 42Hz
i2cWriteBits(address, MPU6050_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42);
//Sets sample rate to 8000/1+7 = 1000Hz
unsigned char divider = SMPLRT_DIV;
i2cWrite(address, MPU6050_SMPLRT_DIV, ÷r, 1); //50Hz
//set gyro range
i2cWriteBits(address, MPU6050_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS);
//set accel range
i2cWriteBits(address, MPU6050_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS);
//set sleep disabled
mpu6050_setSleepDisabled(address);
}
void Accelerometer::setDMPEnabled(bool enabled)
{
i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, 1, enabled);
}
void Accelerometer::resetFIFO()
{
i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1, true);
}
Accelerometer::Accelerometer()
{
m_i2cfd = open("/dev/i2c-1", O_RDWR);
if(!m_i2cfd < 0)
abort();
if(ioctl(m_i2cfd, I2C_SLAVE, 0x68) < 0)
abort();
i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO); // Set Clock source
i2cWriteBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250); // Full scale gyro range
i2cWriteBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2); // Full scale accel range
i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1, false); // Disable sleep mode
i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1, true); // Reset
usleep(30000);
i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1, false); // Disable sleep mode
setMemoryBank(0x10, true, true); // Get MPU hardware revision
uint8_t value = 0x06; // Alright, this is dirty but a pointer to this value will be sent to every i2cWriteBytes...
uint16_t value16; // Yeah... I know, I won't go to heaven but that's ok
i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &value); // Set memory start address
i2cReadBytes(MPU6050_RA_MEM_R_W, 1, buffer); // Get hardware revision
uint8_t hwRevision __attribute__((__unused__)) = buffer[0];
setMemoryBank(0, false, false);
i2cReadBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 1, buffer);
uint8_t otpValid __attribute__((__unused__)) = buffer[0]; // Check OTP bank validity
i2cReadBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
int8_t xgOffset = buffer[0];
i2cReadBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
int8_t ygOffset = buffer[0];
i2cReadBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
int8_t zgOffset = buffer[0];
value = 0x7F; // Setup weird slave stuff
i2cWriteBytes(MPU6050_RA_I2C_SLV0_ADDR, 1, &value);
i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 1, false); // Disable I2C Master Mode
value = 0x68;
i2cWriteBytes(MPU6050_RA_I2C_SLV0_ADDR, 1, &value);
i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1, true); // Reset I2C Master
usleep(20000);
if (writeMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, true, true)) { // Load DMP code into memory banks
if (writeDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE, true)) { // Let's write the DMP configuration
i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO); // Set clock source
value = 0x12;
i2cWriteBytes(MPU6050_RA_INT_ENABLE, 1, &value); // Enable 0x12 interuption
value = 4; // 1khz / (1 + 4) = 200 Hz
i2cWriteBytes(MPU6050_RA_SMPLRT_DIV, 1, &value);
i2cWriteBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L); // External Frame Sync
i2cWriteBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); // DLPF Mode
i2cWriteBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000); // Full scale gyro range
value = 0x03;
i2cWriteBytes(MPU6050_RA_DMP_CFG_1, 1, &value);
value = 0x00;
i2cWriteBytes(MPU6050_RA_DMP_CFG_2, 1, &value);
// Setting X Y and Z gyro offsets to previous values
i2cWriteBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, xgOffset);
i2cWriteBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, ygOffset);
i2cWriteBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, zgOffset);
value16 = 0; // And gyro offsets users to 0
i2cWriteWords(MPU6050_RA_XG_OFFS_USRH, 1, &value16);
i2cWriteWords(MPU6050_RA_YG_OFFS_USRH, 1, &value16);
i2cWriteWords(MPU6050_RA_ZG_OFFS_USRH, 1, &value16);
// Memory update 1/7
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]);
//Memory update 2/7
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]);
resetFIFO();
uint8_t fifoCount = getFIFOCount();
uint8_t fifoBuffer[128];
value = 2;
i2cWriteBytes(MPU6050_RA_MOT_THR, 1, &value); // Motion threshold
value = 156;
i2cWriteBytes(MPU6050_RA_ZRMOT_THR, 1, &value); // Zero Motion threshold
value = 80;
i2cWriteBytes(MPU6050_RA_MOT_DUR, 1, &value); // Motion duration
value = 0;
i2cWriteBytes(MPU6050_RA_ZRMOT_DUR, 1, &value); // Zero Motion duration
resetFIFO();
setFIFOEnabled(true);
setDMPEnabled(true);
i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1, true); // Reset DMP
// Memory update 3/7
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]);
// Memory update 4/7
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]);
// Memory update 5/7
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);
getFIFOBytes(fifoBuffer, fifoCount);
uint8_t mpuIntStatus __attribute__((__unused__)) = getIntStatus();
// Memory update 6/7
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 ((fifoCount = getFIFOCount()) < 3);
getFIFOBytes(fifoBuffer, fifoCount);
mpuIntStatus = getIntStatus();
// Memory update 7/7
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]);
setDMPEnabled(false);
dmpPacketSize = 42;
resetFIFO();
getIntStatus();
} else
abort();
} else
abort();
i2cReadBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
if(buffer[0] != 0x34)
abort();
setDMPEnabled(true);
zeroValues[0] = 0;
zeroValues[1] = 0;
zeroValues[2] = 0;
}
Accelerometer::~Accelerometer()
{
close(m_i2cfd);
i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1, true); // Enable sleep mode
}
/*
* set gyro offset Z
*/
void mpu6050_setZGyroOffset(char address, char offset) {
i2cWriteBits(address, MPU6050_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
}
