Added ITG3200 wrapper.

2 files changed, 50 insertions, 7 deletions

diff --git a/itg3200.h b/itg3200.h
new file mode 100644
index 0000000..2e90f62
--- /dev/null
+++ b/itg3200.h
@@ -0,0 +1,32 @@
+#ifndef ITG3200_H
+#define ITG3200_H
+
+#include "i2c.h"
+
+class ITG3200 {
+	private:
+		I2C& i2c;
+	
+	public:
+		int16_t x, y, z;
+		
+		ITG3200(I2C& i2c_bus) : i2c(i2c_bus) {
+			
+		}
+		
+		void init() {
+			i2c.write_reg(0x68, 0x3e, 0x03); // Select clock reference.
+			i2c.write_reg(0x68, 0x16, 0x18 | 0x02); // 2000 deg/sec range, 98 Hz bandwidth.
+		}
+		
+		void update() {
+			uint8_t buf[6];
+			i2c.read_reg(0x68, 0x1d, 6, buf);
+			
+			x = (buf[0] << 8 | buf[1]) - 0;
+			y = (buf[2] << 8 | buf[3]) - 0;
+			z = (buf[4] << 8 | buf[5]) - 0;
+		}
+};
+
+#endif
diff --git a/main.cpp b/main.cpp
index eb36134..6bd8f6e 100644
--- a/main.cpp
+++ b/main.cpp
@@ -4,6 +4,7 @@
 #include "ppmsum.h"
 #include "i2c.h"
 
+#include "itg3200.h"
 
 int main() {
 	RCC.enable(RCC.AFIO);
@@ -17,13 +18,10 @@ int main() {
 	GPIOB.CRH = 0x4444ff44;
 	
 	I2C i2c;
-	
 	i2c.enable();
 	
-	i2c.write_reg(0x68, 0x3e, 0x03);
-	i2c.write_reg(0x68, 0x16, 0x18 | 0x02);
-	
-	uint8_t buf[6];
+	ITG3200 gyro(i2c);
+	gyro.init();
 	
 	PPMSum ppmsum;
 	ppmsum.enable();
@@ -42,7 +40,20 @@ int main() {
 		while(!(TIM2.SR & 0x01));
 		TIM2.SR = 0;
 		
-			i2c.read_reg(0x68, 0x1d, 6, buf);
-			//xbee_send(6, buf);
+		// Read sensors.
+		gyro.update();
+		
+		// Update filter.
+		
+		// Generate motor mix.
+		int16_t throttle = ppmsum.channels[2] - 1000;
+		int16_t pitch = 0;
+		int16_t yaw = gyro.z >> 7;
+		int16_t roll = 0;
+		
+		TIM2.CCR1 = 1000 + throttle + pitch + roll - yaw;
+		TIM2.CCR2 = 1000 + throttle + pitch - roll + yaw;
+		TIM2.CCR3 = 1000 + throttle - pitch + roll + yaw;
+		TIM2.CCR4 = 1000 + throttle - pitch - roll - yaw;
 	}
 }