/* * 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 }