From dfbb403fcd4d3d6ceec22d2e70091686c53606f3 Mon Sep 17 00:00:00 2001 From: Sebastian Reichel Date: Thu, 7 Jun 2012 19:54:38 +0200 Subject: initial code import --- ctrl/flight-control.vala | 142 +++++++++++++++++++++++++++++++++++++++++++++ ctrl/kinematics.vala | 148 +++++++++++++++++++++++++++++++++++++++++++++++ ctrl/pid-controller.vala | 55 ++++++++++++++++++ 3 files changed, 345 insertions(+) create mode 100644 ctrl/flight-control.vala create mode 100644 ctrl/kinematics.vala create mode 100644 ctrl/pid-controller.vala (limited to 'ctrl') 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 + * 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 . + */ + +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 + * 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 . + */ + +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 + * 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 . + */ + +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(); + } +} -- cgit v1.2.3