From 8a08b9dce9cfc161494ddb07a1c979c7796780ff Mon Sep 17 00:00:00 2001 From: Sebastian Reichel Date: Fri, 15 Jun 2012 22:56:00 +0200 Subject: restructure code it is now loading its components from plugins --- ctrl/copter-model.vala | 3 +- ctrl/flight-control.vala | 115 +++++++++++++++++--------------------------- ctrl/models/Makefile | 14 ++++++ ctrl/models/hexa.vala | 64 ------------------------ ctrl/models/hexacopter.vala | 73 ++++++++++++++++++++++++++++ ctrl/pid-controller.vala | 4 -- 6 files changed, 133 insertions(+), 140 deletions(-) create mode 100644 ctrl/models/Makefile delete mode 100644 ctrl/models/hexa.vala create mode 100644 ctrl/models/hexacopter.vala (limited to 'ctrl') 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 (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 - * - * 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; - } -} 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 + * + * 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); +} 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(); -- cgit v1.2.3