run kinematics update with sensor data

1 files 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);
 	}