Thomas Umscheid
02/17/15
Dynamics of Flight
Code Check #3
Phase 5


acSim

#include "stdafx.h"
ofstream fout("acdata.txt");

int main()
{
	aircraft		ac;
	double		 t, tend = 100.0;
	
	fout << "t" << "\t" << "Pn" << "\t" << "Pe" << "\t" << "Pd" << "\t" << "u" << "\t" << "v" << "\t" << "w" << "\t" << "phi" << "\t" << "theta" << "\t" << "psi" << "\t" << "p" << "\t" << "q" << "\t" << "r" << "\t" << "as1" << "\t" << "as2" << "\t" << "es1" << "\t" << "es2" << "\t" << "rs1" << "\t" << "rs2" << "\t" << "ts1" << "\t" << "ts2" << endl;
	for (t = 0.; t < tend;)
	{
		ac.control(t);
		t = ac.rk(t);

		cout << t << "\t" << h << "\t" << ac.get_state(2) <<  endl;
		fout << t;
		for (int i = 0; i < N; i++)
		{
			fout << "\t" << ac.get_state(i);
			
		}
		fout << endl;
	}
	return 0;
}



aircraft.cpp

#include "stdafx.h"

//Kp,   Ki,		Kd,		Kr, ui_max, uc_max, uc_min, trim
Controller La(.01, .001, 0, 0, 4.0, 4.0, -4.0, 0);
Controller Gt(10.0, 5.6, .4, 0, .25, .75, -.75, 0);
Controller Ro(.3, .4, 0, .1, .3, 1.0, -1.0, 0);
Controller Lo(.3, .2, 0, 0, 4.0, 5.0, -5.0, 0);
Controller Ve(-.1, -.001, -.025, 0, .3, .7, -.7, 0);
Controller Pi(-.5, -.167, 0, -.25, .25, 1.0, -1.0, -.02508);
Controller En(.02, .02, .01, 0, 1.0, 1.2, -1.2, .574778);

aircraft::aircraft()
{
	mass = 13.5;	Jx = 0.8244;	Jy = 1.135;	Jz = 1.759;	Jxz = 0.1204;
	gamma = Jx*Jz - Jxz*Jxz; gamma1 = Jxz*(Jx - Jy + Jz) / gamma; gamma2 = (Jz*(Jz - Jy) + Jxz*Jxz) / gamma; gamma3 = Jz / gamma;
	gamma4 = Jxz / gamma; gamma5 = (Jz - Jx) / Jy; gamma6 = Jxz / Jy; gamma7 = (Jx*(Jx - Jy) + Jxz*Jxz) / gamma; gamma8 = Jx / gamma;
	S = 0.55; b = 2.8956; c = 0.18994; Sprop = 0.2027; Kmotor = 80.;
	CL0 = 0.28; CD0 = 0.03; Cm0 = -0.02338; CLa = 3.45; CDa = 0.30; Cma = -0.38; CLq = 0.0; CDq = 0.0; Cmq = -3.6; CLde = -0.36;
	CDde = 0.0; Cmde = -0.5;
	Cprop = 1.0;
	CY0 = 0.0; Cl0 = 0.0; Cn0 = 0.0; CYb = -0.98; Clb = -0.12; Cnb = 0.25; CYp = 0.0; Clp = -0.26; Cnp = 0.022;
	CYr = 0.0; Clr = 0.14; Cnr = -0.35; CYda = 0.0; Clda = 0.08; Cnda = 0.06; CYdr = -0.17; Cldr = 0.105; Cndr = -0.032;
	//  Initialize all parameters
	init_state();

}


aircraft::~aircraft() {}

	double	aircraft::rk(double t)
	{
		int	i;

		for (i = 0; i<N; i++) y[i] = x[i];
		function(0);
		for (i = 0; i<N; i++) y[i] = x[i] + 0.5*h*K[0][i];
		t += 0.5*h;
		function(1);
		for (i = 0; i<N; i++) y[i] = x[i] + 0.5*h*K[1][i];
		function(2);
		for (i = 0; i<N; i++) y[i] = x[i] + h*K[2][i];
		t += 0.5*h;
		function(3);
		for (i = 0; i<N; i++)
		{
			xdot[i] = (K[0][i] + 2.*K[1][i] + 2.*K[2][i] + K[3][i]) / 6.;
			x[i] += h*xdot[i];
		}
		// other computation as needed
		return t;
	

}

