/*
* Resets the MPU and enables I2C communication with it
*/
int mpu_enable(void)
{
int ret=0;
i2c_enable();
if(!mpu_reset())
printf("Could not reset the MPU");
else
ret=1;
return ret;
}
void mpu_init() {
// configure clocks
RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_DMA2EN;
RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN | RCC_APB2ENR_SPI1EN;
__DMB();
// configure receive stream on DMA
rx_stream->PAR = (uint32_t)&spi->DR;
rx_stream->M0AR = (uint32_t)read_buf;
rx_stream->NDTR = sizeof(read_buf);
rx_stream->CR = (3 << DMA_SxCR_CHSEL_Pos) | DMA_SxCR_MINC | DMA_SxCR_TCIE;
util_enable_irq(DMA2_Stream0_IRQn, IRQ_PRI_KERNEL);
// configure transmit stream on DMA
tx_stream->PAR = (uint32_t)&spi->DR;
tx_stream->M0AR = (uint32_t)read_cmd;
tx_stream->NDTR = sizeof(read_cmd);
tx_stream->CR = (3 << DMA_SxCR_CHSEL_Pos) | DMA_SxCR_MINC | DMA_SxCR_DIR_MEM2PER;
// enable SPI
spi->CR2 = SPI_CR2_TXDMAEN | SPI_CR2_RXDMAEN;
spi->CR1 = SPI_CR1_SSM | SPI_CR1_SSI | SPI_CR1_BR_DIV128 | SPI_CR1_MSTR | SPI_CR1_CPOL | SPI_CR1_CPHA | SPI_CR1_SPE;
// configure GPIOs
GPIOA->BSRRL = (1 << PIN_NSS);
GPIOA->AFR[0] |= AFRL(PIN_SCK, AF_SPI1) | AFRL(PIN_MISO, AF_SPI1) | AFRL(PIN_MOSI, AF_SPI1);
GPIOA->MODER |= MODER_OUT(PIN_NSS) | MODER_AF(PIN_SCK) | MODER_AF(PIN_MISO) | MODER_AF(PIN_MOSI);
// enable EXTI
SYSCFG->EXTICR[1] |= SYSCFG_EXTICR2_EXTI4_PC;
EXTI->RTSR |= (1 << PIN_INT); // enable rising edge
EXTI->IMR |= (1 << PIN_INT); // enable interrupt from EXTI
// set up MPU
mpu_reset(AccelFS::FS4G, GyroFS::FS500DS, 1, 0);
util_enable_irq(EXTI0_IRQn + PIN_INT, IRQ_PRI_HIGH); // enable interrupt in NVIC
}
/*
* Put the MPU to sleep and turn off all sensors.
* After reset, the default state is sleep.
*/
int mpu_sleep(void)
{
mpu_reset();
}
static int __devinit mpu_probe(struct i2c_client *client,
const struct i2c_device_id *id)
{
struct mpu_data *mpu;
int ret;
dev_info(&client->dev, "probe start. - new\n");
mpu = kzalloc(sizeof(struct mpu_data), GFP_KERNEL);
if (!mpu) {
ret = -ENOMEM;
dev_err(&client->dev, "failed to alloc memory: %d\n", ret);
goto exit_alloc_failed;
}
mutex_init(&mpu->lock);
mpu->client = client;
i2c_set_clientdata(client, mpu);
mpu->poll_interval = DEFAULT_POLL_INTERVAL;
mpu->gpio = client->irq;
ret = mpu_input_init(mpu);
if (ret < 0) {
dev_err(&client->dev, "input init failed\n");
goto err_input_init_fail;
}
ret = mpu_reset(mpu);
if (ret < 0) {
dev_err(&client->dev, "reset failed\n");
goto err_reset;
}
ret = mpu_hw_init(mpu);
if (ret < 0) {
dev_err(&client->dev, "hw init failed\n");
goto err_hw_init;
}
ret = mpu_setup_irq(mpu);
if (ret < 0) {
dev_err(&client->dev,
"fail to setup irq for gpio %d\n", mpu->gpio);
goto err_setup_irq;
}
mpu->enabled = 0;
mpu_disable(mpu);
dev_info(&client->dev, "probed\n");
return 0;
err_setup_irq:
err_hw_init:
mpu_disable(mpu);
err_reset:
input_unregister_device(mpu->input_dev);
remove_sysfs_interfaces(&client->dev);
err_input_init_fail:
kfree(mpu);
exit_alloc_failed:
dev_err(&client->dev, "Driver Init failed\n");
return ret;
}
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
}