rendered paste body/*
* Analysis C
*/
#include <stdio.h>
#include <math.h>
#include "PitchBlkOut.h"
#define MAXN 10 // used as the max index in alot of the array declarations
#define MAX_ITERATIONS 10
double Pressure26(double alt);
double Temperature26(double alt);
double Celerity26(double temp, double alt);
double Gravity26(double alt);
double convertMilliBarsToKSM(double millibars);
double func(double x[MAXN], int degree, double xVal);
void Search26(int MaxIter, double endBoundConditions[], double Pert[], double Tol[]);
void OneRun(double X[]);
// Runge-Kutta functions
void RK4_System(int n, double h, double *tref, double X[], int nsteps);
void XP_System(int Neq, double T, double Y[], double DY[]);
void output(int k, double T, double Y[]);
// Gauss functions
void gaussSolve(double a[MAXN][MAXN], double b[MAXN], double x[MAXN], int n);
void gauss(double a[MAXN][MAXN], int l[MAXN], int n);
void solve(double a[MAXN][MAXN], double b[MAXN], double x[MAXN], int l[MAXN], int n);
double max(double a, double b);
void header();
struct pair {
double x;
double y;
};
#define MAXSTAGES 10
int numStages;
int PCONTROL=0, TEMP_PCONTROL=0;
int currentStage;
double COT[MAXSTAGES], Thrust[MAXSTAGES], Weight[MAXSTAGES];
double Flow[MAXSTAGES], CrossSection[MAXSTAGES], CD[MAXSTAGES], TI[MAXSTAGES], AZ, PANG, VEND, PEND;
double PLAT,PLONG,HEIGHT;
double Velocity, Gravity, Pressure, Temperature, Celerity, Mach, Q, AF, Acceleration, Range;
double C[3], F[3], G[3]={0.0, 0.0, -1.0};
// double Latitude, Longitude, Altitude, Acceleration, Range;
int main()
{
double iLat, iLong, iHeight;
double iThrust[MAXSTAGES], iWeight[MAXSTAGES], iFlow[MAXSTAGES], iCrossSection[MAXSTAGES], iDrag[MAXSTAGES], iCutOff[MAXSTAGES], iStepSize[MAXSTAGES];
double iVertRise, iPitchAngle, iPitchEnd, iLaunchAzimuth;
double iDesiredValue[3], iPerturbation[3], iTolerance[3];
int i;
header();
// get all the input from stdin
printf("PPCONTROL values:\n");
printf("\tNumber of stages: ");
scanf("%d", &numStages);
printf("\tPrint PPCONTROL: ");
scanf("%d", &TEMP_PCONTROL);
printf("Launch Point\n");
printf("\tLatitude (degrees): ");
scanf("%lf", &iLat);
printf("\tLongitude (degrees): ");
scanf("%lf", &iLong);
printf("\tHeight (meters): ");
scanf("%lf", &iHeight);
for(i=0; i<numStages; i++)
{
printf("The vehicle, Stage %d\n", i+1);
printf("\tThrust (pounds): ");
scanf("%lf", &iThrust[i]);
printf("\tWeight (pounds): ");
scanf("%lf", &iWeight[i]);
printf("\tFlow (pounds/sec): ");
scanf("%lf", &iFlow[i]);
printf("\tCross-sectional area (square feet): ");
scanf("%lf", &iCrossSection[i]);
printf("\tDrag coefficient: ");
scanf("%lf", &iDrag[i]);
printf("\tCut-off time (secs): ");
scanf("%lf", &iCutOff[i]);
printf("\tStep size (secs): ");
scanf("%lf", &iStepSize[i]);
}
printf("Pitch program:\n");
printf("\tVertical rise time (secs): ");
scanf("%lf", &iVertRise);
printf("\tPitch angle (degrees from verticle): ");
scanf("%lf", &iPitchAngle);
printf("\tTime of end of pitch program (secs): ");
scanf("%lf", &iPitchEnd);
printf("\tLaunch azimuth (degrees from north): ");
scanf("%lf", &iLaunchAzimuth);
for (i=0; i<3; i++) {
printf("Search Parameters (%d): Desired Value, Perturbation, Tolerance\n", i);
scanf("%lf %lf %lf", &iDesiredValue[i], &iPerturbation[i], &iTolerance[i]);
}
// print out input values
printf("PPCONTROL values:\n");
printf("\tNumber of stages: %d\n", numStages);
printf("\tPrint PPCONTROL: %d\n", TEMP_PCONTROL);
printf("Launch Point\n");
printf("\tLatitude: %lf\n", iLat);
printf("\tLongitude: %lf\n", iLong);
printf("\tHeight: %lf\n", iHeight);
for(i=0; i<numStages; i++)
{
printf("The vehicle, Stage %d\n", i+1);
printf("\tThrust (pounds): %lf\n", iThrust[i]);
printf("\tWeight (pounds): %lf\n", iWeight[i]);
printf("\tFlow (pounds/sec): %lf\n", iFlow[i]);
printf("\tCross-sectional area (square feet): %lf\n", iCrossSection[i]);
printf("\tDrag coefficient: %lf\n", iDrag[i]);
printf("\tCut-off time (secs): %lf\n", iCutOff[i]);
printf("\tStep size (secs): %lf\n", iStepSize[i]);
}
printf("Pitch program:\n");
printf("\tVertical rise time (secs): %lf\n", iVertRise);
printf("\tPitch angle (degrees from verticle): %lf\n", iPitchAngle);
printf("\tTime of end of pitch program (secs): %lf\n", iPitchEnd);
printf("\tLaunch azimuth (degrees from north): %lf\n", iLaunchAzimuth);
for (i=0; i<3; i++) {
printf("Search Parameters (%d): Desired Value, Perturbation, Tolerance\n", i);
printf("\t%lf %lf %lf\n", iDesiredValue[i], iPerturbation[i], iTolerance[i]);
}
// fix units
PLAT = iLat;
PLONG = iLong;
HEIGHT = iHeight;
for(i=0; i<numStages; i++)
{
Thrust[i] = iThrust[i] * 0.4535924;
Weight[i] = iWeight[i] * 0.04625455;
Flow[i] = iFlow[i] * 0.04625455;
CrossSection[i] = iCrossSection[i] * pow(0.3048,2.0);
CD[i] = iDrag[i];
COT[i] = iCutOff[i];
TI[i] = iStepSize[i];
}
VEND = iVertRise;
PANG = iPitchAngle; // don't convert to radians afterall
PEND = iPitchEnd; // don't convert to radians afterall
AZ = iLaunchAzimuth;
Search26( MAX_ITERATIONS, iDesiredValue, iPerturbation, iTolerance);
printf("Azimuth=%lf, Pitch angle=%lf, Thrust=%lf\n", AZ, PANG, Thrust[0]);
return 0;
}
void Search26(int MaxIter, double endBoundConditions[], double Pert[], double Tol[])
{
double X2[MAXN];
double u[MAXN][MAXN]; // for storing the final values for each pertubation, [pertubation][value]
double standard[MAXN];
double A[MAXN][MAXN];
double B[MAXN];
double indepVars[MAXN]; // for storing original independant vars, Azimuth, PitchAngle, Thrust
double AnsX[MAXN]; // for storing results from gauss
double h=0.0;
int n=1, i=0, j=0;
// initialize everything
for (i=0; i<3; i++)
{
standard[i] = 0.0;
B[i] = 0.0;
//AnsX[i] = 0.0;
for (j = 0; j < MAXN; j++)
{
A[i][j] = 0.0;
}
}
indepVars[0] = AZ;
indepVars[1] = PANG;
indepVars[2] = Thrust[0];
for (i=0; i<MaxIter; i++)
{
// standard run
AZ = indepVars[0];
PANG = indepVars[1];
Thrust[0] = indepVars[2];
OneRun(X2);
standard[0] = X2[0];
standard[1] = X2[1];
standard[2] = X2[2];
//printf("x[0]: %lf\n", standard[0]);
//printf("x[1]: %lf\n", standard[1]);
B[0] = endBoundConditions[0] - standard[0];
B[1] = endBoundConditions[1] - standard[1];
B[2] = endBoundConditions[2] - standard[2];
// determine if were close enough already
if ( fabs(B[0]/endBoundConditions[0])<Tol[0] &&
fabs(B[1]/endBoundConditions[1])<Tol[1] &&
fabs(B[2]/endBoundConditions[2])<Tol[2] )
{
printf("Within tolerance. Breaking. Completed in %d iterations.\n", i+1);
break;
}
// Pertubation 1
AZ = indepVars[0] * (1.0+Pert[0]);
PANG = indepVars[1];
Thrust[0] = indepVars[2];
OneRun(X2);
u[0][0] = X2[0];
u[0][1] = X2[1];
u[0][2] = X2[2];
// Pertubation 2
AZ = indepVars[0];
PANG = indepVars[1] * (1.0+Pert[1]);
Thrust[0] = indepVars[2];
OneRun(X2);
u[1][0] = X2[0];
u[1][1] = X2[1];
u[1][2] = X2[2];
// Pertubation 3
AZ = indepVars[0];
PANG = indepVars[1];
Thrust[0] = indepVars[2] * (1.0+Pert[2]);
OneRun(X2);
u[2][0] = X2[0];
u[2][1] = X2[1];
u[2][2] = X2[2];
A[0][0] = (u[0][0] - standard[0]) / (Pert[0] * indepVars[0]);
A[0][1] = (u[1][0] - standard[0]) / (Pert[1] * indepVars[1]);
A[0][2] = (u[2][0] - standard[0]) / (Pert[2] * indepVars[2]);
A[1][0] = (u[0][1] - standard[1]) / (Pert[0] * indepVars[0]);
A[1][1] = (u[1][1] - standard[1]) / (Pert[1] * indepVars[1]);
A[1][2] = (u[2][1] - standard[1]) / (Pert[2] * indepVars[2]);
A[2][0] = (u[0][2] - standard[2]) / (Pert[0] * indepVars[0]);
A[2][1] = (u[1][2] - standard[2]) / (Pert[1] * indepVars[1]);
A[2][2] = (u[2][2] - standard[2]) / (Pert[2] * indepVars[2]);
gaussSolve(A, B, AnsX, 3);
indepVars[0] += AnsX[0];
indepVars[1] += AnsX[1];
indepVars[2] += AnsX[2];
//printf("G1: %lf G2: %lf\n", X[2], X[3]);
}
// run one more time for user output
PCONTROL = TEMP_PCONTROL;
OneRun(X2);
printf("X=%lf, Y=%lf, Z=%lf\n", X2[0], X2[1], X2[2]);
}
void OneRun(double X[])
{
int i, nsteps;
double time = 0.0;
//double X[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
double Fg,Ft,Fa;
for (i=0; i<7; i++) {
X[i] = 0.0;
}
X[2] = HEIGHT;
/*
F[0]=0; F[1]=0; F[2]=1;
C[0]=0; C[1]=0; C[2]=0;
X[2] = HEIGHT;
Pressure = Pressure26(HEIGHT);
Temperature = Temperature26(HEIGHT);
Celerity = Celerity26(Temperature, HEIGHT);
Gravity = Gravity26(HEIGHT);
Velocity = 1;
Mach = Velocity / Celerity;
Q = 0.7 * Pressure * Mach * Mach;
Ft = Thrust[0]; // thrust
Fg = Weight[0] * Gravity; // gravity
Fa = CD[0] * Q * CrossSection[0]; // drag
Acceleration = (Ft - Fa) / (Gravity * Weight[0]);
*/
for (i=0; i<numStages; i++)
{
currentStage = i;
nsteps = COT[i]/TI[i];
X[6] = Weight[i];
RK4_System(7, TI[i], &time, X, nsteps);
}
}
// Runge-Kutta functions
void RK4_System(int n, double h, double *tref, double X[], int nsteps)
{
int i=0, j=0;
double t;
double Y[n];
double K[4][n];
t=*tref;
if (PCONTROL)
{
output(0, t, X);
}
for (j=1; j<=nsteps; j++)
{
XP_System(n, t, X, K[0]);
for (i=0; i<n; i++)
{
Y[i] = X[i] + 0.5*h*K[0][i];
}
XP_System(n, t+h/2.0, Y, K[1]);
for (i=0; i<n; i++)
{
Y[i] = X[i] + 0.5*h*K[1][i];
}
XP_System(n, t+h/2.0, Y, K[2]);
for (i=0; i<n; i++)
{
Y[i] = X[i] + h*K[2][i];
}
XP_System(n, t+h, Y, K[3]);
for (i=0; i<n; i++)
{
X[i] = X[i] + (1.0/6.0)*h*(K[0][i] + 2*K[1][i] + 2*K[2][i] + K[3][i]);
}
t = t + h;
//Time = Time + h;
if (PCONTROL)
{
output(j, t, X);
}
}
*tref = t;
}
void XP_System(int Neq, double T, double Y[], double DY[])
{
double Fg,Ft,Fa;
Pressure = Pressure26(Y[2]);
Temperature = Temperature26(Y[2]);
Celerity = Celerity26(Temperature, Y[2]);
Gravity = Gravity26(Y[2]);
if ( T==0.0 ) {
Velocity = 1;
} else {
Velocity = sqrt( Y[3]*Y[3] + Y[4]*Y[4] + Y[5]*Y[5] );
}
Mach = Velocity / Celerity;
Q = 0.7 * Pressure * Mach * Mach;
Ft = Thrust[currentStage]; // thrust
Fg = Gravity * Y[6]; // gravity
Fa = CD[currentStage] * Q * CrossSection[currentStage]; // drag
Acceleration = (Ft - Fa) / (Gravity * (Y[6]));
//Range = sqrt( Y[0]*Y[0] + Y[1]*Y[1] + Y[2]*Y[2] );
Range = sqrt( Y[0]*Y[0] + Y[1]*Y[1] )/1852.0;
if( T==0.0 )
{
C[0] = 0.0; C[1] = 0.0; C[2] = 0.0;
}
else
{
C[0] = Y[3] / Velocity;
C[1] = Y[4] / Velocity;
C[2] = Y[5] / Velocity;
}
PITCH(T, AZ, PANG, VEND, PEND, F, C);
DY[0] = Y[3];
DY[1] = Y[4];
DY[2] = Y[5];
DY[3] = (Ft * F[0] + Fg * G[0] - Fa * C[0]) / (Y[6]);
DY[4] = (Ft * F[1] + Fg * G[1] - Fa * C[1]) / (Y[6]);
DY[5] = (Ft * F[2] + Fg * G[2] - Fa * C[2]) / (Y[6]);
DY[6] = -Flow[currentStage];
}
void output(int j, double T, double Y[])
{
double Latitude, Longitude;
int k=0, l=0;
Latitude = ((PLAT*1852.0*60.0) + Y[0])/(1852.0*60.0);
Longitude = ((PLAT*1852.0*60.0) + Y[1])/(1852.0*60.0);
/*
if( T==0.0 )
{
C[0] = 0.0; C[1] = 0.0; C[2] = 0.0;
}
else
{
C[0] = Y[3] / Velocity;
C[1] = Y[4] / Velocity;
C[2] = Y[5] / Velocity;
}*/
BLKOUT(1, PCONTROL, T, Y, C, F, G, PANG, VEND, PEND, AZ,
Flow[currentStage], CrossSection[currentStage], Thrust[currentStage],
Gravity, Pressure, Temperature, Mach, Celerity,
Q, CD[currentStage], AF, Latitude, Longitude, Range, Velocity,
Acceleration, COT[currentStage], TI[currentStage]);
}
// End Runge-Kutta functions
// returns pressure in Kilograms/square meter. requires altitude in meters.
double Pressure26(double alt)
{
double PressureCurveFit1[] = { -9.6666667e-04, 7.8333333e-02, -2.3791667e+00, 3.4291667e+01, -2.5116667e+02, 1.0000000e+03};
double PressureCurveFit2[] = { -2.0833333e-06, 5.2083333e-04, -5.1041667e-02, 2.4729167e+00, -6.1125000e+01, 6.6500000e+02};
alt = alt/1000;
if (alt < 25) {
return convertMilliBarsToKSM( func(PressureCurveFit1, 6, alt) );
//return func(PressureCurveFit1, 5, alt);
} else if (alt < 65) {
return convertMilliBarsToKSM( func(PressureCurveFit2, 6, alt) );
//return func(PressureCurveFit2, 5, alt);
} else if (alt >= 65) {
return 0;
} else {
printf("Error: Pressure(%lf): Can't process invalid altitude.\n", alt);
}
}
// returns temperature in Kalvens. requires altitude in meters.
double Temperature26(double alt)
{
int n=15, i=0;
double y=0;
struct pair table[15];
double TemperatureAry[2][15] = { {0.0, 5000.0, 10000.0, 15000.0, 20000.0, 25000.0, 30000.0, 35000.0, 40000.0, 45000.0, 50000.0, 55000.0, 60000.0, 65000.0, 70000.0},
{290.0, 240.0, 220.0, 225.0, 230.0, 245.0, 255.0, 265.0, 270.0, 275.0, 270.0, 255.0, 235.0, 215.0, 200.0} };
if ( alt>65000.0 ) {
return 180.0;
}
// since my lagrange function used a different format for data, I have to convert
for (i=0; i<n; i++)
{
table[i].x = TemperatureAry[0][i];
table[i].y = TemperatureAry[1][i];
}
lagrange_interpolate(n, table, 4, alt, &y, 0);
return y;
}
// returns speed of sound in meters/second. accepts temperature in Kelvans, altitude in meters.
double Celerity26(double temp, double alt)
{
if (alt >= 65000.0)
{
return 1000.0;
}
else
{
return ( pow(1.4 * 8314.32 / 28.9644 * temp, 0.5) );
}
}
// returns acceleration of gravity, not force. requires altitude in meters.
double Gravity26(double alt)
{
return (398622000000000.0/pow(6378178.0+alt,2.0));
}
// converts pressure in milibars to Kilograms/square meter.
double convertMilliBarsToKSM(double millibars)
{
return (millibars * 1000.0 * 0.010197162);
}
// helper function for curve fit, point evaluation. x is array of polynomial coefficients, xVal is X value to be interprated.
double func(double x[MAXN], int degree, double xVal)
{
int i, power;
double sum = 0.0;
power = degree-1;
for (i=0;i<degree;i++)
{
sum = sum + pow(xVal, power) * x[i];
power--;
}
return sum;
}
int lagrange_interpolate( int n, struct pair table[], int k, double x, double *y, int debug )
{
int i=0, j=0;
int halfk=0, halfk_top=0;
int start_point=0, points_less_x=0, points_greater_x=0;
double l=0, ans=0, denom=0;
// if k > n, then fail.
if (k>n) {
return 0;
}
// if less than floor(k/2) data points to left or right
halfk = k/2; // using an int here is on purpose
halfk_top = k - halfk;
// count the number of data points above and below x.
if (debug) { printf("lagrange_interpolate(): calculating location of x in table.\n"); }
for (i=0; i<n; i++) {
if (table[i].x < x) {
points_less_x++;
} else {
points_greater_x++;
}
}
if (debug>2) { printf("lagrange_interpolate(): points_less_x=%i\tpoints_greater_x=%i.\n", points_less_x, points_greater_x); }
// figure out the starting data point for K points
// start_point is 0 indexed
if (debug) { printf("lagrange_interpolate(): calculating starting data point.\n"); }
if ( halfk > points_less_x ) {
// then use all of them
start_point = 0;
} else if ( halfk_top > points_greater_x ) {
// then use all of those
start_point = n - k;
} else {
start_point = points_less_x - halfk;
}
if (debug) { printf("lagrange_interpolate(): starting interpolation.\n"); }
for (i=0; i<k; i++) {
// calc l
l = 1;
for (j=0; j<k; j++) {
if ( i!=j ) {
if (debug>2) { printf("lagrange_interpolate(): i=%i\tj=%i.\n", (start_point+i), (start_point+j)); }
// double check demoinator is not zero
// even though this should never happen
denom = table[start_point+i].x - table[start_point+j].x;
if ( denom ) {
l = l * ( (x - table[start_point+j].x)/denom );
if (debug>3) { printf("lagrange_interpolate(): calculating l%i\tl=%lf.\n", (i+1), l); }
} else {
if (debug>2) { printf("lagrange_interpolate(): strange, demoninator in l%i is zero.\n", (i+1)); }
}
}
}
ans += table[start_point+i].y * l;
if (debug>2) { printf("lagrange_interpolate(): using data point: y%i=%lf.\n", (start_point+i), table[start_point+i].y ); }
if (debug>2) { printf("lagrange_interpolate(): calculating y\ty=%lf.\n", ans); }
}
*y = ans;
return 1;
}
// Gauss functions
void gaussSolve(double a[MAXN][MAXN], double b[MAXN], double x[MAXN], int n)
{
int l[MAXN];
gauss(a, l, n);
solve(a, b, x, l, n);
}
void gauss(double a[MAXN][MAXN], int l[MAXN], int n)
{
double s[MAXN];
int i,j,k,p;
double r,rmax,smax,xmult;
//printf("n: %i\n", n);
for (i=0; i<n; i++)
{
l[i] = i;
smax = 0;
for (j=0; j<n; j++)
{
//printf("p: %lf\n", fabs(a[i][j]));
smax = max(smax, fabs(a[i][j]));
}
s[i] = smax;
}
for (k=0; k<(n-1); k++)
{
rmax = 0;
for (i=k; i<n; i++)
{
r = fabs( a[ l[i] ][k]/s[ l[i] ] );
if (r>rmax)
{
rmax = r;
j = i;
}
}
p = l[k];
l[k] = l[j];
l[j] = p;
for (i=(k+1); i<n; i++)
{
xmult = a[ l[i] ][k]/a[ l[k] ][k];
a[ l[i] ][k] = xmult;
for(j=(k+1); j<n; j++)
{
a[ l[i] ][j] = a[ l[i] ][j] - xmult*a[ l[k] ][j];
}
}
}
}
void solve(double a[MAXN][MAXN], double b[MAXN], double x[MAXN], int l[MAXN], int n)
{
double sum;
int i,j,k;
for (k=0; k<(n-1); k++)
{
for (i=(k+1); i<n; i++)
{
b[l[i]] = b[l[i]] - a[l[i]][k]*b[l[k]];
}
}
x[n-1] = b[l[n-1]]/a[l[n-1]][n-1];
for (i=(n-2); i>=0; i--)
{
sum = b[l[i]];
for (j=(i+1); j<n; j++)
{
sum = sum - a[l[i]][j]*x[j];
}
x[i] = sum/a[l[i]][i];
}
}
double max(double a, double b)
{
return ( (a>b) ? (a):(b));
}
// End Gauss functions
void header()
{
printf("\nJuan Velasquez NN:26 Numerical Analysis\n\n");
}