rendered paste body#include "stdio.h"
#include "math.h"
#include "PitchBlkOut.h"
int NumStages; //Total # of stages
int CONTROL; //Print verbosity 0-2
int STAGE; //Stage we are on
int VEHICLE = 1;
int i,j,k;
double Latitude,Longitude,Altitude,Acc, TIME,COT[5],THRUST[5],WEIGHT[5],FLOW[5],XSEC[5],CD[5], TIN[5],AZ,PANG,VEND,PEND;
double PLAT,PLONG,HEIGHT, Velocity,Gravity,Pressure,Temperature,Celerity,Mach,Q,AF;
double C[3] = {0, 0, 0};
double G[3] = {0, 0, -1};
double F[3] = {0, 0, 0};
double func(double x[20], int degree, double xVal);
void RANDOUT(int V1, int V2,
double V3, double AR1[3], double AR2[3],
double AR3[3], double AR4[3],
double V4, double V5, double V6, double V7,
double V8, double V9, double V10,
double V11, double V12, double V13, double V14,
double V15, double V16, double V17, double V18,
double V19, double V20, double V21, double V22,
double V23, double V24, double V25);
// Finds the product
double lgproduct( double x[], int n , double X, int i, int upperBound,int lowerBound)
{
double product = 1; // Initial Product Value
int j = 0;
for(j = lowerBound; j <= upperBound ; j++)
{
if (j != i)
{
product = (product) * ((X - x[j])/(x[i]- x[j])); // Calculate the product
}
}
return product;
}// For calculating the sum of products
double lgsummation(double x[], double y[], int n, double X, int upperBound, int lowerBound)
{
double summation = 0;
int i = lowerBound;
n = upperBound;
for(i = lowerBound; i <= upperBound; i++)
{
summation = summation + ( y[i] * lgproduct(x,n,X,i,upperBound,lowerBound));
}
return summation;
}
double interpolate(double x[], double y[], int k, double X, int n)
{
int i = 0;
int lowerBound = 0;
int upperBound = 0;
int range = 0;
double fX = 0;
// Finding the upper and lower bounds according to K
for (i = 0; i < n; i++)
{
if (x[i] > X)
{
lowerBound = i-1;
upperBound = i;
break;
}
}
range = k/2-1;
upperBound = upperBound + range;
lowerBound = lowerBound - range;
if (lowerBound < 0)
{
upperBound = abs(lowerBound)+upperBound;
lowerBound = 0;
}
else if (upperBound > n)
{
lowerBound = lowerBound - (abs(upperBound - n));
upperBound = n;
}
fX = lgsummation(x,y,n,X,upperBound,lowerBound);
return fX;
}
double Pressure15(double alt)
{
double A[6];
alt = alt/1000.0;
if (alt <= 25)
{
A[0] = -9.6667e-04;
A[1] = 7.8333e-02;
A[2] = -2.3792;
A[3] = 3.4292e+01;
A[4] = -2.5117e+02;
A[5] = 1000;
return (A[0]*pow(alt,5) + A[1]*pow(alt,4) + A[2]*pow(alt,3) + A[3]*pow(alt,2) + A[4]*pow(alt,1) + A[5])*1000*.010197162;
}
else if(alt <= 65)
{
A[0] = -2.0833e-06;
A[1] = 5.2083e-04;
A[2] = -5.1042e-02;
A[3] = 2.4729;
A[4] = -6.1125e+01;
A[5] = 6.6500e+02;
return (A[0]*pow(alt,5) + A[1]*pow(alt,4) + A[2]*pow(alt,3) + A[3]*pow(alt,2) + A[4]*pow(alt,1) + A[5])*1000*.010197162;
}
else
{
return 0;
}
}
double Temperature15(double alt)
{
double A[15];
double T[15];
alt = alt/1000.0;
A[0] = 0.0;
A[1] = 5.0;
A[2] = 10.0;
A[3] = 15;
A[4] = 20;
A[5] = 25;
A[6] = 30;
A[7] = 35;
A[8] = 40;
A[9] = 45;
A[10] = 50;
A[11] = 55;
A[12] = 60;
A[13] = 65;
A[14] = 70;
T[0] = 290;
T[1] = 240;
T[2] = 220;
T[3] = 225;
T[4] = 230;
T[5] = 245;
T[6] = 255;
T[7] = 265;
T[8] = 270;
T[9] = 275;
T[10] = 270;
T[11] = 255;
T[12] = 235;
T[13] = 215;
T[14] = 200;
if (alt <= 65)
{
return interpolate(A,T,4,alt,15); //Computer Interpolation
}
else
{
return 180;
}
}
double Celerity15(double temp)
{
return sqrt(((1.4)*((8.31432e+03)/(28.9644))*temp));
}
double Gravity15(double alt)
{
return (3.98622e+14)/(pow(((6.378178e+06)+(alt)),2));
}
void output(int j, double T, double Y[])
{
int i;
double tmpLong, tmpLat, tmpZ, range;
double C[3];
Latitude = -(Y[0] /(1852 * 60)) + Latitude;
Longitude = -(Y[1] /(1852 * 60)) + Longitude;
range = sqrt(pow(Y[0], 2.0) + pow(Y[1], 2.0) + pow(Y[2], 2.0));
for (i=0;i<3;i++)
{
C[i] = Y[i + 3];
}
BLKOUT(0, CONTROL,
TIME, Y, C,
F, G,
PANG, VEND, PEND, AZ,
FLOW[STAGE], XSEC[STAGE], THRUST[STAGE],
Gravity, Pressure, Temperature, Mach,
Celerity, Q, CD[STAGE], AF,
Latitude, Longitude, range, Y[5],
Acc, COT[STAGE], TIN[STAGE]);
}
void XP_System(double t, double Y[], double DY[])
{
double Fg,Ft,Fa, mass;
int i;
Altitude = Y[2];
Pressure = Pressure15(Altitude);
Temperature = Temperature15(Altitude);
Celerity = Celerity15(Temperature);
Gravity = Gravity15(Altitude);
Velocity = sqrt(Y[3]*Y[3] + Y[4]*Y[4] + Y[5]*Y[5]);
Mach = Velocity / Celerity;
Q = 0.7 * Pressure * Mach * Mach;
mass = WEIGHT[STAGE] - FLOW[STAGE] * t;
Ft = THRUST[STAGE];
Fg = (mass) * Gravity;
Fa = CD[STAGE] * Q * XSEC[STAGE];
Acc = (Ft - Fa) / (9.80665 * mass);
AF = Fa;
G[0] = 0.0;
G[1] = 0.0;
G[2] = -1;
if(t != 0.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("\nF: ");
PITCH(t,AZ,PANG,VEND,PEND,F,C);
printf("\nMASS: %lf",mass);
printf(" Pressure: %lf ", Pressure15(225));
printf(" Altitude: %lf", Y[2]);
DY[0] = Y[3];
DY[1] = Y[4];
DY[2] = Y[5];
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_System1(int n, double h, double T, double X[], int nsteps)
{
double Y[n];
double K[4][n];
output(0,T,X);
for (j = 0; j < nsteps; j++)
{
XP_System(T, X, K[0]);
for (i=0; i<n; i++)
{
Y[i] = X[i] + .5 * h * K[0][i];
}
XP_System(T + h/2, Y, K[1]);
for (i=0; i<n; i++)
{
Y[i] = X[i] + .5 * h * K[1][i];
}
XP_System(T + h/2, Y, K[2]);
for (i=0; i<n; i++)
{
Y[i] = X[i] + h * K[2][i];
}
XP_System(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 = T;
output(j, T, X);
}
}
void OneRun15(double X[7])
{
int nsteps, i;
TIME = 0.0;
X[0] = 0.0;
X[1] = 0.0;
X[2] = HEIGHT;
X[3] = 0.0;
X[4] = 0.0;
X[5] = 1;
for (i=0; i<NumStages; i++)
{
nsteps = (int) COT[i] / TIN[i];
STAGE = i;
X[6] = WEIGHT[i];
RK4_System1(7, TIN[i], TIME, X, nsteps);
}
}
double func(double x[20], int degree, double xVal)
{
int i, power;
double sum = 0;
power = degree-1;
for (i=0;i<degree;i++)
{
sum = sum + pow(xVal, power) * x[i];
--power;
}
return sum;
}
int main () {
double dblLatitude,dblLongitude,dblHeight;
double dblThrust[5],dblWeight[5],dblFlow[5],dblCrossSection[5],dblDrag[5],dblCutOff[5],dblStepSize[5];
double dlbVertRise,dblPitchAngle,dlbPitchEnd,dblLaunchAzimuth;
double X[7];
int i;
printf("\nControl Values: ");
printf("\n Number of stages: ");
scanf("%d",&NumStages);
printf("\n Print control(full or partial): ");
scanf("%d",&CONTROL);
printf("\nLaunch Point");
printf("\n Latitude(in degrees): ");
scanf("%lf",&dblLatitude);
printf("\n Longitude(in degrees): ");
scanf("%lf",&dblLongitude);
printf("\n Height(in meters): ");
scanf("%lf",&dblHeight);
for(i=0; i<NumStages; i++)
{
printf("\nThe vehicle stage %d:",i);
printf("\nThrust(lbs): ");
scanf("%lf",&dblThrust[i]);
printf("\nWeight(lbs): ");
scanf("%lf",&dblWeight[i]);
printf("\nFlow Rate(lbs/sec): ");
scanf("%lf",&dblFlow[i]);
printf("\nCross-sectional area(sqft): ");
scanf("%lf",&dblCrossSection[i]);
printf("\nDrag coefficient: ");
scanf("%lf",&dblDrag[i]);
printf("\nCut-off time(secs): ");
scanf("%lf",&dblCutOff[i]);
printf("\nStep size(secs): ");
scanf("%lf",&dblStepSize[i]);
}
printf("\nPitch program: ");
printf("\nVertical rise time(secs): ");
scanf("%lf",&dlbVertRise);
printf("\nPitch angle(degrees from vert): ");
scanf("%lf",&dblPitchAngle);
printf("\nPitch Program end time(secs): ");
scanf("%lf",&dlbPitchEnd);
printf("\nLaunch azimuth(degrees from north): ");
scanf("%lf",&dblLaunchAzimuth);
//Echo input
printf("\nControl Values: ");
printf("\n Number of stages: %d",NumStages);
printf("\n CONTROL: %d", CONTROL);
printf("\n Latitude: %lf",dblLatitude);
printf("\n Longitude: %lf",dblLongitude);
printf("\n Height: %lf",dblHeight);
for(i=0; i<NumStages; i++)
{
printf("\nStage: %d :",i+1);
printf("\n Thrust(lbs): %lf",dblThrust[i]);
printf("\n Weight(lbs): %lf",dblWeight[i]);
printf("\n Flow(lbs/sec): %lf",dblFlow[i]);
printf("\n Cross-sectional area(sqft): %lf",dblCrossSection[i]);
printf("\n Drag coefficient: %lf",dblDrag[i]);
printf("\n \"Cut-off\" time(secs): %lf",dblCutOff[i]);
printf("\n Step size(secs): %lf",dblStepSize[i]);
}
printf("\nPitch program: ");
printf("\n Vertical rise time(secs): %lf",dlbVertRise);
printf("\n Pitch angle(degrees from vert): %lf",dblPitchAngle);
printf("\n Pitch Program end time(secs): %lf",dlbPitchEnd);
printf("\n Launch azimuth(degrees from north): %lf",dblLaunchAzimuth);
PLAT = dblLatitude;
PLONG = dblLongitude;
Latitude = PLAT;
Longitude = PLONG;
HEIGHT = dblHeight;
AZ = dblLaunchAzimuth; //* .01745329;
PANG = dblPitchAngle; //* .01745329;
VEND = dlbVertRise;
PEND = dlbPitchEnd;
// Convert units .
for(i=0; i<NumStages; i++)
{
COT[i] = dblCutOff[i];
THRUST[i] = dblThrust[i] * 0.4535924;
FLOW[i] = dblFlow[i] * 0.04625455;
WEIGHT[i] = dblWeight[i] * 0.04625455;
XSEC[i] = dblCrossSection[i] * 0.3048 * 0.3048;
CD[i] = dblDrag[i];
TIN[i] = dblStepSize[i];
}
Velocity = 1.0;
OneRun15(X);
return 0;
}