diff options
Diffstat (limited to 'ctrl')
-rw-r--r-- | ctrl/copter-model.vala | 3 | ||||
-rw-r--r-- | ctrl/flight-control.vala | 115 | ||||
-rw-r--r-- | ctrl/models/Makefile | 14 | ||||
-rw-r--r-- | ctrl/models/hexacopter.vala (renamed from ctrl/models/hexa.vala) | 9 | ||||
-rw-r--r-- | ctrl/pid-controller.vala | 4 |
5 files changed, 69 insertions, 76 deletions
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(); |