initial code import

21 files changed, 1194 insertions, 0 deletions

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..6584551
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+test-shell
diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..67dfc5f
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,8 @@
+test-shell: actuators/*.vala actuators/*/*.vala sensors/*.vala sensors/*/*.vala hw/*.vala \
+            ctrl/*.vala test-shell.vala enums.vala
+	valac -o $@ --pkg posix --pkg linux --pkg readline -X -lreadline -X -lm $^
+
+clean:
+	rm -f test-shell *.c */*.c */*/*.c
+
+.PHONY: clean
diff --git a/actuators/blinkm.vala b/actuators/blinkm.vala
new file mode 100644
index 0000000..be98260
--- /dev/null
+++ b/actuators/blinkm.vala
@@ -0,0 +1,154 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public class BlinkM : I2CDevice {
+	public struct Command {
+		uint8 id;
+		uint8 line;
+		uint8 duration;
+		uint8 cmd;
+		uint8 parameter1;
+		uint8 parameter2;
+		uint8 parameter3;
+	}
+
+	public enum Script {
+		STARTUP,
+		RGB,
+		WHITE_FLASH,
+		RED_FLASH,
+		GREEN_FLASH,
+		BLUE_FLASH,
+		CYAN_FLASH,
+		MAGENTA_FLASH,
+		YELLOW_FLASH,
+		BLACK,
+		HUE_CYCLE,
+		MOOD_LIGHT,
+		VIRTUAL_CANDLE,
+		WATER_REFLECTIONS,
+		OLD_NEON,
+		THE_SEASONS,
+		THUNDERSTORM,
+		STOP_LIGHT,
+		MORSE_CODE
+	}
+
+	public BlinkM(uint8 dev, uint8 addr = 0x09) throws I2CError {
+		base(dev, addr);
+	}
+
+	public void get_firmware_version(out char major, out char minor) throws I2CError {
+		write_byte('Z');
+		major = (char) read_byte();
+		minor = (char) read_byte();
+	}
+
+	public uint8 get_address() throws I2CError {
+		write_byte('a');
+		return read_byte();
+	}
+
+	public void set_address(uint8 addr) throws I2CError {
+		write_byte('A');
+		write_byte(addr);
+		write_byte(0xd0);
+		write_byte(0x0d);
+		write_byte(addr);
+		setup_address(addr);
+	}
+
+	public void script_stop() throws I2CError {
+		write_byte('o');
+	}
+
+	public void script_play(Script id, uint8 repeat = 0, uint8 start = 0) throws I2CError {
+		write_byte('p');
+		write_byte(id);
+		write_byte(repeat);
+		write_byte(start);
+	}
+
+	public void set(uint8 red, uint8 green, uint8 blue) throws I2CError {
+		write_byte('n');
+		write_byte(red);
+		write_byte(green);
+		write_byte(blue);
+	}
+
+	public void get(out uint8 red, out uint8 green, out uint8 blue) throws I2CError {
+		write_byte('g');
+		red = read_byte();
+		green = read_byte();
+		blue = read_byte();
+	}
+
+	public void fade(uint8 red, uint8 green, uint8 blue) throws I2CError {
+		write_byte('c');
+		write_byte(red);
+		write_byte(green);
+		write_byte(blue);
+	}
+
+	public void set_fade_speed(uint8 speed) throws I2CError {
+		write_byte('f');
+		write_byte(speed);
+	}
+
+	public void set_time_adjust(int8 adjust) throws I2CError {
+		write_byte('t');
+		write_byte((uint8) adjust);
+	}
+
+	public void script_set_line(Command cmd) throws I2CError {
+		write_byte('W');
+		write_byte(cmd.id);
+		write_byte(cmd.line);
+		write_byte(cmd.duration);
+		write_byte(cmd.cmd);
+		write_byte(cmd.parameter1);
+		write_byte(cmd.parameter2);
+		write_byte(cmd.parameter3);
+	}
+
+	public void script_get_line(out Command cmd) throws I2CError {
+		write_byte('R');
+		cmd = Command();
+		cmd.id = read_byte();
+		cmd.line = read_byte();
+		cmd.duration = read_byte();
+		cmd.cmd = read_byte();
+		cmd.parameter1 = read_byte();
+		cmd.parameter2 = read_byte();
+		cmd.parameter3 = read_byte();
+	}
+
+	public void script_setup(Script id, uint8 length, uint8 repeats) throws I2CError 
+	requires (id == 0) requires (length <= 50) {
+		write_byte('L');
+		write_byte(id);
+		write_byte(length);
+		write_byte(repeats);
+	}
+
+	public void setup_startup(uint8 mode, Script id, uint8 repeat, uint8 fade_speed, int8 time_adjust) throws I2CError {
+		write_byte('B');
+		write_byte(mode);
+		write_byte(id);
+		write_byte(repeat);
+		write_byte(fade_speed);
+		write_byte(time_adjust);
+	}
+}
diff --git a/actuators/motor-controller.vala b/actuators/motor-controller.vala
new file mode 100644
index 0000000..fb596dd
--- /dev/null
+++ b/actuators/motor-controller.vala
@@ -0,0 +1,19 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public interface MotorController : Device {
+	public abstract void set(uint8 m, uint8 v);
+	public abstract void set_all(uint8 v);
+}
diff --git a/actuators/motor/atmostripe.vala b/actuators/motor/atmostripe.vala
new file mode 100644
index 0000000..0cc4eef
--- /dev/null
+++ b/actuators/motor/atmostripe.vala
@@ -0,0 +1,24 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public class AtmostripeMotorController : SerialDevice, MotorController {
+	public void set(uint8 m, uint8 v) {
+		/* */
+	}
+
+	public void set_all(uint8 v) {
+		/* */
+	}
+}
diff --git a/ctrl/flight-control.vala b/ctrl/flight-control.vala
new file mode 100644
index 0000000..00e7b31
--- /dev/null
+++ b/ctrl/flight-control.vala
@@ -0,0 +1,142 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ * Copyright 2012, Ted Carancho
+ *
+ * Ported from AeroQuad
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+public class FlightControl {
+	MotorController motor;
+	FlightMode mode;
+	PID[] pid;
+	Kinematics k;
+
+	public FlightControl() {
+		motor = new AtmostripeMotorController();
+		pid = new PID[PIDEntry.length];
+		k = new Kinematics(0, 0); // TODO: read compass instead of using 0,0
+	}
+
+	/**
+	 * calculateFlightError
+	 *
+	 * Calculate roll/pitch axis error with gyro/accel data to
+	 * compute motor command thrust so used command are executed
+	 */
+	void calculateFlightError(out double roll, out double pitch) {
+		roll = 0; pitch = 0;
+
+		if (mode == FlightMode.ATTITUDE_FLIGHT_MODE) {
+		}
+#if 0
+		if (mode == FlightMode.ATTITUDE_FLIGHT_MODE) {
+			// 0.0 -> (receiverCommand[AXIS.X] - receiverZero[AXIS.X]) * ATTITUDE_SCALING
+			double rollAttitudeCmd   = pid[PIDEntry.ATTITUDE_XAXIS].update(0.1, k.kinematicsAngle[AXIS.X]);
+			// 0.0 -> (receiverCommand[AXIS.Y] - receiverZero[AXIS.Y]) * ATTITUDE_SCALING
+			double pitchAttitudeCmd  = pid[PIDEntry.ATTITUDE_YAXIS].update(0.1, k.kinematicsAngle[AXIS.Y]);
+
+			roll   = pid[PIDEntry.ATTITUDE_GYRO_XAXIS].update(rollAttitudeCmd, gyroRate[AXIS.X]*1.2);
+			pitch  = pid[PIDEntry.ATTITUDE_GYRO_YAXIS].update(pitchAttitudeCmd, -gyroRate[AXIS.Y]*1.2);
+		}
+#endif
+#if 0
+			double pitchAttitudeCmd = updatePID((receiverCommand[AXIS.Y] - receiverZero[AXIS.Y]) * ATTITUDE_SCALING, -kinematicsAngle[AXIS.Y], &PID[ATTITUDE_YAXIS_PID_IDX]);
+			roll   = updatePID(rollAttitudeCmd, gyroRate[XAXIS]*1.2, &PID[ATTITUDE_GYRO_XAXIS_PID_IDX]);
+			pitch  = updatePID(pitchAttitudeCmd, -gyroRate[YAXIS]*1.2, &PID[ATTITUDE_GYRO_YAXIS_PID_IDX]);
+		}
+		else {
+			roll  = updatePID(getReceiverSIData(XAXIS), gyroRate[XAXIS]*0.8, &PID[RATE_XAXIS_PID_IDX]);
+			pitch = updatePID(getReceiverSIData(YAXIS), -gyroRate[YAXIS]*0.8, &PID[RATE_YAXIS_PID_IDX]);
+		}
+#endif
+	}
+
+#if 0
+	/**
+	 * processThrottleCorrection
+	 *
+	 * This function will add some throttle imput if the craft is angled
+	 * this prevent the craft to loose altitude when angled.
+	 * it also add the battery throttle correction in case
+	 * of we are in auto-descent.
+	 *
+	 * Special thank to Ziojo for this.
+	 */
+	void processThrottleCorrection() {
+		int throttleAsjust = throttle / (cos (radians (kinematicsAngle[XAXIS])) * cos (radians (kinematicsAngle[YAXIS])));
+		throttleAsjust = constrain ((throttleAsjust - throttle), 0, 160); //compensate max  +/- 25 deg XAXIS or YAXIS or  +/- 18 ( 18(XAXIS) + 18(YAXIS))
+		throttle = throttle + throttleAsjust + (int)batteyMonitorThrottleCorrection;
+
+		throttle = constrain(throttle,MINCOMMAND,MAXCOMMAND-150);  // limmit throttle to leave some space for motor correction in max throttle manuever
+	}
+#endif
+
+	/**
+	 * processHeading
+	 *
+	 * This function will calculate the craft heading correction depending
+	 * of the users command. Heading correction is process with the gyro
+	 * or a magnetometer
+	 */
+	private void processHeading() {
+		// TODO
+	}
+
+	/**
+	 * processFlightControl
+	 *
+	 * Main flight control processos function
+	 */
+	public void processFlightControl() {
+		double roll, pitch;
+		calculateFlightError(out roll, out pitch);
+		processHeading();
+
+#if 0
+		/* 50Hz tasks */
+		processAltitudeHold();
+		processThrottleCorrection();
+
+		/* if running */
+		applyMotorCommand();
+
+		processMinMaxCommand();
+
+		/* Allows quad to do acrobatics by lowering power to opposite motors during hard manuevers */
+		//processHardManuevers();
+
+		/* If throttle in minimum position, don't apply yaw */
+		if (receiverCommand[THROTTLE] < MINCHECK) {
+			for (byte motor = 0; motor < LASTMOTOR; motor++) {
+				motorMaxCommand[motor] = minArmedThrottle;
+			}
+		}
+
+		/* Apply limits to motor commands */
+		for (byte motor = 0; motor < LASTMOTOR; motor++) {
+			motorCommand[motor] = constrain(motorCommand[motor], motorMinCommand[motor], motorMaxCommand[motor]);
+		}
+
+		/* ESC Calibration */
+		if (motorArmed == OFF) {
+			processCalibrateESC();
+		}
+
+		if (motorArmed == ON && safetyCheck == ON) {
+			writeMotors();
+		}
+#endif
+	}
+}
diff --git a/ctrl/kinematics.vala b/ctrl/kinematics.vala
new file mode 100644
index 0000000..0e90a8f
--- /dev/null
+++ b/ctrl/kinematics.vala
@@ -0,0 +1,148 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ * Copyright 2012, Ted Carancho
+ *
+ * Ported from AeroQuad
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+public class Kinematics {
+	/* quaternion elements representing the estimated orientation */
+	double q0;
+	double q1;
+	double q2;
+	double q3;
+
+	/* scaled integral error */
+	double exInt = 0.0;
+	double eyInt = 0.0;
+	double ezInt = 0.0;
+
+	/* proportional gain governs rate of convergence to accelerometer */
+	double kpAcc = 0.2;
+
+	/* integral gain governs rate of convergence of gyroscope biases */
+	double kiAcc = 0.0005;
+
+	/* proportional gain governs rate of convergence to magnetometer */
+	double kpMag = 2.0;
+	/* integral gain governs rate of convergence of gyroscope biases */
+	double kiMag = 0.005;
+
+	/* half the sample period*/
+	double halfT = 0.0;
+
+	public double[] kinematicsAngle = new double[3];
+	public double[] correctedRateVector = new double[3];
+	public double[] gyroAngle = new double[2];
+
+	public Kinematics(double hdgX, double hdgY) {
+		for (var axis = AXIS.X; axis <= AXIS.Z; axis+=1)
+			kinematicsAngle[axis] = 0;
+		for (var axis = AXIS.X; axis <= AXIS.Y; axis+=1)
+			gyroAngle[axis] = 0;
+
+		double hdg = Math.atan2(hdgY, hdgX);
+		q0 = Math.cos(hdg/2);
+		q1 = 0;
+		q2 = 0;
+		q3 = Math.sin(hdg/2);
+	}
+
+	public void update(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz, double time) {
+		double norm;
+		double hx, hy, hz, bx, bz;
+		double vx, vy, vz, wx, wy, wz;
+		double exAcc, eyAcc, ezAcc;
+		double exMag, eyMag, ezMag;
+		double q0i, q1i, q2i, q3i;
+
+		halfT = time/2;
+
+		/* normalise the accelerometer measurements */
+		norm = Math.sqrt(ax*ax + ay*ay + az*az);
+		ax = ax / norm;
+		ay = ay / norm;
+		az = az / norm;
+
+		/* normalise the magnometer measurements */
+		norm = Math.sqrt(mx*mx + my*my + mz*mz);
+		mx = mx / norm;
+		my = my / norm;
+		mz = mz / norm;
+
+		/* compute reference direction of flux */
+		hx = mx * 2 * (0.5 - q2 * q2 - q3 * q3) + my * 2 * (q1 * q2 - q0 * q3)       + mz * 2 * (q1 * q3 + q0 * q2);
+		hy = mx * 2 * (q1 * q2 + q0 * q3)       + my * 2 * (0.5 - q1 * q1 - q3 * q3) + mz * 2 * (q2 * q3 - q0 * q1);
+		hz = mx * 2 * (q1 * q3 - q0 * q2)       + my * 2 * (q2 * q3 + q0 * q1)       + mz * 2 * (0.5 - q1 * q1 - q2 * q2);
+
+		bx = Math.sqrt((hx*hx) + (hy*hy));
+		bz = hz;
+
+		/* estimate direction of gravity and flux (v and w) */
+		vx = 2 *(q1 * q3 - q0 * q2);
+		vy = 2 *(q0 * q1 + q2 * q3);
+		vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
+
+		wx = bx * 2*(0.5 - q2 * q2 - q3 * q3) + bz * 2 * (q1 * q3 - q0 * q2);
+		wy = bx * 2*(q1 * q2 - q0 * q3)       + bz * 2 * (q0 * q1 + q2 * q3);
+		wz = bx * 2*(q0 * q2 + q1 * q3)       + bz * 2 * (0.5 - q1 * q1 - q2 * q2);
+
+		/* error is sum of cross product between reference direction of fields and direction measured by sensors */
+		exAcc = (vy*az - vz*ay);
+		eyAcc = (vz*ax - vx*az);
+		ezAcc = (vx*ay - vy*ax);
+
+		exMag = (my*wz - mz*wy);
+		eyMag = (mz*wx - mx*wz);
+		ezMag = (mx*wy - my*wx);
+
+		/* integral error scaled integral gain */
+		exInt = exInt + exAcc*kiAcc + exMag*kiMag;
+		eyInt = eyInt + eyAcc*kiAcc + eyMag*kiMag;
+		ezInt = ezInt + ezAcc*kiAcc + ezMag*kiMag;
+
+		/* adjusted gyroscope measurements */
+		correctedRateVector[AXIS.X] = gx = gx + exAcc*kpAcc + exMag*kpMag + exInt;
+		correctedRateVector[AXIS.Y] = gy = gy + eyAcc*kpAcc + eyMag*kpMag + eyInt;
+		correctedRateVector[AXIS.Z] = gz = gz + ezAcc*kpAcc + ezMag*kpMag + ezInt;
+
+		/* integrate quaternion rate and normalise */
+		q0i = (-q1*gx - q2*gy - q3*gz) * halfT;
+		q1i = ( q0*gx + q2*gz - q3*gy) * halfT;
+		q2i = ( q0*gy - q1*gz + q3*gx) * halfT;
+		q3i = ( q0*gz + q1*gy - q2*gx) * halfT;
+		q0 += q0i;
+		q1 += q1i;
+		q2 += q2i;
+		q3 += q3i;
+
+		/* normalise quaternion */
+		norm = Math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
+		q0 = q0 / norm;
+		q1 = q1 / norm;
+		q2 = q2 / norm;
+		q3 = q3 / norm;
+
+		/* save the adjusted gyroscope measurements */
+		correctedRateVector[AXIS.X] = gx;
+		correctedRateVector[AXIS.Y] = gy;
+		correctedRateVector[AXIS.Z] = gz;
+
+		/* calculate euler angles */
+		kinematicsAngle[AXIS.X] = Math.atan2(2 * (q0*q1 + q2*q3), 1 - 2 *(q1*q1 + q2*q2));
+		kinematicsAngle[AXIS.Y] = Math.asin(2 * (q0*q2 - q1*q3));
+		kinematicsAngle[AXIS.Z] = Math.atan2(2 * (q0*q3 + q1*q2), 1 - 2 *(q2*q2 + q3*q3));
+	}
+}
diff --git a/ctrl/pid-controller.vala b/ctrl/pid-controller.vala
new file mode 100644
index 0000000..e77cda5
--- /dev/null
+++ b/ctrl/pid-controller.vala
@@ -0,0 +1,55 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ * Copyright 2012, Ted Carancho
+ *
+ * Ported from AeroQuad
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+public class PID {
+	double P;
+	double I;
+	double D;
+
+	double lastPosition;
+	uint64 previousPIDTime;
+	double integratedError;
+	double windupGuard;
+
+	private double constrain(double x, double a, double b) {
+		return a > x ? a : b < x ? b : x;
+	}
+
+	public double update(double targetPosition, double currentPosition) {
+		/* calculate time delta */
+		uint64 currentTime = time_t();
+		uint64 deltaPIDTime = currentTime - previousPIDTime;
+		previousPIDTime = currentTime;
+
+		/* calculate error */
+		double error = targetPosition - currentPosition;
+		integratedError += error * deltaPIDTime;
+		integratedError = constrain(integratedError, -windupGuard, windupGuard);
+
+		double dTerm = D * (currentPosition - lastPosition) / (deltaPIDTime * 100);
+		lastPosition = currentPosition;
+
+		return P * error + I * integratedError + dTerm;
+	}
+
+	public void clear() {
+		integratedError = 0;
+		previousPIDTime = time_t();
+	}
+}
diff --git a/enums.vala b/enums.vala
new file mode 100644
index 0000000..e59d9bd
--- /dev/null
+++ b/enums.vala
@@ -0,0 +1,35 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public enum AXIS {
+	X,
+	Y,
+	Z
+}
+
+public enum FlightMode {
+	ATTITUDE_FLIGHT_MODE
+}
+
+public enum PIDEntry {
+	RATE_XAXIS,
+	RATE_YAXIS,
+	ATTITUDE_XAXIS,
+	ATTITUDE_YAXIS,
+	ATTITUDE_GYRO_XAXIS,
+	ATTITUDE_GYRO_YAXIS,
+	/* must be the last element */
+	length
+}
diff --git a/hw/device.vala b/hw/device.vala
new file mode 100644
index 0000000..adee58a
--- /dev/null
+++ b/hw/device.vala
@@ -0,0 +1,17 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public class Device {
+}
diff --git a/hw/i2c-device.vala b/hw/i2c-device.vala
new file mode 100644
index 0000000..21193f2
--- /dev/null
+++ b/hw/i2c-device.vala
@@ -0,0 +1,90 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public errordomain I2CError {
+	IO_ERROR,
+	ERRNO,
+}
+
+public class I2CDevice : Device {
+	private int fd;
+
+	public I2CDevice(uint8 dev, uint8 addr) throws I2CError {
+		string file = "/dev/i2c-%u".printf(dev);
+		fd = Posix.open(file, Posix.O_RDWR);
+
+		if(fd < 0)
+			throw new I2CError.IO_ERROR("Could not open device!");
+
+		setup_address(addr);
+	}
+
+	~I2CDevice() {
+		Posix.close(fd);
+	}
+
+	protected void setup_address(uint8 addr) throws I2CError {
+		if(Posix.ioctl(fd, Linux.I2C.SLAVE, addr) < 0)
+			throw new I2CError.IO_ERROR("Could not seek to I2C device 0x%02x!", addr);
+	}
+
+	protected void write_byte(uint8 byte) throws I2CError {
+		if(Linux.I2C.SMBUS.write_byte(fd, byte) < 0) {
+			throw new I2CError.ERRNO("Could not write to I2C device: %s!", Posix.strerror(Posix.errno));
+		}
+	}
+
+	protected uint8 read_byte() throws I2CError {
+		int rc = Linux.I2C.SMBUS.read_byte(fd);
+		if(rc < 0)
+			throw new I2CError.ERRNO("Could not read from I2C device: %s!", Posix.strerror(Posix.errno));
+		return (uint8) rc;
+	}
+
+	protected uint8 get_byte(uint8 addr) throws I2CError {
+		int rc = Linux.I2C.SMBUS.read_word_data(fd, addr);
+		if(rc < 0)
+			throw new I2CError.ERRNO("Could not get byte %d from I2C device: %s!", addr, Posix.strerror(Posix.errno));
+		return (uint8) rc;
+	}
+
+	protected void set_byte(uint8 addr, uint8 val) throws I2CError {
+		int rc = Linux.I2C.SMBUS.write_word_data(fd, addr, val);
+		if(rc < 0)
+			throw new I2CError.ERRNO("Could not set byte %d to %d from I2C device: %s!", addr, val, Posix.strerror(Posix.errno));
+	}
+
+	protected uint16 get_big_word(uint8 addr) throws I2CError {
+		int rc = Linux.I2C.SMBUS.read_word_data(fd, addr);
+		if(rc < 0)
+			throw new I2CError.ERRNO("Could get word %d from I2C device: %s!", addr, Posix.strerror(Posix.errno));
+		return uint16.from_big_endian((uint16) rc);
+	}
+
+	protected uint16 get_little_word(uint8 addr) throws I2CError {
+		int rc = Linux.I2C.SMBUS.read_word_data(fd, addr);
+		if(rc < 0)
+			throw new I2CError.ERRNO("Could get word %d from I2C device: %s!", addr, Posix.strerror(Posix.errno));
+		return uint16.from_little_endian((uint16) rc);
+	}
+
+	protected uint8[] get_block(uint8 addr, uint8 size) throws I2CError {
+		uint8[] data = new uint8[size];
+		int rc = Linux.I2C.SMBUS.read_i2c_block_data(fd, addr, data);
+		if(rc < 0)
+			throw new I2CError.ERRNO("Could get word %d from I2C device: %s!", addr, Posix.strerror(Posix.errno));
+		return data;
+	}
+}
diff --git a/hw/serial-device.vala b/hw/serial-device.vala
new file mode 100644
index 0000000..bbd7698
--- /dev/null
+++ b/hw/serial-device.vala
@@ -0,0 +1,20 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public class SerialDevice : Device {
+
+	/* TODO */
+
+}
diff --git a/sensors/accelerometer.vala b/sensors/accelerometer.vala
new file mode 100644
index 0000000..27b47f0
--- /dev/null
+++ b/sensors/accelerometer.vala
@@ -0,0 +1,18 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public interface Accelerometer : Device {
+	public abstract void get_data(out uint16 x, out uint16 y, out uint16 z) throws Error;
+}
diff --git a/sensors/accelerometer/accelerometer-adxl345.vala b/sensors/accelerometer/accelerometer-adxl345.vala
new file mode 100644
index 0000000..ed9947f
--- /dev/null
+++ b/sensors/accelerometer/accelerometer-adxl345.vala
@@ -0,0 +1,44 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public class ADXL345 : I2CDevice, Accelerometer {
+	public ADXL345(uint8 dev, uint8 addr = 0x53) throws I2CError {
+			base(dev, addr);
+	}
+
+	public uint8 get_address() throws I2CError {
+		var addr = get_byte(0x00);
+		assert(addr == 0xe5);
+		return addr;
+	}
+
+	public void get_offset(out uint8 x, out uint8 y, out uint8 z) throws I2CError {
+		x = get_byte(0x1E);
+		y = get_byte(0x1F);
+		z = get_byte(0x20);
+	}
+
+	public void set_offset(uint8 x, uint8 y, uint8 z) throws I2CError {
+		set_byte(0x1E, x);
+		set_byte(0x1F, y);
+		set_byte(0x20, z);
+	}
+
+	public void get_data(out uint16 x, out uint16 y, out uint16 z) throws I2CError {
+		x = get_big_word(0x32);
+		y = get_big_word(0x34);
+		z = get_big_word(0x36);
+	}
+}
diff --git a/sensors/barometer.vala b/sensors/barometer.vala
new file mode 100644
index 0000000..5c1f4f9
--- /dev/null
+++ b/sensors/barometer.vala
@@ -0,0 +1,23 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public interface Barometer : Device {
+	public abstract int32 get_pressure() throws Error;
+
+	public double get_altitude(int32 base_pressure) throws Error {
+		int32 p = get_pressure();
+		return (44330.0 * (1.0 - Posix.pow(p / base_pressure, 1.0 / 5.255)));
+	}
+}
diff --git a/sensors/barometer/barometer-bmp085.vala b/sensors/barometer/barometer-bmp085.vala
new file mode 100644
index 0000000..92ea917
--- /dev/null
+++ b/sensors/barometer/barometer-bmp085.vala
@@ -0,0 +1,128 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public class BMP085 : I2CDevice, Barometer {
+	public uint8 oversampling_setting = 0x03;
+	private Calibration cal = Calibration();
+
+	private int64 next_temp_measurement;
+	private int raw_temp;
+	private int raw_pressure;
+
+	private int64 temp_correction_coefficient;
+
+	struct Calibration {
+		int16 AC1;
+		int16 AC2;
+		int16 AC3;
+		uint16 AC4;
+		uint16 AC5;
+		uint16 AC6;
+		int16 B1;
+		int16 B2;
+		int16 MB;
+		int16 MC;
+		int16 MD;
+	}
+
+	public BMP085(uint8 dev, uint8 addr = 0x77) throws I2CError {
+			base(dev, addr);
+			read_calibration_values();
+	}
+
+	private void read_calibration_values() throws I2CError {
+		var data = get_block(0xaa, 22);
+		cal.AC1 =  data[0] << 8 | data[1];
+		cal.AC2 =  data[2] << 8 | data[3];
+		cal.AC3 =  data[4] << 8 | data[5];
+		cal.AC4 =  data[6] << 8 | data[7];
+		cal.AC5 =  data[8] << 8 | data[9];
+		cal.AC6 = data[10] << 8 | data[11];
+		cal.B1  = data[12] << 8 | data[13];
+		cal.B2  = data[14] << 8 | data[15];
+		cal.MB  = data[16] << 8 | data[17];
+		cal.MC  = data[18] << 8 | data[19];
+
+		cal.MD  = data[20] << 8 | data[21];
+	}
+
+	private void msleep(uint msecs) {
+		Posix.usleep(msecs*1000);
+	}
+
+	private void update_raw_temperature() throws I2CError {
+		set_byte(0xf4, 0x2e);
+		msleep(5);
+		var data = get_block(0xf6, 2);
+		raw_temp = data[0] << 8 | data[1];
+		next_temp_measurement = time_t() + 1;
+	}
+
+	private void update_raw_pressure() throws I2CError {
+		set_byte(0xf4, 0x34 + (oversampling_setting<<6));
+		msleep(2+(3 << oversampling_setting<<1));
+		var data = get_block(0xf6, 3);
+		raw_pressure = data[0] << 16 | data[1] << 8 | data[2];
+		raw_pressure >>= 8-oversampling_setting;
+	}
+
+	public int32 get_temperature() throws I2CError {
+		if(next_temp_measurement+1 < time_t())
+			update_raw_temperature();
+
+		int64 x1 = ((raw_temp - cal.AC6) * cal.AC5) >> 15;
+		int64 x2 = (cal.MC << 11) / (x1 + cal.MD);
+		temp_correction_coefficient = x1 + x2 - 4000;
+
+		return (int32) (x1+x2+8) >> 4;
+	}
+
+	public int32 get_pressure() throws I2CError {
+		int64 x1, x2, x3, b3, p;
+		uint64 b4, b7;
+
+		if(next_temp_measurement+1 < time_t())
+			get_temperature();
+
+		update_raw_pressure();
+
+		x1 = (temp_correction_coefficient * temp_correction_coefficient) >> 12;
+		x1 *= cal.B2;
+		x1 >>= 11;
+
+		x2 = cal.AC2 * temp_correction_coefficient;
+		x2 >>= 11;
+
+		x3 = x1 + x2;
+
+		b3 = (((((int64)cal.AC1) * 4 + x3) << oversampling_setting) + 2) >> 2;
+
+		x1 = (cal.AC3 * temp_correction_coefficient) >> 13;
+		x2 = (cal.B1 * ((temp_correction_coefficient * temp_correction_coefficient) >> 12)) >> 16;
+		x3 = (x1 + x2 + 2) >> 2;
+		b4 = (cal.AC4 * (uint64)(x3 + 32768)) >> 15;
+
+		b7 = ((uint64)raw_pressure - b3) * (50000 >> oversampling_setting);
+		p = (int64) ((b7 < 0x80000000) ? ((b7 << 1) / b4) : ((b7 / b4) * 2));
+
+		x1 = p >> 8;
+		x1 *= x1;
+		x1 = (x1 * 3038) >> 16;
+		x2 = (-7357 * p) >> 16;
+		p += (x1 + x2 + 3791) >> 4;
+
+		return (int32) p;
+	}
+}
diff --git a/sensors/compass.vala b/sensors/compass.vala
new file mode 100644
index 0000000..b62cb3c
--- /dev/null
+++ b/sensors/compass.vala
@@ -0,0 +1,18 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public interface Compass : Device {
+	public abstract void get_data(out uint16 x, out uint16 y, out uint16 z) throws Error;
+}
diff --git a/sensors/compass/compass-hmc5843.vala b/sensors/compass/compass-hmc5843.vala
new file mode 100644
index 0000000..b09b054
--- /dev/null
+++ b/sensors/compass/compass-hmc5843.vala
@@ -0,0 +1,26 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public class HMC5843 : I2CDevice, Compass {
+	public HMC5843(uint8 dev, uint8 addr = 0x1e) throws I2CError {
+			base(dev, addr);
+	}
+
+	public void get_data(out uint16 x, out uint16 y, out uint16 z) throws I2CError {
+		x = get_big_word(0x03);
+		y = get_big_word(0x05);
+		z = get_big_word(0x07);
+	}
+}
diff --git a/sensors/gyroscope.vala b/sensors/gyroscope.vala
new file mode 100644
index 0000000..165df73
--- /dev/null
+++ b/sensors/gyroscope.vala
@@ -0,0 +1,18 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public interface Gyroscope : Device {
+	public abstract void get_data(out int16 x, out int16 y, out int16 z) throws Error;
+}
diff --git a/sensors/gyroscope/gyroscope-itg3200.vala b/sensors/gyroscope/gyroscope-itg3200.vala
new file mode 100644
index 0000000..c57aa55
--- /dev/null
+++ b/sensors/gyroscope/gyroscope-itg3200.vala
@@ -0,0 +1,48 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public class ITG3200 : I2CDevice, Gyroscope {
+	public ITG3200(uint8 dev, uint8 addr = 0x68) throws I2CError {
+			base(dev, addr);
+	}
+
+	public uint8 get_address() throws I2CError {
+		return (uint8) get_byte(0x00);
+	}
+
+	/* TODO: add method for register 0x16 (frequency + range) */
+	/* TODO: add method for register 0x17 (interrupt config) */
+	/* TODO: add method for register 0x3E (power management) */
+
+	public uint8 get_sample_rate_divider() throws I2CError {
+		/* sample rate = internal frequency / (divider + 1) */
+		return (uint8) get_byte(0x15);
+	}
+
+	public void set_sample_rate_divider(uint8 val) throws I2CError {
+		set_byte(0x15, val);
+	}
+
+	public int16 get_temperature() throws I2CError {
+		int16 raw = (int16) get_big_word(0x1b);
+		return 350 + ((raw + 13200) / 28);
+	}
+
+	public void get_data(out int16 x, out int16 y, out int16 z) throws I2CError {
+		x = (int16) get_big_word(0x1d);
+		y = (int16) get_big_word(0x1f);
+		z = (int16) get_big_word(0x21);
+	}
+}
diff --git a/test-shell.vala b/test-shell.vala
new file mode 100644
index 0000000..4e78406
--- /dev/null
+++ b/test-shell.vala
@@ -0,0 +1,158 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+static int i;
+
+public static string? generator(string cmd, int state) {
+	string[] commands = { "firmware", "get", "set", "stop", "play", "help", "pressure", "temperature", "altitude", "gyro-temperature", "gyro-data" };
+
+	if(state == 0)
+		i = -1;
+
+	while(i < commands.length-1) {
+		i++;
+		if(commands[i].has_prefix(cmd)) {
+			return commands[i];
+		}
+	}
+
+	return null;
+}
+
+public static string[]? completion(string cmd, int start, int end) {
+	string[] matches = null;
+
+	if(start == 0)
+		matches = Readline.completion_matches(cmd, generator);
+
+	return matches;
+}
+
+public static bool shell(out string cmd) {
+	cmd = Readline.readline("[blinkm] $ ");
+	Readline.History.add(cmd);
+
+	if(cmd == "exit")
+		cmd = null;
+
+	if(cmd == null)
+		stdout.printf("exit\n");
+
+	return (cmd != null) ? true : false;
+}
+
+public static int main(string[] args) {
+	Readline.attempted_completion_function = completion;
+	Readline.bind_key('\t', Readline.complete);
+
+	try {
+		string cmd;
+		BlinkM led = new BlinkM(3, 0x10);
+#if BMP085
+		BMP085 bmp085 = new BMP085(3);
+#endif
+		ITG3200 itg3200 = new ITG3200(3);
+
+		while(shell(out cmd)) {
+			var cmdparts = cmd.split(" ");
+			switch(cmdparts[0]) {
+				case null:
+					break;
+				case "firmware":
+					char major, minor;
+					led.get_firmware_version(out major, out minor);
+					stdout.printf("firmware: (%c.%c)\n", major, minor);
+					break;
+				case "get":
+					switch(cmdparts[1]) {
+						case "address":
+							stdout.printf("address: %02x\n", led.get_address());
+							break;
+						default:
+							stdout.printf("unknown get parameter!\n");
+							break;
+					}
+					break;
+				case "stop":
+					led.script_stop();
+					break;
+				case "play":
+					led.script_play((BlinkM.Script) uint64.parse(cmdparts[1]));
+					break;
+				case "set":
+					if(cmdparts[1] == "fade" && cmdparts[2] == "speed") {
+						if(cmdparts.length == 4)
+							led.set_fade_speed((uint8) uint64.parse(cmdparts[3]));
+						else
+							stdout.printf("\tset fade speed <speed>\n");
+					} else if(cmdparts[1] == "time" && cmdparts[2] == "adjust") {
+						if(cmdparts.length == 4)
+							led.set_time_adjust((int8) int.parse(cmdparts[3]));
+						else
+							stdout.printf("\tset time adjust <time>\n");
+					} else {
+						if(cmdparts.length == 4)
+							led.set((uint8) uint64.parse(cmdparts[1]), (uint8) uint64.parse(cmdparts[2]), (uint8) uint64.parse(cmdparts[3]));
+						else
+							stdout.printf("\tset <red> <green> <blue>\n");
+					}
+					break;
+#if BMP085
+				case "temperature":
+					var temp = bmp085.get_temperature();
+					stdout.printf("temperature: %d.%d °C\n", temp / 10, temp % 10);
+					break;
+				case "pressure":
+					var pressure = bmp085.get_pressure();
+					stdout.printf("pressure: %d.%d hPa\n", pressure / 100, pressure % 100);
+					break;
+				case "altitude":
+					var altitude = bmp085.get_altitude();
+					stdout.printf("altitude: %f m\n", altitude);
+					break;
+#endif
+				case "gyro-temperature":
+					var temp = itg3200.get_temperature();
+					stdout.printf("temperature 2: %d.%d °C\n", temp / 10, temp % 10);
+					break;
+				case "gyro-data":
+					int16 x, y, z;
+					itg3200.get_data(out x, out y, out z);
+					stdout.printf("data: %d %d %d\n", x, y, z);
+					break;
+				case "help":
+					stdout.printf("commands: \n");
+					stdout.printf("\tfirmware\n");
+					stdout.printf("\taddress get\n");
+					stdout.printf("\tset <red> <green> <blue>\n");
+					stdout.printf("\tset fade speed <speed>\n");
+					stdout.printf("\tset time adjust <time>\n");
+					stdout.printf("\tstop\n");
+					stdout.printf("\tplay <id>\n");
+					stdout.printf("\texit\n");
+					break;
+				default:
+					stdout.printf("unknown command!\n");
+					break;
+			}
+		}
+	} catch(I2CError e) {
+		stderr.printf("%s\n", e.message);
+	} catch(Error e) {
+		stderr.printf("%s\n", e.message);
+	}
+
+	return 0;
+}