Enabled gyro.

1 files changed, 30 insertions, 1 deletions

diff --git a/main.cpp b/main.cpp
index 2dda662..e14eb3d 100644
--- a/main.cpp
+++ b/main.cpp
@@ -10,6 +10,7 @@
 #include "i2c.h"
 
 #include "lsm303dlm.h"
+#include "l3gd20.h"
 
 static Pin& led_status = PA4;
 static Pin& led_error = PC4;
@@ -26,6 +27,9 @@ static Pin& jtag_tdi = PC15;
 static Pin& i2c_scl = PB10;
 static Pin& i2c_sda = PB11;
 
+static Pin& cs_pressure = PB4;
+static Pin& cs_gyro = PB5;
+
 bool jtag_tick(bool tdi, bool tms) {
 	bool tdo = jtag_tdo.get();
 	jtag_tdi.set(tdi);
@@ -281,6 +285,7 @@ uint32_t usb_stack[1024];
 
 Thread usb_thread(usb_stack, sizeof(usb_stack), usb_main);
 
+L3GD20 gyro(cs_gyro, SPI1);
 LSM303DLM_A accel(I2C2);
 LSM303DLM_M magn(I2C2);
 
@@ -309,16 +314,40 @@ int main() {
 	
 	usb_thread.start();
 	
+	RCC.enable(RCC.SPI1);
+	
+	PA5.set_mode(Pin::AF);
+	PA5.set_af(5);
+	PA6.set_mode(Pin::AF);
+	PA6.set_af(5);
+	PA7.set_mode(Pin::AF);
+	PA7.set_af(5);
+	
+	cs_gyro.on();
+	cs_gyro.set_mode(Pin::Output);
+	cs_pressure.on();
+	cs_pressure.set_mode(Pin::Output);
+	
+	SPI1.reg.CR1 = 0x37f;
+	
+	Time::sleep(1000);
+	
+	gyro.init();
 	accel.init();
 	magn.init();
 	
 	while(1) {
 		led_error.toggle();
-		Time::sleep(100);
+		Time::sleep(10);
+		
+		gyro.update();
 		accel.update();
 		magn.update();
 		
 		int16_t buf[] = {
+			gyro.x,
+			gyro.y,
+			gyro.z,
 			accel.x,
 			accel.y,
 			accel.z,