Пример #1
0
/*
 * 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;
}
Пример #2
0
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
}
Пример #3
0
/*
 * 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;
}
Пример #5
0
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
}