From 8dfd4d6e87a150bd3bc2f023cb4be8d84f7ef6de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Atle=20H=2E=20Havs=C3=B8?= Date: Thu, 31 Jul 2014 22:36:52 +0200 Subject: Added driver to grab accel and gyro data from the MPU-6000 chip. --- drivers/mpu6000.h | 90 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ main.cpp | 14 +++++++-- 2 files changed, 102 insertions(+), 2 deletions(-) create mode 100644 drivers/mpu6000.h diff --git a/drivers/mpu6000.h b/drivers/mpu6000.h new file mode 100644 index 0000000..1297810 --- /dev/null +++ b/drivers/mpu6000.h @@ -0,0 +1,90 @@ +#ifndef MPU6000_H +#define MPU6000_H + +#include +#include +#include + +class MPU6000 { + private: + Pin cs; + SPI_t& spi; + uint8_t raw_data[14]; + + void read_register(uint8_t reg, uint8_t len, uint8_t *data) { + cs.off(); + spi.transfer_byte(reg | (1 << 7)); // Read bit + while(len--) { + *data++ = spi.transfer_byte(); + } + cs.on(); + } + + void write_register(uint8_t reg, uint8_t data) { + cs.off(); + spi.transfer_byte(reg); + spi.transfer_byte(data); + cs.on(); + } + + public: + MPU6000(Pin cs_pin, SPI_t& spi_bus) : cs(cs_pin), spi(spi_bus) {} + float gyro [3] = {0,0,0}; //X Y Z + float accel [3] = {0,0,0}; //X Y Z + + void init() { + spi.reg.CR1 = (1 << 9) | (1 << 8) | (1 << 6) | (2 << 3) | (1 << 2) | (1 << 1) | (1 << 0); // SSM On | SSI 1 | SPE On | BR /8 | CPOL CK 1 Idle | CPHA Second clock data capture + spi.reg.CR2 = (1 << 12) | ( 7 << 8); // FRXTH RXNE if FIFO >= 8bit | DS 8-bit + write_register(0x6B, (1 << 7)); // Device Reset + Time::sleep(150); + uint8_t who_am_i = 0x00; + while (who_am_i != 0x68) { + read_register(0x75, 1, &who_am_i); //WHO_AM_I register + } + write_register(0x19, 4); // SAMPLE DIV 4 + Time::sleep(1); + write_register(0x1A, (3 << 0)); // CONFIG - DLPF_CFG 42hz + Time::sleep(1); + write_register(0x1B, 0); // GYRO_CFG + Time::sleep(1); + write_register(0x1C, 0); // ACCEL_CFG + Time::sleep(1); + write_register(0x6A, (1 << 4)); // USER_CTRL - I2C Off + Time::sleep(1); + write_register(0x6B, (3 << 0)); // PWR_MGMT_1 - CLKSEL PLL_Z + Time::sleep(1); + write_register(0x6C, 0); // PWR_MGMT_2 + Time::sleep(150); + } + + + void update() { + uint16_t temp; + spi.reg.CR1 = (1 << 9) | (1 << 8) | (1 << 6) | (2 << 3) | (1 << 2) | (1 << 1) | (1 << 0); // SSM On | SSI 1 | SPE On | BR /8 | CPOL CK 1 Idle | CPHA Second clock data capture + spi.reg.CR2 = (1 << 12) | ( 7 << 8); // FRXTH RXNE if FIFO >= 8bit | DS 8-bit + + read_register(0x3B, 14, raw_data); // Accel 0-5, Temp 6-7, Gyro 8-13 + + temp = raw_data[0] << 8; + temp |= raw_data[1]; + accel[0] = float(temp); + temp = raw_data[2] << 8; + temp |= raw_data[3]; + accel[1] = float(temp); + temp = raw_data[4] << 8; + temp |= raw_data[5]; + accel[2] = float(temp); + + temp = raw_data[8] << 8; + temp |= raw_data[9]; + gyro[0] = float(temp); + temp = raw_data[10] << 8; + temp |= raw_data[11]; + gyro[1] = float(temp); + temp = raw_data[12] << 8; + temp |= raw_data[13]; + gyro[2] = float(temp); + } +}; + +#endif diff --git a/main.cpp b/main.cpp index 6a698bc..08072f7 100644 --- a/main.cpp +++ b/main.cpp @@ -8,6 +8,7 @@ #include #include "hmc5983.h" +#include "mpu6000.h" Pin usb_vbus = GPIOB[5]; Pin usb_dm = GPIOA[11]; @@ -58,6 +59,7 @@ class USB_TM : public USB_class_driver { USB_TM usb_tm(usb); HMC5983 magn(mag_cs, SPI2); +MPU6000 mpu(mpu_cs, SPI2); int main() { rcc_init(); @@ -96,19 +98,27 @@ int main() { flash_cs.on(); magn.init(); - float buf[3] = {0,0,0}; + mpu.init(); + float buf[9] = {0,0,0,0,0,0,0,0,0}; while(1) { usb.process(); uint8_t magn_status = magn.update(); + mpu.update(); buf[0] = magn.x; buf[1] = magn.y; buf[2] = magn.z; - + buf[3] = mpu.gyro[0]; + buf[4] = mpu.gyro[1]; + buf[5] = mpu.gyro[2]; + buf[6] = mpu.accel[0]; + buf[7] = mpu.accel[1]; + buf[8] = mpu.accel[2]; if(usb.ep_ready(1)) { usb.write(1, (uint32_t*)buf, sizeof(buf)); + //usb.write(1, (uint32_t*)mpu.gyro, sizeof(mpu.gyro)); //usb.write(1, (uint32_t*)&magn_status, sizeof(magn_status)); } } -- cgit v1.2.3