void	aircraft::function(int i)
	{
		Sphi = sin(y[phi]);
		Sth = sin(y[theta]);
		Spsi = sin(y[psi]);
		Cphi = cos(y[phi]);
		Cth = cos(y[theta]);
		Cpsi = cos(y[psi]);
		Tth = tan(y[theta]);

		force();

		K[i][Pn] = Cth*Cpsi*y[u] + (Sphi*Sth*Cpsi - Cphi*Spsi)*y[v] + (Cphi*Sth*Cpsi + Sphi*Spsi)*y[w];
		K[i][Pe] = Cth*Spsi*y[u] + (Sphi*Sth*Spsi + Cphi*Cpsi)*y[v] + (Cphi*Sth*Spsi - Sphi*Cpsi)*y[w];
		K[i][Pd] = -Sth*y[u] + Sphi*Cth*y[v] + Cphi*Cth*y[w];
		K[i][u] = y[r] * y[v] - y[q] * y[w] + Fx / mass;
		K[i][v] = y[p] * y[w] - y[r] * y[u] + Fy / mass;
		K[i][w] = y[q] * y[u] - y[p] * y[v] + Fz / mass;

		moment();

		K[i][phi] = y[p] + Sphi*Tth*y[q] + Cphi*Tth*y[r];
		K[i][theta] = Cphi*y[q] - Sphi*y[r];
		K[i][psi] = Sphi / Cth*y[q] + Cphi / Cth*y[r];
		K[i][p] = gamma1*y[p] * y[q] - gamma2*y[q] * y[r] + gamma3*Mx + gamma4*Mz;
		K[i][q] = gamma5*y[p] * y[r] - gamma6*y[p] * y[p] - y[r] * y[r] + 1 / Jy*My;
		K[i][r] = gamma7*y[p] * y[q] - gamma1*y[q] * y[r] + gamma4*Mx + gamma8*Mz;

		
		K[i][as1] = Wna*(uc[aa] - y[as1]);
		K[i][as2] = Wna*(y[as1] - y[as2]);

		K[i][es1] = Wna*(uc[ee] - y[es1]);
		K[i][es2] = Wna*(y[es1] - y[es2]);

		K[i][rs1] = Wna*(uc[rr] - y[rs1]);
		K[i][rs2] = Wna*(y[rs1] - y[rs2]);

		K[i][ts1] = Wna*(uc[tt] - y[ts1]);
		K[i][ts2] = Wna*(y[ts1] - y[ts2]);
		

	}
void	aircraft::force(void)
	{
		Va = sqrt(y[u] * y[u] + y[v] * y[v] + y[w] * y[w]);
		alpha = atan(y[w] / y[u]);
		beta = asin(y[v] / Va);

		
		if (y[as2] >= amax) d[aa] = amax;
		else if (y[as2] < -amax) d[aa] = -amax;
		else d[aa] = y[as2];

		if (y[es2] >= amax) d[ee] = emax;
		else if (y[es2] < -amax) d[ee] = -emax;
		else d[ee] = y[es2];

		if (y[as2] >= amax) d[ee] = rmax;
		else if (y[rs2] < -amax) d[ee] = -rmax;
		else d[ee] = y[rs2];

		if (y[ts2] >= amax) d[tt] = tmax;
		else if (y[ts2] < -amax) d[tt] = -tmax;
		else d[tt] = y[ts2];
		

		Flift = .5*roe*Va*Va*S*(CL0 + CLa*alpha + CLq*(c / (2 * Va))*y[q] + CLde*de);
		Fdrag = .5*roe*Va*Va*S*(CD0 + CDa*alpha + CDq*(c / (2 * Va))*y[q] + CDde*de);

		Fx = -cos(alpha)*Fdrag + sin(alpha)*Flift - mass*gr*Sth + .5*roe*Sprop*Cprop*((Kmotor*dt)*(Kmotor*dt) - Va*Va);
		Fy = .5*roe*Va*Va*S*(CY0 + CYb*beta + CYp*(b / (2 * Va))*y[p] + CYr*(b / (2 * Va))*y[r] + CYda*da + CYdr*dr)+mass*gr*Cth*Sphi;
		Fz = -sin(alpha)*Fdrag - cos(alpha)*Flift+mass*gr*Cth*Cphi;
	}
