Added driver to grab accel and gyro data from the MPU-6000 chip.

2 files changed, 102 insertions, 2 deletions

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 <spi/spi.h>
+#include <gpio/gpio.h>
+#include <os/time.h>
+
+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 <usb/descriptor.h>
 
 #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));
 		}
 	}