rendered paste body#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include "PitchBlkOut.h"
#define RK_order 4
#define N_Max 100
static const int Neq = 2;
void XP_System(int Neq, double T, double Y[], double DY[]);
void RK4_System(int n, double h, double T, double x[], int nsteps);
double interpolate(int numberofpairs, double x[], double y[], double bigx, int k);
double product(double x[], double bigx, int i, int upperbound,int lowerbound) ;
double summation(double x[], double y[], int numberofpairs, double bigx, int upperbound, int lowerbound);
double pressure23(double altitude);
double temperature23(double altitude);
double gravity23(double alt);
double celerity23(double temperature);
double max(double a, double b);
void OneRun23();
void output(int j, double T, double Y[]);
void Gauss(int n, double a[10][10], double b[10], double x[10]);
void Search23();
double X[100];
int anykey;
int MaxIter=10;
double G1,G2,G3,Pert1,Pert2,Pert3;
double B1,B2,B3,Tol1,Tol2,Tol3;
double Xmin,Xmax;
int NSUB=20;
double TIME[2000],Ys[2000];
int anykey;
int numstages;
int control;
int stage=1;
//A compute in atmospheric routine
double pressure;
double temperature;
//F compute in the function evaluation part of the integration
double velocity;
double c[3]; //unit vector for velocity
//
double celerity;
double mach;
double q;
double af;
//G compute in gravity routine
double gravity;
double g[3];
//O compute in output routine
double lattitude;
double longitude;
double altitude;
double acceleration;
double range;
//P compute in pitch program routine
double az;
double pang;
double vend; //vertical rise time
double pend;
//
double f[3];
//s adjust values at staging
double COT[2];
double thrust[2];
double weight[2];
double flow[2];
double crosssection[2];
double CD[2];
double TI[2];
//I input quantities
double plat;
double plong;
double height;
//three coefficients for unit vectors
double ft;
double fg;
double fa;
//saved c matrix
/*
double X1final;
double X2final;
double X3final;
*/
void Search23()
{
double sta, stb, stc;
double pert1a, pert1b, pert1c;
double pert2a, pert2b, pert2c;
double pert3a, pert3b, pert3c;
double error1, error2, error3;
int i = 0, j = 0;
double A[10][10], B[10];
//X[10];
double temp, temp2, temp3;
double resultX[10];
double saveAZ, savePA, saveTH;
saveAZ = az;
savePA = pang;
saveTH = thrust[0];
for (i = 0; i < 10; i++)
{
B[i] = 0.0;
X[i] = 0.0;
for (j = 0; j < 10; j++)
A[i][j] = 0.0;
}
//printf("you are here2 MaxIter is %d \n", MaxIter);
for(i = 0; i < MaxIter; i++)
{ /*
G1 =az;
G2 =pang;
G3 =thrust[0];
*/
int p=0;
int q=0;
az = saveAZ;
pang = savePA;
thrust[0] = saveTH;
OneRun23(); //do a regular run
sta = X[0]; //save the regular results
stb = X[1];
stc = X[2];
//printf("sta is %lf stb is %lf stc is %lf",sta,stb,stc);
//printf("B1 is %lf B2 is %lf B3 is %lf",B1,B2,B3);
B[0] = (B1 - sta);
B[1] = (B2 - stb);
B[2] = (B3 - stc);
/*
if (fabs(X[0] - B1) < Tol1 && fabs(X[1] - B2) < Tol2 && fabs(X[2] - B3) < Tol3)
{
printf("Tolerence met. Completed in %d iterations.\n", i+1);
break;
}
*/
// printf ("stc - B3 = %lf\n", fabs(stc-B3));
// printf ("stc - B3 / B3 \n ", fabs(fabs(stc-B3)/fabs(B3));
error1 = (fabs(B1 - sta) / fabs(B1));
error2 = (fabs(B2 - stb) / fabs(B2));
error3 = (fabs(B2 - stc) / fabs(B3));
printf ("erro1 is %lf error2 is %lf error3 is %lf\n tol1 is %lf tol2 is %lf tol3 is %lf" ,error1, error2, error3, Tol1, Tol2, Tol3);
//if (fabs(fabs(B[0])/fabs(B1)) < Tol1 && fabs(fabs(B[1])/fabs(B2)) < Tol2 && fabs(fabs(B[2])/fabs(B3)) < Tol3) // check for tolerance
//if ((error1 < Tol1) && (error2 < Tol2) && (error3 < Tol3))
if (fabs(fabs(B[0])/fabs(B1)) < Tol1 && fabs(fabs(B[1])/fabs(B2)) < Tol2 && fabs(fabs(B[2])/fabs(B3)) < Tol3) // check for tolerance
{
printf("Tolerence met. Completed in %d iterations.\n", i+1);
break;
}
//change AZ
az = ( saveAZ * (Pert1 +1));
G1 = az;
//restore other 2
pang=savePA;
thrust[0]=saveTH;
OneRun23(); //perturbation run 1
pert1a = X[0];
pert1b = X[1];
pert1c = X[2];
//change PANG
pang = ( savePA * (Pert2 +1));
G2 = pang;
//restore other 2
az=saveAZ;
thrust[0]=saveTH;
OneRun23(); //perturbation run 2
pert2a = X[0];
pert2b = X[1];
pert2c = X[2];
//change thrust
thrust[0] = ( saveTH * (Pert3 +1));
G3 = thrust[0];
//restore other 2
az = saveAZ;
pang=savePA;
OneRun23(); //perturbation run 3
pert3a = X[0];
pert3b = X[1];
pert3c = X[2];
/*
G1 = saveAZ;
G2 = savePA;
G3 = saveTH;
*/
A[0][0] = (pert1a - sta) / (Pert1 * G1);
A[0][1] = (pert2a - sta) / (Pert1 * G2);
A[0][2] = (pert3a - sta) / (Pert1 * G3);
A[1][0] = (pert1b - stb) / (Pert2 * G1);
A[1][1] = (pert2b - stb) / (Pert2 * G2);
A[1][2] = (pert3b - stb) / (Pert2 * G3);
A[2][0]= (pert1c - stc) / (Pert3 * G1);
A[2][1]= (pert2c - stc) / (Pert3 * G2);
A[2][2]= (pert3c - stc) / (Pert3 * G3);
/*
for (p =0; p <3; p++)
{
for (q=0; q<3; q++)
{
//printf(" %lf ", A[p][q]);
}
//printf("\n");
}
*/
printf("Azimuth=%lf, Pitch angle=%lf, Thrust=%lf\n", az, pang, thrust[0]);
printf("X[0] = %lf, X[1] = %lf, X[2] = %lf\n", X[0], X[1], X[2]); // feed back
X[0] = 0;
X[1] = 0;
X[2] = 0;
Gauss(3, A, B, resultX);
saveAZ += resultX[0];
savePA += resultX[1];
saveTH += resultX[2];
/*
az += resultX[0];
pang += resultX[1];
thrust[0] += resultX[2];
*/
}
}
void OneRun23()
{
int nsteps;
int tempsteps;
int i=0;
double y[7];
g[0] = 0.0;
g[1] = 0.0;
g[2] = -1.0;
X[0] =0.0;
X[1] =0.0;
X[2] = height;
X[3] =0.0;
X[4] =0.0;
X[5] =0.0;
X[6] = weight[0];
velocity =1.0;
acceleration =2.0;
temperature = temperature23(height);
celerity = celerity23(temperature);
gravity = gravity23(height);
mach = velocity/celerity;
pressure = pressure23(height);
q = 0.7 * pressure * (mach * mach);
f[2] =1.0;
for (i = 0; i < numstages; i++)
{
stage =i;
nsteps = (int) COT[i] / TI[i];
y[6] = weight[i];
//printf("you are here\n");
if (i == 0)
{
//printf("runing if i =0\n");
RK4_System(7, TI[i], 0, X, nsteps);
}
else
{
//printf("runing else\n");
RK4_System(7, TI[i], 100, X, ( (int) ((COT[i]-COT[i-1]) / TI[i]) ) );
}
}
}
void XP_System(int Neq, double T, double Y[], double DY[])
{
int i;
double mass;
temperature = temperature23(Y[2]);
celerity = celerity23(temperature);
velocity = sqrt( (Y[3]*Y[3]) + (Y[4]*Y[4]) + (Y[5]*Y[5]) );
gravity = gravity23(Y[2]);
mach = velocity / celerity;
//printf("\n\n pressure is currently %lf sending y2 to pressure. y2 is %lf \n\n", pressure, Y[2]);
pressure = pressure23(Y[2]);
//printf("\n\n pressure is currently %lf \n\n", pressure);
q = 0.7 * pressure * (mach * mach);
mass = weight[stage] - flow[stage] * T;
ft = thrust[stage];
fg = mass*gravity;
fa = CD[stage] * q * crosssection[stage];
acceleration = (ft - fa) / (9.80655 * mass);
af = fa;
if (T!=0)
{
c[0]=Y[3]/velocity;
c[1]=Y[4]/velocity;
c[2]=Y[5]/velocity;
}
else
{
c[0] = 0.0;
c[1] = 0.0;
c[2] = 0.0;
}
//printf("pressure = %lf, temperature = %lf, celerity = %lf, gravity = %lf\n", pressure, temperature, celerity, gravity);
PITCH(T, az, pang, vend, pend, f, c);
DY[0] = Y[3];
DY[1] = Y[4];
DY[2] = Y[5];
/*
DY[3] = ((thrust[stage] * f[0]) - (fa * c[0])) / Y[6];
DY[4] = ((thrust[stage] * f[1]) - (fa * c[1])) / Y[6];
DY[5] = ((thrust[stage] * f[2]) - g[2] - (fa * c[2])) / Y[6];
*/
DY[3] = (ft * f[0] + fg * g[0] - fa * c[0]) / mass;
DY[4] = (ft * f[1] + fg * g[1] - fa * c[1]) / mass;
DY[5] = (ft * f[2] + fg * g[2] - fa * c[2]) / mass;
DY[6] = -(flow[stage]);
}
void RK4_System(int n, double h, double T, double x[], int nsteps)
{
double Y[n];
double K[4][n];
int i = 0;
int j = 0;
//output(0, T, x);
//scanf("%d",&anykey);
for(j = 1; j <= nsteps; j++)
{
//printf("step %d \n", 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 + 0.5 * h, Y, K[1]);
for(i = 0; i < n; i++)
{
Y[i] = x[i] + 0.5 * h * K[1][i];
}
XP_System(n, T + 0.5 * h, 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+=h;
// output(j, T, x);
}
}
double product(double x[], double bigx, int i, int upperbound,int lowerbound)
{
double result = 1;
int j = 0;
for(j=lowerbound; j<=upperbound; j++)
{
if (j!=i)
{
result = (result) * ((bigx - x[j]) / (x[i]- x[j]));
}
}
return result;
}
double summation(double x[], double y[], int numberofpairs, double bigx, int upperbound, int lowerbound)
{
double sum = 0;
int i=lowerbound;
numberofpairs=upperbound;
/*
printf("inside summation\n");
printf("bigx is %lf\n", bigx);
printf("upperbound is is %d lowerbound is %d\n", upperbound, lowerbound);
printf("npair is %d\n", numberofpairs); */
for(i=lowerbound; i<=upperbound; i++)
{
sum+=(y[i] * product(x,bigx,i,upperbound,lowerbound));
}
return sum;
}
double interpolate(int numberofpairs, double x[], double y[], double bigx, int k)
{
int i=0,j=0,width=0;
int upperbound=0,lowerbound = 0;
double fx=0;
/*
printf("bigx is %lf\n", bigx);
printf("k is %d\n", k);
printf("npair is %d\n", numberofpairs);
*/
for (i=0;i<numberofpairs;i++)
{
if (x[i]>bigx)
{
lowerbound=i-1;
upperbound=i;
break;
}
}
width = (k/2) - 1;
upperbound+=width;
lowerbound-=width;
if (lowerbound < 0)
{
upperbound = (abs(lowerbound)+upperbound);
lowerbound = 0;
}
else
{
if(upperbound > numberofpairs)
{
lowerbound=lowerbound-(abs(upperbound-numberofpairs));
upperbound=numberofpairs;
}
}
fx = summation(x,y,numberofpairs,bigx,upperbound,lowerbound);
//printf("fx is %lf\n", fx);
return fx;
}
double temperature23(double altitude)
{
double tempalt;
double X[20];
double Y[20];
double temp = 0;
X[0]=0.0, Y[0]=290.0;
X[1]=5000.0, Y[1]=240.0;
X[2]=10000.0, Y[2]=220.0;
X[3]=15000.0, Y[3]=225.0;
X[4]=20000.0, Y[4]=230.0;
X[5]=25000.0, Y[5]=245.0;
X[6]=30000.0, Y[6]=255.0;
X[7]=35000.0, Y[7]=265.0;
X[8]=40000.0, Y[8]=270.0;
X[9]=45000.0, Y[9]=275.0;
X[10]=50000.0, Y[10]=270.0;
X[11]=55000.0, Y[11]=255.0;
X[12]=60000.0, Y[12]=235.0;
X[13]=65000.0, Y[13]=215.0;
X[14]=70000.0, Y[14]=200.0;
tempalt = altitude;
temp= interpolate(15,X,Y,tempalt,4);
if (altitude >= 65000)
{
temp = 180;
}
return temp;
}
double pressure23(double alt)
{
double press =0.0;
double X[6];
//X = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
double results[6];
int i =0;
double tempsum=0;
alt = alt/1000;
if (alt > 65)
{
press =0;
}
else
{
if (alt > 25)
{ //we got these values by running gauss....
/* X[0] = -0.000002;
X[1] = 0.000521;
X[2] = -0.051042;
X[3] = 2.472917;
X[4] = -61.125000;
X[5] = 665.000000;
*/
X[0] = -2.0833e-06;
X[1] = 5.2083e-04;
X[2] = -5.1042e-02;
X[3] = 2.4729;
X[4] = -6.1125e+01;
X[5] = 6.6500e+02;
}
else
{
/*
X[0] = -0.000967;
X[1] = 0.078333;
X[2] = -2.379167;
X[3] = 34.291670;
X[4] = -251.166700;
X[5] = 1000.000000;
*/
X[0] = -9.6667e-04;
X[1] = 7.8333e-02;
X[2] = -2.3792;
X[3] = 3.4292e+01;
X[4] = -2.5117e+02;
X[5] = 1000;
}
results[0] = (X[0] * (pow ( alt, 5)) );
results[1] = (X[1] * (pow ( alt, 4)) );
results[2] = (X[2] * (pow ( alt, 3)) );
results[3] = (X[3] * (pow ( alt, 2)) );
results[4] = (X[4] * (pow ( alt, 1)) );
results[5] = (X[5]);
/*
printf("%lf * %lf ^ 5 = %lf\n", X[0], alt, results[0]);
printf("%lf * %lf ^ 4 = %lf\n", X[1], alt, results[1]);
printf("%lf * %lf ^ 3 = %lf\n", X[2], alt, results[2]);
printf("%lf * %lf ^ 2 = %lf\n", X[3], alt, results[3]);
printf("%lf * %lf ^ 1 = %lf\n", X[4], alt, results[4]);
printf("%lf * %lf ^ 0 = %lf\n", X[5], alt, results[5]);
*/
press = results[0] + results[1] + results[2] + results[3] + results[4] + results[5];
}
return press * 1000 * .010197162;
}
void output(int j, double T, double Y[])
{
double chglongt, chglatt;
double mass;
temperature = temperature23(Y[2]);
celerity = celerity23(temperature);
velocity = sqrt( (Y[3]*Y[3]) + (Y[4]*Y[4]) + (Y[5]*Y[5]) );
gravity = gravity23(Y[2]);
mach = velocity / celerity;
pressure = pressure23(Y[2]);
q = 0.7 * pressure * (mach * mach);
mass = weight[stage] - flow[stage] * T;
ft = thrust[stage];
fg = mass*gravity;
fa = CD[stage] * q * crosssection[stage];
acceleration = (ft - fa) / (9.80655 * mass);
chglatt = (fabs(Y[0]) /(1852.0 * 60.0)) + lattitude;
chglongt = (fabs(Y[1]) /(1852.0 * 60.0)) + longitude;
BLKOUT(1, control, T, Y, c, f, g, pang, vend, pend, az, -flow[stage], crosssection[stage], thrust[stage], gravity, pressure,
temperature, mach, celerity, q, CD[stage], fa, chglatt, chglongt,
sqrt(pow(Y[0], 2.0) + pow(Y[1], 2.0))/1852.0, velocity, acceleration, COT[stage], TI[stage]);
}
double gravity23(double alt)
{
return (3.98622e+14) / (pow(((6.378178e+06) + (alt)),2));
}
double celerity23(double temperature)
{
if (temperature == 180)
{
return 1000;
}
else
{
return sqrt(((1.4)*((8.31432e+03) / (28.9644)) * temperature));
}
}
double max(double a, double b)
{
if(a > b)
{
return a;
}
else
{
return b;
}
}
void Gauss(int n, double a[10][10], double b[10], double x[10])
{
int I[10];
double s[10];
int i = 0;
int j = 0;
int k = 0;
double r;
double rmax = 0;
double smax = 0;
double xmult = 0;
double temp = 0;
double sum = 0;
for(i=0; i < n; i++)
{
I[i] = i;
smax = 0;
for(j=0; j<n; 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[I[i]][k] / s[I[i]]);
if(r > rmax)
{
rmax = r;
j = i;
}
}
temp = I[j];
I[j] = I[k];
I[k] = temp;
for(i = k + 1; i<n; i++)
{
xmult = a[I[i]][k] / a[I[k]][k];
a[I[i]][k] = xmult;
for(j = k + 1; j < n; j++)
{
a[I[i]][j] = a[I[i]][j] - (xmult)*(a[I[k]][j]);
}
}
}
for(i = 0; i < 4 ; i++)
x[i] = 0;
i = k = sum = 0;
for(k=0; k<n-1; k++)
{
for(i=k+1; i<n; i++)
{
b[I[i]] = b[I[i]] - a[I[i]][k] * b[I[k]];
}
}
x[n-1] = b[I[n-1]] / a[I[n-1]][n-1];
for(i = n-2; i >=0; i--)
{
sum = b[I[i]];
for(j=i+1; j<n; j++)
{
sum = sum - a[I[i]][j] *x[j];
}
x[i] = sum / a[I[i]][i];
}
}
int main()
{
int i;
double testalt=0.0;
double testpress=0.0;
//gather input section==================================================================
/*
printf("gotta test the pressure enter an altitude in KM\n");
scanf("%lf",&testalt);
testpress = pressure23(testalt);
printf("the pressure is %lf \n", testpress);
*/
printf("enter the number of stages\n");
scanf("%d",&numstages);
printf("enter control variable\n");
scanf("%d",&control);
printf("enter lattitude\n");
scanf("%lf",&lattitude);
printf("enter longitude\n");
scanf("%lf",&longitude);
printf("enter height\n");
scanf("%lf",&height);
//loop twice
for(i=0;i<2;i++)
{
printf("enter thrust\n");
scanf("%lf",&thrust[i]);
thrust[i] = thrust[i] * 0.4535924;
printf("enter weight\n");
scanf("%lf",&weight[i]);
weight[i] = weight[i] * .04625455;
printf("enter flow rate\n");
scanf("%lf",&flow[i]);
flow[i] = flow[i] * .04625455;
//flow[i] = (flow[i] * -1);
printf("enter cross sectional area\n");
scanf("%lf",&crosssection[i]);
crosssection[i] = crosssection[i] * (0.3048 * 0.3048);
printf("enter drag coefficient\n");
scanf("%lf",&CD[i]);
printf("enter cut off time\n");
scanf("%lf",&COT[i]);
printf("enter integration step size\n");
scanf("%lf",&TI[i]);
}
printf("enter vertical rise time\n");
scanf("%lf",&vend);
printf("enter pitch angle\n");
scanf("%lf",&pang);
printf("enter time of end of pitch program\n");
scanf("%lf",&pend);
printf("enter launch azimuth (in degrees from north)\n");
scanf("%lf",&az);
c[0]=0.0;
c[1]=0.0;
c[2]=0.0;
G1 =az;
G2 =pang;
G3 =thrust[0];
scanf("\n enter B1 \t");
scanf("%lf",&B1);
scanf("\n enter pert1 \t");
scanf("%lf",&Pert1);
scanf("\n enter Tol1 \t");
scanf("%lf",&Tol1);
scanf("\n enter B2 \t");
scanf("%lf",&B2);
scanf("\n enter pert2 \t");
scanf("%lf",&Pert2);
scanf("\n enter Tol2 \t");
scanf("%lf",&Tol2);
scanf("\n enter B3 \t");
scanf("%lf",&B3);
scanf("\n enter pert3 \t");
scanf("%lf",&Pert3);
scanf("\n enter Tol3 \t");
scanf("%lf",&Tol3);
//ECHO input section==================================================================
printf("\n echo\n");
printf("\nenter the number of stages\t");
printf("%d",numstages);
printf("\nenter control variable\t");
printf("%d",control);
printf("\nenter lattitude\t");
printf("%lf",lattitude);
printf("\nenter longitude\t");
printf("%lf",longitude);
printf("\nenter height\t");
printf("%lf",height);
//loop twice
for(i=0;i<2;i++)
{
printf("\nenter thrust\t");
printf("%lf",thrust[i]);
printf("\nenter weight\t");
printf("%lf",weight[i]);
printf("\nenter flow rate\t");
printf("%lf",flow[i]);
printf("\nenter cross sectional area\t");
printf("%lf",crosssection[i]);
printf("\nenter drag coefficient\t");
printf("%lf",CD[i]);
printf("\nenter cut off time\t");
printf("%lf",COT[i]);
printf("\nenter integration step size \t");
printf("%lf",TI[i]);
}
printf("\nenter vertical rise time \t");
printf("%lf",vend);
printf("\nenter pitch angle \t");
printf("%lf",pang);
printf("\nenter time of end of pitch program \t");
printf("%lf",pend);
printf("\nenter launch azimuth (in degrees from north) \t");
printf("%lf",az);
printf("\n enter b1 \t");
printf("%lf",B1);
printf("\n enter pert1 \t");
printf("%lf",Pert1);
printf("\n enter Tol1 \t");
printf("%lf",Tol1);
printf("\n enter B2 \t");
printf("%lf",B2);
printf("\n enter pert2 \t");
printf("%lf",Pert2);
printf("\n enter Tol2 \t");
printf("%lf",Tol2);
printf("\n enter B3 \t");
printf("%lf",B3);
printf("\n enter pert3 \t");
printf("%lf",Pert3);
printf("\n enter Tol3 \t");
printf("%lf",Tol3);
//=========================================================================
Search23();
printf("search completed\n final answer for Azimuth, Pich angle and Thrust is\n");
printf("Azimuth=%lf, Pitch angle=%lf, Thrust=%lf\n", az, pang, thrust[0]);
/*
printf("Azimuth=%lf, Pitch angle=%lf, Thrust=%lf\n", az, pang, thrust[0]);
printf("X[0] = %lf, X[1] = %lf, X[2] = %lf\n", X[0], X[1], X[2]); // feed back
*/
return 0;
}