summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
authorSebastian Reichel <sre@ring0.de>2012-06-15 22:56:00 +0200
committerSebastian Reichel <sre@ring0.de>2012-06-15 22:56:00 +0200
commit8a08b9dce9cfc161494ddb07a1c979c7796780ff (patch)
treee86e8a83212c617da44e1b9aed480fe6b50374af
parent626580d3a16d01f3ba5481d291c4c0c844ef4c46 (diff)
downloadmicrocopterd-8a08b9dce9cfc161494ddb07a1c979c7796780ff.tar.bz2
restructure code
it is now loading its components from plugins
-rw-r--r--.gitignore5
-rw-r--r--Makefile29
-rw-r--r--actuators/blinkm.vala6
-rw-r--r--actuators/motor-controller.vala6
-rw-r--r--actuators/motor/Makefile14
-rw-r--r--actuators/motor/atmostripe.vala19
-rw-r--r--config.mk2
-rw-r--r--config.vala114
-rw-r--r--ctrl/copter-model.vala3
-rw-r--r--ctrl/flight-control.vala115
-rw-r--r--ctrl/models/Makefile14
-rw-r--r--ctrl/models/hexacopter.vala (renamed from ctrl/models/hexa.vala)9
-rw-r--r--ctrl/pid-controller.vala4
-rw-r--r--hw/device.vala2
-rw-r--r--hw/i2c-device.vala2
-rw-r--r--main.vala63
-rw-r--r--microcopter.cfg31
-rw-r--r--plugin-loader.vala49
-rw-r--r--receiver.vala21
-rw-r--r--receiver/Makefile14
-rw-r--r--receiver/PS3Pad.vala38
-rw-r--r--sensors/accelerometer.vala1
-rw-r--r--sensors/accelerometer/Makefile14
-rw-r--r--sensors/accelerometer/adxl345.vala (renamed from sensors/accelerometer/accelerometer-adxl345.vala)13
-rw-r--r--sensors/barometer.vala1
-rw-r--r--sensors/barometer/Makefile14
-rw-r--r--sensors/barometer/bmp085.vala (renamed from sensors/barometer/barometer-bmp085.vala)14
-rw-r--r--sensors/compass.vala1
-rw-r--r--sensors/compass/Makefile14
-rw-r--r--sensors/compass/hmc5843.vala (renamed from sensors/compass/compass-hmc5843.vala)13
-rw-r--r--sensors/gyroscope.vala13
-rw-r--r--sensors/gyroscope/Makefile14
-rw-r--r--sensors/gyroscope/itg3200.vala (renamed from sensors/gyroscope/gyroscope-itg3200.vala)17
-rw-r--r--test-shell.vala158
-rw-r--r--utils.vala26
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;
+}