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);
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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 {
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);
//******************************************************************************
{
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");
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"))
} 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"));
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");
}
&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);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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
+++ /dev/null
-/*
- * 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
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);
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
FGPropagate::FGPropagate(FGFDMExec* fdmex)
- : FGModel(fdmex),
- VehicleRadius(0)
+ : FGModel(fdmex)
{
Debug(0);
Name = "FGPropagate";
// 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.
// Set auxilliary state variables
RecomputeLocalTerrainVelocity();
- VehicleRadius = GetRadius(); // Calculate current aircraft radius from center of planet
VState.vPQR = VState.vPQRi - Ti2b * in.vOmegaPlanet;
UpdateLocationMatrices();
SetInertialOrientation(vstate.qAttitudeECI);
RecomputeLocalTerrainVelocity();
- VehicleRadius = GetRadius();
VState.vUVW = vstate.vUVW;
vVel = Tb2l * VState.vUVW;
VState.vPQR = vstate.vPQR;
void FGPropagate::UpdateVehicleState(void)
{
RecomputeLocalTerrainVelocity();
- VehicleRadius = GetRadius();
VState.vInertialPosition = Tec2i * VState.vLocation;
UpdateLocationMatrices();
UpdateBodyMatrices();
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
@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 $
*/
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void SetRadius(double r)
{
VState.vLocation.SetRadius(r);
- VehicleRadius = r;
VState.vInertialPosition = Tec2i * VState.vLocation;
}
FGQuaternion Qec2b;
- double VehicleRadius;
FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity;
eIntegrateType integrator_rotational_rate;
projects / flightgear.git / commitdiff