void	aircraft::moment(void)
	{
		Va = sqrt(y[u]*y[u] + y[v]*y[v] + y[w]*y[w]);
		alpha = atan(y[w] / y[u]);
		beta = asin(y[v] / Va);

		Mx = .5*roe*Va*Va*S*b*(Cl0 + Clb*beta + Clp*(b / (2 * Va))*y[p] + Clr*(b / (2 * Va))*y[r] + Clda*da + Cldr*dr);
		My = .5*roe*Va*Va*S*c*(Cm0 + Cma*alpha + Cmq*(c / (2 * Va))*y[q] + Cmde*de);
		Mz = .5*roe*Va*Va*S*b*(Cn0 + Cnb*beta + Cnp*(b / (2 * Va))*y[p] + Cnr*(b / (2 * Va))*y[r] + Cnda*da + Cndr*dr);
	}
void	aircraft::init_state(void)
	{
		int i;

		for (i = 0; i<N; i++) x[i] = 0.0;
		Va = 44.7;
		//x[phi] = .05;

		//x[theta] = .1;
		x[u] = Va*cos(x[theta]);
		x[w] = Va*sin(x[theta]);

		d[aa] = 0;
		d[ee] = -.025077;
		d[rr] = 0.0;
		d[tt] = .57478;

		uc[aa] = 0;
		uc[ee] = -.025077;
		uc[rr] = 0.0;
		uc[tt] = .57478;

		x[as1] = 0;
		x[as2] = 0;

		x[es1] = -.025077;
		x[es2] = -.025077;

		x[rs1] = 0.0;
		x[rs2] = 0.0;

		x[ts1] = .57478;
		x[ts2] = .57478;

		Wna = 20;
		Wne = 20;
		Wnr = 20;
		Wnt = .7;

		amax = .8;
		emax = .8;
		rmax = .8;
		tmax = 1.2;

		GT = 0;
		altitude = -x[Pd];
		Energy = .5*Va*Va + gr*altitude;

		return;
	}
void	aircraft::nav_temp(double time)
	{
		Vc = 44.7;
		Pnc = Vc*time;
		Pec = 0.0;
		Altc = 0.0;
		Rollc = 0.0;
		Pitchc = -0.0285307;
		roll_ratec = 0.0;
		pitch_ratec = 0.0;
		Lac = (Pec - x[Pe]);
		Loc = (Pnc - x[Pn]);
		Gtc = 0.0;
	}
void aircraft::control(double t)
	{
		nav_temp(t);

		double u_La, u_Gt, u_Ro, u_Lo, u_Ve, u_Pi, u_En;

		//Input, Command, Measured, Rate_Command, Rate_Measured
		u_La = La.pidr(Lac, 0, 0, 0, 0);
		u_Gt = Gt.pidr(u_La, Gtc, GT, 0, 0);
		u_Ro = Ro.pidr(u_Gt, Rollc, x[phi],roll_ratec, x[p]);
		u_Lo = Lo.pidr(Loc,0,0,0,0);
		u_Ve = Ve.pidr(u_Lo,Vc,Va,0,0);
		u_Pi = Pi.pidr(u_Ve,Pitchc,x[theta],pitch_ratec,x[q]);

		Energyc = 0.5*(Vc + u_Lo)*(Vc + u_Lo) + gr*Altc;
		u_En = En.pidr(0,Energyc,Energy,0,0);

		uc[aa] = u_Ro;
		uc[ee] = u_Pi;
		uc[tt] = u_En;

		return;

	}
