Skip to content
Snippets Groups Projects
FGSimplexTrim.cpp 9.24 KiB
/*
 * FGSimplexTrim.cpp
 * Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
 *
 * FGSimplexTrim.cpp 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 2 of the License, or
 * (at your option) any later version.
 *
 * FGSimplexTrim.cpp 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 <http://www.gnu.org/licenses/>.
 */

#include "FGSimplexTrim.h"
#include <ctime>

namespace JSBSim {

FGSimplexTrim::FGSimplexTrim(FGFDMExec * fdmPtr, TrimMode mode)
{
	using namespace JSBSim;

	std::clock_t time_start=clock(), time_trimDone, time_linDone;

	// variables
	FGFDMExec & fdm = *fdmPtr;
	fdm.Setdt(1./120);
	FGTrimmer::Constraints constraints;

	std::cout << "\n-----Performaing Simplex Based Trim --------------\n" << std::endl;

	// defaults
	constraints.velocity = fdm.GetAuxiliary()->GetVt();
	constraints.altitude = fdm.GetPropagate()->GetAltitudeASL();
	std::string aircraft = fdm.GetAircraft()->GetAircraftName();
	double rtol = 10*std::numeric_limits<float>::epsilon();
	double abstol = 10*std::numeric_limits<double>::epsilon();
	double speed = 1.8; // must be > 1, 2 typical
	double random = 0.0; // random scale factor added to all simplex calcs
	int iterMax = 2000;
	bool showConvergeStatus = false;
	bool pause = false;
	bool showSimplex = false;
	bool variablePropPitch = false;
	int debugLevel = 0;
	std::string fileName = aircraft;

	// input
	//std::cout << "input ( press enter to accept [default] )\n" << std::endl;

	// load model
	std::string aircraftName = fdm.GetAircraft()->GetAircraftName();
	//prompt("\tdebug level\t\t",debugLevel);
	//fdm.SetDebugLevel(debugLevel);
	//std::cout << "model selection" << std::endl;
	//while (1)
	//{
		//prompt("\taircraft\t\t",aircraft);
		//prompt("\toutput file name\t",fileName);
		//fdm.LoadModel("../aircraft","../engine","../systems",aircraft);
		//aircraftName = fdm.GetAircraft()->GetAircraftName();
		//if (aircraftName == "")
		//{
			//std::cout << "\tfailed to load aircraft" << std::endl;
		//}
		//else
		//{
			//std::cout << "\tsuccessfully loaded: " <<  aircraftName << std::endl;
			//break;
		//}
	//}

	// Turn on propulsion system
	fdm.GetPropulsion()->InitRunning(-1);

	// get propulsion pointer to determine type/ etc.
	FGEngine * engine0 = fdm.GetPropulsion()->GetEngine(0);
	FGThruster * thruster0 = engine0->GetThruster();

	// flight conditions
	//std::cout << "\nflight conditions: " << std::endl;
	//prompt("\taltitude, ft\t\t",constraints.altitude);
	//prompt("\tvelocity, ft/s\t\t",constraints.velocity);
	//prompt("\tgamma, deg\t\t",constraints.gamma); constraints.gamma = constraints.gamma*M_PI/180;
	
	double phi = fdm.GetPropagate()->GetEuler(1);
	double theta = fdm.GetPropagate()->GetEuler(2);
	double psi = fdm.GetPropagate()->GetEuler(3);

	// TODO check that this works properly
	constraints.gamma = theta;

	//if (thruster0->GetType()==FGThruster::ttPropeller)
		//prompt("\tvariable prop pitch?\t\t",variablePropPitch);
	// FIXME, enable
	// mode menu
	while (1)
	{
		//prompt("\tmode",mode);
		constraints.rollRate = 0;
		constraints.pitchRate = 0;
		constraints.yawRate = 0;
		if (mode == tLongitudinal) break;
		else if (mode == tRoll)
		{
			prompt("\troll rate, rad/s",constraints.rollRate);
			prompt("\tstability axis roll",constraints.stabAxisRoll);
			// TODO check that this works properly
			constraints.rollRate = fdm.GetAuxiliary()->GetEulerRates(1);
			constraints.stabAxisRoll = true; // FIXME, make this an option
			break;
		}
		else if (mode == tPullup)
		{
			prompt("\tpitch rate, rad/s",constraints.pitchRate);
			// TODO check that this works properly
			constraints.pitchRate = fdm.GetAuxiliary()->GetEulerRates(2);
			break;
		}
		else if (mode == tTurn)
		{
			//prompt("\tyaw rate, rad/s",constraints.yawRate);
			// TODO check that this works properly
			double gd=fdm.GetInertial()->gravity();
			constraints.yawRate = tan(phi)*gd*cos(theta)/constraints.velocity;
			break;
		}
		else {
			std::cerr << "\tunknown mode: " << mode << std::endl;
			exit(1);
		}
	}

	// solver properties
	// TODO make these options
	//std::cout << "\nsolver properties: " << std::endl;
	//std::cout << std::scientific;
	//prompt("\tshow converge status?\t",showConvergeStatus);
	//prompt("\tshow simplex?\t\t",showSimplex);
	//prompt("\tpause?\t\t\t",pause);
	//prompt("\trelative tolerance\t",rtol);
	//prompt("\tabsolute tolerance\t",abstol);
	//prompt("\tmax iterations\t\t",iterMax);
	//prompt("\tconvergence speed\t",speed);
	//prompt("\trandomization ratio\t",random);
	//std::cout << std::fixed;

	// initial solver state
	int n = 6;
	std::vector<double> initialGuess(n), lowerBound(n), upperBound(n), initialStepSize(n);

	lowerBound[0] = 0; //throttle
	lowerBound[1] = -1; // elevator
	lowerBound[2] = -20*M_PI/180; // alpha
	lowerBound[3] = -1; // aileron
	lowerBound[4] = -1; // rudder
	lowerBound[5] = -20*M_PI/180; // beta

	upperBound[0] = 1; //throttle
	upperBound[1] = 1; // elevator
	upperBound[2] = 20*M_PI/180; // alpha
	upperBound[3] = 1; // aileron
	upperBound[4] = 1; // rudder
	upperBound[5] = 20*M_PI/180; // beta

	initialStepSize[0] = 0.2; //throttle
	initialStepSize[1] = 0.1; // elevator
	initialStepSize[2] = 0.1; // alpha
	initialStepSize[3] = 0.1; // aileron
	initialStepSize[4] = 0.1; // rudder
	initialStepSize[5] = 0.1; // beta

	initialGuess[0] = 0.5; // throttle
	initialGuess[1] = 0; // elevator
	initialGuess[2] = 0; // alpha
	initialGuess[3] = 0; // aileron
	initialGuess[4] = 0; // rudder
	initialGuess[5] = 0; // beta

	// solve
	FGTrimmer trimmer(fdm, constraints);
	Callback callback(fileName,&trimmer);
	FGNelderMead * solver;
	try
	{
		 solver = new FGNelderMead(trimmer,initialGuess,
			lowerBound, upperBound, initialStepSize,iterMax,rtol,
			abstol,speed,random,showConvergeStatus,showSimplex,pause,&callback);
		 while(solver->status()==1) solver->update();
	}
	catch (const std::runtime_error & e)
	{
		std::cout << e.what() << std::endl;
		//exit(1);
	}

	// output
	try
	{
		trimmer.printSolution(solver->getSolution()); // this also loads the solution into the fdm
		std::cout << "\nfinal cost: " << std::scientific << std::setw(10) << trimmer.eval(solver->getSolution()) << std::endl;
	}
	catch(std::runtime_error & e)
	{
		std::cout << "caught std::runtime error" << std::endl;
		std::cout << "exception: " << e.what() << std::endl;
		exit(1);
	}

	time_trimDone = std::clock();
	std::cout << "\ntrim computation time: " << (time_trimDone - time_start)/double(CLOCKS_PER_SEC) << "s \n" << std::endl;

	//std::cout << "\nsimulating flight to determine trim stability" << std::endl;

	//std::cout << "\nt = 5 seconds" << std::endl;
	//for (int i=0;i<5*120;i++) fdm.Run();
	//trimmer.printState();

	//std::cout << "\nt = 10 seconds" << std::endl;
	//for (int i=0;i<5*120;i++) fdm.Run();
	//trimmer.printState();

	std::cout << "\nlinearization: " << std::endl;
	FGStateSpace ss(fdm);

	ss.x.add(new FGStateSpace::Vt);
	ss.x.add(new FGStateSpace::Alpha);
	ss.x.add(new FGStateSpace::Theta);
	ss.x.add(new FGStateSpace::Q);

	if (thruster0->GetType()==FGThruster::ttPropeller)
	{
		ss.x.add(new FGStateSpace::Rpm0);
		if (variablePropPitch) ss.x.add(new FGStateSpace::PropPitch);
		int numEngines = fdm.GetPropulsion()->GetNumEngines();
		if (numEngines>1) ss.x.add(new FGStateSpace::Rpm1);
		if (numEngines>2) ss.x.add(new FGStateSpace::Rpm2);
		if (numEngines>3) ss.x.add(new FGStateSpace::Rpm3);
	}
	ss.x.add(new FGStateSpace::Beta);
	ss.x.add(new FGStateSpace::Phi);
	ss.x.add(new FGStateSpace::P);
	ss.x.add(new FGStateSpace::Psi);
	ss.x.add(new FGStateSpace::R);
	ss.x.add(new FGStateSpace::Latitude);
	ss.x.add(new FGStateSpace::Longitude);
	ss.x.add(new FGStateSpace::Alt);

	ss.u.add(new FGStateSpace::ThrottleCmd);
	ss.u.add(new FGStateSpace::DaCmd);
	ss.u.add(new FGStateSpace::DeCmd);
	ss.u.add(new FGStateSpace::DrCmd);

	// state feedback
	ss.y = ss.x;

	std::vector< std::vector<double> > A,B,C,D;
	std::vector<double> x0 = ss.x.get(), u0 = ss.u.get();
	std::vector<double> y0 = x0; // state feedback
	std::cout << ss << std::endl;

	ss.linearize(x0,u0,y0,A,B,C,D);

	int width=10;
	std::cout.precision(3);
	std::cout
		<< std::fixed
		<< std::right
		<< "\nA=\n" << std::setw(width) << A
		<< "\nB=\n" << std::setw(width) << B
		<< "\nC=\n" << std::setw(width) << C
		<< "\nD=\n" << std::setw(width) << D
		<< std::endl;

	// write scicoslab file
	std::ofstream scicos(std::string(aircraft+"_lin.sce").c_str());
	scicos.precision(10);
	width=20;
	scicos
	<< std::scientific
	<< aircraft << ".x0=..\n" << std::setw(width) << x0 << ";\n"
	<< aircraft << ".u0=..\n" << std::setw(width) << u0 << ";\n"
	<< aircraft << ".sys = syslin('c',..\n"
	<< std::setw(width) << A << ",..\n"
	<< std::setw(width) << B << ",..\n"
	<< std::setw(width) << C << ",..\n"
	<< std::setw(width) << D << ");\n"
	<< aircraft << ".tfm = ss2tf(" << aircraft << ".sys);\n"
	<< std::endl;

	time_linDone = std::clock();
	std::cout << "\nlinearization computation time: " << (time_linDone - time_trimDone)/double(CLOCKS_PER_SEC) << " s\n" << std::endl;
}

} // JSBSim

// vim:ts=4:sw=4