restructure code

it is now loading its components from plugins

35 files changed, 620 insertions, 253 deletions

diff --git a/.gitignore b/.gitignore
index 53a22f5..a0fcae9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,4 @@
-*.c
+microcopterd
 *.so
-test-shell
+*.c
+microcopterd
diff --git a/Makefile b/Makefile
index 4253f89..ffdc860 100644
--- a/Makefile
+++ b/Makefile
@@ -1,8 +1,27 @@
-test-shell: actuators/*.vala actuators/*/*.vala sensors/*.vala sensors/*/*.vala hw/*.vala \
-            ctrl/*.vala ctrl/*/*.vala test-shell.vala enums.vala
-	valac -o $@ --pkg posix --pkg linux --pkg readline -X -lreadline -X -lm $^
+VALAC=valac
+CC=gcc
+
+ACCELEROMETERS      := $(wildcard sensors/accelerometers/*.vala)
+OBJECTS      := $(patsubst %.vala, %.o, $(SOURCES))
+
+all: microcopterd plugins
+
+microcopterd: actuators/*.vala sensors/*.vala hw/*.vala \
+              ctrl/*.vala receiver/*.vala *.vala
+	$(VALAC) -g -o $@ --pkg posix --pkg linux --pkg gmodule-2.0 --pkg readline -X -lreadline -X -lm $^
+
+plugins:
+	@cd actuators/motor ; make
+	@cd ctrl/models ; make
+	@cd receiver ; make
+	@cd sensors/accelerometer ; make
+	@cd sensors/barometer ; make
+	@cd sensors/compass ; make
+	@cd sensors/gyroscope ; make
 
 clean:
-	rm -f test-shell *.c */*.c */*/*.c
+	rm -f microcopterd
+	rm -f *.so */*.so */*/*.so
+	rm -f *.c */*.c */*/*.c
 
-.PHONY: clean
+.PHONY: clean plugins
diff --git a/actuators/blinkm.vala b/actuators/blinkm.vala
index be98260..8068666 100644
--- a/actuators/blinkm.vala
+++ b/actuators/blinkm.vala
@@ -47,7 +47,7 @@ public class BlinkM : I2CDevice {
 	}
 
 	public BlinkM(uint8 dev, uint8 addr = 0x09) throws I2CError {
-		base(dev, addr);
+		setup(dev, addr);
 	}
 
 	public void get_firmware_version(out char major, out char minor) throws I2CError {
@@ -81,14 +81,14 @@ public class BlinkM : I2CDevice {
 		write_byte(start);
 	}
 
-	public void set(uint8 red, uint8 green, uint8 blue) throws I2CError {
+	public void set_color(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 {
+	public void get_color(out uint8 red, out uint8 green, out uint8 blue) throws I2CError {
 		write_byte('g');
 		red = read_byte();
 		green = read_byte();
diff --git a/actuators/motor-controller.vala b/actuators/motor-controller.vala
index fb596dd..d348e53 100644
--- a/actuators/motor-controller.vala
+++ b/actuators/motor-controller.vala
@@ -14,6 +14,10 @@
  */
 
 public interface MotorController : Device {
-	public abstract void set(uint8 m, uint8 v);
+	public abstract uint8 min { get ; }
+	public abstract uint8 max { get ; }
+
+	public abstract void init(KeyFile cfg) throws Error;
+	public abstract void set_single(uint8 m, uint8 v);
 	public abstract void set_all(uint8 v);
 }
diff --git a/actuators/motor/Makefile b/actuators/motor/Makefile
new file mode 100644
index 0000000..bd35015
--- /dev/null
+++ b/actuators/motor/Makefile
@@ -0,0 +1,14 @@
+include ../../config.mk
+
+all: AtmostripeMotorController.so
+
+clean:
+	rm -f *.c *.so
+
+atmostripe.c: atmostripe.vala
+	$(VALAC) --pkg gmodule-2.0 --pkg posix --pkg linux -C $< ../motor-controller.vala ../../hw/serial-device.vala ../../hw/device.vala
+
+AtmostripeMotorController.so: atmostripe.c
+	$(CC) -shared -fPIC `pkg-config --cflags --libs glib-2.0 gobject-2.0 gmodule-2.0` -o $@ $<
+
+.PHONY: all clean
diff --git a/actuators/motor/atmostripe.vala b/actuators/motor/atmostripe.vala
index 0cc4eef..ad996e7 100644
--- a/actuators/motor/atmostripe.vala
+++ b/actuators/motor/atmostripe.vala
@@ -14,7 +14,19 @@
  */
 
 public class AtmostripeMotorController : SerialDevice, MotorController {
-	public void set(uint8 m, uint8 v) {
+	public uint8 min {
+		get { return 0; }
+	}
+
+	public uint8 max {
+		get { return 255; }
+	}
+
+	public void init(KeyFile cfg) throws Error {
+		/* */
+	}
+
+	public void set_single(uint8 m, uint8 v) {
 		/* */
 	}
 
@@ -22,3 +34,8 @@ public class AtmostripeMotorController : SerialDevice, MotorController {
 		/* */
 	}
 }
+
+public Type register_plugin (Module module) {
+	// types are registered automatically
+	return typeof(AtmostripeMotorController);
+}
diff --git a/config.mk b/config.mk
new file mode 100644
index 0000000..462dcaa
--- /dev/null
+++ b/config.mk
@@ -0,0 +1,2 @@
+VALAC=valac
+CC=gcc
diff --git a/config.vala b/config.vala
new file mode 100644
index 0000000..4b28296
--- /dev/null
+++ b/config.vala
@@ -0,0 +1,114 @@
+/* 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 Config : KeyFile {
+	public Config() throws Error {
+		if(!load_from_file("microcopter.cfg", KeyFileFlags.NONE)) {
+			throw new FileError.FAILED("Could not load configuration file");;
+		}
+	}
+
+	public MotorController get_motor_controller() throws Error {
+		string name = get_string("config", "motorcontroller");
+		string path = Path.build_path("/", Environment.get_variable("PWD"), "actuators", "motor", "%s.so".printf(name));
+
+		var plugin = new PluginLoader<MotorController>(path);
+		if(!plugin.load())
+			throw new KeyFileError.INVALID_VALUE("Could not load Plugin");
+
+		var result = plugin.new_object();
+		result.init(this);
+
+		return result;
+	}
+
+	public Gyroscope get_gyroscope() throws Error {
+		string name = get_string("config", "gyroscope");
+		string path = Path.build_path("/", Environment.get_variable("PWD"), "sensors", "gyroscope", "%s.so".printf(name));
+		var plugin = new PluginLoader<Gyroscope>(path);
+		if(!plugin.load())
+			throw new KeyFileError.INVALID_VALUE("Could not load Plugin");
+
+		var result = plugin.new_object();
+		result.init(this);
+
+		return result;
+	}
+
+	public Barometer get_barometer() throws Error {
+		string name = get_string("config", "barometer");
+		string path = Path.build_path("/", Environment.get_variable("PWD"), "sensors", "barometer", "%s.so".printf(name));
+		var plugin = new PluginLoader<Barometer>(path);
+		if(!plugin.load())
+			throw new KeyFileError.INVALID_VALUE("Could not load Plugin");
+
+		var result = plugin.new_object();
+		result.init(this);
+
+		return result;
+	}
+
+	public Accelerometer get_accelerometer() throws Error {
+		string name = get_string("config", "accelerometer");
+		string path = Path.build_path("/", Environment.get_variable("PWD"), "sensors", "accelerometer", "%s.so".printf(name));
+		var plugin = new PluginLoader<Accelerometer>(path);
+		if(!plugin.load())
+			throw new KeyFileError.INVALID_VALUE("Could not load Plugin");
+
+		var result = plugin.new_object();
+		result.init(this);
+
+		return result;
+	}
+
+	public Compass get_compass() throws Error {
+		string name = get_string("config", "compass");
+		string path = Path.build_path("/", Environment.get_variable("PWD"), "sensors", "compass", "%s.so".printf(name));
+		var plugin = new PluginLoader<Compass>(path);
+		if(!plugin.load())
+			throw new KeyFileError.INVALID_VALUE("Could not load Plugin");
+
+		var result = plugin.new_object();
+		result.init(this);
+
+		return result;
+	}
+
+	public CopterModel get_model() throws Error {
+		string name = get_string("config", "model");
+		string path = Path.build_path("/", Environment.get_variable("PWD"), "ctrl", "models", "%s.so".printf(name));
+		var plugin = new PluginLoader<CopterModel>(path);
+		if(!plugin.load())
+			throw new KeyFileError.INVALID_VALUE("Could not load Plugin");
+
+		var result = plugin.new_object();
+		result.init(this);
+
+		return result;
+	}
+
+	public Receiver get_receiver() throws Error {
+		string name = get_string("config", "receiver");
+		string path = Path.build_path("/", Environment.get_variable("PWD"), "receiver", "%s.so".printf(name));
+		var plugin = new PluginLoader<Receiver>(path);
+		if(!plugin.load())
+			throw new KeyFileError.INVALID_VALUE("Could not load Plugin");
+
+		var result = plugin.new_object();
+		result.init(this);
+
+		return result;
+	}
+}
diff --git a/ctrl/copter-model.vala b/ctrl/copter-model.vala
index 09a780d..377b2bf 100644
--- a/ctrl/copter-model.vala
+++ b/ctrl/copter-model.vala
@@ -13,7 +13,8 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
-public abstract class CopterModel {
+public abstract class CopterModel : Object {
+	public abstract void init(KeyFile cfg) throws Error;
 	public abstract uint8 get_motor_amount();
 
 	public abstract void set_max(uint8 motor, int val);
diff --git a/ctrl/flight-control.vala b/ctrl/flight-control.vala
index 00e7b31..c2df730 100644
--- a/ctrl/flight-control.vala
+++ b/ctrl/flight-control.vala
@@ -18,15 +18,17 @@
  */
 
 public class FlightControl {
-	MotorController motor;
 	FlightMode mode;
 	PID[] pid;
-	Kinematics k;
 
-	public FlightControl() {
-		motor = new AtmostripeMotorController();
+	int throttle;
+
+	const double ATTITUDE_SCALING = 1.0;
+
+	public FlightControl() throws Error {
 		pid = new PID[PIDEntry.length];
-		k = new Kinematics(0, 0); // TODO: read compass instead of using 0,0
+		for(int i=0; i<PIDEntry.length; i++)
+			pid[i] = new PID();
 	}
 
 	/**
@@ -35,62 +37,34 @@ public class FlightControl {
 	 * 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) {
+	void calculateFlightError(out int roll, out int pitch) throws Error {
 		roll = 0; pitch = 0;
 
 		if (mode == FlightMode.ATTITUDE_FLIGHT_MODE) {
+			double rollAttitudeCmd   = pid[PIDEntry.ATTITUDE_XAXIS].update(receiver.get_value(AXIS.X) * ATTITUDE_SCALING, kinematics.kinematicsAngle[AXIS.X]);
+			double pitchAttitudeCmd  = pid[PIDEntry.ATTITUDE_YAXIS].update(receiver.get_value(AXIS.Y) * ATTITUDE_SCALING, kinematics.kinematicsAngle[AXIS.Y]);
+			roll   = (int) pid[PIDEntry.ATTITUDE_GYRO_XAXIS].update(rollAttitudeCmd,   gyroscope.rate[AXIS.X]*1.2);
+			pitch  = (int) pid[PIDEntry.ATTITUDE_GYRO_YAXIS].update(pitchAttitudeCmd, -gyroscope.rate[AXIS.Y]*1.2);
+		} else {
+			roll  = (int) pid[PIDEntry.RATE_XAXIS].update(receiver.get_value(AXIS.X),  gyroscope.rate[AXIS.X]*0.8);
+			pitch = (int) pid[PIDEntry.RATE_YAXIS].update(receiver.get_value(AXIS.Y), -gyroscope.rate[AXIS.Y]*0.8);
 		}
-#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;
+		int throttleAdjust = (int) (throttle / (Math.cos(radians(kinematics.kinematicsAngle[AXIS.X])) * Math.cos(radians(kinematics.kinematicsAngle[AXIS.Y]))));
+		throttleAdjust = (int) constrain ((throttleAdjust - throttle), 0, 160); //compensate max  +/- 25 deg XAXIS or YAXIS or  +/- 18 ( 18(XAXIS) + 18(YAXIS))
+		throttle = throttle + throttleAdjust; // + (int)batteyMonitorThrottleCorrection;
 
-		throttle = constrain(throttle,MINCOMMAND,MAXCOMMAND-150);  // limmit throttle to leave some space for motor correction in max throttle manuever
+		/* limit throttle to leave some space for motor correction in max throttle manuever */
+		throttle = constrain_int(throttle,motorCtrl.min, motorCtrl.max - 10);
 	}
-#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() {
+	private void processHeadingHold() {
+		// TODO
+	}
+
+	private void processAltitudeHold() {
 		// TODO
 	}
 
@@ -99,44 +73,43 @@ public class FlightControl {
 	 *
 	 * Main flight control processos function
 	 */
-	public void processFlightControl() {
-		double roll, pitch;
+	public void process() throws Error {
+		int throttle = 0, yaw = 0, roll, pitch;
+		int[] motor_data;
+
 		calculateFlightError(out roll, out pitch);
-		processHeading();
+		processHeadingHold();
 
-#if 0
-		/* 50Hz tasks */
+		/* TODO: 50Hz tasks */
 		processAltitudeHold();
 		processThrottleCorrection();
 
-		/* if running */
-		applyMotorCommand();
-
-		processMinMaxCommand();
-
-		/* Allows quad to do acrobatics by lowering power to opposite motors during hard manuevers */
-		//processHardManuevers();
+		/* TODO: if running */
+		motor_data = model.get_motor_data(throttle, yaw, roll, pitch);
+		//TODO: processMinMaxCommand();
 
+#if 0
 		/* If throttle in minimum position, don't apply yaw */
-		if (receiverCommand[THROTTLE] < MINCHECK) {
-			for (byte motor = 0; motor < LASTMOTOR; motor++) {
+		if (receiver.get_value(THROTTLE) < MINCHECK) {
+			for (uint8 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]);
+		for (uint8 motor = 0; motor < LASTMOTOR; motor++) {
+			motor_data[motor] = constraint_int(motor_data[motor], motorMinCommand[motor], motorMaxCommand[motor]);
 		}
 
 		/* ESC Calibration */
 		if (motorArmed == OFF) {
 			processCalibrateESC();
 		}
-
-		if (motorArmed == ON && safetyCheck == ON) {
-			writeMotors();
-		}
 #endif
+
+		//if (motorArmed == ON && safetyCheck == ON) {
+			for(uint8 i=0; i<motor_data.length; i++)
+				motorCtrl.set_single(i, (uint8) motor_data[i]);
+		//}
 	}
 }
diff --git a/ctrl/models/Makefile b/ctrl/models/Makefile
new file mode 100644
index 0000000..113d1be
--- /dev/null
+++ b/ctrl/models/Makefile
@@ -0,0 +1,14 @@
+include ../../config.mk
+
+all: Hexacopter.so
+
+clean:
+	rm -f *.c *.so
+
+hexacopter.c: hexacopter.vala
+	$(VALAC) --pkg gmodule-2.0 --pkg posix --pkg linux -C $< ../copter-model.vala
+
+Hexacopter.so: hexacopter.c
+	$(CC) -shared -fPIC `pkg-config --cflags --libs glib-2.0 gobject-2.0 gmodule-2.0` -o $@ $<
+
+.PHONY: all clean
diff --git a/ctrl/models/hexa.vala b/ctrl/models/hexacopter.vala
index 176d3d6..9462bc8 100644
--- a/ctrl/models/hexa.vala
+++ b/ctrl/models/hexacopter.vala
@@ -29,6 +29,10 @@ public class Hexacopter : CopterModel {
 		FRONT_LEFT,
 	}
 
+	public override void init(KeyFile cfg) throws Error {
+		/* No Config */
+	}
+
 	public override uint8 get_motor_amount() {
 		return 6;
 	}
@@ -62,3 +66,8 @@ public class Hexacopter : CopterModel {
 		return cmd;
 	}
 }
+
+public Type register_plugin (Module module) {
+	// types are registered automatically
+	return typeof(Hexacopter);
+}
diff --git a/ctrl/pid-controller.vala b/ctrl/pid-controller.vala
index e77cda5..5866fe5 100644
--- a/ctrl/pid-controller.vala
+++ b/ctrl/pid-controller.vala
@@ -27,10 +27,6 @@ public class PID {
 	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();
diff --git a/hw/device.vala b/hw/device.vala
index adee58a..dad4439 100644
--- a/hw/device.vala
+++ b/hw/device.vala
@@ -13,5 +13,5 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
-public class Device {
+public class Device : Object {
 }
diff --git a/hw/i2c-device.vala b/hw/i2c-device.vala
index 21193f2..b6d285c 100644
--- a/hw/i2c-device.vala
+++ b/hw/i2c-device.vala
@@ -21,7 +21,7 @@ public errordomain I2CError {
 public class I2CDevice : Device {
 	private int fd;
 
-	public I2CDevice(uint8 dev, uint8 addr) throws I2CError {
+	public void setup(uint8 dev, uint8 addr) throws I2CError {
 		string file = "/dev/i2c-%u".printf(dev);
 		fd = Posix.open(file, Posix.O_RDWR);
 
diff --git a/main.vala b/main.vala
new file mode 100644
index 0000000..76fd3ab
--- /dev/null
+++ b/main.vala
@@ -0,0 +1,63 @@
+/* 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.
+ */
+
+Config cfg;
+Gyroscope gyroscope;
+Accelerometer accelerometer;
+Compass compass;
+Barometer barometer;
+MotorController motorCtrl;
+Receiver receiver;
+CopterModel model;
+FlightControl flightCtrl;
+Kinematics kinematics;
+
+bool loop() {
+	try {
+		/* TODO: get sensor values */
+		/* TODO: update kinematics */
+
+		flightCtrl.process();
+	} catch(Error e) {
+		stderr.printf("%s\n", e.message);
+	}
+
+	return true;
+}
+
+int main(string[] args) {
+	try {
+		cfg = new Config();
+
+		gyroscope     = cfg.get_gyroscope();
+		accelerometer = cfg.get_accelerometer();
+		compass       = cfg.get_compass();
+		barometer     = cfg.get_barometer();
+		motorCtrl     = cfg.get_motor_controller();
+		model         = cfg.get_model();
+		receiver      = cfg.get_receiver();
+
+		kinematics   = new Kinematics(0, 0); // TODO: Compass
+		flightCtrl   = new FlightControl();
+
+		Timeout.add(10, loop, Priority.HIGH);
+	} catch(Error e) {
+		stderr.printf("%s\n", e.message);
+		return 1;
+	}
+
+	new MainLoop(null, false).run();
+	return 0;
+}
diff --git a/microcopter.cfg b/microcopter.cfg
new file mode 100644
index 0000000..a471fe0
--- /dev/null
+++ b/microcopter.cfg
@@ -0,0 +1,31 @@
+[config]
+model = Hexacopter
+receiver = PS3Pad
+motorcontroller = AtmostripeMotorController
+gyroscope = ITG3200
+accelerometer = ADXL345
+compass = HMC5843
+barometer = BMP085
+
+[ITG3200]
+i2c-adapter = 3
+#i2c-address = 0x68
+i2c-address = 104
+
+[ADXL345]
+i2c-adapter = 3
+#i2c-address = 0x53
+i2c-address = 83
+
+[HMC5843]
+i2c-adapter = 3
+#i2c-address = 0x1e
+i2c-address = 30
+
+[BMP085]
+i2c-adapter = 3
+#i2c-address = 0x77
+i2c-address = 119
+
+[AtmostripeMotorController]
+device = /dev/ttyUSB0
diff --git a/plugin-loader.vala b/plugin-loader.vala
new file mode 100644
index 0000000..67070b4
--- /dev/null
+++ b/plugin-loader.vala
@@ -0,0 +1,49 @@
+/* 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 PluginLoader<T> : Object {
+
+    public string path { get; private set; }
+
+    private Type type;
+    private Module module;
+
+    private delegate Type RegisterPluginFunction(Module module);
+
+    public PluginLoader(string path) {
+        assert(Module.supported());
+        this.path = path;
+    }
+
+    public bool load() {
+        module = Module.open(path, ModuleFlags.BIND_LAZY);
+        if(module == null) {
+            return false;
+        }
+
+		module.make_resident();
+
+        void* function;
+        module.symbol("register_plugin", out function);
+
+        type = ((RegisterPluginFunction) function)(module);
+        stdout.printf("Loaded Plugin: %s\n", type.name());
+        return true;
+    }
+
+    public T new_object() {
+        return Object.new (type);
+    }
+}
diff --git a/receiver.vala b/receiver.vala
new file mode 100644
index 0000000..48feeeb
--- /dev/null
+++ b/receiver.vala
@@ -0,0 +1,21 @@
+/* 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 abstract class Receiver : Object {
+	public abstract uint8 size { get; }
+	public abstract void init(KeyFile cfg) throws Error;
+	public abstract double get_value(uint8 channel) throws Error;
+	public abstract void set_value(uint8 channel, double value) throws Error;
+}
diff --git a/receiver/Makefile b/receiver/Makefile
new file mode 100644
index 0000000..96d1bb8
--- /dev/null
+++ b/receiver/Makefile
@@ -0,0 +1,14 @@
+include ../config.mk
+
+all: PS3Pad.so
+
+clean:
+	rm -f *.c *.so
+
+PS3Pad.c: PS3Pad.vala
+	$(VALAC) --pkg gmodule-2.0 --pkg posix --pkg linux -C $< ../receiver.vala
+
+PS3Pad.so: PS3Pad.c
+	$(CC) -shared -fPIC `pkg-config --cflags --libs glib-2.0 gobject-2.0 gmodule-2.0` -o $@ $<
+
+.PHONY: all clean
diff --git a/receiver/PS3Pad.vala b/receiver/PS3Pad.vala
new file mode 100644
index 0000000..0ceb98d
--- /dev/null
+++ b/receiver/PS3Pad.vala
@@ -0,0 +1,38 @@
+/* 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 PS3Pad : Receiver {
+	public override uint8 size {
+		get { return 0; }
+	}
+
+	public override void init(KeyFile cfg) throws KeyFileError {
+		/* TODO */
+	}
+
+	public override double get_value(uint8 channel) {
+		/* TODO */
+		return 0;
+	}
+
+	public override void set_value(uint8 channel, double value) {
+		/* TODO */
+	}
+}
+
+public Type register_plugin (Module module) {
+	// types are registered automatically
+	return typeof(PS3Pad);
+}
diff --git a/sensors/accelerometer.vala b/sensors/accelerometer.vala
index 27b47f0..d9563f9 100644
--- a/sensors/accelerometer.vala
+++ b/sensors/accelerometer.vala
@@ -14,5 +14,6 @@
  */
 
 public interface Accelerometer : Device {
+	public abstract void init(KeyFile cfg) throws Error;
 	public abstract void get_data(out uint16 x, out uint16 y, out uint16 z) throws Error;
 }
diff --git a/sensors/accelerometer/Makefile b/sensors/accelerometer/Makefile
new file mode 100644
index 0000000..1bd2a03
--- /dev/null
+++ b/sensors/accelerometer/Makefile
@@ -0,0 +1,14 @@
+include ../../config.mk
+
+all: ADXL345.so
+
+clean:
+	rm -f *.c *.so
+
+adxl345.c: adxl345.vala
+	$(VALAC) --pkg gmodule-2.0 --pkg posix --pkg linux -C $< ../accelerometer.vala ../../hw/i2c-device.vala ../../hw/device.vala
+
+ADXL345.so: adxl345.c
+	$(CC) -shared -fPIC `pkg-config --cflags --libs glib-2.0 gobject-2.0 gmodule-2.0` -o $@ $<
+
+.PHONY: all clean
diff --git a/sensors/accelerometer/accelerometer-adxl345.vala b/sensors/accelerometer/adxl345.vala
index ed9947f..b8aa5e0 100644
--- a/sensors/accelerometer/accelerometer-adxl345.vala
+++ b/sensors/accelerometer/adxl345.vala
@@ -15,7 +15,13 @@
 
 public class ADXL345 : I2CDevice, Accelerometer {
 	public ADXL345(uint8 dev, uint8 addr = 0x53) throws I2CError {
-			base(dev, addr);
+			setup(dev, addr);
+	}
+
+	public void init(KeyFile cfg) throws KeyFileError, I2CError {
+		var adapter = cfg.get_uint64("ADXL345", "i2c-adapter");
+		var address = cfg.get_uint64("ADXL345", "i2c-address");
+		setup((uint8) adapter, (uint8) address);
 	}
 
 	public uint8 get_address() throws I2CError {
@@ -42,3 +48,8 @@ public class ADXL345 : I2CDevice, Accelerometer {
 		z = get_big_word(0x36);
 	}
 }
+
+public Type register_plugin (Module module) {
+	// types are registered automatically
+	return typeof(ADXL345);
+}
diff --git a/sensors/barometer.vala b/sensors/barometer.vala
index 5c1f4f9..d891ca0 100644
--- a/sensors/barometer.vala
+++ b/sensors/barometer.vala
@@ -14,6 +14,7 @@
  */
 
 public interface Barometer : Device {
+	public abstract void init(KeyFile cfg) throws Error;
 	public abstract int32 get_pressure() throws Error;
 
 	public double get_altitude(int32 base_pressure) throws Error {
diff --git a/sensors/barometer/Makefile b/sensors/barometer/Makefile
new file mode 100644
index 0000000..9a23671
--- /dev/null
+++ b/sensors/barometer/Makefile
@@ -0,0 +1,14 @@
+include ../../config.mk
+
+all: BMP085.so
+
+clean:
+	rm -f *.c *.so
+
+bmp085.c: bmp085.vala
+	$(VALAC) --pkg gmodule-2.0 --pkg posix --pkg linux -C $< ../barometer.vala ../../hw/i2c-device.vala ../../hw/device.vala
+
+BMP085.so: bmp085.c
+	$(CC) -shared -fPIC `pkg-config --cflags --libs glib-2.0 gobject-2.0 gmodule-2.0` -o $@ $<
+
+.PHONY: all clean
diff --git a/sensors/barometer/barometer-bmp085.vala b/sensors/barometer/bmp085.vala
index 92ea917..f0da790 100644
--- a/sensors/barometer/barometer-bmp085.vala
+++ b/sensors/barometer/bmp085.vala
@@ -38,10 +38,17 @@ public class BMP085 : I2CDevice, Barometer {
 	}
 
 	public BMP085(uint8 dev, uint8 addr = 0x77) throws I2CError {
-			base(dev, addr);
+			setup(dev, addr);
 			read_calibration_values();
 	}
 
+	public void init(KeyFile cfg) throws KeyFileError, I2CError {
+		var adapter = cfg.get_uint64("BMP085", "i2c-adapter");
+		var address = cfg.get_uint64("BMP085", "i2c-address");
+		setup((uint8) adapter, (uint8) address);
+		read_calibration_values();
+	}
+
 	private void read_calibration_values() throws I2CError {
 		var data = get_block(0xaa, 22);
 		cal.AC1 =  data[0] << 8 | data[1];
@@ -126,3 +133,8 @@ public class BMP085 : I2CDevice, Barometer {
 		return (int32) p;
 	}
 }
+
+public Type register_plugin (Module module) {
+	// types are registered automatically
+	return typeof(BMP085);
+}
diff --git a/sensors/compass.vala b/sensors/compass.vala
index b62cb3c..42d43ad 100644
--- a/sensors/compass.vala
+++ b/sensors/compass.vala
@@ -14,5 +14,6 @@
  */
 
 public interface Compass : Device {
+	public abstract void init(KeyFile cfg) throws Error;
 	public abstract void get_data(out uint16 x, out uint16 y, out uint16 z) throws Error;
 }
diff --git a/sensors/compass/Makefile b/sensors/compass/Makefile
new file mode 100644
index 0000000..178ae55
--- /dev/null
+++ b/sensors/compass/Makefile
@@ -0,0 +1,14 @@
+include ../../config.mk
+
+all: HMC5843.so
+
+clean:
+	rm -f *.c *.so
+
+hmc5843.c: hmc5843.vala
+	$(VALAC) --pkg gmodule-2.0 --pkg posix --pkg linux -C $< ../compass.vala ../../hw/i2c-device.vala ../../hw/device.vala
+
+HMC5843.so: hmc5843.c
+	$(CC) -shared -fPIC `pkg-config --cflags --libs glib-2.0 gobject-2.0 gmodule-2.0` -o $@ $<
+
+.PHONY: all clean
diff --git a/sensors/compass/compass-hmc5843.vala b/sensors/compass/hmc5843.vala
index b09b054..393f985 100644
--- a/sensors/compass/compass-hmc5843.vala
+++ b/sensors/compass/hmc5843.vala
@@ -15,7 +15,13 @@
 
 public class HMC5843 : I2CDevice, Compass {
 	public HMC5843(uint8 dev, uint8 addr = 0x1e) throws I2CError {
-			base(dev, addr);
+			setup(dev, addr);
+	}
+
+	public void init(KeyFile cfg) throws KeyFileError, I2CError {
+		var adapter = cfg.get_uint64("HMC5843", "i2c-adapter");
+		var address = cfg.get_uint64("HMC5843", "i2c-address");
+		setup((uint8) adapter, (uint8) address);
 	}
 
 	public void get_data(out uint16 x, out uint16 y, out uint16 z) throws I2CError {
@@ -24,3 +30,8 @@ public class HMC5843 : I2CDevice, Compass {
 		z = get_big_word(0x07);
 	}
 }
+
+public Type register_plugin (Module module) {
+	// types are registered automatically
+	return typeof(HMC5843);
+}
diff --git a/sensors/gyroscope.vala b/sensors/gyroscope.vala
index 165df73..8cdc0cd 100644
--- a/sensors/gyroscope.vala
+++ b/sensors/gyroscope.vala
@@ -14,5 +14,16 @@
  */
 
 public interface Gyroscope : Device {
-	public abstract void get_data(out int16 x, out int16 y, out int16 z) throws Error;
+#if 0
+	double[] rate = {0.0, 0.0, 0.0};
+	int[] zero = {0, 0, 0};
+	long[] sample = {0, 0, 0};
+	double smoothFactor = 1.0;
+	double scaleFactor = 0.0;
+	double heading = 0.0;
+	ulong gyroLastMesuredTime = 0;
+#endif
+	public abstract void init(KeyFile cfg) throws Error;
+	public abstract void get_data(out int16 x, out int16 y, out int16 z) throws I2CError;
+	public abstract int[] rate { get; set; }
 }
diff --git a/sensors/gyroscope/Makefile b/sensors/gyroscope/Makefile
new file mode 100644
index 0000000..f7add97
--- /dev/null
+++ b/sensors/gyroscope/Makefile
@@ -0,0 +1,14 @@
+include ../../config.mk
+
+all: ITG3200.so
+
+clean:
+	rm -f *.c *.so
+
+itg3200.c: itg3200.vala
+	$(VALAC) -g --pkg gmodule-2.0 --pkg posix --pkg linux -C $< ../gyroscope.vala ../../hw/i2c-device.vala ../../hw/device.vala
+
+ITG3200.so: itg3200.c
+	$(CC) -g -shared -fPIC `pkg-config --cflags --libs glib-2.0 gobject-2.0 gmodule-2.0` -o $@ $<
+
+.PHONY: all clean
diff --git a/sensors/gyroscope/gyroscope-itg3200.vala b/sensors/gyroscope/itg3200.vala
index c57aa55..2b7f03e 100644
--- a/sensors/gyroscope/gyroscope-itg3200.vala
+++ b/sensors/gyroscope/itg3200.vala
@@ -14,8 +14,18 @@
  */
 
 public class ITG3200 : I2CDevice, Gyroscope {
+	public int[] rate { get; set; }
+
 	public ITG3200(uint8 dev, uint8 addr = 0x68) throws I2CError {
-			base(dev, addr);
+			setup(dev, addr);
+	}
+
+	public void init(KeyFile cfg) throws KeyFileError, I2CError {
+		rate = new int[3];
+
+		var adapter = cfg.get_uint64("ITG3200", "i2c-adapter");
+		var address = cfg.get_uint64("ITG3200", "i2c-address");
+		setup((uint8) adapter, (uint8) address);
 	}
 
 	public uint8 get_address() throws I2CError {
@@ -46,3 +56,8 @@ public class ITG3200 : I2CDevice, Gyroscope {
 		z = (int16) get_big_word(0x21);
 	}
 }
+
+public Type register_plugin (Module module) {
+	// types are registered automatically
+	return typeof(ITG3200);
+}
diff --git a/test-shell.vala b/test-shell.vala
deleted file mode 100644
index 4e78406..0000000
--- a/test-shell.vala
+++ /dev/null
@@ -1,158 +0,0 @@
-/* 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;
-}
diff --git a/utils.vala b/utils.vala
new file mode 100644
index 0000000..e204548
--- /dev/null
+++ b/utils.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 double radians(double degree) {
+	return degree * (Math.PI/180.0);
+}
+
+public double constrain(double x, double a, double b) {
+	return a > x ? a : b < x ? b : x;
+}
+
+public int constrain_int(int x, int a, int b) {
+	return a > x ? a : b < x ? b : x;
+}