double	aircraft::get_state(int i)
	{
		return	x[i];
	}


aircraft.h

class aircraft	{
	private:
		enum{ Pn, Pe, Pd, u, v, w, phi, theta, psi, p, q, r, as1, as2, es1, es2, rs1, rs2, ts1, ts2};
		double	x[N], xdot[N], y[N], K[4][N];
		double	Sphi, Cphi, Sth, Cth, Tth, Spsi, Cpsi;
		double	Fx, Fy, Fz, Mx, My, Mz, Flift, Fdrag;
		double	mass, Jx, Jy, Jz, Jxz, gamma, gamma1, gamma2, gamma3, gamma4, gamma5, gamma6, gamma7, gamma8;
		double S, b, c, Sprop, Kmotor;
		double CL0, CD0, Cm0, CLa, CDa, Cma, CLq, CDq, Cmq, CLde;
		double CDde, Cmde;
		double Cprop;
		double CY0, Cl0, Cn0, CYb, Clb, Cnb, CYp, Clp, Cnp;
		double CYr, Clr, Cnr, CYda, Clda, Cnda, CYdr, Cldr, Cndr;
		enum{aa, ee, rr, tt};
		double d[4];
		double uc[4];
		double amax, emax, rmax, tmax, Wna, Va, alpha, beta, de, dr, dt, da;
		//  other parameters as needed
	public:
		aircraft();			// constructor
		~aircraft();			// destructor
		double	rk(double);
		void		function(int);
		void		force(void);
		void		moment(void);
		void		init_state(void);
		double 	get_state(int);
		// other functions as needed


};


Controller.cpp

#include "stdafx.h"


//Kp, Ki, Kd, Kr, ui_max, uc_max, uc_min, trim
Controller::Controller(double p, double i, double d, double r, double uimax, double ucmax, double ucmin, double Trim)
{
	Kp = p; Ki = i; Kd = d; Kr = r; ui_max = uimax; uc_max = ucmax; uc_min = ucmin; trim = Trim; error_previous = 0; ui = 0;
}
Controller::~Controller()
{
}
//Input, Command, Measured, Rate_Command, Rate_Measured.
double	Controller::pidr(double Input, double Command, double Measured, double Rate_Command, double Rate_Measured)
{
	double Output;

	error = Command + Input - Measured;
	rate_error = Rate_Command - Rate_Measured;

	up = Kp*error;
	ui += Ki*error*h;

	if (ui >= ui_max) ui = ui_max;
	else if (ui < -ui_max) ui = -ui_max;

	ud = Kd*(error-error_previous)/h;
	ur = Kr*rate_error;

	error_previous = error;

	uc = up+ui+ud+ur+trim;

	if (uc >= uc_max) uc=uc_max;
	else if (uc < uc_min) uc = uc_min;

	Output = uc;

	return Output;
}


controller.h


#include "stdafx.h"


class Controller {
private:
	double		Kp, Ki, Kd, Kr;
	// Proportional, Integral, Derivative and Rate Controller Gains
	double		error, up, ui, ud, ur, error_previous;
	double		ui_max, uc_max, uc_min, uc, rate_error, trim;

public:
	// Variables passed to the Constructor:  Proportional Gain (Kp), Integral Gain (Ki),
	// Derivative Gain (Kd), Rate Gain (Kr), Integral Windeup limit (ui_max), 
	// Output Limiter Max (uc_max), Output Limiter Min (uc_min), Output Trim value (trim).

	Controller(double, double, double, double, double, double, double, double);
	// constructor
	~Controller();		// destructor

	// Variables passed to the pidr function:  Input, Command, Measured, 
	// Rate_Command, Rate_Measured.

	double	pidr(double, double, double, double, double);

};



stdafx.h

#pragma once
#define gr 9.80665		// gravity (m/s^2)
#define roe 1.2682		// air density (kg/m^3)
#define pi 3.1416
#define d2r	pi/180.
#define N  	20		// # states
#define	h	0.01	// numerical integration step-size (s)




#include <stdio.h>
#include <tchar.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include "aircraft.h"

using namespace std;