From 46a3dea3b9daa5be5be2e56a9d76d22c8386670d Mon Sep 17 00:00:00 2001
From: Sebastian Reichel <sre@ring0.de>
Date: Fri, 12 Oct 2012 19:45:47 +0200
Subject: run kinematics update with sensor data

---
 main.vala | 15 +++++++++++----
 1 file changed, 11 insertions(+), 4 deletions(-)

diff --git a/main.vala b/main.vala
index 790e0fc..8dce275 100644
--- a/main.vala
+++ b/main.vala
@@ -26,11 +26,18 @@ Kinematics kinematics;
 
 bool loop() {
 	try {
-		/* TODO: get sensor values */
-		/* TODO: get remote control values */
-		/* TODO: update kinematics */
+		/* get sensor data */
+		int16 gx, gy, gz;
+		uint16 ax, ay, az, mx, my, mz;
+		gyroscope.get_data(out gx, out gy, out gz);
+		accelerometer.get_data(out ax, out ay, out az);
+		compass.get_data(out mx, out my, out mz);
 
-		//flightCtrl.process();
+		/* calculate kinematics (TODO: calculate time diff) */
+		kinematics.update(gx, gy, gz, ax, ay, az, mx, my, mz, 0.1);
+
+		/* update motors */
+		flightCtrl.process();
 	} catch(Error e) {
 		stderr.printf("%s\n", e.message);
 	}
-- 
cgit v1.2.3