1 files changed, 73 insertions, 0 deletions
diff --git a/ctrl/models/hexacopter.vala b/ctrl/models/hexacopter.vala
new file mode 100644
index 0000000..9462bc8
--- /dev/null
+++ b/ctrl/models/hexacopter.vala
@@ -0,0 +1,73 @@
+/* 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 Hexacopter : CopterModel {
+	int[] min = new int[6];
+	int[] max = new int[6];
+	int[] cmd = new int[6];
+
+	const int YAW_DIRECTION = 1;
+
+	private enum Position {
+		FRONT,
+		FRONT_RIGHT,
+		REAR_RIGHT,
+		REAR,
+		REAR_LEFT,
+		FRONT_LEFT,
+	}
+
+	public override void init(KeyFile cfg) throws Error {
+		/* No Config */
+	}
+
+	public override uint8 get_motor_amount() {
+		return 6;
+	}
+
+	public override void set_max(uint8 motor, int val) {
+		max[motor] = val;
+	}
+
+	public override int get_max(uint8 motor) {
+		return max[motor];
+	}
+
+	public override void set_min(uint8 motor, int val) {
+		min[motor] = val;
+	}
+
+	public override int get_min(uint8 motor) {
+		return min[motor];
+	}
+
+	public override int[] get_motor_data(int throttle, int motorAxisCommandYaw, int motorAxisCommandRoll, int motorAxisCommandPitch) {
+		int throttleCorrection = (motorAxisCommandYaw*3/6).abs();
+
+		cmd[Position.FRONT]       = (throttle - throttleCorrection) + motorAxisCommandRoll/2 - motorAxisCommandPitch/2 - (YAW_DIRECTION * motorAxisCommandYaw);
+		cmd[Position.FRONT_RIGHT] = (throttle - throttleCorrection) - motorAxisCommandRoll/2 + motorAxisCommandPitch/2 + (YAW_DIRECTION * motorAxisCommandYaw);
+		cmd[Position.REAR_RIGHT]  = (throttle - throttleCorrection) - motorAxisCommandRoll/2 - motorAxisCommandPitch/2 - (YAW_DIRECTION * motorAxisCommandYaw);
+		cmd[Position.REAR]        = (throttle - throttleCorrection) + motorAxisCommandRoll/2 + motorAxisCommandPitch/2 + (YAW_DIRECTION * motorAxisCommandYaw);
+		cmd[Position.REAR_LEFT]   = (throttle - throttleCorrection)                          - motorAxisCommandPitch   + (YAW_DIRECTION * motorAxisCommandYaw);
+		cmd[Position.FRONT_LEFT]  = (throttle - throttleCorrection)                          + motorAxisCommandPitch   - (YAW_DIRECTION * motorAxisCommandYaw);
+
+		return cmd;
+	}
+}
+
+public Type register_plugin (Module module) {
+	// types are registered automatically
+	return typeof(Hexacopter);
+}