void FGAtmosphere::Calculate(double altitude)
{
FGPropertyNode* node = PropertyManager->GetNode();
if (!PropertyManager->HasNode("atmosphere/override/temperature"))
Temperature = GetTemperature(altitude);
else
Temperature = node->GetDouble("atmosphere/override/temperature");
if (!PropertyManager->HasNode("atmosphere/override/pressure"))
Pressure = GetPressure(altitude);
else
Pressure = node->GetDouble("atmosphere/override/pressure");
if (!PropertyManager->HasNode("atmosphere/override/density"))
Density = Pressure/(Reng*Temperature);
else
Density = node->GetDouble("atmosphere/override/density");
Soundspeed = sqrt(SHRatio*Reng*(Temperature));
PressureAltitude = altitude;
DensityAltitude = altitude;
Viscosity = Beta * pow(Temperature, 1.5) / (SutherlandConstant + Temperature);
KinematicViscosity = Viscosity / Density;
}
FGSimplexTrim::FGSimplexTrim(FGFDMExec * fdm, TrimMode mode)
{
std::clock_t time_start=clock(), time_trimDone;
// variables
FGTrimmer::Constraints constraints;
if (fdm->GetDebugLevel() > 0) {
std::cout << "\n-----Performing Simplex Based Trim --------------\n" << std::endl;
}
// defaults
std::string aircraftName = fdm->GetAircraft()->GetAircraftName();
FGPropertyNode* node = fdm->GetPropertyManager()->GetNode();
double rtol = node->GetDouble("trim/solver/rtol");
double abstol = node->GetDouble("trim/solver/abstol");
double speed = node->GetDouble("trim/solver/speed"); // must be > 1, 2 typical
double random = node->GetDouble("trim/solver/random");
int iterMax = int(node->GetDouble("trim/solver/iterMax"));
bool showConvergence = node->GetBool("trim/solver/showConvergence");
bool pause = node->GetBool("trim/solver/pause");
bool showSimplex = node->GetBool("trim/solver/showSimplex");
// flight conditions
double phi = fdm->GetIC()->GetPhiRadIC();
double theta = fdm->GetIC()->GetThetaRadIC();
double gd = fdm->GetInertial()->gravity();
constraints.velocity = fdm->GetIC()->GetVtrueFpsIC();
constraints.altitude = fdm->GetIC()->GetAltitudeASLFtIC();
constraints.gamma = fdm->GetIC()->GetFlightPathAngleRadIC();
constraints.rollRate = 0;
constraints.pitchRate = 0;
constraints.yawRate = tan(phi)*gd*cos(theta)/constraints.velocity;
constraints.stabAxisRoll = true; // FIXME, make this an option
// initial solver state
int n = 6;
std::vector<double> initialGuess(n), lowerBound(n), upperBound(n), initialStepSize(n);
lowerBound[0] = node->GetDouble("trim/solver/throttleMin");
lowerBound[1] = node->GetDouble("trim/solver/elevatorMin");
lowerBound[2] = node->GetDouble("trim/solver/alphaMin");
lowerBound[3] = node->GetDouble("trim/solver/aileronMin");
lowerBound[4] = node->GetDouble("trim/solver/rudderMin");
lowerBound[5] = node->GetDouble("trim/solver/betaMin");
upperBound[0] = node->GetDouble("trim/solver/throttleMax");
upperBound[1] = node->GetDouble("trim/solver/elevatorMax");
upperBound[2] = node->GetDouble("trim/solver/alphaMax");
upperBound[3] = node->GetDouble("trim/solver/aileronMax");
upperBound[4] = node->GetDouble("trim/solver/rudderMax");
upperBound[5] = node->GetDouble("trim/solver/betaMax");
initialStepSize[0] = node->GetDouble("trim/solver/throttleStep");
initialStepSize[1] = node->GetDouble("trim/solver/elevatorStep");
initialStepSize[2] = node->GetDouble("trim/solver/alphaStep");
initialStepSize[3] = node->GetDouble("trim/solver/aileronStep");
initialStepSize[4] = node->GetDouble("trim/solver/rudderStep");
initialStepSize[5] = node->GetDouble("trim/solver/betaStep");
initialGuess[0] = node->GetDouble("trim/solver/throttleGuess");
initialGuess[1] = node->GetDouble("trim/solver/elevatorGuess");
initialGuess[2] = node->GetDouble("trim/solver/alphaGuess");
initialGuess[3] = node->GetDouble("trim/solver/aileronGuess");
initialGuess[4] = node->GetDouble("trim/solver/rudderGuess");
initialGuess[5] = node->GetDouble("trim/solver/betaGuess");
// solve
FGTrimmer * trimmer = new FGTrimmer(fdm, &constraints);
Callback callback(aircraftName, trimmer);
FGNelderMead * solver = NULL;
solver = new FGNelderMead(trimmer,initialGuess,
lowerBound, upperBound, initialStepSize,iterMax,rtol,
abstol,speed,random,showConvergence,showSimplex,pause,&callback);
while(solver->status()==1) solver->update();
time_trimDone = std::clock();
// output
if (fdm->GetDebugLevel() > 0) {
trimmer->printSolution(std::cout,solver->getSolution());
std::cout << "\nfinal cost: " << std::scientific << std::setw(10) << trimmer->eval(solver->getSolution()) << std::endl;
std::cout << "\ntrim computation time: " << (time_trimDone - time_start)/double(CLOCKS_PER_SEC) << "s \n" << std::endl;
}
delete solver;
delete trimmer;
}