diff options
| -rw-r--r-- | Makefile | 2 | ||||
| -rw-r--r-- | ctrl/copter-model.vala | 26 | ||||
| -rw-r--r-- | ctrl/models/hexa.vala | 64 |
3 files changed, 91 insertions, 1 deletions
@@ -1,5 +1,5 @@ test-shell: actuators/*.vala actuators/*/*.vala sensors/*.vala sensors/*/*.vala hw/*.vala \ - ctrl/*.vala test-shell.vala enums.vala + ctrl/*.vala ctrl/*/*.vala test-shell.vala enums.vala valac -o $@ --pkg posix --pkg linux --pkg readline -X -lreadline -X -lm $^ clean: diff --git a/ctrl/copter-model.vala b/ctrl/copter-model.vala new file mode 100644 index 0000000..09a780d --- /dev/null +++ b/ctrl/copter-model.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 abstract class CopterModel { + public abstract uint8 get_motor_amount(); + + public abstract void set_max(uint8 motor, int val); + public abstract int get_max(uint8 motor); + + public abstract void set_min(uint8 motor, int val); + public abstract int get_min(uint8 motor); + + public abstract int[] get_motor_data(int throttle, int motorAxisCommandYaw, int motorAxisCommandRoll, int motorAxisCommandPitch); +} diff --git a/ctrl/models/hexa.vala b/ctrl/models/hexa.vala new file mode 100644 index 0000000..176d3d6 --- /dev/null +++ b/ctrl/models/hexa.vala @@ -0,0 +1,64 @@ +/* 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 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; + } +} |