initial code import

3 files changed, 345 insertions, 0 deletions

diff --git a/ctrl/flight-control.vala b/ctrl/flight-control.vala
new file mode 100644
index 0000000..00e7b31
--- /dev/null
+++ b/ctrl/flight-control.vala
@@ -0,0 +1,142 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ * Copyright 2012, Ted Carancho
+ *
+ * Ported from AeroQuad
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+public class FlightControl {
+	MotorController motor;
+	FlightMode mode;
+	PID[] pid;
+	Kinematics k;
+
+	public FlightControl() {
+		motor = new AtmostripeMotorController();
+		pid = new PID[PIDEntry.length];
+		k = new Kinematics(0, 0); // TODO: read compass instead of using 0,0
+	}
+
+	/**
+	 * calculateFlightError
+	 *
+	 * 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) {
+		roll = 0; pitch = 0;
+
+		if (mode == FlightMode.ATTITUDE_FLIGHT_MODE) {
+		}
+#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;
+
+		throttle = constrain(throttle,MINCOMMAND,MAXCOMMAND-150);  // limmit throttle to leave some space for motor correction in max throttle manuever
+	}
+#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() {
+		// TODO
+	}
+
+	/**
+	 * processFlightControl
+	 *
+	 * Main flight control processos function
+	 */
+	public void processFlightControl() {
+		double roll, pitch;
+		calculateFlightError(out roll, out pitch);
+		processHeading();
+
+#if 0
+		/* 50Hz tasks */
+		processAltitudeHold();
+		processThrottleCorrection();
+
+		/* if running */
+		applyMotorCommand();
+
+		processMinMaxCommand();
+
+		/* Allows quad to do acrobatics by lowering power to opposite motors during hard manuevers */
+		//processHardManuevers();
+
+		/* If throttle in minimum position, don't apply yaw */
+		if (receiverCommand[THROTTLE] < MINCHECK) {
+			for (byte 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]);
+		}
+
+		/* ESC Calibration */
+		if (motorArmed == OFF) {
+			processCalibrateESC();
+		}
+
+		if (motorArmed == ON && safetyCheck == ON) {
+			writeMotors();
+		}
+#endif
+	}
+}
diff --git a/ctrl/kinematics.vala b/ctrl/kinematics.vala
new file mode 100644
index 0000000..0e90a8f
--- /dev/null
+++ b/ctrl/kinematics.vala
@@ -0,0 +1,148 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ * Copyright 2012, Ted Carancho
+ *
+ * Ported from AeroQuad
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+public class Kinematics {
+	/* quaternion elements representing the estimated orientation */
+	double q0;
+	double q1;
+	double q2;
+	double q3;
+
+	/* scaled integral error */
+	double exInt = 0.0;
+	double eyInt = 0.0;
+	double ezInt = 0.0;
+
+	/* proportional gain governs rate of convergence to accelerometer */
+	double kpAcc = 0.2;
+
+	/* integral gain governs rate of convergence of gyroscope biases */
+	double kiAcc = 0.0005;
+
+	/* proportional gain governs rate of convergence to magnetometer */
+	double kpMag = 2.0;
+	/* integral gain governs rate of convergence of gyroscope biases */
+	double kiMag = 0.005;
+
+	/* half the sample period*/
+	double halfT = 0.0;
+
+	public double[] kinematicsAngle = new double[3];
+	public double[] correctedRateVector = new double[3];
+	public double[] gyroAngle = new double[2];
+
+	public Kinematics(double hdgX, double hdgY) {
+		for (var axis = AXIS.X; axis <= AXIS.Z; axis+=1)
+			kinematicsAngle[axis] = 0;
+		for (var axis = AXIS.X; axis <= AXIS.Y; axis+=1)
+			gyroAngle[axis] = 0;
+
+		double hdg = Math.atan2(hdgY, hdgX);
+		q0 = Math.cos(hdg/2);
+		q1 = 0;
+		q2 = 0;
+		q3 = Math.sin(hdg/2);
+	}
+
+	public void update(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz, double time) {
+		double norm;
+		double hx, hy, hz, bx, bz;
+		double vx, vy, vz, wx, wy, wz;
+		double exAcc, eyAcc, ezAcc;
+		double exMag, eyMag, ezMag;
+		double q0i, q1i, q2i, q3i;
+
+		halfT = time/2;
+
+		/* normalise the accelerometer measurements */
+		norm = Math.sqrt(ax*ax + ay*ay + az*az);
+		ax = ax / norm;
+		ay = ay / norm;
+		az = az / norm;
+
+		/* normalise the magnometer measurements */
+		norm = Math.sqrt(mx*mx + my*my + mz*mz);
+		mx = mx / norm;
+		my = my / norm;
+		mz = mz / norm;
+
+		/* compute reference direction of flux */
+		hx = mx * 2 * (0.5 - q2 * q2 - q3 * q3) + my * 2 * (q1 * q2 - q0 * q3)       + mz * 2 * (q1 * q3 + q0 * q2);
+		hy = mx * 2 * (q1 * q2 + q0 * q3)       + my * 2 * (0.5 - q1 * q1 - q3 * q3) + mz * 2 * (q2 * q3 - q0 * q1);
+		hz = mx * 2 * (q1 * q3 - q0 * q2)       + my * 2 * (q2 * q3 + q0 * q1)       + mz * 2 * (0.5 - q1 * q1 - q2 * q2);
+
+		bx = Math.sqrt((hx*hx) + (hy*hy));
+		bz = hz;
+
+		/* estimate direction of gravity and flux (v and w) */
+		vx = 2 *(q1 * q3 - q0 * q2);
+		vy = 2 *(q0 * q1 + q2 * q3);
+		vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
+
+		wx = bx * 2*(0.5 - q2 * q2 - q3 * q3) + bz * 2 * (q1 * q3 - q0 * q2);
+		wy = bx * 2*(q1 * q2 - q0 * q3)       + bz * 2 * (q0 * q1 + q2 * q3);
+		wz = bx * 2*(q0 * q2 + q1 * q3)       + bz * 2 * (0.5 - q1 * q1 - q2 * q2);
+
+		/* error is sum of cross product between reference direction of fields and direction measured by sensors */
+		exAcc = (vy*az - vz*ay);
+		eyAcc = (vz*ax - vx*az);
+		ezAcc = (vx*ay - vy*ax);
+
+		exMag = (my*wz - mz*wy);
+		eyMag = (mz*wx - mx*wz);
+		ezMag = (mx*wy - my*wx);
+
+		/* integral error scaled integral gain */
+		exInt = exInt + exAcc*kiAcc + exMag*kiMag;
+		eyInt = eyInt + eyAcc*kiAcc + eyMag*kiMag;
+		ezInt = ezInt + ezAcc*kiAcc + ezMag*kiMag;
+
+		/* adjusted gyroscope measurements */
+		correctedRateVector[AXIS.X] = gx = gx + exAcc*kpAcc + exMag*kpMag + exInt;
+		correctedRateVector[AXIS.Y] = gy = gy + eyAcc*kpAcc + eyMag*kpMag + eyInt;
+		correctedRateVector[AXIS.Z] = gz = gz + ezAcc*kpAcc + ezMag*kpMag + ezInt;
+
+		/* integrate quaternion rate and normalise */
+		q0i = (-q1*gx - q2*gy - q3*gz) * halfT;
+		q1i = ( q0*gx + q2*gz - q3*gy) * halfT;
+		q2i = ( q0*gy - q1*gz + q3*gx) * halfT;
+		q3i = ( q0*gz + q1*gy - q2*gx) * halfT;
+		q0 += q0i;
+		q1 += q1i;
+		q2 += q2i;
+		q3 += q3i;
+
+		/* normalise quaternion */
+		norm = Math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
+		q0 = q0 / norm;
+		q1 = q1 / norm;
+		q2 = q2 / norm;
+		q3 = q3 / norm;
+
+		/* save the adjusted gyroscope measurements */
+		correctedRateVector[AXIS.X] = gx;
+		correctedRateVector[AXIS.Y] = gy;
+		correctedRateVector[AXIS.Z] = gz;
+
+		/* calculate euler angles */
+		kinematicsAngle[AXIS.X] = Math.atan2(2 * (q0*q1 + q2*q3), 1 - 2 *(q1*q1 + q2*q2));
+		kinematicsAngle[AXIS.Y] = Math.asin(2 * (q0*q2 - q1*q3));
+		kinematicsAngle[AXIS.Z] = Math.atan2(2 * (q0*q3 + q1*q2), 1 - 2 *(q2*q2 + q3*q3));
+	}
+}
diff --git a/ctrl/pid-controller.vala b/ctrl/pid-controller.vala
new file mode 100644
index 0000000..e77cda5
--- /dev/null
+++ b/ctrl/pid-controller.vala
@@ -0,0 +1,55 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ * Copyright 2012, Ted Carancho
+ *
+ * Ported from AeroQuad
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+public class PID {
+	double P;
+	double I;
+	double D;
+
+	double lastPosition;
+	uint64 previousPIDTime;
+	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();
+		uint64 deltaPIDTime = currentTime - previousPIDTime;
+		previousPIDTime = currentTime;
+
+		/* calculate error */
+		double error = targetPosition - currentPosition;
+		integratedError += error * deltaPIDTime;
+		integratedError = constrain(integratedError, -windupGuard, windupGuard);
+
+		double dTerm = D * (currentPosition - lastPosition) / (deltaPIDTime * 100);
+		lastPosition = currentPosition;
+
+		return P * error + I * integratedError + dTerm;
+	}
+
+	public void clear() {
+		integratedError = 0;
+		previousPIDTime = time_t();
+	}
+}