From 5ef3413373d439490d0bbbbdec5ee4b778da0056 Mon Sep 17 00:00:00 2001 From: Erik Hofman Date: Fri, 29 Jan 2016 12:23:58 +0100 Subject: [PATCH] Add the latest batch of JSBSim changes. Tested with the Shuttle --- src/FDM/JSBSim/FGJSBBase.cpp | 4 +- .../initialization/FGInitialCondition.cpp | 58 +-- .../JSBSim/initialization/FGLinearization.cpp | 115 ------ .../JSBSim/initialization/FGLinearization.h | 47 --- .../JSBSim/initialization/FGSimplexTrim.cpp | 118 ------ src/FDM/JSBSim/initialization/FGSimplexTrim.h | 81 ---- src/FDM/JSBSim/initialization/FGTrimmer.cpp | 381 ------------------ src/FDM/JSBSim/initialization/FGTrimmer.h | 66 --- src/FDM/JSBSim/models/FGPropagate.cpp | 9 +- src/FDM/JSBSim/models/FGPropagate.h | 6 +- 10 files changed, 41 insertions(+), 844 deletions(-) delete mode 100644 src/FDM/JSBSim/initialization/FGLinearization.cpp delete mode 100644 src/FDM/JSBSim/initialization/FGLinearization.h delete mode 100644 src/FDM/JSBSim/initialization/FGSimplexTrim.cpp delete mode 100644 src/FDM/JSBSim/initialization/FGSimplexTrim.h delete mode 100644 src/FDM/JSBSim/initialization/FGTrimmer.cpp delete mode 100644 src/FDM/JSBSim/initialization/FGTrimmer.h diff --git a/src/FDM/JSBSim/FGJSBBase.cpp b/src/FDM/JSBSim/FGJSBBase.cpp index 8a8fbc82f..fa587ca53 100644 --- a/src/FDM/JSBSim/FGJSBBase.cpp +++ b/src/FDM/JSBSim/FGJSBBase.cpp @@ -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 { diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp index 262ef8ada..8b9b2c0fe 100644 --- a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp +++ b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp @@ -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); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/src/FDM/JSBSim/initialization/FGLinearization.cpp b/src/FDM/JSBSim/initialization/FGLinearization.cpp deleted file mode 100644 index 9438f795f..000000000 --- a/src/FDM/JSBSim/initialization/FGLinearization.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/* - * FGLinearization.cpp - * Copyright (C) James Goppert 2011 - * - * 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 . - */ - -#include "FGInitialCondition.h" -#include "FGLinearization.h" -#include - -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 > A,B,C,D; - std::vector x0 = ss.x.get(), u0 = ss.u.get(); - std::vector 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 diff --git a/src/FDM/JSBSim/initialization/FGLinearization.h b/src/FDM/JSBSim/initialization/FGLinearization.h deleted file mode 100644 index e26754430..000000000 --- a/src/FDM/JSBSim/initialization/FGLinearization.h +++ /dev/null @@ -1,47 +0,0 @@ -/* - * FGLinearization.h - * Copyright (C) James Goppert 2010 - * - * 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 . - */ - -#ifndef FGLinearization_H_ -#define FGLinearization_H_ - -#include "initialization/FGTrimmer.h" -#include "math/FGStateSpace.h" -#include -#include -#include "models/FGAircraft.h" -#include "models/propulsion/FGEngine.h" -#include "models/propulsion/FGTurbine.h" -#include "models/propulsion/FGTurboProp.h" -#include "math/FGNelderMead.h" -#include -#include -#include - -namespace JSBSim { - -class FGLinearization -{ -public: - FGLinearization(FGFDMExec * fdmPtr, int mode); -}; - -} // JSBSim - -#endif //FGLinearization_H_ - -// vim:ts=4:sw=4 diff --git a/src/FDM/JSBSim/initialization/FGSimplexTrim.cpp b/src/FDM/JSBSim/initialization/FGSimplexTrim.cpp deleted file mode 100644 index 5aa104b5b..000000000 --- a/src/FDM/JSBSim/initialization/FGSimplexTrim.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/* - * FGSimplexTrim.cpp - * Copyright (C) James Goppert 2010 - * - * 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 . - */ - -#include "FGTrim.h" -#include "FGSimplexTrim.h" -#include - -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 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 diff --git a/src/FDM/JSBSim/initialization/FGSimplexTrim.h b/src/FDM/JSBSim/initialization/FGSimplexTrim.h deleted file mode 100644 index 9bfe51c3e..000000000 --- a/src/FDM/JSBSim/initialization/FGSimplexTrim.h +++ /dev/null @@ -1,81 +0,0 @@ -/* - * FGSimplexTrim.h - * Copyright (C) James Goppert 2010 - * - * 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 . - */ - -#ifndef FGSimplexTrim_H_ -#define FGSimplexTrim_H_ - -#include "initialization/FGTrimmer.h" -#include "math/FGStateSpace.h" -#include -#include -#include "models/FGAircraft.h" -#include "models/propulsion/FGEngine.h" -#include "models/propulsion/FGTurbine.h" -#include "models/propulsion/FGTurboProp.h" -#include "math/FGNelderMead.h" -#include -#include -#include - -namespace JSBSim { - -class FGSimplexTrim -{ -public: - FGSimplexTrim(FGFDMExec * fdmPtr, TrimMode mode); -private: - template - 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 &v) - { - _outputFile << _trimmer->eval(v) << std::endl;; - //std::cout << "v: "; - //for (int i=0;i - * - * 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 . - */ - -#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 -#include -#include -#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 FGTrimmer::constrain(const std::vector & 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; iGetPropulsion()->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; iGetPropulsion()->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::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 data; - data.push_back(phi); - data.push_back(theta); - return data; -} - -void FGTrimmer::printSolution(std::ostream & stream, const std::vector & 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;iGetPropulsion()->GetNumTanks();i++) { - stream - << "\n\ttank " << i << ": fuel (lbm)\t\t\t:\t" - << m_fdm->GetPropulsion()->GetTank(i)->GetContents(); - } - - for (unsigned int i=0;iGetPropulsion()->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 & v) -{ - constrain(v); - return compute_cost(); -} - -} // JSBSim - - -// vim:ts=4:sw=4:expandtab diff --git a/src/FDM/JSBSim/initialization/FGTrimmer.h b/src/FDM/JSBSim/initialization/FGTrimmer.h deleted file mode 100644 index 66be2f02e..000000000 --- a/src/FDM/JSBSim/initialization/FGTrimmer.h +++ /dev/null @@ -1,66 +0,0 @@ -/* - * FGTrimmer.h - * Copyright (C) James Goppert 2010 - * - * 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 . - */ - -#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 constrain(const std::vector & v); - void printSolution(std::ostream & stream, const std::vector & v); - void printState(std::ostream & stream); - double compute_cost(); - double eval(const std::vector & v); - static void limit(double min, double max, double &val) - { - if (valmax) val=max; - } - void setFdm(FGFDMExec * fdm) {m_fdm = fdm; } -private: - FGFDMExec * m_fdm; - Constraints * m_constraints; -}; - -} // JSBSim - -#endif - -// vim:ts=4:sw=4 diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index a2aebf58c..8ccdda3bd 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -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(); diff --git a/src/FDM/JSBSim/models/FGPropagate.h b/src/FDM/JSBSim/models/FGPropagate.h index 682c555b5..b2ee9703b 100644 --- a/src/FDM/JSBSim/models/FGPropagate.h +++ b/src/FDM/JSBSim/models/FGPropagate.h @@ -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; -- 2.39.5