fly/flightgear
1
0
Fork 0
Code Activity

Add the latest batch of JSBSim changes. Tested with the Shuttle

Browse source
This commit is contained in:
Erik Hofman 2016-01-29 12:23:58 +01:00
parent 9172a8a8a9
commit 5ef3413373
10 changed files with 41 additions and 844 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
src/FDM/JSBSim/FGJSBBase.cpp
View file

@ -46,7 +46,7 @@ using namespace std;
namespace JSBSim {
IDENT(IdSrc,"$Id: FGJSBBase.cpp,v 1.41 2016/01/10 12:07:49 bcoconni Exp $");
IDENT(IdSrc,"$Id: FGJSBBase.cpp,v 1.42 2016/01/17 18:42:52 bcoconni Exp $");
IDENT(IdHdr,ID_JSBBASE);
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -277,7 +277,7 @@ double FGJSBBase::GaussianRandomNumber(void)
double FGJSBBase::PitotTotalPressure(double mach, double p)
{
if (mach < 0) return 0;
if (mach < 0) return p;
if (mach < 1) //calculate total pressure assuming isentropic flow
return p*pow((1 + 0.2*mach*mach),3.5);
else {

58
src/FDM/JSBSim/initialization/FGInitialCondition.cpp
View file

@ -58,7 +58,7 @@ using namespace std;
namespace JSBSim {
IDENT(IdSrc,"$Id: FGInitialCondition.cpp,v 1.104 2016/01/10 16:35:28 bcoconni Exp $");
IDENT(IdSrc,"$Id: FGInitialCondition.cpp,v 1.107 2016/01/24 18:18:38 bcoconni Exp $");
IDENT(IdHdr,ID_INITIALCONDITION);
//******************************************************************************
@ -909,6 +909,21 @@ bool FGInitialCondition::Load_v1(Element* document)
{
bool result = true;
if (document->FindElement("longitude"))
SetLongitudeRadIC(document->FindElementValueAsNumberConvertTo("longitude", "RAD"));
if (document->FindElement("elevation"))
SetTerrainElevationFtIC(document->FindElementValueAsNumberConvertTo("elevation", "FT"));
if (document->FindElement("altitude")) // This is feet above ground level
SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitude", "FT"));
else if (document->FindElement("altitudeAGL")) // This is feet above ground level
SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"));
else if (document->FindElement("altitudeMSL")) // This is feet above sea level
SetAltitudeASLFtIC(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
double altitude = GetAltitudeASLFtIC();
double longitude = GetLongitudeRadIC();
Element* latitude_el = document->FindElement("latitude");
if (latitude_el) {
double latitude = document->FindElementValueAsNumberConvertTo("latitude", "RAD");
@ -927,23 +942,11 @@ bool FGInitialCondition::Load_v1(Element* document)
string lat_type = latitude_el->GetAttributeValue("type");
if (lat_type == "geod" || lat_type == "geodetic")
position.SetPositionGeodetic(0.0, latitude, 0.0); // Longitude and altitude will be set later on
position.SetPositionGeodetic(longitude, latitude, altitude); // Longitude and altitude will be set later on
else
position.SetLatitude(latitude);
}
if (document->FindElement("longitude"))
SetLongitudeRadIC(document->FindElementValueAsNumberConvertTo("longitude", "RAD"));
if (document->FindElement("elevation"))
SetTerrainElevationFtIC(document->FindElementValueAsNumberConvertTo("elevation", "FT"));
if (document->FindElement("altitude")) // This is feet above ground level
SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitude", "FT"));
else if (document->FindElement("altitudeAGL")) // This is feet above ground level
SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"));
else if (document->FindElement("altitudeMSL")) // This is feet above sea level
SetAltitudeASLFtIC(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"));
FGColumnVector3 vOrient = orientation.GetEuler();
if (document->FindElement("phi"))
@ -1047,15 +1050,6 @@ bool FGInitialCondition::Load_v2(Element* document)
} else if (frame == "ecef") {
if (!position_el->FindElement("x") && !position_el->FindElement("y") && !position_el->FindElement("z")) {
Element* latitude_el = position_el->FindElement("latitude");
if (latitude_el) {
string lat_type = latitude_el->GetAttributeValue("type");
double latitude = position_el->FindElementValueAsNumberConvertTo("latitude", "RAD");
if (lat_type == "geod" || lat_type == "geodetic")
position.SetPositionGeodetic(0.0, latitude, 0.0); // Longitude and altitude will be set later on
else
position.SetLatitude(latitude);
}
if (position_el->FindElement("longitude"))
position.SetLongitude(position_el->FindElementValueAsNumberConvertTo("longitude", "RAD"));
@ -1070,6 +1064,18 @@ bool FGInitialCondition::Load_v2(Element* document)
result = false;
}
double altitude = position.GetAltitudeASL();
double longitude = position.GetLongitude();
if (latitude_el) {
string lat_type = latitude_el->GetAttributeValue("type");
double latitude = position_el->FindElementValueAsNumberConvertTo("latitude", "RAD");
if (lat_type == "geod" || lat_type == "geodetic")
position.SetPositionGeodetic(longitude, latitude, altitude);
else
position.SetLatitude(latitude);
}
} else {
position = position_el->FindElementTripletConvertTo("FT");
}
@ -1433,6 +1439,12 @@ void FGInitialCondition::bind(FGPropertyManager* PropertyManager)
&FGInitialCondition::GetRRadpsIC,
&FGInitialCondition::SetRRadpsIC,
true);
PropertyManager->Tie("ic/lat-geod-rad", &position,
&FGLocation::GetGeodLatitudeRad);
PropertyManager->Tie("ic/lat-geod-deg", &position,
&FGLocation::GetGeodLatitudeDeg);
PropertyManager->Tie("ic/geod-alt-ft", &position,
&FGLocation::GetGeodAltitude);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

115
src/FDM/JSBSim/initialization/FGLinearization.cpp
View file

@ -1,115 +0,0 @@
/*
* FGLinearization.cpp
* Copyright (C) James Goppert 2011 <james.goppert@gmail.com>
*
* FGLinearization.h is free software: you can redistribute it and/or modify it
* under the terms of the GNU Lesser General Public License as published by the
* Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* FGLinearization.h 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "FGInitialCondition.h"
#include "FGLinearization.h"
#include <ctime>
namespace JSBSim {
// TODO make FGLinearization have X,U,Y selectable by xml config file
FGLinearization::FGLinearization(FGFDMExec * fdm, int mode)
{
std::cout << "\nlinearization: " << std::endl;
std::clock_t time_start=clock(), time_linDone;
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);
// get propulsion pointer to determine type/ etc.
FGEngine * engine0 = fdm->GetPropulsion()->GetEngine(0);
FGThruster * thruster0 = engine0->GetThruster();
if (thruster0->GetType()==FGThruster::ttPropeller)
{
ss.x.add(new FGStateSpace::Rpm0);
// TODO add variable prop pitch property
// 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);
if (numEngines>4) {
std::cerr << "more than 4 engines not currently handled" << std::endl;
}
}
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
<< "\n* note: C should be identity, if not, indicates problem with model"
<< "\nD=\n" << std::setw(width) << D
<< std::endl;
// write scicoslab file
std::string aircraft = fdm->GetAircraft()->GetAircraftName();
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_start)/double(CLOCKS_PER_SEC) << " s\n" << std::endl;
}
} // JSBSim
// vim:ts=4:sw=4

47
src/FDM/JSBSim/initialization/FGLinearization.h
View file

@ -1,47 +0,0 @@
/*
* FGLinearization.h
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
*
* FGLinearization.h is free software: you can redistribute it and/or modify it
* under the terms of the GNU Lesser General Public License as published by the
* Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* FGLinearization.h 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef FGLinearization_H_
#define FGLinearization_H_
#include "initialization/FGTrimmer.h"
#include "math/FGStateSpace.h"
#include <iomanip>
#include <fstream>
#include "models/FGAircraft.h"
#include "models/propulsion/FGEngine.h"
#include "models/propulsion/FGTurbine.h"
#include "models/propulsion/FGTurboProp.h"
#include "math/FGNelderMead.h"
#include <stdexcept>
#include <fstream>
#include <cstdlib>
namespace JSBSim {
class FGLinearization
{
public:
FGLinearization(FGFDMExec * fdmPtr, int mode);
};
} // JSBSim
#endif //FGLinearization_H_
// vim:ts=4:sw=4

118
src/FDM/JSBSim/initialization/FGSimplexTrim.cpp
View file

@ -1,118 +0,0 @@
/*
* 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 Lesser 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "FGTrim.h"
#include "FGSimplexTrim.h"
#include <ctime>
namespace JSBSim {
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 = node->GetInt("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;
}
} // JSBSim
// vim:ts=4:sw=4

81
src/FDM/JSBSim/initialization/FGSimplexTrim.h
View file

@ -1,81 +0,0 @@
/*
* FGSimplexTrim.h
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
*
* FGSimplexTrim.h is free software: you can redistribute it and/or modify it
* under the terms of the GNU Lesser General Public License as published by the
* Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* FGSimplexTrim.h 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef FGSimplexTrim_H_
#define FGSimplexTrim_H_
#include "initialization/FGTrimmer.h"
#include "math/FGStateSpace.h"
#include <iomanip>
#include <fstream>
#include "models/FGAircraft.h"
#include "models/propulsion/FGEngine.h"
#include "models/propulsion/FGTurbine.h"
#include "models/propulsion/FGTurboProp.h"
#include "math/FGNelderMead.h"
#include <stdexcept>
#include <fstream>
#include <cstdlib>
namespace JSBSim {
class FGSimplexTrim
{
public:
FGSimplexTrim(FGFDMExec * fdmPtr, TrimMode mode);
private:
template <class varType>
void prompt(const std::string & str, varType & var)
{
std::cout << str + " [" << std::setw(10) << var << "]\t: ";
if (std::cin.peek() != '\n')
{
std::cin >> var;
std::cin.ignore(1000, '\n');
}
else std::cin.get();
}
class Callback : public JSBSim::FGNelderMead::Callback
{
private:
std::ofstream _outputFile;
JSBSim::FGTrimmer * _trimmer;
public:
Callback(std::string fileName, JSBSim::FGTrimmer * trimmer) :
_outputFile((fileName + std::string("_simplexTrim.log")).c_str()),
_trimmer(trimmer) {
}
virtual ~Callback() {
_outputFile.close();
}
void eval(const std::vector<double> &v)
{
_outputFile << _trimmer->eval(v) << std::endl;;
//std::cout << "v: ";
//for (int i=0;i<v.size();i++) std::cout << v[i] << " ";
//std::cout << std::endl;
}
};
};
} // JSBSim
#endif //FGSimplexTrim_H_
// vim:ts=4:sw=4

381
src/FDM/JSBSim/initialization/FGTrimmer.cpp
View file

@ -1,381 +0,0 @@
/*
* FGTrimmer.cpp
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
*
* FGTrimmer.cpp is free software: you can redistribute it and/or modify it
* under the terms of the GNU Lesser General Public License as published by the
* Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* FGTrimmer.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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "FGTrimmer.h"
#include "models/FGFCS.h"
#include "models/FGPropulsion.h"
#include "models/FGAccelerations.h"
#include "models/propulsion/FGEngine.h"
#include "models/propulsion/FGThruster.h"
#include "models/propulsion/FGTank.h"
#include "models/FGMassBalance.h"
#include "models/FGAuxiliary.h"
#include "models/FGAircraft.h"
#include <iomanip>
#include <cstdlib>
#include <stdexcept>
#include "simgear/misc/stdint.hxx"
#include "FGInitialCondition.h"
namespace JSBSim
{
FGTrimmer::FGTrimmer(FGFDMExec * fdm, Constraints * constraints) :
m_fdm(fdm), m_constraints(constraints)
{
}
FGTrimmer::~FGTrimmer()
{
}
std::vector<double> FGTrimmer::constrain(const std::vector<double> & dv)
{
// unpack design vector
double throttle = dv[0];
double elevator = dv[1];
double alpha = dv[2];
double aileron = dv[3];
double rudder = dv[4];
double beta = dv[5];
// initialize constraints
double vt = m_constraints->velocity;
double altitude = m_constraints->altitude;
double gamma = m_constraints->gamma;
double phi = m_fdm->GetIC()->GetPhiRadIC();
double theta = m_fdm->GetIC()->GetThetaRadIC();
double psi = m_fdm->GetIC()->GetPsiRadIC();
double p = 0.0, q = 0.0, r= 0.0;
double u = vt*cos(alpha)*cos(beta);
double v = vt*sin(beta);
double w = vt*sin(alpha)*cos(beta);
double lat = m_fdm->GetIC()->GetLatitudeRadIC();
double lon = m_fdm->GetIC()->GetLongitudeRadIC();
// precomputation
double sGam = sin(gamma);
double sBeta = sin(beta);
double cBeta = cos(beta);
double tAlpha = tan(alpha);
double cAlpha = cos(alpha);
// turn coordination constraint, lewis pg. 190
double gd = m_fdm->GetInertial()->gravity();
double gc = m_constraints->yawRate*vt/gd;
double a = 1 - gc*tAlpha*sBeta;
double b = sGam/cBeta;
double c = 1 + gc*gc*cBeta*cBeta;
phi = atan((gc*cBeta*((a-b*b)+
b*tAlpha*sqrt(c*(1-b*b)+gc*gc*sBeta*sBeta)))/
(cAlpha*(a*a-b*b*(1+c*tAlpha*tAlpha))));
// rate of climb constraint
a = cAlpha*cBeta;
b = sin(phi)*sBeta+cos(phi)*sin(alpha)*cBeta;
theta = atan((a*b+sGam*sqrt(a*a-sGam*sGam+b*b))/(a*a-sGam*sGam));
// turn rates
if (m_constraints->rollRate != 0.0) // rolling
{
p = m_constraints->rollRate;
q = 0.0;
if (m_constraints->stabAxisRoll) // stability axis roll
{
r = m_constraints->rollRate*tan(alpha);
}
else // body axis roll
{
r = m_constraints->rollRate;
}
}
else if (m_constraints->yawRate != 0.0) // yawing
{
p = -m_constraints->yawRate*sin(theta);
q = m_constraints->yawRate*sin(phi)*cos(theta);
r = m_constraints->yawRate*cos(phi)*cos(theta);
}
else if (m_constraints->pitchRate != 0.0) // pitching
{
p = 0.0;
q = m_constraints->pitchRate;
r = 0.0;
}
// apply state
m_fdm->GetIC()->ResetIC(u, v, w,
p, q, r,
alpha, beta,
phi, theta, psi,
lat, lon, altitude,
gamma);
// set controls
m_fdm->GetFCS()->SetDeCmd(elevator);
m_fdm->GetFCS()->SetDePos(ofNorm,elevator);
m_fdm->GetFCS()->SetDaCmd(aileron);
m_fdm->GetFCS()->SetDaLPos(ofNorm,aileron);
m_fdm->GetFCS()->SetDaRPos(ofNorm,aileron);
m_fdm->GetFCS()->SetDrPos(ofNorm,rudder);
m_fdm->GetFCS()->SetDrCmd(rudder);
for (unsigned int i=0; i<m_fdm->GetPropulsion()->GetNumEngines(); i++)
{
m_fdm->GetFCS()->SetThrottleCmd(i,throttle);
m_fdm->GetFCS()->SetThrottlePos(i,throttle);
}
// initialize
m_fdm->Initialize(m_fdm->GetIC());
for (unsigned int i=0; i<m_fdm->GetPropulsion()->GetNumEngines(); i++) {
m_fdm->GetPropulsion()->GetEngine(i)->InitRunning();
}
// wait for stable state
double cost = compute_cost();
for(int i=0;;i++) {
m_fdm->GetPropulsion()->GetSteadyState();
m_fdm->SetTrimStatus(true);
m_fdm->DisableOutput();
m_fdm->SuspendIntegration();
m_fdm->Run();
m_fdm->SetTrimStatus(false);
m_fdm->EnableOutput();
m_fdm->ResumeIntegration();
double costNew = compute_cost();
double dcost = fabs(costNew - cost);
if (dcost < std::numeric_limits<double>::epsilon()) {
if(m_fdm->GetDebugLevel() > 1) {
std::cout << "cost convergd, i: " << i << std::endl;
}
break;
}
if (i > 1000) {
if(m_fdm->GetDebugLevel() > 1) {
std::cout << "cost failed to converge, dcost: "
<< std::scientific
<< dcost << std::endl;
}
break;
}
cost = costNew;
}
std::vector<double> data;
data.push_back(phi);
data.push_back(theta);
return data;
}
void FGTrimmer::printSolution(std::ostream & stream, const std::vector<double> & v)
{
eval(v);
//double dt = m_fdm->GetDeltaT();
double elevator = m_fdm->GetFCS()->GetDePos(ofNorm);
double aileron = m_fdm->GetFCS()->GetDaLPos(ofNorm);
double rudder = m_fdm->GetFCS()->GetDrPos(ofNorm);
double throttle = m_fdm->GetFCS()->GetThrottlePos(0);
double lat = m_fdm->GetPropagate()->GetLatitudeDeg();
double lon = m_fdm->GetPropagate()->GetLongitudeDeg();
double vt = m_fdm->GetAuxiliary()->GetVt();
//double dthrust = (m_fdm->GetPropulsion()->GetEngine(0)->
//GetThruster()->GetThrust()-thrust)/dt;
//double delevator = (m_fdm->GetFCS()->GetDePos(ofNorm)-elevator)/dt;
//double daileron = (m_fdm->GetFCS()->GetDaLPos(ofNorm)-aileron)/dt;
//double drudder = (m_fdm->GetFCS()->GetDrPos(ofNorm)-rudder)/dt;
//double dthrottle = (m_fdm->GetFCS()->GetThrottlePos(0)-throttle)/dt;
//double dlat = (m_fdm->GetPropagate()->GetLatitudeDeg()-lat)/dt;
//double dlon = (m_fdm->GetPropagate()->GetLongitudeDeg()-lon)/dt;
//double dvt = (m_fdm->GetAuxiliary()->GetVt()-vt)/dt;
// reinitialize with correct state
eval(v);
// state
stream << std::setw(10)
// aircraft state
<< "\naircraft state"
<< "\n\tvt\t\t:\t" << vt
<< "\n\talpha, deg\t:\t" << m_fdm->GetIC()->GetAlphaDegIC()
<< "\n\ttheta, deg\t:\t" << m_fdm->GetIC()->GetThetaDegIC()
<< "\n\tq, rad/s\t:\t" << m_fdm->GetIC()->GetQRadpsIC()
<< "\n\tthrust, lbf\t:\t" << m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetThrust()
<< "\n\tbeta, deg\t:\t" << m_fdm->GetIC()->GetBetaDegIC()
<< "\n\tphi, deg\t:\t" << m_fdm->GetIC()->GetPhiDegIC()
<< "\n\tp, rad/s\t:\t" << m_fdm->GetIC()->GetPRadpsIC()
<< "\n\tr, rad/s\t:\t" << m_fdm->GetIC()->GetRRadpsIC()
<< "\n\tmass (lbm)\t:\t" << m_fdm->GetMassBalance()->GetWeight()
// actuator states
<< "\n\nactuator state"
<< "\n\tthrottle, %\t:\t" << 100*throttle
<< "\n\televator, %\t:\t" << 100*elevator
<< "\n\taileron, %\t:\t" << 100*aileron
<< "\n\trudder, %\t:\t" << 100*rudder
// nav state
<< "\n\nnav state"
<< "\n\taltitude, ft\t:\t" << m_fdm->GetIC()->GetAltitudeASLFtIC()
<< "\n\tpsi, deg\t:\t" << m_fdm->GetIC()->GetPsiDegIC()
<< "\n\tlat, deg\t:\t" << lat
<< "\n\tlon, deg\t:\t" << lon
// d/dt aircraft state
<< "\n\naircraft d/dt state"
<< std::scientific
//<< "\n\td/dt vt\t\t\t:\t" << dvt
<< "\n\td/dt alpha, deg/s\t:\t" << m_fdm->GetAuxiliary()->Getadot()*180/M_PI
<< "\n\td/dt theta, deg/s\t:\t" << m_fdm->GetAuxiliary()->GetEulerRates(2)*180/M_PI
<< "\n\td/dt q, rad/s^2\t\t:\t" << m_fdm->GetAccelerations()->GetPQRdot(2)
//<< "\n\td/dt thrust, lbf\t:\t" << dthrust
<< "\n\td/dt beta, deg/s\t:\t" << m_fdm->GetAuxiliary()->Getbdot()*180/M_PI
<< "\n\td/dt phi, deg/s\t\t:\t" << m_fdm->GetAuxiliary()->GetEulerRates(1)*180/M_PI
<< "\n\td/dt p, rad/s^2\t\t:\t" << m_fdm->GetAccelerations()->GetPQRdot(1)
<< "\n\td/dt r, rad/s^2\t\t:\t" << m_fdm->GetAccelerations()->GetPQRdot(3)
// d/dt actuator states
//<< "\n\nd/dt actuator state"
//<< "\n\td/dt throttle, %/s\t:\t" << dthrottle
//<< "\n\td/dt elevator, %/s\t:\t" << delevator
//<< "\n\td/dt aileron, %/s\t:\t" << daileron
//<< "\n\td/dt rudder, %/s\t:\t" << drudder
// nav state
<< "\n\nd/dt nav state"
<< "\n\td/dt altitude, ft/s\t:\t" << m_fdm->GetPropagate()->Gethdot()
<< "\n\td/dt psi, deg/s\t\t:\t" << m_fdm->GetAuxiliary()->GetEulerRates(3)
//<< "\n\td/dt lat, deg/s\t\t:\t" << dlat
//<< "\n\td/dt lon, deg/s\t\t:\t" << dlon
<< std::fixed
<< "\n\npropulsion system state"
<< std::scientific << std::setw(10);
for (unsigned int i=0;i<m_fdm->GetPropulsion()->GetNumTanks();i++) {
stream
<< "\n\ttank " << i << ": fuel (lbm)\t\t\t:\t"
<< m_fdm->GetPropulsion()->GetTank(i)->GetContents();
}
for (unsigned int i=0;i<m_fdm->GetPropulsion()->GetNumEngines();i++) {
m_fdm->GetPropulsion()->GetEngine(i)->CalcFuelNeed();
stream
<< "\n\tengine " << i
<< "\n\t\tfuel flow rate (lbm/s)\t\t:\t" << m_fdm->GetPropulsion()->GetEngine(i)->GetFuelFlowRate()
<< "\n\t\tfuel flow rate (gph)\t\t:\t" << m_fdm->GetPropulsion()->GetEngine(i)->GetFuelFlowRateGPH()
<< "\n\t\tstarved\t\t\t\t:\t" << m_fdm->GetPropulsion()->GetEngine(i)->GetStarved()
<< "\n\t\trunning\t\t\t\t:\t" << m_fdm->GetPropulsion()->GetEngine(i)->GetRunning()
<< std::endl;
}
}
void FGTrimmer::printState(std::ostream & stream)
{
// state
stream << std::setw(10)
// interval method comparison
//<< "\n\ninterval method comparison"
//<< "\nAngle of Attack: \t:\t" << m_fdm->GetAuxiliary()->Getalpha(ofDeg) << "\twdot: " << m_fdm->GetAccelerations()->GetUVWdot(3)
//<< "\nThrottle: \t:\t" << 100*m_fdm->GetFCS()->GetThrottlePos(0) << "\tudot: " << m_fdm->GetAccelerations()->GetUVWdot(1)
//<< "\nPitch Trim: \t:\t" << 100*m_fdm->GetFCS()->GetDePos(ofNorm) << "\tqdot: " << m_fdm->GetAccelerations()->GetPQRdot(2)
//<< "\nSideslip: \t:\t" << m_fdm->GetAuxiliary()->Getbeta(ofDeg) << "\tvdot: " << m_fdm->GetAccelerations()->GetUVWdot(2)
//<< "\nAilerons: \t:\t" << 100*m_fdm->GetFCS()->GetDaLPos(ofNorm) << "\tpdot: " << m_fdm->GetAccelerations()->GetPQRdot(1)
//<< "\nRudder: \t:\t" << 100*m_fdm->GetFCS()->GetDrPos(ofNorm) << "\trdot: " << m_fdm->GetAccelerations()->GetPQRdot(3)
<< "\n\naircraft state"
<< "\nvt\t\t:\t" << m_fdm->GetAuxiliary()->GetVt()
<< "\nalpha, deg\t:\t" << m_fdm->GetAuxiliary()->Getalpha(ofDeg)
<< "\ntheta, deg\t:\t" << m_fdm->GetPropagate()->GetEuler(2)*180/M_PI
<< "\nq, rad/s\t:\t" << m_fdm->GetPropagate()->GetPQR(2)
<< "\nthrust, lbf\t:\t" << m_fdm->GetPropulsion()->GetEngine(0)->GetThruster()->GetThrust()
<< "\nbeta, deg\t:\t" << m_fdm->GetAuxiliary()->Getbeta(ofDeg)
<< "\nphi, deg\t:\t" << m_fdm->GetPropagate()->GetEuler(1)*180/M_PI
<< "\np, rad/s\t:\t" << m_fdm->GetPropagate()->GetPQR(1)
<< "\nr, rad/s\t:\t" << m_fdm->GetPropagate()->GetPQR(3)
// actuator states
<< "\n\nactuator state"
<< "\nthrottle, %\t:\t" << 100*m_fdm->GetFCS()->GetThrottlePos(0)
<< "\nelevator, %\t:\t" << 100*m_fdm->GetFCS()->GetDePos(ofNorm)
<< "\naileron, %\t:\t" << 100*m_fdm->GetFCS()->GetDaLPos(ofNorm)
<< "\nrudder, %\t:\t" << 100*m_fdm->GetFCS()->GetDrPos(ofNorm)
// nav state
<< "\n\nnav state"
<< "\naltitude, ft\t:\t" << m_fdm->GetPropagate()->GetAltitudeASL()
<< "\npsi, deg\t:\t" << m_fdm->GetPropagate()->GetEuler(3)*180/M_PI
<< "\nlat, deg\t:\t" << m_fdm->GetPropagate()->GetLatitudeDeg()
<< "\nlon, deg\t:\t" << m_fdm->GetPropagate()->GetLongitudeDeg()
// input
<< "\n\ninput"
<< "\nthrottle cmd, %\t:\t" << 100*m_fdm->GetFCS()->GetThrottleCmd(0)
<< "\nelevator cmd, %\t:\t" << 100*m_fdm->GetFCS()->GetDeCmd()
<< "\naileron cmd, %\t:\t" << 100*m_fdm->GetFCS()->GetDaCmd()
<< "\nrudder cmd, %\t:\t" << 100*m_fdm->GetFCS()->GetDrCmd()
<< std::endl;
}
double FGTrimmer::compute_cost()
{
double dvt = (m_fdm->GetPropagate()->GetUVW(1)*m_fdm->GetAccelerations()->GetUVWdot(1) +
m_fdm->GetPropagate()->GetUVW(2)*m_fdm->GetAccelerations()->GetUVWdot(2) +
m_fdm->GetPropagate()->GetUVW(3)*m_fdm->GetAccelerations()->GetUVWdot(3))/
m_fdm->GetAuxiliary()->GetVt(); // from lewis, vtrue dot
double dalpha = m_fdm->GetAuxiliary()->Getadot();
double dbeta = m_fdm->GetAuxiliary()->Getbdot();
double dp = m_fdm->GetAccelerations()->GetPQRdot(1);
double dq = m_fdm->GetAccelerations()->GetPQRdot(2);
double dr = m_fdm->GetAccelerations()->GetPQRdot(3);
if(m_fdm->GetDebugLevel() > 1) {
std::cout
<< "dvt: " << dvt
<< "\tdalpha: " << dalpha
<< "\tdbeta: " << dbeta
<< "\tdp: " << dp
<< "\tdq: " << dq
<< "\tdr: " << dr
<< std::endl;
}
return dvt*dvt +
100.0*(dalpha*dalpha + dbeta*dbeta) +
10.0*(dp*dp + dq*dq + dr*dr);
}
double FGTrimmer::eval(const std::vector<double> & v)
{
constrain(v);
return compute_cost();
}
} // JSBSim
// vim:ts=4:sw=4:expandtab

66
src/FDM/JSBSim/initialization/FGTrimmer.h
View file

@ -1,66 +0,0 @@
/*
* FGTrimmer.h
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
*
* FGTrimmer.h is free software: you can redistribute it and/or modify it
* under the terms of the GNU Lesser General Public License as published by the
* Free Software Foundation, either version 2 of the License, or
* (at your option) any later version.
*
* FGTrimmer.h 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 Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef JSBSim_FGTrimmer_H
#define JSBSim_FGTrimmer_H
#include "math/FGNelderMead.h"
#include "FGFDMExec.h"
#include "models/FGInertial.h"
namespace JSBSim
{
class FGTrimmer : public FGNelderMead::Function
{
public:
struct Constraints
{
Constraints() :
velocity(100), altitude(1000), gamma(0),
rollRate(0), pitchRate(0), yawRate(0),
coordinatedTurn(true), stabAxisRoll(true)
{
}
double velocity, altitude, gamma;
double rollRate, pitchRate, yawRate;
bool coordinatedTurn, stabAxisRoll;
};
FGTrimmer(FGFDMExec * fdm, Constraints * constraints);
~FGTrimmer();
std::vector<double> constrain(const std::vector<double> & v);
void printSolution(std::ostream & stream, const std::vector<double> & v);
void printState(std::ostream & stream);
double compute_cost();
double eval(const std::vector<double> & v);
static void limit(double min, double max, double &val)
{
if (val<min) val=min;
else if (val>max) val=max;
}
void setFdm(FGFDMExec * fdm) {m_fdm = fdm; }
private:
FGFDMExec * m_fdm;
Constraints * m_constraints;
};
} // JSBSim
#endif
// vim:ts=4:sw=4

9
src/FDM/JSBSim/models/FGPropagate.cpp
View file

@ -79,7 +79,7 @@ using namespace std;
namespace JSBSim {
IDENT(IdSrc,"$Id: FGPropagate.cpp,v 1.127 2015/08/22 18:09:00 bcoconni Exp $");
IDENT(IdSrc,"$Id: FGPropagate.cpp,v 1.128 2016/01/23 10:48:11 bcoconni Exp $");
IDENT(IdHdr,ID_PROPAGATE);
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -87,8 +87,7 @@ CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
FGPropagate::FGPropagate(FGFDMExec* fdmex)
: FGModel(fdmex),
VehicleRadius(0)
: FGModel(fdmex)
{
Debug(0);
Name = "FGPropagate";
@ -172,7 +171,6 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
// Compute local terrain velocity
RecomputeLocalTerrainVelocity();
VehicleRadius = GetRadius();
// Set the angular velocities of the body frame relative to the ECEF frame,
// expressed in the body frame.
@ -254,7 +252,6 @@ bool FGPropagate::Run(bool Holding)
// Set auxilliary state variables
RecomputeLocalTerrainVelocity();
VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
@ -551,7 +548,6 @@ void FGPropagate::SetVState(const VehicleState& vstate)
UpdateLocationMatrices();
SetInertialOrientation(vstate.qAttitudeECI);
RecomputeLocalTerrainVelocity();
VehicleRadius = GetRadius();
VState.vUVW = vstate.vUVW;
vVel = Tb2l * VState.vUVW;
VState.vPQR = vstate.vPQR;
@ -564,7 +560,6 @@ void FGPropagate::SetVState(const VehicleState& vstate)
void FGPropagate::UpdateVehicleState(void)
{
RecomputeLocalTerrainVelocity();
VehicleRadius = GetRadius();
VState.vInertialPosition = Tec2i * VState.vLocation;
UpdateLocationMatrices();
UpdateBodyMatrices();

6
src/FDM/JSBSim/models/FGPropagate.h
View file

@ -49,7 +49,7 @@ INCLUDES
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.82 2015/08/22 18:09:00 bcoconni Exp $"
#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.83 2016/01/23 10:48:11 bcoconni Exp $"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
@ -92,7 +92,7 @@ CLASS DOCUMENTATION
@endcode
@author Jon S. Berndt, Mathias Froehlich, Bertrand Coconnier
@version $Id: FGPropagate.h,v 1.82 2015/08/22 18:09:00 bcoconni Exp $
@version $Id: FGPropagate.h,v 1.83 2016/01/23 10:48:11 bcoconni Exp $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -550,7 +550,6 @@ public:
void SetRadius(double r)
{
VState.vLocation.SetRadius(r);
VehicleRadius = r;
VState.vInertialPosition = Tec2i * VState.vLocation;
}
@ -618,7 +617,6 @@ private:
FGQuaternion Qec2b;
double VehicleRadius;
FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
eIntegrateType integrator_rotational_rate;