diff options
35 files changed, 620 insertions, 253 deletions
@@ -1,3 +1,4 @@ -*.c +microcopterd *.so -test-shell +*.c +microcopterd @@ -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; +} |