From fab1284f83d6397a3b83c112192b2dff49047d20 Mon Sep 17 00:00:00 2001 From: ThorstenB Date: Fri, 21 Jan 2011 19:55:42 +0100 Subject: [PATCH 01/13] GPWS: avoid "altitude_callout_voice != NULL" assertion Properly remember which active alerts were already voiced. Added NULL-pointer safety check --- src/Instrumentation/mk_viii.cxx | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/Instrumentation/mk_viii.cxx b/src/Instrumentation/mk_viii.cxx index 745ed6c2e..8fb871a4a 100755 --- a/src/Instrumentation/mk_viii.cxx +++ b/src/Instrumentation/mk_viii.cxx @@ -2319,6 +2319,8 @@ MK_VIII::VoicePlayer::get_sample (const char *name) void MK_VIII::VoicePlayer::play (Voice *_voice, unsigned int flags) { + if (!_voice) + return; if (test_bits(flags, PLAY_NOW) || ! voice || voice->element->silence) { if (voice) @@ -2978,9 +2980,10 @@ MK_VIII::AlertHandler::update () mk->voice_player.play(mk_voice(bank_angle_pause_bank_angle)); } - // set new state - - old_alerts = voice_alerts; + // remember all alerts voiced so far... + old_alerts |= voice_alerts; + // ... forget those no longer active + old_alerts &= alerts; repeated_alerts = 0; } From ac4d22628e565987a777a88932fca0b1454a362d Mon Sep 17 00:00:00 2001 From: Torsten Dreyer Date: Fri, 21 Jan 2011 20:44:35 +0100 Subject: [PATCH 02/13] remove debug message spam --- src/Input/FGLinuxEventInput.cxx | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/Input/FGLinuxEventInput.cxx b/src/Input/FGLinuxEventInput.cxx index 4139dd7f5..86c04494e 100644 --- a/src/Input/FGLinuxEventInput.cxx +++ b/src/Input/FGLinuxEventInput.cxx @@ -311,6 +311,7 @@ void FGLinuxInputDevice::Open() continue; } absinfo[i] = ai; +/* SG_LOG( SG_INPUT, SG_INFO, "Axis #" << i << ": value=" << ai.value << ": minimum=" << ai.minimum << @@ -318,6 +319,7 @@ void FGLinuxInputDevice::Open() ": fuzz=" << ai.fuzz << ": flat=" << ai.flat << ": resolution=" << ai.resolution ); +*/ // kick an initial event struct input_event event; From f32656b2a5b567a0725e5ef2e2c254bd4f090185 Mon Sep 17 00:00:00 2001 From: ThorstenB Date: Fri, 21 Jan 2011 23:44:23 +0100 Subject: [PATCH 03/13] Fix crash in GPS module. Fixes crash on exit (during property untie). Also potential run-time crash. --- src/Instrumentation/gps.cxx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index 853d49d46..f0ecf6f04 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -1158,7 +1158,7 @@ double GPS::getWP1Bearing() const double GPS::getWP1MagBearing() const { - if (!_dataValid) { + if (!_dataValid || !_wayptController.get()) { return -9999.0; } From 110894408772ac7a8d8f0c530e8a05de5e42fb00 Mon Sep 17 00:00:00 2001 From: James Turner Date: Sat, 22 Jan 2011 18:49:54 +0000 Subject: [PATCH 04/13] Fix Apple libsvn include for isysroot (SDK) builds. --- utils/TerraSync/CMakeLists.txt | 11 +++++++---- utils/TerraSync/terrasync.cxx | 2 ++ 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/utils/TerraSync/CMakeLists.txt b/utils/TerraSync/CMakeLists.txt index 86526eb0b..88b208c16 100644 --- a/utils/TerraSync/CMakeLists.txt +++ b/utils/TerraSync/CMakeLists.txt @@ -1,7 +1,4 @@ -if(LIBSVN_FOUND) - include_directories(${LIBSVN_INCLUDE_DIR}) -endif(LIBSVN_FOUND) add_executable(terrasync terrasync.cxx) @@ -12,7 +9,13 @@ target_link_libraries(terrasync if(LIBSVN_FOUND) target_link_libraries(terrasync ${LIBSVN_LIBRARIES}) - set_property(TARGET terrasync APPEND PROPERTY COMPILE_FLAGS ${APR_CFLAGS}) + set_property(TARGET terrasync APPEND PROPERTY COMPILE_FLAGS "${APR_CFLAGS}") + + IF(APPLE) + set_property(SOURCE terrasync.cxx PROPERTY COMPILE_FLAGS "-iwithsysroot ${LIBSVN_INCLUDE_DIR}") + ELSE() + include_directories(${LIBSVN_INCLUDE_DIR}) + ENDIF(APPLE) endif() diff --git a/utils/TerraSync/terrasync.cxx b/utils/TerraSync/terrasync.cxx index 8776cdc5a..f1a2cc46b 100644 --- a/utils/TerraSync/terrasync.cxx +++ b/utils/TerraSync/terrasync.cxx @@ -183,8 +183,10 @@ int mysvn_setup(void) { if (err) return svn_cmdline_handle_exit_error(err, pool, "terrasync: "); mysvn_ctx->auth_baton = ab; +#if (SVN_VER_MINOR >= 5) mysvn_ctx->conflict_func = NULL; mysvn_ctx->conflict_baton = NULL; +#endif // Now our magic revisions mysvn_rev = (svn_opt_revision_t*) apr_palloc(pool, sizeof(svn_opt_revision_t)); From 7f846d20c79f0c10daa0689bbcc83757c8ebaf3e Mon Sep 17 00:00:00 2001 From: ThorstenB Date: Sun, 23 Jan 2011 00:45:06 +0100 Subject: [PATCH 05/13] Make route editing sane. Drag&drop, insert and remove now update the current waypoint as expected. --- src/Autopilot/route_mgr.cxx | 16 ++++++++++------ src/GUI/WaypointList.cxx | 6 ++++++ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx index 57f7d5d5c..562c0ec68 100644 --- a/src/Autopilot/route_mgr.cxx +++ b/src/Autopilot/route_mgr.cxx @@ -354,11 +354,6 @@ flightgear::WayptRef FGRouteMgr::removeWayptAtIndex(int aIndex) SG_LOG(SG_AUTOPILOT, SG_WARN, "removeWayptAtIndex with invalid index:" << aIndex); return NULL; } - - if (_currentIndex > index) { - --_currentIndex; // shift current index down if necessary - } - WayptVec::iterator it = _route.begin(); it += index; @@ -366,6 +361,15 @@ flightgear::WayptRef FGRouteMgr::removeWayptAtIndex(int aIndex) _route.erase(it); update_mirror(); + + if (_currentIndex == index) { + currentWaypointChanged(); // current waypoint was removed + } + else + if (_currentIndex > index) { + --_currentIndex; // shift current index down if necessary + } + _edited->fireValueChanged(); checkFinished(); @@ -682,7 +686,7 @@ void FGRouteMgr::insertWayptAtIndex(Waypt* aWpt, int aIndex) WayptVec::iterator it = _route.begin(); it += index; - if (_currentIndex > index) { + if (_currentIndex >= index) { ++_currentIndex; } diff --git a/src/GUI/WaypointList.cxx b/src/GUI/WaypointList.cxx index b014d1cd3..ded3d46ec 100644 --- a/src/GUI/WaypointList.cxx +++ b/src/GUI/WaypointList.cxx @@ -83,9 +83,15 @@ public: --destIndex; } + int currentWpIndex = currentWaypoint(); WayptRef w(_rm->removeWayptAtIndex(srcIndex)); SG_LOG(SG_GENERAL, SG_INFO, "wpt:" << w->ident()); _rm->insertWayptAtIndex(w, destIndex); + + if (srcIndex == currentWpIndex) { + // current waypoint was moved + _rm->jumpToIndex(destIndex); + } } virtual void setUpdateCallback(SGCallback* cb) From 65239f8f53bbe924007c7a2b954884dbf5c6c0ab Mon Sep 17 00:00:00 2001 From: Frederic Bouvier Date: Sun, 23 Jan 2011 09:42:13 +0100 Subject: [PATCH 06/13] Build Terrasync with libsvnclient under Windows --- CMakeModules/FindSvnClient.cmake | 62 +++++++++++++++++++++++++++----- 1 file changed, 54 insertions(+), 8 deletions(-) diff --git a/CMakeModules/FindSvnClient.cmake b/CMakeModules/FindSvnClient.cmake index 717b8b04d..4b5e21a5d 100644 --- a/CMakeModules/FindSvnClient.cmake +++ b/CMakeModules/FindSvnClient.cmake @@ -19,7 +19,17 @@ if(HAVE_APR_CONFIG) string(STRIP ${RAW_APR_LIBS} APR_LIBS) else(HAVE_APR_CONFIG) - message(STATUS "apr-1-config not found, implement manual search for APR") + FIND_LIBRARY(APR_LIBS + NAMES libapr-1 apr-1 + HINTS + PATH_SUFFIXES lib64 lib libs64 libs libs/Win32 libs/Win64 + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local + /usr + /opt + ) endif(HAVE_APR_CONFIG) find_path(LIBSVN_INCLUDE_DIR svn_client.h @@ -32,17 +42,53 @@ find_path(LIBSVN_INCLUDE_DIR svn_client.h /opt ) -check_library_exists(svn_client-1 svn_client_checkout "" HAVE_LIB_SVNCLIENT) -check_library_exists(svn_subr-1 svn_cmdline_init "" HAVE_LIB_SVNSUBR) -check_library_exists(svn_ra-1 svn_ra_initialize "" HAVE_LIB_SVNRA) +FIND_LIBRARY(SVNCLIENT_LIBRARY + NAMES libsvn_client-1 svn_client-1 + HINTS + PATH_SUFFIXES lib64 lib libs64 libs libs/Win32 libs/Win64 + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local + /usr + /opt +) + +FIND_LIBRARY(SVNSUBR_LIBRARY + NAMES libsvn_subr-1 svn_subr-1 + HINTS + PATH_SUFFIXES lib64 lib libs64 libs libs/Win32 libs/Win64 + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local + /usr + /opt +) + +FIND_LIBRARY(SVNRA_LIBRARY + NAMES libsvn_ra-1 svn_ra-1 + HINTS + PATH_SUFFIXES lib64 lib libs64 libs libs/Win32 libs/Win64 + PATHS + ~/Library/Frameworks + /Library/Frameworks + /usr/local + /usr + /opt +) include(FindPackageHandleStandardArgs) FIND_PACKAGE_HANDLE_STANDARD_ARGS(LIBSVN DEFAULT_MSG - HAVE_LIB_SVNSUBR - HAVE_LIB_SVNCLIENT - HAVE_LIB_SVNRA + SVNSUBR_LIBRARY + SVNCLIENT_LIBRARY + SVNRA_LIBRARY LIBSVN_INCLUDE_DIR) if(LIBSVN_FOUND) - set(LIBSVN_LIBRARIES "svn_client-1" "svn_subr-1" "svn_ra-1" ${APR_LIBS}) + message(STATUS "LIBSVN_FOUND 1") + + set(LIBSVN_LIBRARIES ${SVNCLIENT_LIBRARY} ${SVNSUBR_LIBRARY} ${SVNRA_LIBRARY} ${APR_LIBS}) +else(LIBSVN_FOUND) + message(STATUS "LIBSVN_FOUND 0") endif(LIBSVN_FOUND) From 58550d9e70169bc844b36616f3bce7498c8edf8f Mon Sep 17 00:00:00 2001 From: Frederic Bouvier Date: Sun, 23 Jan 2011 09:49:04 +0100 Subject: [PATCH 07/13] Cmake: remove debug messages --- CMakeModules/FindSvnClient.cmake | 4 ---- 1 file changed, 4 deletions(-) diff --git a/CMakeModules/FindSvnClient.cmake b/CMakeModules/FindSvnClient.cmake index 4b5e21a5d..b238d3f56 100644 --- a/CMakeModules/FindSvnClient.cmake +++ b/CMakeModules/FindSvnClient.cmake @@ -86,9 +86,5 @@ FIND_PACKAGE_HANDLE_STANDARD_ARGS(LIBSVN DEFAULT_MSG LIBSVN_INCLUDE_DIR) if(LIBSVN_FOUND) - message(STATUS "LIBSVN_FOUND 1") - set(LIBSVN_LIBRARIES ${SVNCLIENT_LIBRARY} ${SVNSUBR_LIBRARY} ${SVNRA_LIBRARY} ${APR_LIBS}) -else(LIBSVN_FOUND) - message(STATUS "LIBSVN_FOUND 0") endif(LIBSVN_FOUND) From feab53b4621cafec0739f6e2fe63607bbc5b9b8b Mon Sep 17 00:00:00 2001 From: ThorstenB Date: Sun, 23 Jan 2011 15:07:09 +0100 Subject: [PATCH 08/13] Added sanity checks to MP receive/send. Stop invalid data (NaN values) from being sent or received via MP. --- src/MultiPlayer/multiplaymgr.cxx | 49 ++++++++++++++++++++++++++++---- src/MultiPlayer/multiplaymgr.hxx | 3 +- 2 files changed, 46 insertions(+), 6 deletions(-) diff --git a/src/MultiPlayer/multiplaymgr.cxx b/src/MultiPlayer/multiplaymgr.cxx index 1d36ec257..89d2eb85a 100644 --- a/src/MultiPlayer/multiplaymgr.cxx +++ b/src/MultiPlayer/multiplaymgr.cxx @@ -1,6 +1,6 @@ ////////////////////////////////////////////////////////////////////// // -// multiplaymgr.hpp +// multiplaymgr.cxx // // Written by Duncan McCreanor, started February 2003. // duncan.mccreanor@airservicesaustralia.com @@ -526,14 +526,44 @@ union FGMultiplayMgr::MsgBuf T_MsgHdr Header; }; +bool +FGMultiplayMgr::isSane(const FGExternalMotionData& motionInfo) +{ + // check for corrupted data (NaNs) + bool isCorrupted = false; + isCorrupted |= ((osg::isNaN(motionInfo.time )) || + (osg::isNaN(motionInfo.lag )) || + (osg::isNaN(motionInfo.orientation(3) ))); + for (unsigned i = 0; (i < 3)&&(!isCorrupted); ++i) + { + isCorrupted |= ((osg::isNaN(motionInfo.position(i) ))|| + (osg::isNaN(motionInfo.orientation(i) ))|| + (osg::isNaN(motionInfo.linearVel(i)) )|| + (osg::isNaN(motionInfo.angularVel(i)) )|| + (osg::isNaN(motionInfo.linearAccel(i)) )|| + (osg::isNaN(motionInfo.angularAccel(i)) )); + } + return !isCorrupted; +} + void FGMultiplayMgr::SendMyPosition(const FGExternalMotionData& motionInfo) { if ((! mInitialised) || (! mHaveServer)) return; + if (! mHaveServer) { - SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::SendMyPosition - no server"); - return; + SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::SendMyPosition - no server"); + return; + } + + if (!isSane(motionInfo)) + { + // Current local data is invalid (NaN), so stop MP transmission. + // => Be nice to older FG versions (no NaN checks) and don't waste bandwidth. + SG_LOG(SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::SendMyPosition - " + << "Local data is invalid (NaN). Data not transmitted."); + return; } static MsgBuf msgBuf; @@ -780,7 +810,7 @@ FGMultiplayMgr::update(double) } if (MsgHdr->Version != PROTO_VER) { SG_LOG( SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::MP_ProcessData - " - << "message has invalid protocoll number!" ); + << "message has invalid protocol number!" ); break; } if (MsgHdr->MsgLen != bytes) { @@ -858,6 +888,15 @@ FGMultiplayMgr::ProcessPosMsg(const FGMultiplayMgr::MsgBuf& Msg, for (unsigned i = 0; i < 3; ++i) motionInfo.angularAccel(i) = XDR_decode_float(PosMsg->angularAccel[i]); + // sanity check: do not allow injection of corrupted data (NaNs) + if (!isSane(motionInfo)) + { + // drop this message, keep old position until receiving valid data + SG_LOG(SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::ProcessPosMsg - " + << "Position message with invalid data (NaN) received from " + << MsgHdr->Callsign); + return; + } //cout << "INPUT MESSAGE\n"; @@ -953,7 +992,7 @@ FGMultiplayMgr::ProcessPosMsg(const FGMultiplayMgr::MsgBuf& Msg, SG_LOG(SG_NETWORK, SG_DEBUG, "Unknown Prop type " << pData->id << " " << pData->type); xdr++; break; - } + } motionInfo.properties.push_back(pData); } diff --git a/src/MultiPlayer/multiplaymgr.hxx b/src/MultiPlayer/multiplaymgr.hxx index 5a08e1aab..8475c0ef1 100644 --- a/src/MultiPlayer/multiplaymgr.hxx +++ b/src/MultiPlayer/multiplaymgr.hxx @@ -1,6 +1,6 @@ ////////////////////////////////////////////////////////////////////// // -// multiplaymgr.hpp +// multiplaymgr.hxx // // Written by Duncan McCreanor, started February 2003. // duncan.mccreanor@airservicesaustralia.com @@ -81,6 +81,7 @@ private: void ProcessPosMsg(const MsgBuf& Msg, const simgear::IPAddress& SenderAddress, long stamp); void ProcessChatMsg(const MsgBuf& Msg, const simgear::IPAddress& SenderAddress); + bool isSane(const FGExternalMotionData& motionInfo); /// maps from the callsign string to the FGAIMultiplayer typedef std::map > MultiPlayerMap; From 4a817a63079733469d76905902509a40af019535 Mon Sep 17 00:00:00 2001 From: Erik Hofman Date: Sun, 23 Jan 2011 15:14:01 +0100 Subject: [PATCH 09/13] Sync. with JSBSim CVS --- src/FDM/JSBSim/FGFDMExec.cpp | 4 +- src/FDM/JSBSim/JSBSim.cxx | 21 - src/FDM/JSBSim/JSBSim.hxx | 1 - .../initialization/FGInitialCondition.cpp | 1557 +++++++++-------- .../initialization/FGInitialCondition.h | 233 +-- src/FDM/JSBSim/input_output/FGScript.cpp | 7 +- src/FDM/JSBSim/math/FGColumnVector3.cpp | 46 +- src/FDM/JSBSim/math/FGColumnVector3.h | 62 +- src/FDM/JSBSim/math/FGMatrix33.cpp | 51 +- src/FDM/JSBSim/math/FGMatrix33.h | 32 +- src/FDM/JSBSim/math/FGQuaternion.cpp | 49 +- src/FDM/JSBSim/math/FGQuaternion.h | 66 +- src/FDM/JSBSim/models/FGAerodynamics.cpp | 15 +- src/FDM/JSBSim/models/FGGasCell.cpp | 28 +- src/FDM/JSBSim/models/FGPropagate.cpp | 109 +- src/FDM/JSBSim/models/FGPropagate.h | 102 +- src/FDM/JSBSim/models/FGPropulsion.cpp | 6 +- src/FDM/JSBSim/models/propulsion/FGRocket.cpp | 6 +- src/FDM/JSBSim/models/propulsion/FGRotor.cpp | 1141 +++++------- src/FDM/JSBSim/models/propulsion/FGRotor.h | 471 +++-- 20 files changed, 1800 insertions(+), 2207 deletions(-) diff --git a/src/FDM/JSBSim/FGFDMExec.cpp b/src/FDM/JSBSim/FGFDMExec.cpp index ab0bf8a9a..b3b7003c2 100644 --- a/src/FDM/JSBSim/FGFDMExec.cpp +++ b/src/FDM/JSBSim/FGFDMExec.cpp @@ -71,7 +71,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGFDMExec.cpp,v 1.83 2010/11/07 13:30:54 jberndt Exp $"; +static const char *IdSrc = "$Id: FGFDMExec.cpp,v 1.84 2011/01/16 16:26:14 bcoconni Exp $"; static const char *IdHdr = ID_FDMEXEC; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -700,7 +700,7 @@ bool FGFDMExec::LoadModel(const string& model, bool addModelToPath) FCS->LateBind(); } catch (string prop) { cerr << endl << fgred << " Could not late bind property " << prop - << ". Aborting." << endl; + << ". Aborting." << reset << endl; result = false; } diff --git a/src/FDM/JSBSim/JSBSim.cxx b/src/FDM/JSBSim/JSBSim.cxx index d92d265f0..e8a96c018 100644 --- a/src/FDM/JSBSim/JSBSim.cxx +++ b/src/FDM/JSBSim/JSBSim.cxx @@ -143,8 +143,6 @@ FGJSBsim::FGJSBsim( double dt ) break; } } - - resetPropertyState(); fdmex = new FGFDMExec( (FGPropertyManager*)globals->get_props() ); @@ -1413,22 +1411,3 @@ void FGJSBsim::update_external_forces(double t_off) fgSetDouble("/fdm/jsbsim/systems/hook/tailhook-pos-deg", fi); } - -void FGJSBsim::resetPropertyState() -{ -// this code works-around bug #222: -// http://code.google.com/p/flightgear-bugs/issues/detail?id=222 -// for whatever reason, having an existing value for the WOW -// property causes the NaNs. Should that be fixed, this code can die - SGPropertyNode* gear = fgGetNode("/fdm/jsbsim/gear", false); - if (!gear) { - return; - } - - int index = 0; - SGPropertyNode* unitNode = NULL; - for (; (unitNode = gear->getChild("unit", index)) != NULL; ++index) { - unitNode->removeChild("WOW", 0, false); - } -} - diff --git a/src/FDM/JSBSim/JSBSim.hxx b/src/FDM/JSBSim/JSBSim.hxx index 1a8037167..6729a141f 100644 --- a/src/FDM/JSBSim/JSBSim.hxx +++ b/src/FDM/JSBSim/JSBSim.hxx @@ -284,7 +284,6 @@ private: void update_external_forces(double t_off); - void resetPropertyState(); }; diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp index cb1fe5710..f9bbc5415 100644 --- a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp +++ b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp @@ -1,7 +1,7 @@ /******************************************************************************* Header: FGInitialCondition.cpp - Author: Tony Peden + Author: Tony Peden, Bertrand Coconnier Date started: 7/1/99 ------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) ------------- @@ -27,6 +27,8 @@ HISTORY -------------------------------------------------------------------------------- 7/1/99 TP Created +11/25/10 BC Complete revision - Use minimal set of variables to prevent + inconsistent states. Wind is correctly handled. FUNCTIONAL DESCRIPTION @@ -44,25 +46,22 @@ INCLUDES #include "FGInitialCondition.h" #include "FGFDMExec.h" +#include "math/FGQuaternion.h" #include "models/FGInertial.h" #include "models/FGAtmosphere.h" -#include "models/FGAerodynamics.h" #include "models/FGPropagate.h" -#include "input_output/FGPropertyManager.h" -#include "input_output/FGXMLElement.h" #include "models/FGPropulsion.h" -#include "input_output/FGXMLParse.h" -#include "math/FGQuaternion.h" +#include "input_output/FGPropertyManager.h" +#include "input_output/string_utilities.h" #include #include #include -#include "input_output/string_utilities.h" using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGInitialCondition.cpp,v 1.50 2010/11/20 16:38:43 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGInitialCondition.cpp,v 1.56 2011/01/23 12:13:44 bcoconni Exp $"; static const char *IdHdr = ID_INITIALCONDITION; //****************************************************************************** @@ -72,8 +71,6 @@ FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) : fdmex(FDMExec) InitializeIC(); if(FDMExec != NULL ) { - fdmex->GetPropagate()->SetAltitudeASL(altitudeASL); - fdmex->GetAtmosphere()->Run(); PropertyManager=fdmex->GetPropertyManager(); Constructing = true; bind(); @@ -101,63 +98,53 @@ void FGInitialCondition::ResetIC(double u0, double v0, double w0, double latRad0, double lonRad0, double altAGLFt0, double gamma0) { + double calpha = cos(alpha0), cbeta = cos(beta0); + double salpha = sin(alpha0), sbeta = sin(beta0); + InitializeIC(); - u = u0; v = v0; w = w0; p = p0; q = q0; r = r0; alpha = alpha0; beta = beta0; phi = phi0; theta = theta0; psi = psi0; - gamma = gamma0; - latitude = latRad0; - longitude = lonRad0; - SetAltitudeAGLFtIC(altAGLFt0); + position.SetPosition(lonRad0, latRad0, altAGLFt0 + terrain_elevation + sea_level_radius); - cphi = cos(phi); sphi = sin(phi); // phi, rad - ctheta = cos(theta); stheta = sin(theta); // theta, rad - cpsi = cos(psi); spsi = sin(psi); // psi, rad - - FGQuaternion Quat( phi, theta, psi ); + FGQuaternion Quat(phi, theta, psi); Quat.Normalize(); + Tl2b = Quat.GetT(); + Tb2l = Quat.GetTInv(); -// const FGMatrix33& _Tl2b = Quat.GetT(); // local to body frame - const FGMatrix33& _Tb2l = Quat.GetTInv(); // body to local + vUVW_NED = Tb2l * FGColumnVector3(u0, v0, w0); + vt = vUVW_NED.Magnitude(); - FGColumnVector3 _vUVW_BODY(u,v,w); - FGColumnVector3 _vUVW_NED = _Tb2l * _vUVW_BODY; - FGColumnVector3 _vWIND_NED(wnorth,weast,wdown); -// FGColumnVector3 _vUVWAero = _Tl2b * ( _vUVW_NED + _vWIND_NED ); - - uw=_vWIND_NED(1); vw=_vWIND_NED(2); ww=_vWIND_NED(3); + Tw2b = FGMatrix33(calpha*cbeta, -calpha*sbeta, -salpha, + sbeta, cbeta, 0.0, + salpha*cbeta, -salpha*sbeta, calpha); + Tb2w = Tw2b.Transposed(); + SetFlightPathAngleRadIC(gamma0); } //****************************************************************************** void FGInitialCondition::InitializeIC(void) { - vt=vc=ve=vg=0; - mach=0; - alpha=beta=gamma=0; + alpha=beta=0; theta=phi=psi=0; - altitudeASL=hdot=0; - latitude=longitude=0; - u=v=w=0; - p=q=r=0; - uw=vw=ww=0; - vnorth=veast=vdown=0; - wnorth=weast=wdown=0; - whead=wcross=0; - wdir=wmag=0; - lastSpeedSet=setvt; - lastWindSet=setwned; - radius_to_vehicle = sea_level_radius = fdmex->GetInertial()->GetRefRadius(); terrain_elevation = 0; + sea_level_radius = fdmex->GetInertial()->GetRefRadius(); + position.SetPosition(0., 0., sea_level_radius); + position.SetEarthPositionAngle(fdmex->GetInertial()->GetEarthPositionAngle()); + vUVW_NED.InitMatrix(); + p=q=r=0; + vt=0; targetNlfIC = 1.0; - salpha=sbeta=stheta=sphi=spsi=sgamma=0; - calpha=cbeta=ctheta=cphi=cpsi=cgamma=1; + Tw2b.InitMatrix(1., 0., 0., 0., 1., 0., 0., 0., 1.); + Tb2w.InitMatrix(1., 0., 0., 0., 1., 0., 0., 0., 1.); + Tl2b.InitMatrix(1., 0., 0., 0., 1., 0., 0., 0., 1.); + Tb2l.InitMatrix(1., 0., 0., 0., 1., 0., 0., 0., 1.); } //****************************************************************************** @@ -197,510 +184,539 @@ void FGInitialCondition::WriteStateFile(int num) //****************************************************************************** -void FGInitialCondition::SetVcalibratedKtsIC(double tt) { - - if(getMachFromVcas(&mach,tt*ktstofps)) { - //cout << "Mach: " << mach << endl; - lastSpeedSet=setvc; - vc=tt*ktstofps; - vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed(); - ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio()); - //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl; - } - else { - cout << "Failed to get Mach number for given Vc and altitude, Vc unchanged." << endl; - cout << "Please mail the set of initial conditions used to apeden@earthlink.net" << endl; - } -} - -//****************************************************************************** - -void FGInitialCondition::SetVequivalentKtsIC(double tt) { - ve=tt*ktstofps; - lastSpeedSet=setve; - vt=ve*1/sqrt(fdmex->GetAtmosphere()->GetDensityRatio()); - mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed(); - vc=calcVcas(mach); -} - -//****************************************************************************** - -void FGInitialCondition::calcAeroEuler(void) +void FGInitialCondition::SetVequivalentKtsIC(double ve) { - double ua = u + uw; - double va = v + vw; - double wa = w + ww; - vt = sqrt( ua*ua + va*va + wa*wa ); + double altitudeASL = position.GetRadius() - sea_level_radius; + double rho = fdmex->GetAtmosphere()->GetDensity(altitudeASL); + double rhoSL = fdmex->GetAtmosphere()->GetDensitySL(); + SetVtrueFpsIC(ve*ktstofps/sqrt(rho/rhoSL)); + lastSpeedSet = setve; +} + +//****************************************************************************** + +void FGInitialCondition::SetMachIC(double mach) +{ + double altitudeASL = position.GetRadius() - sea_level_radius; + double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL); + double soundSpeed = sqrt(SHRatio*Reng*temperature); + SetVtrueFpsIC(mach*soundSpeed); + lastSpeedSet = setmach; +} + +//****************************************************************************** + +void FGInitialCondition::SetVcalibratedKtsIC(double vcas) +{ + double altitudeASL = position.GetRadius() - sea_level_radius; + double mach = getMachFromVcas(fabs(vcas)*ktstofps); + double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL); + double soundSpeed = sqrt(SHRatio*Reng*temperature); + + SetVtrueFpsIC(mach*soundSpeed); + lastSpeedSet = setvc; +} + +//****************************************************************************** +// Updates alpha and beta according to the aircraft true airspeed in the local +// NED frame. + +void FGInitialCondition::calcAeroAngles(const FGColumnVector3& _vt_NED) +{ + FGColumnVector3 _vt_BODY = Tl2b * _vt_NED; + double ua = _vt_BODY(eX); + double va = _vt_BODY(eY); + double wa = _vt_BODY(eZ); + double uwa = sqrt(ua*ua + wa*wa); + double calpha, cbeta; + double salpha, sbeta; + alpha = beta = 0.0; calpha = cbeta = 1.0; salpha = sbeta = 0.0; - double vxz = sqrt( u*u + w*w ); - if( w != 0 ) alpha = atan2( w, u ); - if( vxz != 0 ) { - beta = atan2( v, vxz ); - calpha = u / vxz; - salpha = w / vxz; + + if( wa != 0 ) + alpha = atan2( wa, ua ); + + // alpha cannot be constrained without updating other informations like the + // true speed or the Euler angles. Otherwise we might end up with an inconsistent + // state of the aircraft. + /*alpha = Constrain(fdmex->GetAerodynamics()->GetAlphaCLMin(), alpha, + fdmex->GetAerodynamics()->GetAlphaCLMax());*/ + + if( va != 0 ) + beta = atan2( va, uwa ); + + if (uwa != 0) { + calpha = ua / uwa; + salpha = wa / uwa; } - double vn = sqrt(vxz*vxz + v*v); - if (vn != 0) { - cbeta = vxz / vn; - sbeta = v / vn; + + if (vt != 0) { + cbeta = uwa / vt; + sbeta = va / vt; } + + Tw2b = FGMatrix33(calpha*cbeta, -calpha*sbeta, -salpha, + sbeta, cbeta, 0.0, + salpha*cbeta, -salpha*sbeta, calpha); + Tb2w = Tw2b.Transposed(); } //****************************************************************************** +// Set the ground velocity. Caution it sets the vertical velocity to zero to +// keep backward compatibility. -void FGInitialCondition::SetVgroundFpsIC(double tt) { - vg=tt; - lastSpeedSet=setvg; - vnorth = vg*cos(psi); veast = vg*sin(psi); vdown = 0; - calcUVWfromNED(); - calcAeroEuler(); - mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed(); - vc=calcVcas(mach); - ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio()); -} - -//****************************************************************************** - -void FGInitialCondition::SetVtrueFpsIC(double tt) { - vt=tt; - lastSpeedSet=setvt; - mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed(); - vc=calcVcas(mach); - ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio()); -} - -//****************************************************************************** - -void FGInitialCondition::SetMachIC(double tt) { - mach=tt; - lastSpeedSet=setmach; - vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed(); - vc=calcVcas(mach); - ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio()); - //cout << "Vt: " << vt*fpstokts << " Vc: " << vc*fpstokts << endl; -} - -//****************************************************************************** - -void FGInitialCondition::SetClimbRateFpmIC(double tt) { - SetClimbRateFpsIC(tt/60.0); -} - -//****************************************************************************** - -void FGInitialCondition::SetClimbRateFpsIC(double tt) { - - if(vt > 0.1) { - hdot=tt; - gamma=asin(hdot/vt); - sgamma=sin(gamma); cgamma=cos(gamma); - } -} - -//****************************************************************************** - -void FGInitialCondition::SetFlightPathAngleRadIC(double tt) { - gamma=tt; - sgamma=sin(gamma); cgamma=cos(gamma); - getTheta(); - hdot=vt*sgamma; -} - -//****************************************************************************** - -void FGInitialCondition::SetAlphaRadIC(double tt) { - alpha=tt; - salpha=sin(alpha); calpha=cos(alpha); - getTheta(); -} - -//****************************************************************************** - -void FGInitialCondition::SetThetaRadIC(double tt) { - theta=tt; - stheta=sin(theta); ctheta=cos(theta); - getAlpha(); -} - -//****************************************************************************** - -void FGInitialCondition::SetBetaRadIC(double tt) { - beta=tt; - sbeta=sin(beta); cbeta=cos(beta); - getTheta(); - -} - -//****************************************************************************** - -void FGInitialCondition::SetPhiRadIC(double tt) { - phi=tt; - sphi=sin(phi); cphi=cos(phi); - getTheta(); -} - -//****************************************************************************** - -void FGInitialCondition::SetPsiRadIC(double tt) { - psi=tt; - spsi=sin(psi); cpsi=cos(psi); - calcWindUVW(); -} - -//****************************************************************************** - -void FGInitialCondition::SetUBodyFpsIC(double tt) { - u=tt; - calcAeroEuler(); - lastSpeedSet=setuvw; -} - -//****************************************************************************** - -void FGInitialCondition::SetVBodyFpsIC(double tt) { - v=tt; - calcAeroEuler(); - lastSpeedSet=setuvw; -} - -//****************************************************************************** - -void FGInitialCondition::SetWBodyFpsIC(double tt) { - w=tt; - calcAeroEuler(); - lastSpeedSet=setuvw; -} - - -//****************************************************************************** - -void FGInitialCondition::SetVNorthFpsIC(double tt) { - vnorth = tt; - calcUVWfromNED(); - calcAeroEuler(); - mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed(); - vc=calcVcas(mach); - ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio()); - lastSpeedSet=setned; -} - -//****************************************************************************** - -void FGInitialCondition::SetVEastFpsIC(double tt) { - veast = tt; - calcUVWfromNED(); - calcAeroEuler(); - mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed(); - vc=calcVcas(mach); - ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio()); - lastSpeedSet=setned; -} - -//****************************************************************************** - -void FGInitialCondition::SetVDownFpsIC(double tt) { - vdown = tt; - calcUVWfromNED(); - calcAeroEuler(); - mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed(); - vc=calcVcas(mach); - ve=vt*sqrt(fdmex->GetAtmosphere()->GetDensityRatio()); - SetClimbRateFpsIC(-1*vdown); - lastSpeedSet=setned; -} - -//****************************************************************************** - -double FGInitialCondition::GetUBodyFpsIC(void) const { - if (lastSpeedSet == setvg || lastSpeedSet == setned) - return u; - else - return vt*calpha*cbeta - uw; -} - -//****************************************************************************** - -double FGInitialCondition::GetVBodyFpsIC(void) const { - if (lastSpeedSet == setvg || lastSpeedSet == setned) - return v; - else { - return vt*sbeta - vw; - } -} - -//****************************************************************************** - -double FGInitialCondition::GetWBodyFpsIC(void) const { - if (lastSpeedSet == setvg || lastSpeedSet == setned) - return w; - else - return vt*salpha*cbeta -ww; -} - -//****************************************************************************** - -void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD ) { - wnorth = wN; weast = wE; wdown = wD; - lastWindSet = setwned; - calcWindUVW(); - if(lastSpeedSet == setvg) - SetVgroundFpsIC(vg); -} - -//****************************************************************************** - -void FGInitialCondition::SetCrossWindKtsIC(double cross){ - wcross=cross*ktstofps; - lastWindSet=setwhc; - calcWindUVW(); - if(lastSpeedSet == setvg) - SetVgroundFpsIC(vg); - -} - -//****************************************************************************** - -// positive from left -void FGInitialCondition::SetHeadWindKtsIC(double head){ - whead=head*ktstofps; - lastWindSet=setwhc; - calcWindUVW(); - if(lastSpeedSet == setvg) - SetVgroundFpsIC(vg); - -} - -//****************************************************************************** - -void FGInitialCondition::SetWindDownKtsIC(double wD) { - wdown=wD; - calcWindUVW(); - if(lastSpeedSet == setvg) - SetVgroundFpsIC(vg); -} - -//****************************************************************************** - -void FGInitialCondition::SetWindMagKtsIC(double mag) { - wmag=mag*ktstofps; - lastWindSet=setwmd; - calcWindUVW(); - if(lastSpeedSet == setvg) - SetVgroundFpsIC(vg); -} - -//****************************************************************************** - -void FGInitialCondition::SetWindDirDegIC(double dir) { - wdir=dir*degtorad; - lastWindSet=setwmd; - calcWindUVW(); - if(lastSpeedSet == setvg) - SetVgroundFpsIC(vg); -} - - -//****************************************************************************** - -void FGInitialCondition::calcWindUVW(void) { - - switch(lastWindSet) { - case setwmd: - wnorth=wmag*cos(wdir); - weast=wmag*sin(wdir); - break; - case setwhc: - wnorth=whead*cos(psi) + wcross*cos(psi+M_PI/2); - weast=whead*sin(psi) + wcross*sin(psi+M_PI/2); - break; - case setwned: - break; - } - uw=wnorth*ctheta*cpsi + - weast*ctheta*spsi - - wdown*stheta; - vw=wnorth*( sphi*stheta*cpsi - cphi*spsi ) + - weast*( sphi*stheta*spsi + cphi*cpsi ) + - wdown*sphi*ctheta; - ww=wnorth*(cphi*stheta*cpsi + sphi*spsi) + - weast*(cphi*stheta*spsi - sphi*cpsi) + - wdown*cphi*ctheta; - - - /* cout << "FGInitialCondition::calcWindUVW: wnorth, weast, wdown " - << wnorth << ", " << weast << ", " << wdown << endl; - cout << "FGInitialCondition::calcWindUVW: theta, phi, psi " - << theta << ", " << phi << ", " << psi << endl; - cout << "FGInitialCondition::calcWindUVW: uw, vw, ww " - << uw << ", " << vw << ", " << ww << endl; */ - -} - -//****************************************************************************** - -void FGInitialCondition::SetAltitudeASLFtIC(double tt) +void FGInitialCondition::SetVgroundFpsIC(double vg) { - altitudeASL=tt; - fdmex->GetPropagate()->SetAltitudeASL(altitudeASL); - fdmex->GetAtmosphere()->Run(); - //lets try to make sure the user gets what they intended + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED; + + vUVW_NED(eU) = vg*cos(psi); + vUVW_NED(eV) = vg*sin(psi); + vUVW_NED(eW) = 0.; + _vt_NED = vUVW_NED + _vWIND_NED; + vt = _vt_NED.Magnitude(); + + calcAeroAngles(_vt_NED); + + lastSpeedSet = setvg; +} + +//****************************************************************************** +// Sets the true airspeed. The amplitude of the airspeed is modified but its +// direction is kept unchanged. If there is no wind, the same is true for the +// ground velocity. If there is some wind, the airspeed direction is unchanged +// but this may result in the ground velocity direction being altered. This is +// for backward compatibility. + +void FGInitialCondition::SetVtrueFpsIC(double vtrue) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED; + + if (vt > 0.1) + _vt_NED *= vtrue / vt; + else + _vt_NED = Tb2l * Tw2b * FGColumnVector3(vtrue, 0., 0.); + + vt = vtrue; + vUVW_NED = _vt_NED - _vWIND_NED; + + calcAeroAngles(_vt_NED); + + lastSpeedSet = setvt; +} + +//****************************************************************************** +// When the climb rate is modified, we need to update the angles theta and beta +// to keep the true airspeed amplitude, the AoA and the heading unchanged. +// Beta will be modified if the aircraft roll angle is not null. + +void FGInitialCondition::SetClimbRateFpsIC(double hdot) +{ + if (fabs(hdot) > vt) { + cerr << "The climb rate cannot be higher than the true speed." << endl; + return; + } + + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _WIND_NED = _vt_NED - vUVW_NED; + double hdot0 = _vt_NED(eW); + + if (fabs(hdot0) < vt) { + double scale = sqrt((vt*vt-hdot*hdot)/(vt*vt-hdot0*hdot0)); + _vt_NED(eU) *= scale; + _vt_NED(eV) *= scale; + } + _vt_NED(eW) = hdot; + vUVW_NED = _vt_NED - _WIND_NED; + + // The AoA is not modified here but the function SetAlphaRadIC is updating the + // same angles than SetClimbRateFpsIC needs to update. + // TODO : create a subroutine that only shares the relevant code. + SetAlphaRadIC(alpha); +} + +//****************************************************************************** +// When the AoA is modified, we need to update the angles theta and beta to +// keep the true airspeed amplitude, the climb rate and the heading unchanged. +// Beta will be modified if the aircraft roll angle is not null. + +void FGInitialCondition::SetAlphaRadIC(double alfa) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + + double calpha = cos(alfa), salpha = sin(alfa); + double cpsi = cos(psi), spsi = sin(psi); + double cphi = cos(phi), sphi = sin(phi); + FGMatrix33 Tpsi( cpsi, spsi, 0., + -spsi, cpsi, 0., + 0., 0., 1.); + FGMatrix33 Tphi(1., 0., 0., + 0., cphi, sphi, + 0.,-sphi, cphi); + FGMatrix33 Talpha( calpha, 0., salpha, + 0., 1., 0., + -salpha, 0., calpha); + + FGColumnVector3 v0 = Tpsi * _vt_NED; + FGColumnVector3 n = (Talpha * Tphi).Transposed() * FGColumnVector3(0., 0., 1.); + FGColumnVector3 y = FGColumnVector3(0., 1., 0.); + FGColumnVector3 u = y - DotProduct(y, n) * n; + FGColumnVector3 p = y * n; + + if (DotProduct(p, v0) < 0) p *= -1.0; + p.Normalize(); + + u *= DotProduct(v0, y) / DotProduct(u, y); + + // There are situations where the desired alpha angle cannot be obtained. This + // is not a limitation of the algorithm but is due to the mathematical problem + // not having a solution. This can only be cured by limiting the alpha angle + // or by modifying an additional angle (psi ?). Since this is anticipated to + // be a pathological case (mainly when a high roll angle is required) this + // situation is not addressed below. However if there are complaints about the + // following error being raised too often, we might need to reconsider this + // position. + if (DotProduct(v0, v0) < DotProduct(u, u)) { + cerr << "Cannot modify angle 'alpha' from " << alpha << " to " << alfa << endl; + return; + } + + FGColumnVector3 v1 = u + sqrt(DotProduct(v0, v0) - DotProduct(u, u))*p; + + FGColumnVector3 v0xz(v0(eU), 0., v0(eW)); + FGColumnVector3 v1xz(v1(eU), 0., v1(eW)); + v0xz.Normalize(); + v1xz.Normalize(); + double sinTheta = (v1xz * v0xz)(eY); + theta = asin(sinTheta); + + FGQuaternion Quat(phi, theta, psi); + Quat.Normalize(); + Tl2b = Quat.GetT(); + Tb2l = Quat.GetTInv(); + + FGColumnVector3 v2 = Talpha * Quat.GetT() * _vt_NED; + + alpha = alfa; + beta = atan2(v2(eV), v2(eU)); + double cbeta=0.0, sbeta=0.0; + if (vt != 0.0) { + cbeta = v2(eU) / vt; + sbeta = v2(eV) / vt; + } + Tw2b = FGMatrix33(calpha*cbeta, -calpha*sbeta, -salpha, + sbeta, cbeta, 0.0, + salpha*cbeta, -salpha*sbeta, calpha); + Tb2w = Tw2b.Transposed(); +} + +//****************************************************************************** +// When the beta angle is modified, we need to update the angles theta and psi +// to keep the true airspeed (amplitude and direction - including the climb rate) +// and the alpha angle unchanged. This may result in the aircraft heading (psi) +// being altered especially if there is cross wind. + +void FGInitialCondition::SetBetaRadIC(double bta) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + + beta = bta; + double calpha = cos(alpha), salpha = sin(alpha); + double cbeta = cos(beta), sbeta = sin(beta); + + Tw2b = FGMatrix33(calpha*cbeta, -calpha*sbeta, -salpha, + sbeta, cbeta, 0.0, + salpha*cbeta, -salpha*sbeta, calpha); + Tb2w = Tw2b.Transposed(); + + FGColumnVector3 vf = FGQuaternion(eX, phi).GetTInv() * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 v0xy(_vt_NED(eX), _vt_NED(eY), 0.); + FGColumnVector3 v1xy(sqrt(v0xy(eX)*v0xy(eX)+v0xy(eY)*v0xy(eY)-vf(eY)*vf(eY)),vf(eY),0.); + v0xy.Normalize(); + v1xy.Normalize(); + + if (vf(eX) < 0.) v0xy(eX) *= -1.0; + + double sinPsi = (v1xy * v0xy)(eZ); + double cosPsi = DotProduct(v0xy, v1xy); + psi = atan2(sinPsi, cosPsi); + FGMatrix33 Tpsi( cosPsi, sinPsi, 0., + -sinPsi, cosPsi, 0., + 0., 0., 1.); + + FGColumnVector3 v2xz = Tpsi * _vt_NED; + FGColumnVector3 vfxz = vf; + v2xz(eV) = vfxz(eV) = 0.0; + v2xz.Normalize(); + vfxz.Normalize(); + double sinTheta = (v2xz * vfxz)(eY); + theta = -asin(sinTheta); + + FGQuaternion Quat(phi, theta, psi); + Quat.Normalize(); + Tl2b = Quat.GetT(); + Tb2l = Quat.GetTInv(); +} + +//****************************************************************************** +// Modifies the body frame orientation (roll angle phi). The true airspeed in +// the local NED frame is kept unchanged. Hence the true airspeed in the body +// frame is modified. + +void FGInitialCondition::SetPhiRadIC(double fi) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + + phi = fi; + FGQuaternion Quat = FGQuaternion(phi, theta, psi); + Quat.Normalize(); + Tl2b = Quat.GetT(); + Tb2l = Quat.GetTInv(); + + calcAeroAngles(_vt_NED); +} + +//****************************************************************************** +// Modifies the body frame orientation (pitch angle theta). The true airspeed in +// the local NED frame is kept unchanged. Hence the true airspeed in the body +// frame is modified. + +void FGInitialCondition::SetThetaRadIC(double teta) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + + theta = teta; + FGQuaternion Quat = FGQuaternion(phi, theta, psi); + Quat.Normalize(); + Tl2b = Quat.GetT(); + Tb2l = Quat.GetTInv(); + + calcAeroAngles(_vt_NED); +} + +//****************************************************************************** +// Modifies the body frame orientation (yaw angle psi). The true airspeed in +// the local NED frame is kept unchanged. Hence the true airspeed in the body +// frame is modified. + +void FGInitialCondition::SetPsiRadIC(double psy) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + + psi = psy; + FGQuaternion Quat = FGQuaternion(phi, theta, psi); + Quat.Normalize(); + Tl2b = Quat.GetT(); + Tb2l = Quat.GetTInv(); + + calcAeroAngles(_vt_NED); +} + +//****************************************************************************** +// Modifies an aircraft velocity component (eU, eV or eW) in the body frame. The +// true airspeed is modified accordingly. If there is some wind, the airspeed +// direction modification may differ from the body velocity modification. + +void FGInitialCondition::SetBodyVelFpsIC(int idx, double vel) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED; + FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED; + + _vUVW_BODY(idx) = vel; + vUVW_NED = Tb2l * _vUVW_BODY; + _vt_NED = vUVW_NED + _vWIND_NED; + vt = _vt_NED.Magnitude(); + + calcAeroAngles(_vt_NED); + + lastSpeedSet = setuvw; +} + +//****************************************************************************** +// Modifies an aircraft velocity component (eX, eY or eZ) in the local NED frame. +// The true airspeed is modified accordingly. If there is some wind, the airspeed +// direction modification may differ from the local velocity modification. + +void FGInitialCondition::SetNEDVelFpsIC(int idx, double vel) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED; + + vUVW_NED(idx) = vel; + _vt_NED = vUVW_NED + _vWIND_NED; + vt = _vt_NED.Magnitude(); + + calcAeroAngles(_vt_NED); + + lastSpeedSet = setned; +} + +//****************************************************************************** +// Set wind amplitude and direction in the local NED frame. The aircraft velocity +// with respect to the ground is not changed but the true airspeed is. + +void FGInitialCondition::SetWindNEDFpsIC(double wN, double wE, double wD ) +{ + FGColumnVector3 _vt_NED = vUVW_NED + FGColumnVector3(wN, wE, wD); + vt = _vt_NED.Magnitude(); + + calcAeroAngles(_vt_NED); +} + +//****************************************************************************** +// Set the cross wind velocity (in knots). Here, 'cross wind' means perpendicular +// to the aircraft heading and parallel to the ground. The aircraft velocity +// with respect to the ground is not changed but the true airspeed is. + +void FGInitialCondition::SetCrossWindKtsIC(double cross) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED; + FGColumnVector3 _vCROSS(-sin(psi), cos(psi), 0.); + + // Gram-Schmidt process is used to remove the existing cross wind component + _vWIND_NED -= DotProduct(_vWIND_NED, _vCROSS) * _vCROSS; + // which is now replaced by the new value. + _vWIND_NED += cross * _vCROSS; + _vt_NED = vUVW_NED + _vWIND_NED; + vt = _vt_NED.Magnitude(); + + calcAeroAngles(_vt_NED); +} + +//****************************************************************************** +// Set the head wind velocity (in knots). Here, 'head wind' means parallel +// to the aircraft heading and to the ground. The aircraft velocity +// with respect to the ground is not changed but the true airspeed is. + +void FGInitialCondition::SetHeadWindKtsIC(double head) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED; + FGColumnVector3 _vHEAD(cos(psi), sin(psi), 0.); + + // Gram-Schmidt process is used to remove the existing cross wind component + _vWIND_NED -= DotProduct(_vWIND_NED, _vHEAD) * _vHEAD; + // which is now replaced by the new value. + _vWIND_NED += head * _vHEAD; + _vt_NED = vUVW_NED + _vWIND_NED; + vt = _vt_NED.Magnitude(); + + calcAeroAngles(_vt_NED); +} + +//****************************************************************************** +// Set the vertical wind velocity (in knots). The 'vertical' is taken in the +// local NED frame. The aircraft velocity with respect to the ground is not +// changed but the true airspeed is. + +void FGInitialCondition::SetWindDownKtsIC(double wD) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + + _vt_NED(eW) = vUVW_NED(eW) + wD; + vt = _vt_NED.Magnitude(); + + calcAeroAngles(_vt_NED); +} + +//****************************************************************************** +// Modifies the wind velocity (in knots) while keeping its direction unchanged. +// The vertical component (in local NED frame) is unmodified. The aircraft +// velocity with respect to the ground is not changed but the true airspeed is. + +void FGInitialCondition::SetWindMagKtsIC(double mag) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED; + FGColumnVector3 _vHEAD(_vWIND_NED(eU), _vWIND_NED(eV), 0.); + double windMag = _vHEAD.Magnitude(); + + if (windMag > 0.001) + _vHEAD *= mag / windMag; + else + _vHEAD = FGColumnVector3(mag, 0., 0.); + + _vWIND_NED(eU) = _vHEAD(eU); + _vWIND_NED(eV) = _vHEAD(eV); + _vt_NED = vUVW_NED + _vWIND_NED; + vt = _vt_NED.Magnitude(); + + calcAeroAngles(_vt_NED); +} + +//****************************************************************************** +// Modifies the wind direction while keeping its velocity unchanged. The vertical +// component (in local NED frame) is unmodified. The aircraft velocity with +// respect to the ground is not changed but the true airspeed is. + +void FGInitialCondition::SetWindDirDegIC(double dir) +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED; + double mag = _vWIND_NED.Magnitude(eU, eV); + FGColumnVector3 _vHEAD(mag*cos(dir*degtorad), mag*sin(dir*degtorad), 0.); + + _vWIND_NED(eU) = _vHEAD(eU); + _vWIND_NED(eV) = _vHEAD(eV); + _vt_NED = vUVW_NED + _vWIND_NED; + vt = _vt_NED.Magnitude(); + + calcAeroAngles(_vt_NED); +} + +//****************************************************************************** +// Set the altitude SL. If the airspeed has been previously set with parameters +// that are atmosphere dependent (Mach, VCAS, VEAS) then the true airspeed is +// modified to keep the last set speed to its previous value. + +void FGInitialCondition::SetAltitudeASLFtIC(double alt) +{ + double altitudeASL = position.GetRadius() - sea_level_radius; + double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL); + double soundSpeed = sqrt(SHRatio*Reng*temperature); + double rho = fdmex->GetAtmosphere()->GetDensity(altitudeASL); + double rhoSL = fdmex->GetAtmosphere()->GetDensitySL(); + + double mach0 = vt / soundSpeed; + double vc0 = calcVcas(mach0); + double ve0 = vt * sqrt(rho/rhoSL); + + altitudeASL=alt; + temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL); + soundSpeed = sqrt(SHRatio*Reng*temperature); + rho = fdmex->GetAtmosphere()->GetDensity(altitudeASL); switch(lastSpeedSet) { - case setned: - case setuvw: - case setvt: - SetVtrueKtsIC(vt*fpstokts); - break; - case setvc: - SetVcalibratedKtsIC(vc*fpstokts); - break; - case setve: - SetVequivalentKtsIC(ve*fpstokts); - break; - case setmach: - SetMachIC(mach); - break; - case setvg: - SetVgroundFpsIC(vg); - break; + case setvc: + mach0 = getMachFromVcas(vc0); + case setmach: + SetVtrueFpsIC(mach0 * soundSpeed); + break; + case setve: + SetVtrueFpsIC(ve0 * sqrt(rho/rhoSL)); + break; } + + position.SetRadius(alt + sea_level_radius); } //****************************************************************************** +// Calculate the VCAS. Uses the Rayleigh formula for supersonic speeds +// (See "Introduction to Aerodynamics of a Compressible Fluid - H.W. Liepmann, +// A.E. Puckett - Wiley & sons (1947)" §5.4 pp 75-80) -void FGInitialCondition::SetAltitudeAGLFtIC(double tt) +double FGInitialCondition::calcVcas(double Mach) const { - SetAltitudeASLFtIC(terrain_elevation + tt); -} - -//****************************************************************************** - -void FGInitialCondition::SetSeaLevelRadiusFtIC(double tt) -{ - sea_level_radius = tt; -} - -//****************************************************************************** - -void FGInitialCondition::SetTerrainElevationFtIC(double tt) -{ - terrain_elevation=tt; -} - -//****************************************************************************** - -void FGInitialCondition::calcUVWfromNED(void) -{ - u=vnorth*ctheta*cpsi + - veast*ctheta*spsi - - vdown*stheta; - v=vnorth*( sphi*stheta*cpsi - cphi*spsi ) + - veast*( sphi*stheta*spsi + cphi*cpsi ) + - vdown*sphi*ctheta; - w=vnorth*( cphi*stheta*cpsi + sphi*spsi ) + - veast*( cphi*stheta*spsi - sphi*cpsi ) + - vdown*cphi*ctheta; -} - -//****************************************************************************** - -bool FGInitialCondition::getMachFromVcas(double *Mach,double vcas) { - - bool result=false; - double guess=1.5; - xlo=xhi=0; - xmin=0;xmax=50; - sfunc=&FGInitialCondition::calcVcas; - if(findInterval(vcas,guess)) { - if(solve(&mach,vcas)) - result=true; - } - return result; -} - -//****************************************************************************** - -bool FGInitialCondition::getAlpha(void) { - bool result=false; - double guess=theta-gamma; - - if(vt < 0.01) return 0; - - xlo=xhi=0; - xmin=fdmex->GetAerodynamics()->GetAlphaCLMin(); - xmax=fdmex->GetAerodynamics()->GetAlphaCLMax(); - sfunc=&FGInitialCondition::GammaEqOfAlpha; - if(findInterval(0,guess)){ - if(solve(&alpha,0)){ - result=true; - salpha=sin(alpha); - calpha=cos(alpha); - } - } - calcWindUVW(); - return result; -} - -//****************************************************************************** - -bool FGInitialCondition::getTheta(void) { - bool result=false; - double guess=alpha+gamma; - - if(vt < 0.01) return 0; - - xlo=xhi=0; - xmin=-89;xmax=89; - sfunc=&FGInitialCondition::GammaEqOfTheta; - if(findInterval(0,guess)){ - if(solve(&theta,0)){ - result=true; - stheta=sin(theta); - ctheta=cos(theta); - } - } - calcWindUVW(); - return result; -} - -//****************************************************************************** - -double FGInitialCondition::GammaEqOfTheta(double Theta) { - double a,b,c; - double sTheta,cTheta; - - //theta=Theta; stheta=sin(theta); ctheta=cos(theta); - sTheta=sin(Theta); cTheta=cos(Theta); - calcWindUVW(); - a=wdown + vt*calpha*cbeta + uw; - b=vt*sphi*sbeta + vw*sphi; - c=vt*cphi*salpha*cbeta + ww*cphi; - return vt*sgamma - ( a*sTheta - (b+c)*cTheta); -} - -//****************************************************************************** - -double FGInitialCondition::GammaEqOfAlpha(double Alpha) { - double a,b,c; - double sAlpha,cAlpha; - sAlpha=sin(Alpha); cAlpha=cos(Alpha); - a=wdown + vt*cAlpha*cbeta + uw; - b=vt*sphi*sbeta + vw*sphi; - c=vt*cphi*sAlpha*cbeta + ww*cphi; - - return vt*sgamma - ( a*stheta - (b+c)*ctheta ); -} - -//****************************************************************************** - -double FGInitialCondition::calcVcas(double Mach) { - - double p=fdmex->GetAtmosphere()->GetPressure(); + double altitudeASL = position.GetRadius() - sea_level_radius; + double p=fdmex->GetAtmosphere()->GetPressure(altitudeASL); double psl=fdmex->GetAtmosphere()->GetPressureSL(); double rhosl=fdmex->GetAtmosphere()->GetDensitySL(); - double pt,A,B,D,vcas; + double pt,A,vcas; if (Mach < 0) Mach=0; if (Mach < 1) //calculate total pressure assuming isentropic flow @@ -718,13 +734,10 @@ double FGInitialCondition::calcVcas(double Mach) { // should be small as well. AFAIK, this approach is fairly well accepted // within the aerospace community - B = 5.76*Mach*Mach/(5.6*Mach*Mach - 0.8); - - // The denominator above is zero for Mach ~ 0.38, for which + // The denominator below is zero for Mach ~ 0.38, for which // we'll never be here, so we're safe - D = (2.8*Mach*Mach-0.4)*0.4167; - pt = p*pow(B,3.5)*D; + pt = p*166.92158*pow(Mach,7.0)/pow(7*Mach*Mach-1,2.5); } A = pow(((pt-p)/psl+1),0.28571); @@ -734,104 +747,124 @@ double FGInitialCondition::calcVcas(double Mach) { } //****************************************************************************** +// Reverse the VCAS formula to obtain the corresponding Mach number. For subsonic +// speeds, the reversed formula has a closed form. For supersonic speeds, the +// formula is reversed by the Newton-Raphson algorithm. -bool FGInitialCondition::findInterval(double x,double guess) { - //void find_interval(inter_params &ip,eqfunc f,double y,double constant, int &flag){ - - int i=0; - bool found=false; - double flo,fhi,fguess; - double lo,hi,step; - step=0.1; - fguess=(this->*sfunc)(guess)-x; - lo=hi=guess; - do { - step=2*step; - lo-=step; - hi+=step; - if(lo < xmin) lo=xmin; - if(hi > xmax) hi=xmax; - i++; - flo=(this->*sfunc)(lo)-x; - fhi=(this->*sfunc)(hi)-x; - if(flo*fhi <=0) { //found interval with root - found=true; - if(flo*fguess <= 0) { //narrow interval down a bit - hi=lo+step; //to pass solver interval that is as - //small as possible - } - else if(fhi*fguess <= 0) { - lo=hi-step; - } - } - //cout << "findInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl; - } - while((found == 0) && (i <= 100)); - xlo=lo; - xhi=hi; - return found; -} - -//****************************************************************************** - -bool FGInitialCondition::solve(double *y,double x) +double FGInitialCondition::getMachFromVcas(double vcas) { - double x1,x2,x3,f1,f2,f3,d,d0; - double eps=1E-5; - double const relax =0.9; - int i; - bool success=false; + double altitudeASL = position.GetRadius() - sea_level_radius; + double p=fdmex->GetAtmosphere()->GetPressure(altitudeASL); + double psl=fdmex->GetAtmosphere()->GetPressureSL(); + double rhosl=fdmex->GetAtmosphere()->GetDensitySL(); - //initializations - d=1; - x2 = 0; - x1=xlo;x3=xhi; - f1=(this->*sfunc)(x1)-x; - f3=(this->*sfunc)(x3)-x; - d0=fabs(x3-x1); + double pt = p + psl*(pow(1+vcas*vcas*rhosl/(7.0*psl),3.5)-1); - //iterations - i=0; - while ((fabs(d) > eps) && (i < 100)) { - d=(x3-x1)/d0; - x2 = x1-d*d0*f1/(f3-f1); + if (pt/p < 1.89293) + return sqrt(5.0*(pow(pt/p, 0.2857143) -1)); // Mach < 1 + else { + // Mach >= 1 + double mach = sqrt(0.77666*pt/p); // Initial guess is based on a quadratic approximation of the Rayleigh formula + double delta = 1.; + double target = pt/(166.92158*p); + int iter = 0; - f2=(this->*sfunc)(x2)-x; - //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl; - //cout << " " << f1 << "," << f2 << "," << f3 << endl; - - if(fabs(f2) <= 0.001) { - x1=x3=x2; - } else if(f1*f2 <= 0.0) { - x3=x2; - f3=f2; - f1=relax*f1; - } else if(f2*f3 <= 0) { - x1=x2; - f1=f2; - f3=relax*f3; + // Find the root with Newton-Raphson. Since the differential is never zero, + // the function is monotonic and has only one root with a multiplicity of one. + // Convergence is certain. + while (delta > 1E-5 && iter < 10) { + double m2 = mach*mach; // Mach^2 + double m6 = m2*m2*m2; // Mach^6 + delta = mach*m6/pow(7.0*m2-1.0,2.5) - target; + double diff = 7.0*m6*(2.0*m2-1)/pow(7.0*m2-1.0,3.5); // Never zero when Mach >= 1 + mach -= delta/diff; + iter++; } - //cout << i << endl; - i++; - }//end while - if(i < 100) { - success=true; - *y=x2; - } - //cout << "Success= " << success << " Vcas: " << vcas*fpstokts << " Mach: " << x2 << endl; - return success; + return mach; + } } //****************************************************************************** -double FGInitialCondition::GetWindDirDegIC(void) const { - if(weast != 0.0) - return atan2(weast,wnorth)*radtodeg; - else if(wnorth > 0) - return 0.0; - else - return 180.0; +double FGInitialCondition::GetWindDirDegIC(void) const +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED; + + return _vWIND_NED(eV) == 0.0 ? 0.0 + : atan2(_vWIND_NED(eV), _vWIND_NED(eU))*radtodeg; +} + +//****************************************************************************** + +double FGInitialCondition::GetNEDWindFpsIC(int idx) const +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED; + + return _vWIND_NED(idx); +} + +//****************************************************************************** + +double FGInitialCondition::GetWindFpsIC(void) const +{ + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _vWIND_NED = _vt_NED - vUVW_NED; + + return _vWIND_NED.Magnitude(eU, eV); +} + +//****************************************************************************** + +double FGInitialCondition::GetBodyWindFpsIC(int idx) const +{ + FGColumnVector3 _vt_BODY = Tw2b * FGColumnVector3(vt, 0., 0.); + FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED; + FGColumnVector3 _vWIND_BODY = _vt_BODY - _vUVW_BODY; + + return _vWIND_BODY(idx); +} + +//****************************************************************************** + +double FGInitialCondition::GetVcalibratedKtsIC(void) const +{ + double altitudeASL = position.GetRadius() - sea_level_radius; + double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL); + double soundSpeed = sqrt(SHRatio*Reng*temperature); + double mach = vt / soundSpeed; + return fpstokts * calcVcas(mach); +} + +//****************************************************************************** + +double FGInitialCondition::GetVequivalentKtsIC(void) const +{ + double altitudeASL = position.GetRadius() - sea_level_radius; + double rho = fdmex->GetAtmosphere()->GetDensity(altitudeASL); + double rhoSL = fdmex->GetAtmosphere()->GetDensitySL(); + return fpstokts * vt * sqrt(rho/rhoSL); +} + +//****************************************************************************** + +double FGInitialCondition::GetMachIC(void) const +{ + double altitudeASL = position.GetRadius() - sea_level_radius; + double temperature = fdmex->GetAtmosphere()->GetTemperature(altitudeASL); + double soundSpeed = sqrt(SHRatio*Reng*temperature); + return vt / soundSpeed; +} + +//****************************************************************************** + +double FGInitialCondition::GetBodyVelFpsIC(int idx) const +{ + FGColumnVector3 _vUVW_BODY = Tl2b * vUVW_NED; + + return _vUVW_BODY(idx); } //****************************************************************************** @@ -870,8 +903,23 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath) result = Load_v2(); } else if (version >= 1.0) { result = Load_v1(); - } + } + // Check to see if any engines are specified to be initialized in a running state + FGPropulsion* propulsion = fdmex->GetPropulsion(); + Element* running_elements = document->FindElement("running"); + while (running_elements) { + int n = int(running_elements->GetDataAsNumber()); + try { + propulsion->InitRunning(n); + } catch (string str) { + cerr << str << endl; + result = false; + } + running_elements = document->FindNextElement("running"); + } + + fdmex->RunIC(); fdmex->GetPropagate()->DumpState(); return result; @@ -882,21 +930,32 @@ bool FGInitialCondition::Load(string rstfile, bool useStoredPath) bool FGInitialCondition::Load_v1(void) { bool result = true; - int n; if (document->FindElement("latitude")) - SetLatitudeDegIC(document->FindElementValueAsNumberConvertTo("latitude", "DEG")); + position.SetLatitude(document->FindElementValueAsNumberConvertTo("latitude", "RAD")); if (document->FindElement("longitude")) - SetLongitudeDegIC(document->FindElementValueAsNumberConvertTo("longitude", "DEG")); + position.SetLongitude(document->FindElementValueAsNumberConvertTo("longitude", "RAD")); if (document->FindElement("elevation")) - SetTerrainElevationFtIC(document->FindElementValueAsNumberConvertTo("elevation", "FT")); + terrain_elevation = document->FindElementValueAsNumberConvertTo("elevation", "FT"); if (document->FindElement("altitude")) // This is feet above ground level - SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitude", "FT")); + position.SetRadius(document->FindElementValueAsNumberConvertTo("altitude", "FT") + terrain_elevation + sea_level_radius); else if (document->FindElement("altitudeAGL")) // This is feet above ground level - SetAltitudeAGLFtIC(document->FindElementValueAsNumberConvertTo("altitudeAGL", "FT")); + position.SetRadius(document->FindElementValueAsNumberConvertTo("altitudeAGL", "FT") + terrain_elevation + sea_level_radius); else if (document->FindElement("altitudeMSL")) // This is feet above sea level - SetAltitudeASLFtIC(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT")); + position.SetRadius(document->FindElementValueAsNumberConvertTo("altitudeMSL", "FT") + sea_level_radius); + + if (document->FindElement("phi")) + phi = document->FindElementValueAsNumberConvertTo("phi", "RAD"); + if (document->FindElement("theta")) + theta = document->FindElementValueAsNumberConvertTo("theta", "RAD"); + if (document->FindElement("psi")) + psi = document->FindElementValueAsNumberConvertTo("psi", "RAD"); + + FGQuaternion Quat(phi, theta, psi); + Quat.Normalize(); + Tl2b = Quat.GetT(); + Tb2l = Quat.GetTInv(); if (document->FindElement("ubody")) SetUBodyFpsIC(document->FindElementValueAsNumberConvertTo("ubody", "FT/SEC")); @@ -910,57 +969,35 @@ bool FGInitialCondition::Load_v1(void) SetVEastFpsIC(document->FindElementValueAsNumberConvertTo("veast", "FT/SEC")); if (document->FindElement("vdown")) SetVDownFpsIC(document->FindElementValueAsNumberConvertTo("vdown", "FT/SEC")); - if (document->FindElement("winddir")) - SetWindDirDegIC(document->FindElementValueAsNumberConvertTo("winddir", "DEG")); - if (document->FindElement("vwind")) - SetWindMagKtsIC(document->FindElementValueAsNumberConvertTo("vwind", "KTS")); - if (document->FindElement("hwind")) - SetHeadWindKtsIC(document->FindElementValueAsNumberConvertTo("hwind", "KTS")); - if (document->FindElement("xwind")) - SetCrossWindKtsIC(document->FindElementValueAsNumberConvertTo("xwind", "KTS")); if (document->FindElement("vc")) SetVcalibratedKtsIC(document->FindElementValueAsNumberConvertTo("vc", "KTS")); if (document->FindElement("vt")) SetVtrueKtsIC(document->FindElementValueAsNumberConvertTo("vt", "KTS")); if (document->FindElement("mach")) SetMachIC(document->FindElementValueAsNumber("mach")); - if (document->FindElement("phi")) - SetPhiDegIC(document->FindElementValueAsNumberConvertTo("phi", "DEG")); - if (document->FindElement("theta")) - SetThetaDegIC(document->FindElementValueAsNumberConvertTo("theta", "DEG")); - if (document->FindElement("psi")) - SetPsiDegIC(document->FindElementValueAsNumberConvertTo("psi", "DEG")); - if (document->FindElement("alpha")) - SetAlphaDegIC(document->FindElementValueAsNumberConvertTo("alpha", "DEG")); - if (document->FindElement("beta")) - SetBetaDegIC(document->FindElementValueAsNumberConvertTo("beta", "DEG")); if (document->FindElement("gamma")) SetFlightPathAngleDegIC(document->FindElementValueAsNumberConvertTo("gamma", "DEG")); if (document->FindElement("roc")) SetClimbRateFpsIC(document->FindElementValueAsNumberConvertTo("roc", "FT/SEC")); if (document->FindElement("vground")) SetVgroundKtsIC(document->FindElementValueAsNumberConvertTo("vground", "KTS")); + if (document->FindElement("alpha")) + SetAlphaDegIC(document->FindElementValueAsNumberConvertTo("alpha", "DEG")); + if (document->FindElement("beta")) + SetBetaDegIC(document->FindElementValueAsNumberConvertTo("beta", "DEG")); + if (document->FindElement("vwind")) + SetWindMagKtsIC(document->FindElementValueAsNumberConvertTo("vwind", "KTS")); + if (document->FindElement("winddir")) + SetWindDirDegIC(document->FindElementValueAsNumberConvertTo("winddir", "DEG")); + if (document->FindElement("hwind")) + SetHeadWindKtsIC(document->FindElementValueAsNumberConvertTo("hwind", "KTS")); + if (document->FindElement("xwind")) + SetCrossWindKtsIC(document->FindElementValueAsNumberConvertTo("xwind", "KTS")); if (document->FindElement("targetNlf")) { SetTargetNlfIC(document->FindElementValueAsNumber("targetNlf")); } - // Check to see if any engines are specified to be initialized in a running state - FGPropulsion* propulsion = fdmex->GetPropulsion(); - Element* running_elements = document->FindElement("running"); - while (running_elements) { - n = int(running_elements->GetDataAsNumber()); - try { - propulsion->InitRunning(n); - } catch (string str) { - cerr << str << endl; - result = false; - } - running_elements = document->FindNextElement("running"); - } - - fdmex->RunIC(); - return result; } @@ -968,26 +1005,15 @@ bool FGInitialCondition::Load_v1(void) bool FGInitialCondition::Load_v2(void) { - int n; - double epa = 0.0; - FGColumnVector3 vLoc, vOrient; + FGColumnVector3 vOrient; bool result = true; - FGInertial* Inertial = fdmex->GetInertial(); - FGPropagate* Propagate = fdmex->GetPropagate(); - FGColumnVector3 vOmegaEarth = FGColumnVector3(0.0, 0.0, Inertial->omega()); + FGColumnVector3 vOmegaEarth = FGColumnVector3(0.0, 0.0, fdmex->GetInertial()->omega()); - if (document->FindElement("earth_position_angle")) { - epa = document->FindElementValueAsNumberConvertTo("earth_position_angle", "RAD"); - } - Inertial->SetEarthPositionAngle(epa); - Propagate->GetVState()->vLocation.SetEarthPositionAngle(epa); + if (document->FindElement("earth_position_angle")) + position.SetEarthPositionAngle(document->FindElementValueAsNumberConvertTo("earth_position_angle", "RAD")); - Propagate->SetSeaLevelRadius(GetSeaLevelRadiusFtIC()); - - if (document->FindElement("elevation")) { - SetTerrainElevationFtIC(document->FindElementValueAsNumberConvertTo("elevation", "FT")); - Propagate->SetTerrainElevation(terrain_elevation); - } + if (document->FindElement("elevation")) + terrain_elevation = document->FindElementValueAsNumberConvertTo("elevation", "FT"); // Initialize vehicle position // @@ -995,37 +1021,30 @@ bool FGInitialCondition::Load_v2(void) // - ECI (Earth Centered Inertial) // - ECEF (Earth Centered, Earth Fixed) - Element* position = document->FindElement("position"); - if (position) { - string frame = position->GetAttributeValue("frame"); + Element* position_el = document->FindElement("position"); + if (position_el) { + string frame = position_el->GetAttributeValue("frame"); frame = to_lower(frame); if (frame == "eci") { // Need to transform vLoc to ECEF for storage and use in FGLocation. - vLoc = position->FindElementTripletConvertTo("FT"); - vLoc = Propagate->GetTi2ec()*vLoc; - Propagate->SetLocation(vLoc); + position = position.GetTi2ec() * position_el->FindElementTripletConvertTo("FT"); } else if (frame == "ecef") { - double AltitudeASL = 0.0; - if (!position->FindElement("x") && !position->FindElement("y") && !position->FindElement("z")) { - if (position->FindElement("radius")) { - AltitudeASL = position->FindElementValueAsNumberConvertTo("radius", "FT") - sea_level_radius; - } else if (position->FindElement("altitudeAGL")) { - AltitudeASL = terrain_elevation + position->FindElementValueAsNumberConvertTo("altitudeAGL", "FT"); - } else if (position->FindElement("altitudeMSL")) { - AltitudeASL = position->FindElementValueAsNumberConvertTo("altitudeMSL", "FT"); + if (!position_el->FindElement("x") && !position_el->FindElement("y") && !position_el->FindElement("z")) { + if (position_el->FindElement("radius")) { + position.SetRadius(position_el->FindElementValueAsNumberConvertTo("radius", "FT")); + } else if (position_el->FindElement("altitudeAGL")) { + position.SetRadius(sea_level_radius + terrain_elevation + position_el->FindElementValueAsNumberConvertTo("altitude", "FT")); + } else if (position_el->FindElement("altitudeMSL")) { + position.SetRadius(sea_level_radius + position_el->FindElementValueAsNumberConvertTo("altitudeMSL", "FT")); } else { cerr << endl << " No altitude or radius initial condition is given." << endl; result = false; } - double lat_rad=0.0; - double long_rad = 0.0; - if (position->FindElement("longitude")) - long_rad = position->FindElementValueAsNumberConvertTo("longitude", "RAD"); - if (position->FindElement("latitude")) - lat_rad = position->FindElementValueAsNumberConvertTo("latitude", "RAD"); - Propagate->SetPosition(long_rad, lat_rad, AltitudeASL + GetSeaLevelRadiusFtIC()); + if (position_el->FindElement("longitude")) + position.SetLongitude(position_el->FindElementValueAsNumberConvertTo("longitude", "RAD")); + if (position_el->FindElement("latitude")) + position.SetLatitude(position_el->FindElementValueAsNumberConvertTo("latitude", "RAD")); } else { - vLoc = position->FindElementTripletConvertTo("FT"); - Propagate->SetLocation(vLoc); + position = position_el->FindElementTripletConvertTo("FT"); } } else { cerr << endl << " Neither ECI nor ECEF frame is specified for initial position." << endl; @@ -1044,71 +1063,68 @@ bool FGInitialCondition::Load_v2(void) // - ECEF (Earth Centered, Earth Fixed) // - Local // - // Need to convert the provided orientation to an ECI orientation, using + // Need to convert the provided orientation to a local orientation, using // the given orientation and knowledge of the Earth position angle. // This could be done using matrices (where in the subscript "b/a", - // it is meant "b with respect to a", and where b=body frame, + // it is meant "b with respect to a", and where b=body frame, // i=inertial frame, and e=ecef frame) as: // - // C_b/i = C_b/e * C_e/i + // C_b/l = C_b/e * C_e/l // // Using quaternions (note reverse ordering compared to matrix representation): // - // Q_b/i = Q_e/i * Q_b/e + // Q_b/l = Q_e/l * Q_b/e // // Use the specific matrices as needed. The above example of course is for the whole - // body to inertial orientation. + // body to local orientation. // The new orientation angles can be extracted from the matrix or the quaternion. // ToDo: Do we need to deal with normalization of the quaternions here? Element* orientation_el = document->FindElement("orientation"); - FGQuaternion QuatI2Body; + FGQuaternion QuatLocal2Body; if (orientation_el) { string frame = orientation_el->GetAttributeValue("frame"); frame = to_lower(frame); vOrient = orientation_el->FindElementTripletConvertTo("RAD"); if (frame == "eci") { - QuatI2Body = FGQuaternion(vOrient); + // In this case, we are supplying the Euler angles for the vehicle with + // respect to the inertial system, represented by the C_b/i Matrix. + // We want the body orientation with respect to the local (NED frame): + // + // C_b/l = C_b/i * C_i/l + // + // Or, using quaternions (note reverse ordering compared to matrix representation): + // + // Q_b/l = Q_e/l * Q_b/i + + FGQuaternion QuatI2Body = FGQuaternion(vOrient); + QuatI2Body.Normalize(); + FGQuaternion QuatLocal2I = position.GetTl2i(); + QuatLocal2I.Normalize(); + QuatLocal2Body = QuatLocal2I * QuatI2Body; } else if (frame == "ecef") { // In this case we are given the Euler angles representing the orientation of // the body with respect to the ECEF system, represented by the C_b/e Matrix. - // We want the body orientation with respect to the inertial system: + // We want the body orientation with respect to the local (NED frame): // - // C_b/i = C_b/e * C_e/i + // C_b/l = C_b/e * C_e/l // // Using quaternions (note reverse ordering compared to matrix representation): // - // Q_b/i = Q_e/i * Q_b/e + // Q_b/l = Q_e/l * Q_b/e FGQuaternion QuatEC2Body(vOrient); // Store relationship of Body frame wrt ECEF frame, Q_b/e QuatEC2Body.Normalize(); - FGQuaternion QuatI2EC = Propagate->GetTi2ec(); // Get Q_e/i from matrix - QuatI2EC.Normalize(); - QuatI2Body = QuatI2EC * QuatEC2Body; // Q_b/i = Q_e/i * Q_b/e + FGQuaternion QuatLocal2EC = position.GetTl2ec(); // Get Q_e/l from matrix + QuatLocal2EC.Normalize(); + QuatLocal2Body = QuatLocal2EC * QuatEC2Body; // Q_b/l = Q_e/l * Q_b/e } else if (frame == "local") { - // In this case, we are supplying the Euler angles for the vehicle with - // respect to the local (NED frame), also called the navigation frame. - // This is the most common way of initializing the orientation of - // aircraft. The matrix representation is: - // - // C_b/i = C_b/n * C_n/e * C_e/i - // - // Or, using quaternions (note reverse ordering compared to matrix representation): - // - // Q_b/i = Q_e/i * Q_n/e * Q_b/n - - FGQuaternion QuatLocal2Body = FGQuaternion(vOrient); // Store relationship of Body frame wrt local (NED) frame, Q_b/n - QuatLocal2Body.Normalize(); - FGQuaternion QuatEC2Local = Propagate->GetTec2l(); // Get Q_n/e from matrix - QuatEC2Local.Normalize(); - FGQuaternion QuatI2EC = Propagate->GetTi2ec(); // Get Q_e/i from matrix - QuatI2EC.Normalize(); - QuatI2Body = QuatI2EC * QuatEC2Local * QuatLocal2Body; // Q_b/i = Q_e/i * Q_n/e * Q_b/n + QuatLocal2Body = FGQuaternion(vOrient); } else { @@ -1119,8 +1135,12 @@ bool FGInitialCondition::Load_v2(void) } } - QuatI2Body.Normalize(); - Propagate->SetInertialOrientation(QuatI2Body); + QuatLocal2Body.Normalize(); + phi = QuatLocal2Body.GetEuler(ePhi); + theta = QuatLocal2Body.GetEuler(eTht); + psi = QuatLocal2Body.GetEuler(ePsi); + Tl2b = QuatLocal2Body.GetT(); + Tb2l = QuatLocal2Body.GetTInv(); // Initialize vehicle velocity // Allowable frames @@ -1131,12 +1151,8 @@ bool FGInitialCondition::Load_v2(void) // The vehicle will be defaulted to (0,0,0) in the Body frame if nothing is provided. Element* velocity_el = document->FindElement("velocity"); - FGColumnVector3 vInertialVelocity; FGColumnVector3 vInitVelocity = FGColumnVector3(0.0, 0.0, 0.0); - FGColumnVector3 omega_cross_r = vOmegaEarth * Propagate->GetInertialPosition(); - FGMatrix33 mTl2i = Propagate->GetTl2i(); - FGMatrix33 mTec2i = Propagate->GetTec2i(); // Get C_i/e - FGMatrix33 mTb2i = Propagate->GetTb2i(); + FGMatrix33 mTec2l = position.GetTec2l(); if (velocity_el) { string frame = velocity_el->GetAttributeValue("frame"); @@ -1144,13 +1160,16 @@ bool FGInitialCondition::Load_v2(void) FGColumnVector3 vInitVelocity = velocity_el->FindElementTripletConvertTo("FT/SEC"); if (frame == "eci") { - vInertialVelocity = vInitVelocity; + FGColumnVector3 omega_cross_r = vOmegaEarth * (position.GetTec2i() * position); + vUVW_NED = mTec2l * (vInitVelocity - omega_cross_r); } else if (frame == "ecef") { - vInertialVelocity = mTec2i * vInitVelocity + omega_cross_r; + vUVW_NED = mTec2l * vInitVelocity; } else if (frame == "local") { - vInertialVelocity = mTl2i * vInitVelocity + omega_cross_r; + vUVW_NED = vInitVelocity; + lastSpeedSet = setned; } else if (frame == "body") { - vInertialVelocity = mTb2i * vInitVelocity + omega_cross_r; + vUVW_NED = Tb2l * vInitVelocity; + lastSpeedSet = setuvw; } else { cerr << endl << fgred << " Velocity frame type: \"" << frame @@ -1161,11 +1180,13 @@ bool FGInitialCondition::Load_v2(void) } else { - vInertialVelocity = mTb2i * vInitVelocity + omega_cross_r; + vUVW_NED = Tb2l * vInitVelocity; } - Propagate->SetInertialVelocity(vInertialVelocity); + vt = vUVW_NED.Magnitude(); + + calcAeroAngles(vUVW_NED); // Initialize vehicle body rates // Allowable frames @@ -1173,14 +1194,13 @@ bool FGInitialCondition::Load_v2(void) // - ECEF (Earth Centered, Earth Fixed) // - Body - FGColumnVector3 vInertialRate; + FGColumnVector3 vLocalRate; Element* attrate_el = document->FindElement("attitude_rate"); - double radInv = 1.0/Propagate->GetRadius(); - FGColumnVector3 vVel = Propagate->GetVel(); + double radInv = 1.0 / position.GetRadius(); FGColumnVector3 vOmegaLocal = FGColumnVector3( - radInv*vVel(eEast), - -radInv*vVel(eNorth), - -radInv*vVel(eEast)*Propagate->GetLocation().GetTanLatitude() ); + radInv*vUVW_NED(eEast), + -radInv*vUVW_NED(eNorth), + -radInv*vUVW_NED(eEast)*position.GetTanLatitude() ); if (attrate_el) { @@ -1189,11 +1209,11 @@ bool FGInitialCondition::Load_v2(void) FGColumnVector3 vAttRate = attrate_el->FindElementTripletConvertTo("RAD/SEC"); if (frame == "eci") { - vInertialRate = vAttRate; + vLocalRate = Tl2b * (position.GetTi2l() * (vAttRate - vOmegaEarth) - vOmegaLocal); } else if (frame == "ecef") { - vInertialRate = vAttRate + vOmegaEarth; + vLocalRate = Tl2b * (position.GetTec2l() * vAttRate - vOmegaLocal); } else if (frame == "local") { - vInertialRate = vOmegaEarth + Propagate->GetTl2i() * vOmegaLocal + Propagate->GetTb2i() * vAttRate; + vLocalRate = vAttRate; } else if (!frame.empty()) { // misspelling of frame cerr << endl << fgred << " Attitude rate frame type: \"" << frame @@ -1205,35 +1225,20 @@ bool FGInitialCondition::Load_v2(void) } } else { // Body frame attitude rate assumed 0 relative to local. - vInertialRate = vOmegaEarth + Propagate->GetTl2i() * vOmegaLocal; - } - Propagate->SetInertialRates(vInertialRate); - Propagate->InitializeDerivatives(); - - // Check to see if any engines are specified to be initialized in a running state - FGPropulsion* propulsion = fdmex->GetPropulsion(); - Element* running_elements = document->FindElement("running"); - while (running_elements) { - n = int(running_elements->GetDataAsNumber()); - try { - propulsion->InitRunning(n); - } catch (string str) { - cerr << str << endl; - result = false; - } - running_elements = document->FindNextElement("running"); + vLocalRate.InitMatrix(); } - fdmex->SuspendIntegration(); // saves the integration rate, dt, then sets it to 0.0. - fdmex->Run(); - fdmex->ResumeIntegration(); // Restores the integration rate to what it was. + p = vLocalRate(eP); + q = vLocalRate(eQ); + r = vLocalRate(eR); return result; } //****************************************************************************** -void FGInitialCondition::bind(void){ +void FGInitialCondition::bind(void) +{ PropertyManager->Tie("ic/vc-kts", this, &FGInitialCondition::GetVcalibratedKtsIC, &FGInitialCondition::SetVcalibratedKtsIC, diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.h b/src/FDM/JSBSim/initialization/FGInitialCondition.h index ebd65bda9..308122450 100644 --- a/src/FDM/JSBSim/initialization/FGInitialCondition.h +++ b/src/FDM/JSBSim/initialization/FGInitialCondition.h @@ -47,16 +47,14 @@ SENTRY INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#include "FGFDMExec.h" -#include "FGJSBBase.h" -#include "math/FGColumnVector3.h" #include "input_output/FGXMLFileRead.h" +#include "math/FGLocation.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_INITIALCONDITION "$Id: FGInitialCondition.h,v 1.22 2010/11/20 16:38:43 bcoconni Exp $" +#define ID_INITIALCONDITION "$Id: FGInitialCondition.h,v 1.26 2011/01/16 16:10:59 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -64,8 +62,11 @@ FORWARD DECLARATIONS namespace JSBSim { +class FGFDMExec; +class FGMatrix33; +class FGColumnVector3; + typedef enum { setvt, setvc, setve, setmach, setuvw, setned, setvg } speedset; -typedef enum { setwned, setwmd, setwhc } windset; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS DOCUMENTATION @@ -83,17 +84,27 @@ CLASS DOCUMENTATION With a valid object of FGFDMExec and an aircraft model loaded: @code - FGInitialCondition fgic=new FGInitialCondition(FDMExec); - fgic->SetVcalibratedKtsIC() - fgic->SetAltitudeAGLFtIC(); + FGInitialCondition* fgic = FDMExec->GetIC(); + + // Reset the initial conditions and set VCAS and altitude + fgic->InitializeIC(); + fgic->SetVcalibratedKtsIC(vcas); + fgic->SetAltitudeAGLFtIC(altitude); // directly into Run - FDMExec->GetState()->Initialize(fgic) + FDMExec->GetPropagate()->SetInitialState(fgic); delete fgic; - FDMExec->Run() + FDMExec->Run(); //or to loop the sim w/o integrating - FDMExec->RunIC(fgic) + FDMExec->RunIC(); + @endcode + + Alternatively, you can load initial conditions from an XML file: + + @code + FGInitialCondition* fgic = FDMExec->GetIC(); + fgic->Load(IC_file); @endcode

Speed

@@ -202,7 +213,7 @@ CLASS DOCUMENTATION @property ic/r-rad_sec (read/write) Yaw rate initial condition in radians/second @author Tony Peden - @version "$Id: FGInitialCondition.h,v 1.22 2010/11/20 16:38:43 bcoconni Exp $" + @version "$Id: FGInitialCondition.h,v 1.26 2011/01/16 16:10:59 bcoconni Exp $" */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -227,11 +238,11 @@ public: /** Set true airspeed initial condition in knots. @param vt True airspeed in knots */ - inline void SetVtrueKtsIC(double vt) { SetVtrueFpsIC(vt*ktstofps); } + void SetVtrueKtsIC(double vtrue) { SetVtrueFpsIC(vtrue*ktstofps); } /** Set ground speed initial condition in knots. @param vg Ground speed in knots */ - inline void SetVgroundKtsIC(double vg) { SetVgroundFpsIC(vg*ktstofps); } + void SetVgroundKtsIC(double vg) { SetVgroundFpsIC(vg*ktstofps); } /** Set mach initial condition. @param mach Mach number */ @@ -239,15 +250,15 @@ public: /** Sets angle of attack initial condition in degrees. @param a Alpha in degrees */ - inline void SetAlphaDegIC(double a) { SetAlphaRadIC(a*degtorad); } + void SetAlphaDegIC(double a) { SetAlphaRadIC(a*degtorad); } /** Sets angle of sideslip initial condition in degrees. @param B Beta in degrees */ - inline void SetBetaDegIC(double B) { SetBetaRadIC(B*degtorad);} + void SetBetaDegIC(double b) { SetBetaRadIC(b*degtorad);} /** Sets pitch angle initial condition in degrees. @param theta Theta (pitch) angle in degrees */ - inline void SetThetaDegIC(double theta) { SetThetaRadIC(theta*degtorad); } + void SetThetaDegIC(double theta) { SetThetaRadIC(theta*degtorad); } /** Resets the IC data structure to new values @param u, v, w, ... **/ @@ -258,19 +269,20 @@ public: /** Sets the roll angle initial condition in degrees. @param phi roll angle in degrees */ - inline void SetPhiDegIC(double phi) { SetPhiRadIC(phi*degtorad);} + void SetPhiDegIC(double phi) { SetPhiRadIC(phi*degtorad);} /** Sets the heading angle initial condition in degrees. @param psi Heading angle in degrees */ - inline void SetPsiDegIC(double psi){ SetPsiRadIC(psi*degtorad); } + void SetPsiDegIC(double psi){ SetPsiRadIC(psi*degtorad); } /** Sets the climb rate initial condition in feet/minute. @param roc Rate of Climb in feet/minute */ - void SetClimbRateFpmIC(double roc); + void SetClimbRateFpmIC(double roc) { SetClimbRateFpsIC(roc/60.0); } /** Sets the flight path angle initial condition in degrees. @param gamma Flight path angle in degrees */ - inline void SetFlightPathAngleDegIC(double gamma) { SetFlightPathAngleRadIC(gamma*degtorad); } + void SetFlightPathAngleDegIC(double gamma) + { SetClimbRateFpsIC(vt*sin(gamma*degtorad)); } /** Sets the altitude above sea level initial condition in feet. @param altitudeASL Altitude above sea level in feet */ @@ -278,95 +290,98 @@ public: /** Sets the initial Altitude above ground level. @param agl Altitude above ground level in feet */ - void SetAltitudeAGLFtIC(double agl); + void SetAltitudeAGLFtIC(double agl) + { SetAltitudeASLFtIC(terrain_elevation + agl); } /** Sets the initial sea level radius from planet center @param sl_rad sea level radius in feet */ - void SetSeaLevelRadiusFtIC(double sl_rad); + void SetSeaLevelRadiusFtIC(double sl_rad) { sea_level_radius = sl_rad; } /** Sets the initial terrain elevation. @param elev Initial terrain elevation in feet */ - void SetTerrainElevationFtIC(double elev); + void SetTerrainElevationFtIC(double elev) { terrain_elevation = elev; } /** Sets the initial latitude. @param lat Initial latitude in degrees */ - inline void SetLatitudeDegIC(double lat) { latitude=lat*degtorad; } + void SetLatitudeDegIC(double lat) { position.SetLatitude(lat*degtorad); } /** Sets the initial longitude. @param lon Initial longitude in degrees */ - inline void SetLongitudeDegIC(double lon) { longitude=lon*degtorad; } + void SetLongitudeDegIC(double lon) { position.SetLongitude(lon*degtorad); } /** Gets the initial calibrated airspeed. @return Initial calibrated airspeed in knots */ - inline double GetVcalibratedKtsIC(void) const { return vc*fpstokts; } + double GetVcalibratedKtsIC(void) const; /** Gets the initial equivalent airspeed. @return Initial equivalent airspeed in knots */ - inline double GetVequivalentKtsIC(void) const { return ve*fpstokts; } + double GetVequivalentKtsIC(void) const; /** Gets the initial ground speed. @return Initial ground speed in knots */ - inline double GetVgroundKtsIC(void) const { return vg*fpstokts; } + double GetVgroundKtsIC(void) const { return GetVgroundFpsIC() * fpstokts; } /** Gets the initial true velocity. @return Initial true airspeed in knots. */ - inline double GetVtrueKtsIC(void) const { return vt*fpstokts; } + double GetVtrueKtsIC(void) const { return vt*fpstokts; } /** Gets the initial mach. @return Initial mach number */ - inline double GetMachIC(void) const { return mach; } + double GetMachIC(void) const; /** Gets the initial climb rate. @return Initial climb rate in feet/minute */ - inline double GetClimbRateFpmIC(void) const { return hdot*60; } + double GetClimbRateFpmIC(void) const + { return GetClimbRateFpsIC()*60; } /** Gets the initial flight path angle. @return Initial flight path angle in degrees */ - inline double GetFlightPathAngleDegIC(void)const { return gamma*radtodeg; } + double GetFlightPathAngleDegIC(void) const + { return GetFlightPathAngleRadIC()*radtodeg; } /** Gets the initial angle of attack. @return Initial alpha in degrees */ - inline double GetAlphaDegIC(void) const { return alpha*radtodeg; } + double GetAlphaDegIC(void) const { return alpha*radtodeg; } /** Gets the initial sideslip angle. @return Initial beta in degrees */ - inline double GetBetaDegIC(void) const { return beta*radtodeg; } + double GetBetaDegIC(void) const { return beta*radtodeg; } /** Gets the initial pitch angle. @return Initial pitch angle in degrees */ - inline double GetThetaDegIC(void) const { return theta*radtodeg; } + double GetThetaDegIC(void) const { return theta*radtodeg; } /** Gets the initial roll angle. @return Initial phi in degrees */ - inline double GetPhiDegIC(void) const { return phi*radtodeg; } + double GetPhiDegIC(void) const { return phi*radtodeg; } /** Gets the initial heading angle. @return Initial psi in degrees */ - inline double GetPsiDegIC(void) const { return psi*radtodeg; } + double GetPsiDegIC(void) const { return psi*radtodeg; } /** Gets the initial latitude. @return Initial geocentric latitude in degrees */ - inline double GetLatitudeDegIC(void) const { return latitude*radtodeg; } + double GetLatitudeDegIC(void) const { return position.GetLatitudeDeg(); } /** Gets the initial longitude. @return Initial longitude in degrees */ - inline double GetLongitudeDegIC(void) const { return longitude*radtodeg; } + double GetLongitudeDegIC(void) const { return position.GetLongitudeDeg(); } /** Gets the initial altitude above sea level. @return Initial altitude in feet. */ - inline double GetAltitudeASLFtIC(void) const { return altitudeASL; } + double GetAltitudeASLFtIC(void) const { return position.GetRadius() - sea_level_radius; } /** Gets the initial altitude above ground level. @return Initial altitude AGL in feet */ - inline double GetAltitudeAGLFtIC(void) const { return altitudeASL - terrain_elevation; } + double GetAltitudeAGLFtIC(void) const { return position.GetRadius() - sea_level_radius - terrain_elevation; } /** Gets the initial sea level radius. @return Initial sea level radius */ - inline double GetSeaLevelRadiusFtIC(void) const { return sea_level_radius; } + double GetSeaLevelRadiusFtIC(void) const { return sea_level_radius; } /** Gets the initial terrain elevation. @return Initial terrain elevation in feet */ - inline double GetTerrainElevationFtIC(void) const { return terrain_elevation; } + double GetTerrainElevationFtIC(void) const { return terrain_elevation; } /** Sets the initial ground speed. @param vg Initial ground speed in feet/second */ @@ -378,27 +393,27 @@ public: /** Sets the initial body axis X velocity. @param ubody Initial X velocity in feet/second */ - void SetUBodyFpsIC(double ubody); + void SetUBodyFpsIC(double ubody) { SetBodyVelFpsIC(eU, ubody); } /** Sets the initial body axis Y velocity. @param vbody Initial Y velocity in feet/second */ - void SetVBodyFpsIC(double vbody); + void SetVBodyFpsIC(double vbody) { SetBodyVelFpsIC(eV, vbody); } /** Sets the initial body axis Z velocity. @param wbody Initial Z velocity in feet/second */ - void SetWBodyFpsIC(double wbody); + void SetWBodyFpsIC(double wbody) { SetBodyVelFpsIC(eW, wbody); } /** Sets the initial local axis north velocity. @param vn Initial north velocity in feet/second */ - void SetVNorthFpsIC(double vn); + void SetVNorthFpsIC(double vn) { SetNEDVelFpsIC(eU, vn); } /** Sets the initial local axis east velocity. @param ve Initial east velocity in feet/second */ - void SetVEastFpsIC(double ve); + void SetVEastFpsIC(double ve) { SetNEDVelFpsIC(eV, ve); } /** Sets the initial local axis down velocity. @param vd Initial down velocity in feet/second */ - void SetVDownFpsIC(double vd); + void SetVDownFpsIC(double vd) { SetNEDVelFpsIC(eW, vd); } /** Sets the initial roll rate. @param P Initial roll rate in radians/second */ @@ -444,39 +459,39 @@ public: /** Gets the initial ground velocity. @return Initial ground velocity in feet/second */ - inline double GetVgroundFpsIC(void) const { return vg; } + double GetVgroundFpsIC(void) const { return vUVW_NED.Magnitude(eU, eV); } /** Gets the initial true velocity. @return Initial true velocity in feet/second */ - inline double GetVtrueFpsIC(void) const { return vt; } + double GetVtrueFpsIC(void) const { return vt; } /** Gets the initial body axis X wind velocity. @return Initial body axis X wind velocity in feet/second */ - inline double GetWindUFpsIC(void) const { return uw; } + double GetWindUFpsIC(void) const { return GetBodyWindFpsIC(eU); } /** Gets the initial body axis Y wind velocity. @return Initial body axis Y wind velocity in feet/second */ - inline double GetWindVFpsIC(void) const { return vw; } + double GetWindVFpsIC(void) const { return GetBodyWindFpsIC(eV); } /** Gets the initial body axis Z wind velocity. @return Initial body axis Z wind velocity in feet/second */ - inline double GetWindWFpsIC(void) const { return ww; } + double GetWindWFpsIC(void) const { return GetBodyWindFpsIC(eW); } /** Gets the initial wind velocity in local frame. @return Initial wind velocity toward north in feet/second */ - inline double GetWindNFpsIC(void) const { return wnorth; } + double GetWindNFpsIC(void) const { return GetNEDWindFpsIC(eX); } /** Gets the initial wind velocity in local frame. @return Initial wind velocity eastwards in feet/second */ - inline double GetWindEFpsIC(void) const { return weast; } + double GetWindEFpsIC(void) const { return GetNEDWindFpsIC(eY); } /** Gets the initial wind velocity in local frame. @return Initial wind velocity downwards in feet/second */ - inline double GetWindDFpsIC(void) const { return wdown; } + double GetWindDFpsIC(void) const { return GetNEDWindFpsIC(eZ); } /** Gets the initial total wind velocity in feet/sec. @return Initial wind velocity in feet/second */ - inline double GetWindFpsIC(void) const { return sqrt(wnorth*wnorth + weast*weast); } + double GetWindFpsIC(void) const; /** Gets the initial wind direction. @return Initial wind direction in feet/second */ @@ -484,31 +499,35 @@ public: /** Gets the initial climb rate. @return Initial rate of climb in feet/second */ - inline double GetClimbRateFpsIC(void) const { return hdot; } + double GetClimbRateFpsIC(void) const + { + FGColumnVector3 _vt_NED = Tb2l * Tw2b * FGColumnVector3(vt, 0., 0.); + return _vt_NED(eW); + } /** Gets the initial body axis X velocity. @return Initial body axis X velocity in feet/second. */ - double GetUBodyFpsIC(void) const; + double GetUBodyFpsIC(void) const { return GetBodyVelFpsIC(eU); } /** Gets the initial body axis Y velocity. @return Initial body axis Y velocity in feet/second. */ - double GetVBodyFpsIC(void) const; + double GetVBodyFpsIC(void) const { return GetBodyVelFpsIC(eV); } /** Gets the initial body axis Z velocity. @return Initial body axis Z velocity in feet/second. */ - double GetWBodyFpsIC(void) const; + double GetWBodyFpsIC(void) const { return GetBodyVelFpsIC(eW); } /** Gets the initial local frame X (North) velocity. @return Initial local frame X (North) axis velocity in feet/second. */ - double GetVNorthFpsIC(void) const {return vnorth;}; + double GetVNorthFpsIC(void) const { return vUVW_NED(eU); } /** Gets the initial local frame Y (East) velocity. @return Initial local frame Y (East) axis velocity in feet/second. */ - double GetVEastFpsIC(void) const {return veast;}; + double GetVEastFpsIC(void) const { return vUVW_NED(eV); } /** Gets the initial local frame Z (Down) velocity. @return Initial local frame Z (Down) axis velocity in feet/second. */ - double GetVDownFpsIC(void) const {return vdown;}; + double GetVDownFpsIC(void) const { return vUVW_NED(eW); } /** Gets the initial body axis roll rate. @return Initial body axis roll rate in radians/second */ @@ -524,7 +543,8 @@ public: /** Sets the initial flight path angle. @param gamma Initial flight path angle in radians */ - void SetFlightPathAngleRadIC(double gamma); + void SetFlightPathAngleRadIC(double gamma) + { SetClimbRateFpsIC(vt*sin(gamma)); } /** Sets the initial angle of attack. @param alpha Initial angle of attack in radians */ @@ -548,59 +568,57 @@ public: /** Sets the initial latitude. @param lat Initial latitude in radians */ - inline void SetLatitudeRadIC(double lat) { latitude=lat; } + void SetLatitudeRadIC(double lat) { position.SetLatitude(lat); } /** Sets the initial longitude. @param lon Initial longitude in radians */ - inline void SetLongitudeRadIC(double lon) { longitude=lon; } + void SetLongitudeRadIC(double lon) { position.SetLongitude(lon); } /** Sets the target normal load factor. @param nlf Normal load factor*/ - inline void SetTargetNlfIC(double nlf) { targetNlfIC=nlf; } + void SetTargetNlfIC(double nlf) { targetNlfIC=nlf; } /** Gets the initial flight path angle. + If total velocity is zero, this function returns zero. @return Initial flight path angle in radians */ - inline double GetFlightPathAngleRadIC(void) const { return gamma; } + double GetFlightPathAngleRadIC(void) const + { return (vt == 0.0)?0.0:asin(GetClimbRateFpsIC() / vt); } /** Gets the initial angle of attack. @return Initial alpha in radians */ - inline double GetAlphaRadIC(void) const { return alpha; } + double GetAlphaRadIC(void) const { return alpha; } /** Gets the initial angle of sideslip. @return Initial sideslip angle in radians */ - inline double GetBetaRadIC(void) const { return beta; } + double GetBetaRadIC(void) const { return beta; } /** Gets the initial roll angle. @return Initial roll angle in radians */ - inline double GetPhiRadIC(void) const { return phi; } + double GetPhiRadIC(void) const { return phi; } /** Gets the initial latitude. @return Initial latitude in radians */ - inline double GetLatitudeRadIC(void) const { return latitude; } + double GetLatitudeRadIC(void) const { return position.GetLatitude(); } /** Gets the initial longitude. @return Initial longitude in radians */ - inline double GetLongitudeRadIC(void) const { return longitude; } + double GetLongitudeRadIC(void) const { return position.GetLongitude(); } /** Gets the initial pitch angle. @return Initial pitch angle in radians */ - inline double GetThetaRadIC(void) const { return theta; } + double GetThetaRadIC(void) const { return theta; } /** Gets the initial heading angle. @return Initial heading angle in radians */ - inline double GetPsiRadIC(void) const { return psi; } + double GetPsiRadIC(void) const { return psi; } /** Gets the initial speedset. @return Initial speedset */ - inline speedset GetSpeedSet(void) { return lastSpeedSet; } - - /** Gets the initial windset. - @return Initial windset */ - inline windset GetWindSet(void) { return lastWindSet; } + speedset GetSpeedSet(void) const { return lastSpeedSet; } /** Gets the target normal load factor set from IC. @return target normal load factor set from IC*/ - inline double GetTargetNlfIC(void) { return targetNlfIC; } + double GetTargetNlfIC(void) const { return targetNlfIC; } /** Loads the initial conditions. @param rstname The name of an initial conditions file @@ -610,39 +628,26 @@ public: /** Get init-file name */ - string GetInitFile(void) {return init_file_name;} + string GetInitFile(void) const {return init_file_name;} /** Set init-file name */ void SetInitFile(string f) { init_file_name = f;} void WriteStateFile(int num); private: - double vt,vc,ve,vg; - double mach; - double altitudeASL,hdot; - double latitude,longitude; - double u,v,w; + FGColumnVector3 vUVW_NED; + FGLocation position; + double vt; double p,q,r; - double uw,vw,ww; - double vnorth,veast,vdown; - double wnorth,weast,wdown; - double whead, wcross, wdir, wmag; double sea_level_radius; double terrain_elevation; - double radius_to_vehicle; double targetNlfIC; - double alpha, beta, theta, phi, psi, gamma; - double salpha,sbeta,stheta,sphi,spsi,sgamma; - double calpha,cbeta,ctheta,cphi,cpsi,cgamma; - - double xlo, xhi,xmin,xmax; - - typedef double (FGInitialCondition::*fp)(double x); - fp sfunc; + FGMatrix33 Tw2b, Tb2w; + FGMatrix33 Tl2b, Tb2l; + double alpha, beta, theta, phi, psi; speedset lastSpeedSet; - windset lastWindSet; FGFDMExec *fdmex; FGPropertyManager *PropertyManager; @@ -651,20 +656,16 @@ private: bool Load_v2(void); bool Constructing; - bool getAlpha(void); - bool getTheta(void); - bool getMachFromVcas(double *Mach,double vcas); - double GammaEqOfTheta(double Theta); void InitializeIC(void); - double GammaEqOfAlpha(double Alpha); - double calcVcas(double Mach); - void calcUVWfromNED(void); - void calcWindUVW(void); - void calcAeroEuler(void); - - bool findInterval(double x,double guess); - bool solve(double *y, double x); + void SetBodyVelFpsIC(int idx, double vel); + void SetNEDVelFpsIC(int idx, double vel); + double GetBodyWindFpsIC(int idx) const; + double GetNEDWindFpsIC(int idx) const; + double GetBodyVelFpsIC(int idx) const; + double getMachFromVcas(double vcas); + double calcVcas(double Mach) const; + void calcAeroAngles(const FGColumnVector3& _vt_BODY); void bind(void); void Debug(int from); diff --git a/src/FDM/JSBSim/input_output/FGScript.cpp b/src/FDM/JSBSim/input_output/FGScript.cpp index 1697e7ce7..e20e8c1f2 100755 --- a/src/FDM/JSBSim/input_output/FGScript.cpp +++ b/src/FDM/JSBSim/input_output/FGScript.cpp @@ -54,7 +54,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGScript.cpp,v 1.42 2010/11/24 12:58:39 jberndt Exp $"; +static const char *IdSrc = "$Id: FGScript.cpp,v 1.43 2011/01/16 15:27:34 jberndt Exp $"; static const char *IdHdr = ID_FGSCRIPT; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -266,8 +266,9 @@ bool FGScript::LoadScript(string script, double deltaT) } else { cout << endl << fgred << " Could not find the property named " << notifyPropertyName << " in script" << endl << " \"" - << ScriptName << "\". This unknown property will not be " - << "echoed for notification." << reset << endl; + << ScriptName << "\". Execution is aborted. Please recheck " + << "your input files and scripts." << reset << endl; + return false; } notify_property_element = notify_element->FindNextElement("property"); } diff --git a/src/FDM/JSBSim/math/FGColumnVector3.cpp b/src/FDM/JSBSim/math/FGColumnVector3.cpp index 5b91dcdb6..26c04fbe0 100644 --- a/src/FDM/JSBSim/math/FGColumnVector3.cpp +++ b/src/FDM/JSBSim/math/FGColumnVector3.cpp @@ -47,7 +47,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGColumnVector3.cpp,v 1.13 2010/08/08 00:19:21 jberndt Exp $"; +static const char *IdSrc = "$Id: FGColumnVector3.cpp,v 1.14 2010/12/07 12:57:14 jberndt Exp $"; static const char *IdHdr = ID_COLUMNVECTOR3; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -125,47 +125,11 @@ FGColumnVector3& FGColumnVector3::Normalize(void) return *this; } -/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -// The bitmasked value choices are as follows: -// unset: In this case (the default) JSBSim would only print -// out the normally expected messages, essentially echoing -// the config files as they are read. If the environment -// variable is not set, debug_lvl is set to 1 internally -// 0: This requests JSBSim not to output any messages -// whatsoever. -// 1: This value explicity requests the normal JSBSim -// startup messages -// 2: This value asks for a message to be printed out when -// a class is instantiated -// 4: When this value is set, a message is displayed when a -// FGModel object executes its Run() method -// 8: When this value is set, various runtime state variables -// are printed out periodically -// 16: When set various parameters are sanity checked and -// a message is printed out when they go out of bounds +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGColumnVector3::Debug(int from) -{ - if (debug_lvl <= 0) return; - - if (debug_lvl & 1) { // Standard console startup message output - } - if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGColumnVector3" << endl; - if (from == 1) cout << "Destroyed: FGColumnVector3" << endl; - } - if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects - } - if (debug_lvl & 8 ) { // Runtime state variables - } - if (debug_lvl & 16) { // Sanity checking - } - if (debug_lvl & 64) { - if (from == 0) { // Constructor - cout << IdSrc << endl; - cout << IdHdr << endl; - } - } +double FGColumnVector3::Magnitude(const int idx1, const int idx2) const { + return sqrt( data[idx1-1]*data[idx1-1] + data[idx2-1]*data[idx2-1] ); } + } // namespace JSBSim diff --git a/src/FDM/JSBSim/math/FGColumnVector3.h b/src/FDM/JSBSim/math/FGColumnVector3.h index b92a47400..16000fc85 100644 --- a/src/FDM/JSBSim/math/FGColumnVector3.h +++ b/src/FDM/JSBSim/math/FGColumnVector3.h @@ -41,13 +41,12 @@ INCLUDES #include #include -#include "FGJSBBase.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_COLUMNVECTOR3 "$Id: FGColumnVector3.h,v 1.12 2010/06/30 03:13:40 jberndt Exp $" +#define ID_COLUMNVECTOR3 "$Id: FGColumnVector3.h,v 1.16 2010/12/07 12:57:14 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -61,14 +60,14 @@ CLASS DOCUMENTATION /** This class implements a 3 element column vector. @author Jon S. Berndt, Tony Peden, et. al. - @version $Id: FGColumnVector3.h,v 1.12 2010/06/30 03:13:40 jberndt Exp $ + @version $Id: FGColumnVector3.h,v 1.16 2010/12/07 12:57:14 jberndt Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS DECLARATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -class FGColumnVector3 : public FGJSBBase +class FGColumnVector3 { public: /** Default initializer. @@ -80,11 +79,10 @@ public: @param Y value of the y-conponent. @param Z value of the z-conponent. Create a vector from the doubles given in the arguments. */ - FGColumnVector3(double X, double Y, double Z) { + FGColumnVector3(const double X, const double Y, const double Z) { data[0] = X; data[1] = Y; data[2] = Z; -// Debug(0); } /** Copy constructor. @@ -94,25 +92,24 @@ public: data[0] = v.data[0]; data[1] = v.data[1]; data[2] = v.data[2]; -// Debug(0); } /// Destructor. - ~FGColumnVector3(void) { /* Debug(1); */ } + ~FGColumnVector3(void) { } /** Read access the entries of the vector. @param idx the component index. Return the value of the matrix entry at the given index. Indices are counted starting with 1. Note that the index given in the argument is unchecked. */ - double operator()(unsigned int idx) const { return data[idx-1]; } + double operator()(const unsigned int idx) const { return data[idx-1]; } /** Write access the entries of the vector. @param idx the component index. Return a reference to the vector entry at the given index. Indices are counted starting with 1. Note that the index given in the argument is unchecked. */ - double& operator()(unsigned int idx) { return data[idx-1]; } + double& operator()(const unsigned int idx) { return data[idx-1]; } /** Read access the entries of the vector. @param idx the component index. @@ -122,7 +119,7 @@ public: operator()(unsigned int idx) const function. It is used internally to access the elements in a more convenient way. Note that the index given in the argument is unchecked. */ - double Entry(unsigned int idx) const { return data[idx-1]; } + double Entry(const unsigned int idx) const { return data[idx-1]; } /** Write access the entries of the vector. @param idx the component index. @@ -132,7 +129,7 @@ public: operator()(unsigned int idx) function. It is used internally to access the elements in a more convenient way. Note that the index given in the argument is unchecked. */ - double& Entry(unsigned int idx) { return data[idx-1]; } + double& Entry(const unsigned int idx) { return data[idx-1]; } /** Prints the contents of the vector @param delimeter the item separator (tab or comma) @@ -181,34 +178,34 @@ public: Compute and return the cross product of the current vector with the given argument. */ FGColumnVector3 operator*(const FGColumnVector3& V) const { - return FGColumnVector3( data[1] * V(3) - data[2] * V(2), - data[2] * V(1) - data[0] * V(3), - data[0] * V(2) - data[1] * V(1) ); + return FGColumnVector3( data[1] * V.data[2] - data[2] * V.data[1], + data[2] * V.data[0] - data[0] * V.data[2], + data[0] * V.data[1] - data[1] * V.data[0] ); } /// Addition operator. FGColumnVector3 operator+(const FGColumnVector3& B) const { - return FGColumnVector3( data[0] + B(1), data[1] + B(2), data[2] + B(3) ); + return FGColumnVector3( data[0] + B.data[0], data[1] + B.data[1], data[2] + B.data[2] ); } /// Subtraction operator. FGColumnVector3 operator-(const FGColumnVector3& B) const { - return FGColumnVector3( data[0] - B(1), data[1] - B(2), data[2] - B(3) ); + return FGColumnVector3( data[0] - B.data[0], data[1] - B.data[1], data[2] - B.data[2] ); } /// Subtract an other vector. FGColumnVector3& operator-=(const FGColumnVector3 &B) { - data[0] -= B(1); - data[1] -= B(2); - data[2] -= B(3); + data[0] -= B.data[0]; + data[1] -= B.data[1]; + data[2] -= B.data[2]; return *this; } /// Add an other vector. FGColumnVector3& operator+=(const FGColumnVector3 &B) { - data[0] += B(1); - data[1] += B(2); - data[2] += B(3); + data[0] += B.data[0]; + data[1] += B.data[1]; + data[2] += B.data[2]; return *this; } @@ -224,8 +221,8 @@ public: FGColumnVector3& operator/=(const double scalar); void InitMatrix(void) { data[0] = data[1] = data[2] = 0.0; } - void InitMatrix(double a) { data[0] = data[1] = data[2] = a; } - void InitMatrix(double a, double b, double c) { + void InitMatrix(const double a) { data[0] = data[1] = data[2] = a; } + void InitMatrix(const double a, const double b, const double c) { data[0]=a; data[1]=b; data[2]=c; } @@ -236,9 +233,7 @@ public: /** Length of the vector in a coordinate axis plane. Compute and return the euclidean norm of this vector projected into the coordinate axis plane idx1-idx2. */ - double Magnitude(int idx1, int idx2) const { - return sqrt( data[idx1-1]*data[idx1-1] + data[idx2-1]*data[idx2-1] ); - } + double Magnitude(const int idx1, const int idx2) const; /** Normalize. Normalize the vector to have the Magnitude() == 1.0. If the vector @@ -249,19 +244,18 @@ public: Compute and return the euclidean dot (or scalar) product of two vectors v1 and v2 */ friend inline double DotProduct(const FGColumnVector3& v1, const FGColumnVector3& v2) { - return v1(1)*v2(1) + v1(2)*v2(2) + v1(3)*v2(3); + return v1.data[0]*v2.data[0] + v1.data[1]*v2.data[1] + v1.data[2]*v2.data[2]; } private: double data[3]; - - void Debug(int from); }; /** Scalar multiplication. @param scalar scalar value to multiply with. @param A Vector to multiply. - Multiply the Vector with a scalar value.*/ + Multiply the Vector with a scalar value. Note: At this time, this + operator MUST be inlined, or a multiple definition link error will occur.*/ inline FGColumnVector3 operator*(double scalar, const FGColumnVector3& A) { // use already defined operation. return A*scalar; @@ -269,8 +263,8 @@ inline FGColumnVector3 operator*(double scalar, const FGColumnVector3& A) { /** Write vector to a stream. @param os Stream to write to. - @param M Matrix to write. - Write the matrix to a stream.*/ + @param col vector to write. + Write the vector to a stream.*/ std::ostream& operator<<(std::ostream& os, const FGColumnVector3& col); } // namespace JSBSim diff --git a/src/FDM/JSBSim/math/FGMatrix33.cpp b/src/FDM/JSBSim/math/FGMatrix33.cpp index 379b37004..a9e1213c4 100644 --- a/src/FDM/JSBSim/math/FGMatrix33.cpp +++ b/src/FDM/JSBSim/math/FGMatrix33.cpp @@ -48,7 +48,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGMatrix33.cpp,v 1.10 2010/07/01 23:13:19 jberndt Exp $"; +static const char *IdSrc = "$Id: FGMatrix33.cpp,v 1.11 2010/12/07 12:57:14 jberndt Exp $"; static const char *IdHdr = ID_MATRIX33; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -61,8 +61,6 @@ FGMatrix33::FGMatrix33(void) { data[0] = data[1] = data[2] = data[3] = data[4] = data[5] = data[6] = data[7] = data[8] = 0.0; - - // Debug(0); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -476,49 +474,4 @@ FGColumnVector3 FGMatrix33::operator*(const FGColumnVector3& v) const return FGColumnVector3( tmp1, tmp2, tmp3 ); } -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// The bitmasked value choices are as follows: -// unset: In this case (the default) JSBSim would only print -// out the normally expected messages, essentially echoing -// the config files as they are read. If the environment -// variable is not set, debug_lvl is set to 1 internally -// 0: This requests JSBSim not to output any messages -// whatsoever. -// 1: This value explicity requests the normal JSBSim -// startup messages -// 2: This value asks for a message to be printed out when -// a class is instantiated -// 4: When this value is set, a message is displayed when a -// FGModel object executes its Run() method -// 8: When this value is set, various runtime state variables -// are printed out periodically -// 16: When set various parameters are sanity checked and -// a message is printed out when they go out of bounds - -void FGMatrix33::Debug(int from) -{ - if (debug_lvl <= 0) return; - - if (debug_lvl & 1) { // Standard console startup message output - if (from == 0) { // Constructor - - } - } - if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGMatrix33" << endl; - if (from == 1) cout << "Destroyed: FGMatrix33" << endl; - } - if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects - } - if (debug_lvl & 8 ) { // Runtime state variables - } - if (debug_lvl & 16) { // Sanity checking - } - if (debug_lvl & 64) { - if (from == 0) { // Constructor - cout << IdSrc << endl; - cout << IdHdr << endl; - } - } -} -} +} \ No newline at end of file diff --git a/src/FDM/JSBSim/math/FGMatrix33.h b/src/FDM/JSBSim/math/FGMatrix33.h index d937f910a..6f31ef2e2 100644 --- a/src/FDM/JSBSim/math/FGMatrix33.h +++ b/src/FDM/JSBSim/math/FGMatrix33.h @@ -44,13 +44,12 @@ INCLUDES #include #include "FGColumnVector3.h" -#include "FGJSBBase.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_MATRIX33 "$Id: FGMatrix33.h,v 1.12 2010/08/21 17:13:47 jberndt Exp $" +#define ID_MATRIX33 "$Id: FGMatrix33.h,v 1.14 2010/12/07 12:57:14 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -72,7 +71,7 @@ CLASS DOCUMENTATION DECLARATION: MatrixException %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -class MatrixException : public FGJSBBase +class MatrixException //: public FGJSBBase { public: std::string Message; @@ -90,7 +89,7 @@ CLASS DOCUMENTATION DECLARATION: FGMatrix33 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -class FGMatrix33 : public FGJSBBase +class FGMatrix33 { public: @@ -122,8 +121,6 @@ public: data[6] = M.data[6]; data[7] = M.data[7]; data[8] = M.data[8]; - - // Debug(0); } /** Initialization by given values. @@ -140,9 +137,10 @@ public: Create a matrix from the doubles given in the arguments. */ - FGMatrix33(double m11, double m12, double m13, - double m21, double m22, double m23, - double m31, double m32, double m33) { + FGMatrix33(const double m11, const double m12, const double m13, + const double m21, const double m22, const double m23, + const double m31, const double m32, const double m33) + { data[0] = m11; data[1] = m21; data[2] = m31; @@ -152,13 +150,11 @@ public: data[6] = m13; data[7] = m23; data[8] = m33; - - // Debug(0); } /** Destructor. */ - ~FGMatrix33(void) { /* Debug(1); */ } + ~FGMatrix33(void) {} /** Prints the contents of the matrix. @param delimeter the item separator (tab or comma) @@ -263,9 +259,10 @@ public: /** Initialize the matrix. This function initializes a matrix to user specified values. */ - void InitMatrix(double m11, double m12, double m13, - double m21, double m22, double m23, - double m31, double m32, double m33) { + void InitMatrix(const double m11, const double m12, const double m13, + const double m21, const double m22, const double m23, + const double m31, const double m32, const double m33) + { data[0] = m11; data[1] = m21; data[2] = m31; @@ -309,7 +306,8 @@ public: Copy the content of the matrix given in the argument into *this. */ - FGMatrix33& operator=(const FGMatrix33& A) { + FGMatrix33& operator=(const FGMatrix33& A) + { data[0] = A.data[0]; data[1] = A.data[1]; data[2] = A.data[2]; @@ -434,8 +432,6 @@ public: private: double data[eRows*eColumns]; - - void Debug(int from); }; /** Scalar multiplication. diff --git a/src/FDM/JSBSim/math/FGQuaternion.cpp b/src/FDM/JSBSim/math/FGQuaternion.cpp index 0d6e83030..e983550a6 100644 --- a/src/FDM/JSBSim/math/FGQuaternion.cpp +++ b/src/FDM/JSBSim/math/FGQuaternion.cpp @@ -57,7 +57,7 @@ using std::endl; namespace JSBSim { -static const char *IdSrc = "$Id: FGQuaternion.cpp,v 1.17 2010/11/28 13:15:26 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGQuaternion.cpp,v 1.19 2010/12/07 12:57:14 jberndt Exp $"; static const char *IdHdr = ID_QUATERNION; //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -254,55 +254,14 @@ void FGQuaternion::ComputeDerivedUnconditional(void) const mEulerCosines(ePhi) = cos(mEulerAngles(ePhi)); mEulerCosines(eTht) = cos(mEulerAngles(eTht)); mEulerCosines(ePsi) = cos(mEulerAngles(ePsi)); - -// Debug(2); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// The bitmasked value choices are as follows: -// unset: In this case (the default) JSBSim would only print -// out the normally expected messages, essentially echoing -// the config files as they are read. If the environment -// variable is not set, debug_lvl is set to 1 internally -// 0: This requests JSBSim not to output any messages -// whatsoever. -// 1: This value explicity requests the normal JSBSim -// startup messages -// 2: This value asks for a message to be printed out when -// a class is instantiated -// 4: When this value is set, a message is displayed when a -// FGModel object executes its Run() method -// 8: When this value is set, various runtime state variables -// are printed out periodically -// 16: When set various parameters are sanity checked and -// a message is printed out when they go out of bounds -void FGQuaternion::Debug(int from) const +std::ostream& operator<<(std::ostream& os, const FGQuaternion& q) { - if (debug_lvl <= 0) return; - - if (debug_lvl & 1) { // Standard console startup message output - } - if (debug_lvl & 2 ) { // Instantiation/Destruction notification - if (from == 0) cout << "Instantiated: FGQuaternion" << endl; - if (from == 1) cout << "Destroyed: FGQuaternion" << endl; - } - if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects - } - if (debug_lvl & 8 ) { // Runtime state variables - } - if (debug_lvl & 16) { // Sanity checking - if (!EqualToRoundoff(Magnitude(), 1.0)) { - cout << "Quaternion magnitude differs from 1.0 !" << endl; - cout << "\tdelta from 1.0: " << std::scientific << Magnitude()-1.0 << endl; - } - } - if (debug_lvl & 64) { - if (from == 0) { // Constructor - cout << IdSrc << endl; - cout << IdHdr << endl; - } - } + os << q(1) << " , " << q(2) << " , " << q(3) << " , " << q(4); + return os; } } // namespace JSBSim diff --git a/src/FDM/JSBSim/math/FGQuaternion.h b/src/FDM/JSBSim/math/FGQuaternion.h index 69f304eb3..8486b49f8 100644 --- a/src/FDM/JSBSim/math/FGQuaternion.h +++ b/src/FDM/JSBSim/math/FGQuaternion.h @@ -47,7 +47,7 @@ SENTRY DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_QUATERNION "$Id: FGQuaternion.h,v 1.18 2010/09/29 02:19:05 jberndt Exp $" +#define ID_QUATERNION "$Id: FGQuaternion.h,v 1.22 2010/12/07 12:57:14 jberndt Exp $" namespace JSBSim { @@ -88,7 +88,7 @@ class FGMatrix33; CLASS DECLARATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -class FGQuaternion : virtual FGJSBBase { +class FGQuaternion : public FGJSBBase { public: /** Default initializer. Default initializer, initializes the class with the identity rotation. */ @@ -293,10 +293,10 @@ public: @return reference to a quaternion object */ const FGQuaternion& operator=(const FGQuaternion& q) { // Copy the master values ... - data[0] = q(1); - data[1] = q(2); - data[2] = q(3); - data[3] = q(4); + data[0] = q.data[0]; + data[1] = q.data[1]; + data[2] = q.data[2]; + data[3] = q.data[3]; ComputeDerived(); // .. and copy the derived values if they are valid mCacheValid = q.mCacheValid; @@ -317,8 +317,8 @@ public: @param q a quaternion reference @return true if both quaternions represent the same rotation. */ bool operator==(const FGQuaternion& q) const { - return data[0] == q(1) && data[1] == q(2) - && data[2] == q(3) && data[3] == q(4); + return data[0] == q.data[0] && data[1] == q.data[1] + && data[2] == q.data[2] && data[3] == q.data[3]; } /** Comparison operator "!=". @@ -327,10 +327,10 @@ public: bool operator!=(const FGQuaternion& q) const { return ! operator==(q); } const FGQuaternion& operator+=(const FGQuaternion& q) { // Copy the master values ... - data[0] += q(1); - data[1] += q(2); - data[2] += q(3); - data[3] += q(4); + data[0] += q.data[0]; + data[1] += q.data[1]; + data[2] += q.data[2]; + data[3] += q.data[3]; mCacheValid = false; return *this; } @@ -340,10 +340,10 @@ public: @return a quaternion reference representing Q, where Q = Q - q. */ const FGQuaternion& operator-=(const FGQuaternion& q) { // Copy the master values ... - data[0] -= q(1); - data[1] -= q(2); - data[2] -= q(3); - data[3] -= q(4); + data[0] -= q.data[0]; + data[1] -= q.data[1]; + data[2] -= q.data[2]; + data[3] -= q.data[3]; mCacheValid = false; return *this; } @@ -371,16 +371,16 @@ public: @param q a quaternion to be summed. @return a quaternion representing Q, where Q = Q + q. */ FGQuaternion operator+(const FGQuaternion& q) const { - return FGQuaternion(data[0]+q(1), data[1]+q(2), - data[2]+q(3), data[3]+q(4)); + return FGQuaternion(data[0]+q.data[0], data[1]+q.data[1], + data[2]+q.data[2], data[3]+q.data[3]); } /** Arithmetic operator "-". @param q a quaternion to be subtracted. @return a quaternion representing Q, where Q = Q - q. */ FGQuaternion operator-(const FGQuaternion& q) const { - return FGQuaternion(data[0]-q(1), data[1]-q(2), - data[2]-q(3), data[3]-q(4)); + return FGQuaternion(data[0]-q.data[0], data[1]-q.data[1], + data[2]-q.data[2], data[3]-q.data[3]); } /** Arithmetic operator "*". @@ -388,10 +388,10 @@ public: @param q a quaternion to be multiplied. @return a quaternion representing Q, where Q = Q * q. */ FGQuaternion operator*(const FGQuaternion& q) const { - return FGQuaternion(data[0]*q(1)-data[1]*q(2)-data[2]*q(3)-data[3]*q(4), - data[0]*q(2)+data[1]*q(1)+data[2]*q(4)-data[3]*q(3), - data[0]*q(3)-data[1]*q(4)+data[2]*q(1)+data[3]*q(2), - data[0]*q(4)+data[1]*q(3)-data[2]*q(2)+data[3]*q(1)); + return FGQuaternion(data[0]*q.data[0]-data[1]*q.data[1]-data[2]*q.data[2]-data[3]*q.data[3], + data[0]*q.data[1]+data[1]*q.data[0]+data[2]*q.data[3]-data[3]*q.data[2], + data[0]*q.data[2]-data[1]*q.data[3]+data[2]*q.data[0]+data[3]*q.data[1], + data[0]*q.data[3]+data[1]*q.data[2]-data[2]*q.data[1]+data[3]*q.data[0]); } /** Arithmetic operator "*=". @@ -399,10 +399,10 @@ public: @param q a quaternion to be multiplied. @return a quaternion reference representing Q, where Q = Q * q. */ const FGQuaternion& operator*=(const FGQuaternion& q) { - double q0 = data[0]*q(1)-data[1]*q(2)-data[2]*q(3)-data[3]*q(4); - double q1 = data[0]*q(2)+data[1]*q(1)+data[2]*q(4)-data[3]*q(3); - double q2 = data[0]*q(3)-data[1]*q(4)+data[2]*q(1)+data[3]*q(2); - double q3 = data[0]*q(4)+data[1]*q(3)-data[2]*q(2)+data[3]*q(1); + double q0 = data[0]*q.data[0]-data[1]*q.data[1]-data[2]*q.data[2]-data[3]*q.data[3]; + double q1 = data[0]*q.data[1]+data[1]*q.data[0]+data[2]*q.data[3]-data[3]*q.data[2]; + double q2 = data[0]*q.data[2]-data[1]*q.data[3]+data[2]*q.data[0]+data[3]*q.data[1]; + double q3 = data[0]*q.data[3]+data[1]*q.data[2]-data[2]*q.data[1]+data[3]*q.data[0]; data[0] = q0; data[1] = q1; data[2] = q2; @@ -506,8 +506,6 @@ private: mutable FGColumnVector3 mEulerSines; mutable FGColumnVector3 mEulerCosines; - void Debug(int from) const; - void InitializeFromEulerAngles(double phi, double tht, double psi); }; @@ -519,9 +517,15 @@ private: Multiply the Vector with a scalar value. */ inline FGQuaternion operator*(double scalar, const FGQuaternion& q) { - return FGQuaternion(scalar*q(1), scalar*q(2), scalar*q(3), scalar*q(4)); + return FGQuaternion(scalar*q.data[0], scalar*q.data[1], scalar*q.data[2], scalar*q.data[3]); } +/** Write quaternion to a stream. + @param os Stream to write to. + @param q Quaternion to write. + Write the quaternion to a stream.*/ +std::ostream& operator<<(std::ostream& os, const FGQuaternion& q); + } // namespace JSBSim #include "FGMatrix33.h" diff --git a/src/FDM/JSBSim/models/FGAerodynamics.cpp b/src/FDM/JSBSim/models/FGAerodynamics.cpp index 07e639860..21583363b 100644 --- a/src/FDM/JSBSim/models/FGAerodynamics.cpp +++ b/src/FDM/JSBSim/models/FGAerodynamics.cpp @@ -52,7 +52,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGAerodynamics.cpp,v 1.35 2010/11/18 12:38:06 jberndt Exp $"; +static const char *IdSrc = "$Id: FGAerodynamics.cpp,v 1.36 2011/01/19 12:41:19 jberndt Exp $"; static const char *IdHdr = ID_AERODYNAMICS; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -388,23 +388,28 @@ void FGAerodynamics::DetermineAxisSystem() string axis; while (axis_element) { axis = axis_element->GetAttributeValue("name"); - if (axis == "LIFT" || axis == "DRAG" || axis == "SIDE") { + if (axis == "LIFT" || axis == "DRAG") { if (axisType == atNone) axisType = atLiftDrag; else if (axisType != atLiftDrag) { cerr << endl << " Mixed aerodynamic axis systems have been used in the" - << " aircraft config file." << endl; + << " aircraft config file. (LIFT DRAG)" << endl; + } + } else if (axis == "SIDE") { + if (axisType != atNone && axisType != atLiftDrag && axisType != atAxialNormal) { + cerr << endl << " Mixed aerodynamic axis systems have been used in the" + << " aircraft config file. (SIDE)" << endl; } } else if (axis == "AXIAL" || axis == "NORMAL") { if (axisType == atNone) axisType = atAxialNormal; else if (axisType != atAxialNormal) { cerr << endl << " Mixed aerodynamic axis systems have been used in the" - << " aircraft config file." << endl; + << " aircraft config file. (NORMAL AXIAL)" << endl; } } else if (axis == "X" || axis == "Y" || axis == "Z") { if (axisType == atNone) axisType = atBodyXYZ; else if (axisType != atBodyXYZ) { cerr << endl << " Mixed aerodynamic axis systems have been used in the" - << " aircraft config file." << endl; + << " aircraft config file. (XYZ)" << endl; } } else if (axis != "ROLL" && axis != "PITCH" && axis != "YAW") { // error cerr << endl << " An unknown axis type, " << axis << " has been specified" diff --git a/src/FDM/JSBSim/models/FGGasCell.cpp b/src/FDM/JSBSim/models/FGGasCell.cpp index 8edabcd9a..bdab72172 100644 --- a/src/FDM/JSBSim/models/FGGasCell.cpp +++ b/src/FDM/JSBSim/models/FGGasCell.cpp @@ -53,7 +53,7 @@ using std::max; namespace JSBSim { -static const char *IdSrc = "$Id: FGGasCell.cpp,v 1.12 2009/10/24 22:59:30 jberndt Exp $"; +static const char *IdSrc = "$Id: FGGasCell.cpp,v 1.13 2010/12/29 22:39:25 andgi Exp $"; static const char *IdHdr = ID_GASCELL; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -206,20 +206,20 @@ FGGasCell::FGGasCell(FGFDMExec* exec, Element* el, int num) : FGForce(exec) base_property_name = CreateIndexedPropertyName("buoyant_forces/gas-cell", CellNum); property_name = base_property_name + "/max_volume-ft3"; - PropertyManager->Tie( property_name.c_str(), &MaxVolume ); + PropertyManager->Tie( property_name.c_str(), &MaxVolume, false ); PropertyManager->SetWritable( property_name, false ); property_name = base_property_name + "/temp-R"; - PropertyManager->Tie( property_name.c_str(), &Temperature ); + PropertyManager->Tie( property_name.c_str(), &Temperature, false ); property_name = base_property_name + "/pressure-psf"; - PropertyManager->Tie( property_name.c_str(), &Pressure ); + PropertyManager->Tie( property_name.c_str(), &Pressure, false ); property_name = base_property_name + "/volume-ft3"; - PropertyManager->Tie( property_name.c_str(), &Volume ); + PropertyManager->Tie( property_name.c_str(), &Volume, false ); property_name = base_property_name + "/buoyancy-lbs"; - PropertyManager->Tie( property_name.c_str(), &Buoyancy ); + PropertyManager->Tie( property_name.c_str(), &Buoyancy, false ); property_name = base_property_name + "/contents-mol"; - PropertyManager->Tie( property_name.c_str(), &Contents ); + PropertyManager->Tie( property_name.c_str(), &Contents, false ); property_name = base_property_name + "/valve_open"; - PropertyManager->Tie( property_name.c_str(), &ValveOpen ); + PropertyManager->Tie( property_name.c_str(), &ValveOpen, false ); Debug(0); @@ -646,23 +646,23 @@ FGBallonet::FGBallonet(FGFDMExec* exec, Element* el, int num, FGGasCell* parent) base_property_name = CreateIndexedPropertyName(base_property_name + "/ballonet", CellNum); property_name = base_property_name + "/max_volume-ft3"; - PropertyManager->Tie( property_name, &MaxVolume ); + PropertyManager->Tie( property_name, &MaxVolume, false ); PropertyManager->SetWritable( property_name, false ); property_name = base_property_name + "/temp-R"; - PropertyManager->Tie( property_name, &Temperature ); + PropertyManager->Tie( property_name, &Temperature, false ); property_name = base_property_name + "/pressure-psf"; - PropertyManager->Tie( property_name, &Pressure ); + PropertyManager->Tie( property_name, &Pressure, false ); property_name = base_property_name + "/volume-ft3"; - PropertyManager->Tie( property_name, &Volume ); + PropertyManager->Tie( property_name, &Volume, false ); property_name = base_property_name + "/contents-mol"; - PropertyManager->Tie( property_name, &Contents ); + PropertyManager->Tie( property_name, &Contents, false ); property_name = base_property_name + "/valve_open"; - PropertyManager->Tie( property_name, &ValveOpen ); + PropertyManager->Tie( property_name, &ValveOpen, false ); Debug(0); diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp index 5405c8639..8a04cb750 100644 --- a/src/FDM/JSBSim/models/FGPropagate.cpp +++ b/src/FDM/JSBSim/models/FGPropagate.cpp @@ -71,7 +71,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.74 2010/11/28 13:02:43 bcoconni Exp $"; +static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.76 2011/01/16 16:10:59 bcoconni Exp $"; static const char *IdHdr = ID_PROPAGATE; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -157,8 +157,8 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC) VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); - Ti2ec = GetTi2ec(); // ECI to ECEF transform - Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform + Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform + Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform VState.vInertialPosition = Tec2i * VState.vLocation; @@ -260,8 +260,8 @@ bool FGPropagate::Run(void) VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); // 2. Update the Ti2ec and Tec2i transforms from the updated EPA - Ti2ec = GetTi2ec(); // ECI to ECEF transform - Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform + Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform + Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform // 3. Update the location from the updated Ti2ec and inertial position VState.vLocation = Ti2ec*VState.vInertialPosition; @@ -561,7 +561,7 @@ void FGPropagate::ResolveFrictionForces(double dt) // Prepare the linear system for the Gauss-Seidel algorithm : // 1. Compute the right hand side member 'rhs' - // 2. Divide every line of 'a' and 'lhs' by a[i,i]. This is in order to save + // 2. Divide every line of 'a' and 'rhs' by a[i,i]. This is in order to save // a division computation at each iteration of Gauss-Seidel. for (int i=0; i < n; i++) { double d = 1.0 / a[i*n+i]; @@ -619,22 +619,22 @@ void FGPropagate::ResolveFrictionForces(double dt) void FGPropagate::UpdateLocationMatrices(void) { - Tl2ec = GetTl2ec(); // local to ECEF transform - Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform - Ti2l = GetTi2l(); - Tl2i = Ti2l.Transposed(); + Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform + Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform + Ti2l = VState.vLocation.GetTi2l(); // ECI to local frame transform + Tl2i = Ti2l.Transposed(); // local to ECI transform } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGPropagate::UpdateBodyMatrices(void) { - Ti2b = GetTi2b(); // ECI to body frame transform - Tb2i = Ti2b.Transposed(); // body to ECI frame transform - Tl2b = Ti2b*Tl2i; // local to body frame transform - Tb2l = Tl2b.Transposed(); // body to local frame transform - Tec2b = Tl2b * Tec2l; // ECEF to body frame transform - Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform + Ti2b = VState.qAttitudeECI.GetT(); // ECI to body frame transform + Tb2i = Ti2b.Transposed(); // body to ECI frame transform + Tl2b = Ti2b*Tl2i; // local to body frame transform + Tb2l = Tl2b.Transposed(); // body to local frame transform + Tec2b = Tl2b * Tec2l; // ECEF to body frame transform + Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -651,7 +651,7 @@ void FGPropagate::SetInertialOrientation(FGQuaternion Qi) { void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) { VState.vInertialVelocity = Vi; CalculateUVW(); - vVel = GetTb2l() * VState.vUVW; + vVel = Tb2l * VState.vUVW; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -715,36 +715,6 @@ double FGPropagate::GetTerrainElevation(void) const return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius; } -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -//Todo: when should this be called - when should the new EPA be used to -// calculate the transformation matrix, so that the matrix is not a step -// ahead of the sim and the associated calculations? -const FGMatrix33& FGPropagate::GetTi2ec(void) -{ - return VState.vLocation.GetTi2ec(); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -const FGMatrix33& FGPropagate::GetTec2i(void) -{ - return VState.vLocation.GetTec2i(); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -void FGPropagate::SetAltitudeASL(double altASL) -{ - VState.vLocation.SetRadius( altASL + SeaLevelRadius ); -} - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -double FGPropagate::GetLocalTerrainRadius(void) const -{ - return LocalTerrainRadius; -} - //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% double FGPropagate::GetDistanceAGL(void) const @@ -754,9 +724,48 @@ double FGPropagate::GetDistanceAGL(void) const //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGPropagate::SetDistanceAGL(double tt) +void FGPropagate::SetVState(const VehicleState& vstate) { - VState.vLocation.SetRadius( tt + LocalTerrainRadius ); + VState.vLocation = vstate.vLocation; + VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); + Ti2ec = VState.vLocation.GetTi2ec(); // useless ? + Tec2i = Ti2ec.Transposed(); + UpdateLocationMatrices(); + SetInertialOrientation(vstate.qAttitudeECI); + RecomputeLocalTerrainRadius(); + VehicleRadius = GetRadius(); + VState.vUVW = vstate.vUVW; + vVel = Tb2l * VState.vUVW; + VState.vPQR = vstate.vPQR; + VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth; + VState.vPQRi_i = Tb2i * VState.vPQRi; + VState.vInertialPosition = vstate.vInertialPosition; + + InitializeDerivatives(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPropagate::UpdateVehicleState(void) +{ + RecomputeLocalTerrainRadius(); + VehicleRadius = GetRadius(); + VState.vInertialPosition = Tec2i * VState.vLocation; + UpdateLocationMatrices(); + UpdateBodyMatrices(); + vVel = Tb2l * VState.vUVW; + VState.qAttitudeLocal = Tl2b.GetQuaternion(); +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +void FGPropagate::SetLocation(const FGLocation& l) +{ + VState.vLocation = l; + VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle()); + Ti2ec = VState.vLocation.GetTi2ec(); // useless ? + Tec2i = Ti2ec.Transposed(); + UpdateVehicleState(); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -784,7 +793,7 @@ void FGPropagate::DumpState(void) cout << endl << " " << underon << "Velocity" << underoff << endl; cout << " ECI: " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl; - cout << " ECEF: " << (GetTb2ec() * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl; + cout << " ECEF: " << (Tb2ec * VState.vUVW).Dump(", ") << " (x,y,z in ft/s)" << endl; cout << " Local: " << GetVel() << " (n,e,d in ft/sec)" << endl; cout << " Body: " << GetUVW() << " (u,v,w in ft/sec)" << endl; diff --git a/src/FDM/JSBSim/models/FGPropagate.h b/src/FDM/JSBSim/models/FGPropagate.h index 00de83c7f..107b3989b 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.53 2010/11/28 13:02:43 bcoconni Exp $" +#define ID_PROPAGATE "$Id: FGPropagate.h,v 1.55 2011/01/16 16:10:59 bcoconni Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -102,7 +102,7 @@ CLASS DOCUMENTATION @endcode @author Jon S. Berndt, Mathias Froehlich - @version $Id: FGPropagate.h,v 1.53 2010/11/28 13:02:43 bcoconni Exp $ + @version $Id: FGPropagate.h,v 1.55 2011/01/16 16:10:59 bcoconni Exp $ */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -441,7 +441,7 @@ public: units feet @return distance of the local terrain from the center of the earth. */ - double GetLocalTerrainRadius(void) const; + double GetLocalTerrainRadius(void) const { return LocalTerrainRadius; } double GetSeaLevelRadius(void) const { return SeaLevelRadius; } double GetTerrainElevation(void) const; @@ -466,13 +466,13 @@ public: The quaternion class, being the means by which the orientation of the vehicle is stored, manages the local-to-body transformation matrix. @return a reference to the local-to-body transformation matrix. */ - const FGMatrix33& GetTl2b(void) const { return VState.qAttitudeLocal.GetT(); } + const FGMatrix33& GetTl2b(void) const { return Tl2b; } /** Retrieves the body-to-local transformation matrix. The quaternion class, being the means by which the orientation of the vehicle is stored, manages the body-to-local transformation matrix. @return a reference to the body-to-local matrix. */ - const FGMatrix33& GetTb2l(void) const { return VState.qAttitudeLocal.GetTInv(); } + const FGMatrix33& GetTb2l(void) const { return Tb2l; } /** Retrieves the ECEF-to-body transformation matrix. @return a reference to the ECEF-to-body transformation matrix. */ @@ -484,55 +484,43 @@ public: /** Retrieves the ECI-to-body transformation matrix. @return a reference to the ECI-to-body transformation matrix. */ - const FGMatrix33& GetTi2b(void) const { return VState.qAttitudeECI.GetT(); } + const FGMatrix33& GetTi2b(void) const { return Ti2b; } /** Retrieves the body-to-ECI transformation matrix. @return a reference to the body-to-ECI matrix. */ - const FGMatrix33& GetTb2i(void) const { return VState.qAttitudeECI.GetTInv(); } + const FGMatrix33& GetTb2i(void) const { return Tb2i; } /** Retrieves the ECEF-to-ECI transformation matrix. @return a reference to the ECEF-to-ECI transformation matrix. */ - const FGMatrix33& GetTec2i(void); + const FGMatrix33& GetTec2i(void) const { return Tec2i; } /** Retrieves the ECI-to-ECEF transformation matrix. @return a reference to the ECI-to-ECEF matrix. */ - const FGMatrix33& GetTi2ec(void); + const FGMatrix33& GetTi2ec(void) const { return Ti2ec; } /** Retrieves the ECEF-to-local transformation matrix. Retrieves the ECEF-to-local transformation matrix. Note that the so-called local from is also know as the NED frame (for North, East, Down). @return a reference to the ECEF-to-local matrix. */ - const FGMatrix33& GetTec2l(void) const { return VState.vLocation.GetTec2l(); } + const FGMatrix33& GetTec2l(void) const { return Tec2l; } /** Retrieves the local-to-ECEF transformation matrix. Retrieves the local-to-ECEF transformation matrix. Note that the so-called local from is also know as the NED frame (for North, East, Down). @return a reference to the local-to-ECEF matrix. */ - const FGMatrix33& GetTl2ec(void) const { return VState.vLocation.GetTl2ec(); } + const FGMatrix33& GetTl2ec(void) const { return Tl2ec; } /** Retrieves the local-to-inertial transformation matrix. @return a reference to the local-to-inertial transformation matrix. */ - const FGMatrix33& GetTl2i(void) { return VState.vLocation.GetTl2i(); } + const FGMatrix33& GetTl2i(void) const { return Tl2i; } /** Retrieves the inertial-to-local transformation matrix. @return a reference to the inertial-to-local matrix. */ - const FGMatrix33& GetTi2l(void) { return VState.vLocation.GetTi2l(); } + const FGMatrix33& GetTi2l(void) const { return Ti2l; } - VehicleState* GetVState(void) { return &VState; } + const VehicleState& GetVState(void) const { return VState; } - void SetVState(VehicleState* vstate) { - VState.vLocation = vstate->vLocation; - UpdateLocationMatrices(); - SetInertialOrientation(vstate->qAttitudeECI); - VehicleRadius = GetRadius(); - VState.vUVW = vstate->vUVW; - vVel = GetTb2l() * VState.vUVW; - VState.vPQR = vstate->vPQR; - VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth; - VState.vInertialPosition = vstate->vInertialPosition; - - InitializeDerivatives(); - } + void SetVState(const VehicleState& vstate); void InitializeDerivatives(void); @@ -554,50 +542,46 @@ public: // SET functions - void SetLongitude(double lon) { - VState.vLocation.SetLongitude(lon); - VState.vInertialPosition = GetTec2i() * VState.vLocation; - UpdateLocationMatrices(); + void SetLongitude(double lon) + { + VState.vLocation.SetLongitude(lon); + UpdateVehicleState(); } void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); } - void SetLatitude(double lat) { - VState.vLocation.SetLatitude(lat); - VState.vInertialPosition = GetTec2i() * VState.vLocation; - UpdateLocationMatrices(); + void SetLatitude(double lat) + { + VState.vLocation.SetLatitude(lat); + UpdateVehicleState(); } void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); } - void SetRadius(double r) { - VState.vLocation.SetRadius(r); - VState.vInertialPosition = GetTec2i() * VState.vLocation; - UpdateLocationMatrices(); + void SetRadius(double r) + { + VState.vLocation.SetRadius(r); + VehicleRadius = r; + VState.vInertialPosition = Tec2i * VState.vLocation; } - void SetAltitudeASL(double altASL); - void SetAltitudeASLmeters(double altASL) {SetAltitudeASL(altASL/fttom);} + void SetAltitudeASL(double altASL) { SetRadius(altASL + SeaLevelRadius); } + void SetAltitudeASLmeters(double altASL) { SetRadius(altASL/fttom + SeaLevelRadius); } void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt; } void SetTerrainElevation(double tt); - void SetDistanceAGL(double tt); + void SetDistanceAGL(double tt) { SetRadius(tt + LocalTerrainRadius); } void SetInitialState(const FGInitialCondition *); - void SetPosition(const double Lon, const double Lat, const double Radius) { - VState.vLocation.SetPosition(Lon, Lat, Radius); - VState.vInertialPosition = GetTec2i() * VState.vLocation; - VehicleRadius = GetRadius(); - UpdateLocationMatrices(); + void SetLocation(const FGLocation& l); + void SetLocation(const FGColumnVector3& lv) + { + FGLocation l = FGLocation(lv); + SetLocation(l); } - void SetLocation(const FGLocation& l) { - VState.vLocation = l; - VState.vInertialPosition = GetTec2i() * VState.vLocation; - UpdateLocationMatrices(); - } - void SetLocation(const FGColumnVector3& l) { - VState.vLocation = l; - VState.vInertialPosition = GetTec2i() * VState.vLocation; - UpdateLocationMatrices(); + void SetPosition(const double Lon, const double Lat, const double Radius) + { + FGLocation l = FGLocation(Lon, Lat, Radius); + SetLocation(l); } void RecomputeLocalTerrainRadius(void); void NudgeBodyLocation(FGColumnVector3 deltaLoc) { - vDeltaXYZEC = GetTb2ec()*deltaLoc; + vDeltaXYZEC = Tb2ec*deltaLoc; VState.vLocation -= vDeltaXYZEC; } @@ -678,13 +662,11 @@ private: void UpdateLocationMatrices(void); void UpdateBodyMatrices(void); + void UpdateVehicleState(void); void bind(void); void Debug(int from); }; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -#include "initialization/FGInitialCondition.h" - #endif diff --git a/src/FDM/JSBSim/models/FGPropulsion.cpp b/src/FDM/JSBSim/models/FGPropulsion.cpp index 57d2852f3..32d8b42a1 100644 --- a/src/FDM/JSBSim/models/FGPropulsion.cpp +++ b/src/FDM/JSBSim/models/FGPropulsion.cpp @@ -307,18 +307,23 @@ bool FGPropulsion::Load(Element* el) try { if (type == "piston_engine") { HavePistonEngine = true; + if (!IsBound) bind(); Engines.push_back(new FGPiston(FDMExec, document, numEngines)); } else if (type == "turbine_engine") { HaveTurbineEngine = true; + if (!IsBound) bind(); Engines.push_back(new FGTurbine(FDMExec, document, numEngines)); } else if (type == "turboprop_engine") { HaveTurboPropEngine = true; + if (!IsBound) bind(); Engines.push_back(new FGTurboProp(FDMExec, document, numEngines)); } else if (type == "rocket_engine") { HaveRocketEngine = true; + if (!IsBound) bind(); Engines.push_back(new FGRocket(FDMExec, document, numEngines)); } else if (type == "electric_engine") { HaveElectricEngine = true; + if (!IsBound) bind(); Engines.push_back(new FGElectric(FDMExec, document, numEngines)); } else { cerr << "Unknown engine type: " << type << endl; @@ -345,7 +350,6 @@ bool FGPropulsion::Load(Element* el) if (el->FindElement("dump-rate")) DumpRate = el->FindElementValueAsNumberConvertTo("dump-rate", "LBS/MIN"); - if (!IsBound) bind(); PostLoad(el, PropertyManager); return true; diff --git a/src/FDM/JSBSim/models/propulsion/FGRocket.cpp b/src/FDM/JSBSim/models/propulsion/FGRocket.cpp index 1a40501a1..3fef49616 100644 --- a/src/FDM/JSBSim/models/propulsion/FGRocket.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGRocket.cpp @@ -49,7 +49,7 @@ using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGRocket.cpp,v 1.20 2010/08/21 17:13:48 jberndt Exp $"; +static const char *IdSrc = "$Id: FGRocket.cpp,v 1.22 2010/12/30 13:35:09 jberndt Exp $"; static const char *IdHdr = ID_ROCKET; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -215,9 +215,9 @@ void FGRocket::ConsumeFuel(void) if (Tank->GetContents() > 0.0 && Tank->GetSelected() && SourceTanks[i] > 0) ++TanksWithFuel; break; case FGTank::ttOXIDIZER: - if (Tank->GetContents() > 0.0 && Tank->GetSelected() && SourceTanks[i] > 0) { + if (Tank->GetSelected() && SourceTanks[i] > 0) { haveOxTanks = true; - ++TanksWithOxidizer; + if (Tank->GetContents() > 0.0) ++TanksWithOxidizer; } break; } diff --git a/src/FDM/JSBSim/models/propulsion/FGRotor.cpp b/src/FDM/JSBSim/models/propulsion/FGRotor.cpp index b1c1661d0..5c69728f2 100644 --- a/src/FDM/JSBSim/models/propulsion/FGRotor.cpp +++ b/src/FDM/JSBSim/models/propulsion/FGRotor.cpp @@ -5,7 +5,7 @@ Date started: 08/24/00 Purpose: Encapsulates the rotor object - ------------- Copyright (C) 2000 Jon S. Berndt (jsb@hal-pc.org) ------------- + ------------- Copyright (C) 2000 Jon S. Berndt (jon@jsbsim.org) ------------- This program 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 @@ -31,6 +31,9 @@ HISTORY -------------------------------------------------------------------------------- 08/24/00 JSB Created 01/01/10 T.Kreitler test implementation +11/15/10 T.Kreitler treated flow solver bug, flow and torque calculations + simplified, tiploss influence removed from flapping angles +01/10/11 T.Kreitler changed to single rotor model %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INCLUDES @@ -48,131 +51,187 @@ INCLUDES #include "input_output/FGXMLElement.h" -#include "math/FGRungeKutta.h" using namespace std; namespace JSBSim { -static const char *IdSrc = "$Id: FGRotor.cpp,v 1.9 2010/06/05 12:12:34 jberndt Exp $"; +static const char *IdSrc = "$Id: FGRotor.cpp,v 1.11 2011/01/17 22:09:59 jberndt Exp $"; static const char *IdHdr = ID_ROTOR; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% MISC %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -static int dump_req; // debug schwafel flag - static inline double sqr(double x) { return x*x; } /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS IMPLEMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -// starting with 'inner' rotor, FGRotor constructor is further down - -FGRotor::rotor::~rotor() { } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// hmm, not a real alternative to a pretty long initializer list +// Constructor -void FGRotor::rotor::zero() { - FGColumnVector3 zero_vec(0.0, 0.0, 0.0); +FGRotor::FGRotor(FGFDMExec *exec, Element* rotor_element, int num) + : FGThruster(exec, rotor_element, num), + - flags = 0; - parent = NULL ; + // environment + dt(0.0), rho(0.002356), - reports = 0; + // configuration parameters + Radius(0.0), BladeNum(0), - // configuration - Radius = 0.0 ; - BladeNum = 0 ; - RelDistance_xhub = 0.0 ; - RelShift_yhub = 0.0 ; - RelHeight_zhub = 0.0 ; - NominalRPM = 0.0 ; - MinRPM = 0.0 ; - BladeChord = 0.0 ; - LiftCurveSlope = 0.0 ; - BladeFlappingMoment = 0.0 ; - BladeTwist = 0.0 ; - BladeMassMoment = 0.0 ; - TipLossB = 0.0 ; - PolarMoment = 0.0 ; - InflowLag = 0.0 ; - ShaftTilt = 0.0 ; - HingeOffset = 0.0 ; - HingeOffset_hover = 0.0 ; - CantAngleD3 = 0.0 ; + Sense(1.0), NominalRPM(0.0), ExternalRPM(0), RPMdefinition(0), ExtRPMsource(NULL), - theta_shaft = 0.0 ; - phi_shaft = 0.0 ; + BladeChord(0.0), LiftCurveSlope(0.0), BladeTwist(0.0), HingeOffset(0.0), + BladeFlappingMoment(0.0), BladeMassMoment(0.0), PolarMoment(0.0), + InflowLag(0.0), + TipLossB(0.0), + + GroundEffectExp(0.0), GroundEffectShift(0.0), // derived parameters - LockNumberByRho = 0.0 ; - solidity = 0.0 ; - RpmRatio = 0.0 ; - - for (int i=0; i<5; i++) R[i] = 0.0; - for (int i=0; i<6; i++) B[i] = 0.0; - - BodyToShaft.InitMatrix(); - ShaftToBody.InitMatrix(); + LockNumberByRho(0.0), Solidity(0.0), // dynamic values - ActualRPM = 0.0 ; - Omega = 0.0 ; - beta_orient = 0.0 ; - a0 = 0.0 ; - a_1 = b_1 = a_dw = 0.0 ; - a1s = b1s = 0.0 ; - H_drag = J_side = 0.0 ; + RPM(0.0), Omega(0.0), - Torque = 0.0 ; - Thrust = 0.0 ; - Ct = 0.0 ; - lambda = 0.0 ; - mu = 0.0 ; - nu = 0.0 ; - v_induced = 0.0 ; + beta_orient(0.0), + a0(0.0), a_1(0.0), b_1(0.0), a_dw(0.0), a1s(0.0), b1s(0.0), - force = zero_vec ; - moment = zero_vec ; + H_drag(0.0), J_side(0.0), Torque(0.0), C_T(0.0), + lambda(-0.001), mu(0.0), nu(0.001), v_induced(0.0), + theta_downwash(0.0), phi_downwash(0.0), + + // control + ControlMap(eMainCtrl), + CollectiveCtrl(0.0), LateralCtrl(0.0), LongitudinalCtrl(0.0) + +{ + FGColumnVector3 location(0.0, 0.0, 0.0), orientation(0.0, 0.0, 0.0); + Element *thruster_element; + + // initialise/set remaining variables + SetTransformType(FGForce::tCustom); + PropertyManager = exec->GetPropertyManager(); + Type = ttRotor; + GearRatio = 1.0; + + dt = exec->GetDeltaT(); + for (int i=0; i<5; i++) R[i] = 0.0; + for (int i=0; i<5; i++) B[i] = 0.0; + + // get positions + thruster_element = rotor_element->GetParent()->FindElement("sense"); + if (thruster_element) { + double s = thruster_element->GetDataAsNumber(); + if (s < -0.1) { + Sense = -1.0; // 'CW' as seen from above + } else if (s < 0.1) { + Sense = 0.0; // 'coaxial' + } else { + Sense = 1.0; // 'CCW' as seen from above + } + } + + thruster_element = rotor_element->GetParent()->FindElement("location"); + if (thruster_element) { + location = thruster_element->FindElementTripletConvertTo("IN"); + } else { + cerr << "No thruster location found." << endl; + } + + thruster_element = rotor_element->GetParent()->FindElement("orient"); + if (thruster_element) { + orientation = thruster_element->FindElementTripletConvertTo("RAD"); + } else { + cerr << "No thruster orientation found." << endl; + } + + SetLocation(location); + SetAnglesToBody(orientation); + InvTransform = Transform().Transposed(); + + // wire controls + ControlMap = eMainCtrl; + if (rotor_element->FindElement("controlmap")) { + string cm = rotor_element->FindElementValue("controlmap"); + cm = to_upper(cm); + if (cm == "TAIL") { + ControlMap = eTailCtrl; + } else if (cm == "TANDEM") { + ControlMap = eTandemCtrl; + } else { + cerr << "# found unknown controlmap: '" << cm << "' using main rotor config." << endl; + } + } + + // ExternalRPM -- is the RPM dictated ? + if (rotor_element->FindElement("ExternalRPM")) { + ExternalRPM = 1; + RPMdefinition = (int) rotor_element->FindElementValueAsNumber("ExternalRPM"); + } + + // configure the rotor parameters + Configure(rotor_element); + + // shaft representation - a rather simple transform, + // but using a matrix is safer. + TboToHsr.InitMatrix( 0.0, 0.0, 1.0, + 0.0, 1.0, 0.0, + -1.0, 0.0, 0.0 ); + HsrToTbo = TboToHsr.Transposed(); + + // smooth out jumps in hagl reported, otherwise the ground effect + // calculation would cause jumps too. 1Hz seems sufficient. + damp_hagl = Filter(1.0,dt); + + // enable import-export + BindModel(); + + Debug(0); + +} // Constructor + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +FGRotor::~FGRotor(){ + Debug(1); } + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // 5in1: value-fetch-convert-default-return function -double FGRotor::rotor::cnf_elem( const string& ename, double default_val, +double FGRotor::ConfigValueConv( Element* el, const string& ename, double default_val, const string& unit, bool tell) { Element *e = NULL; - double val=default_val; + double val = default_val; - std::string pname = "*No parent element*"; + string pname = "*No parent element*"; - if (parent) { - e = parent->FindElement(ename); - pname = parent->GetName(); + if (el) { + e = el->FindElement(ename); + pname = el->GetName(); } if (e) { if (unit.empty()) { - // val = e->FindElementValueAsNumber(ename); - // yields to: Attempting to get single data value from multiple lines - val = parent->FindElementValueAsNumber(ename); + val = e->GetDataAsNumber(); } else { - // val = e->FindElementValueAsNumberConvertTo(ename,unit); - // yields to: Attempting to get non-existent element diameter + crash, why ? - val = parent->FindElementValueAsNumberConvertTo(ename,unit); + val = el->FindElementValueAsNumberConvertTo(ename,unit); } } else { if (tell) { - cerr << pname << ": missing element '" << ename <<"' using estimated value: " << default_val << endl; + cerr << pname << ": missing element '" << ename << + "' using estimated value: " << default_val << endl; } } @@ -181,135 +240,102 @@ double FGRotor::rotor::cnf_elem( const string& ename, double default_val, //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -double FGRotor::rotor::cnf_elem(const string& ename, double default_val, bool tell) +double FGRotor::ConfigValue(Element* el, const string& ename, double default_val, bool tell) { - return cnf_elem(ename, default_val, "", tell); + return ConfigValueConv(el, ename, default_val, "", tell); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // 1. read configuration and try to fill holes, ymmv // 2. calculate derived parameters and transforms -void FGRotor::rotor::configure(int f, const rotor *xmain) +void FGRotor::Configure(Element* rotor_element) { double estimate; const bool yell = true; const bool silent = false; - flags = f; - estimate = (xmain) ? 2.0 * xmain->Radius * 0.2 : 42.0; - Radius = 0.5 * cnf_elem("diameter", estimate, "FT", yell); - - estimate = (xmain) ? xmain->BladeNum : 2.0; - estimate = Constrain(1.0,estimate,4.0); - BladeNum = (int) cnf_elem("numblades", estimate, yell); - - estimate = (xmain) ? - xmain->Radius * 1.05 - Radius : - 0.025 * Radius ; - RelDistance_xhub = cnf_elem("xhub", estimate, "FT", yell); - - RelShift_yhub = cnf_elem("yhub", 0.0, "FT", silent); + Radius = 0.5 * ConfigValueConv(rotor_element, "diameter", 42.0, "FT", yell); + Radius = Constrain(1e-3, Radius, 1e9); - estimate = - 0.1 * Radius - 4.0; - RelHeight_zhub = cnf_elem("zhub", estimate, "FT", yell); + BladeNum = (int) ConfigValue(rotor_element, "numblades", 3 , yell); + GearRatio = ConfigValue(rotor_element, "gearratio", 1.0, yell); + // make sure that v_tip (omega*r) is below 0.7mach ~ 750ft/s estimate = (750.0/Radius)/(2.0*M_PI) * 60.0; // 7160/Radius - NominalRPM = cnf_elem("nominalrpm", estimate, yell); + NominalRPM = ConfigValue(rotor_element, "nominalrpm", estimate, yell); - MinRPM = cnf_elem("minrpm", 1.0, silent); - MinRPM = Constrain(1.0, MinRPM, NominalRPM-1.0); - - estimate = (xmain) ? 0.12 : 0.07; // guess solidity + estimate = Constrain(0.07, 2.0/Radius , 0.14); // guess solidity estimate = estimate * M_PI*Radius/BladeNum; - BladeChord = cnf_elem("chord", estimate, "FT", yell); + BladeChord = ConfigValueConv(rotor_element, "chord", estimate, "FT", yell); - LiftCurveSlope = cnf_elem("liftcurveslope", 6.0, yell); // "1/RAD" + LiftCurveSlope = ConfigValue(rotor_element, "liftcurveslope", 6.0); // "1/RAD" + BladeTwist = ConfigValueConv(rotor_element, "twist", -0.17, "RAD"); - estimate = sqr(BladeChord) * sqr(Radius) * 0.57; - BladeFlappingMoment = cnf_elem("flappingmoment", estimate, "SLUG*FT2", yell); - BladeFlappingMoment = Constrain(0.1, BladeFlappingMoment, 1e9); + HingeOffset = ConfigValueConv(rotor_element, "hingeoffset", 0.05 * Radius, "FT" ); - BladeTwist = cnf_elem("twist", -0.17, "RAD", yell); + estimate = sqr(BladeChord) * sqr(Radius - HingeOffset) * 0.57; + BladeFlappingMoment = ConfigValueConv(rotor_element, "flappingmoment", estimate, "SLUG*FT2"); + BladeFlappingMoment = Constrain(0.001, BladeFlappingMoment, 1e9); - estimate = sqr(BladeChord) * BladeChord * 15.66; // might be really wrong! - BladeMassMoment = cnf_elem("massmoment", estimate, yell); // slug-ft - BladeMassMoment = Constrain(0.1, BladeMassMoment, 1e9); + // guess mass from moment of a thin stick, and multiply by the blades cg distance + estimate = ( 3.0 * BladeFlappingMoment / sqr(Radius) ) * (0.45 * Radius) ; + BladeMassMoment = ConfigValue(rotor_element, "massmoment", estimate); // unit is slug-ft + BladeMassMoment = Constrain(0.001, BladeMassMoment, 1e9); - TipLossB = cnf_elem("tiplossfactor", 0.98, silent); + TipLossB = ConfigValue(rotor_element, "tiplossfactor", 1.0, silent); estimate = 1.1 * BladeFlappingMoment * BladeNum; - PolarMoment = cnf_elem("polarmoment", estimate, "SLUG*FT2", silent); - PolarMoment = Constrain(0.1, PolarMoment, 1e9); + PolarMoment = ConfigValueConv(rotor_element, "polarmoment", estimate, "SLUG*FT2"); + PolarMoment = Constrain(0.001, PolarMoment, 1e9); - InflowLag = cnf_elem("inflowlag", 0.2, silent); // fixme, depends on size + InflowLag = ConfigValue(rotor_element, "inflowlag", 0.2, yell); // fixme, depends on size + InflowLag = Constrain(1e-6, InflowLag, 2.0); - estimate = (xmain) ? 0.0 : -0.06; - ShaftTilt = cnf_elem("shafttilt", estimate, "RAD", silent); - // ignore differences between teeter/hingeless/fully-articulated constructions - estimate = 0.05 * Radius; - HingeOffset = cnf_elem("hingeoffset", estimate, "FT", (xmain) ? silent : yell); + // ground effect + if (rotor_element->FindElement("cgroundeffect")) { + double cge,gee; + cge = rotor_element->FindElementValueAsNumber("cgroundeffect"); + cge = Constrain(1e-9, cge, 1.0); + gee = 1.0 / ( 2.0*Radius * cge ); + cerr << "# *** 'cgroundeffect' is defunct." << endl; + cerr << "# *** use 'groundeffectexp' with: " << gee << endl; + } - CantAngleD3 = cnf_elem("cantangle", 0.0, "RAD", silent); - - // derived parameters + GroundEffectExp = ConfigValue(rotor_element, "groundeffectexp", 0.0); + GroundEffectShift = ConfigValueConv(rotor_element, "groundeffectshift", 0.0, "FT"); // precalc often used powers R[0]=1.0; R[1]=Radius; R[2]=R[1]*R[1]; R[3]=R[2]*R[1]; R[4]=R[3]*R[1]; - B[0]=1.0; B[1]=TipLossB; B[2]=B[1]*B[1]; B[3]=B[2]*B[1]; B[4]=B[3]*B[1]; B[5]=B[4]*B[1]; + B[0]=1.0; B[1]=TipLossB; B[2]=B[1]*B[1]; B[3]=B[2]*B[1]; B[4]=B[3]*B[1]; + // derived parameters LockNumberByRho = LiftCurveSlope * BladeChord * R[4] / BladeFlappingMoment; - solidity = BladeNum * BladeChord / (M_PI * Radius); + Solidity = BladeNum * BladeChord / (M_PI * Radius); - // use simple orientations at the moment - if (flags & eTail) { // axis parallel to Y_body - theta_shaft = 0.0; // no tilt - phi_shaft = 0.5*M_PI; - - // opposite direction if main rotor is spinning CW - if (xmain && (xmain->flags & eRotCW) ) { - phi_shaft = -phi_shaft; - } - } else { // more or less upright - theta_shaft = ShaftTilt; - phi_shaft = 0.0; - } - - // setup Shaft-Body transforms, see /SH79/ eqn(17,18) - double st = sin(theta_shaft); - double ct = cos(theta_shaft); - double sp = sin(phi_shaft); - double cp = cos(phi_shaft); - - ShaftToBody.InitMatrix( ct, st*sp, st*cp, - 0.0, cp, -sp, - -st, ct*sp, ct*cp ); - - BodyToShaft = ShaftToBody.Inverse(); - - // misc defaults - nu = 0.001; // help the flow solver by providing some moving molecules - return; -} +} // Configure //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // calculate control-axes components of total airspeed at the hub. // sets rotor orientation angle (beta) as side effect. /SH79/ eqn(19-22) -FGColumnVector3 FGRotor::rotor::hub_vel_body2ca( const FGColumnVector3 &uvw, +FGColumnVector3 FGRotor::hub_vel_body2ca( const FGColumnVector3 &uvw, const FGColumnVector3 &pqr, double a_ic, double b_ic) { - FGColumnVector3 v_r, v_shaft, v_w; - FGColumnVector3 pos(RelDistance_xhub,0.0,RelHeight_zhub); + FGColumnVector3 pos; + + pos = fdmex->GetMassBalance()->StructuralToBody(GetActingLocation()); v_r = uvw + pqr*pos; - v_shaft = BodyToShaft * v_r; + v_shaft = TboToHsr * InvTransform * v_r; beta_orient = atan2(v_shaft(eV),v_shaft(eU)); @@ -324,11 +350,14 @@ FGColumnVector3 FGRotor::rotor::hub_vel_body2ca( const FGColumnVector3 &uvw, // express fuselage angular velocity in control axes /SH79/ eqn(30,31) -FGColumnVector3 FGRotor::rotor::fus_angvel_body2ca( const FGColumnVector3 &pqr) +FGColumnVector3 FGRotor::fus_angvel_body2ca( const FGColumnVector3 &pqr) { FGColumnVector3 av_s_fus, av_w_fus; - av_s_fus = BodyToShaft * pqr; + // for comparison: + // av_s_fus = BodyToShaft * pqr; /SH79/ + // BodyToShaft = TboToHsr * InvTransform + av_s_fus = TboToHsr * InvTransform * pqr; av_w_fus(eP)= av_s_fus(eP)*cos(beta_orient) + av_s_fus(eQ)*sin(beta_orient); av_w_fus(eQ)= - av_s_fus(eP)*sin(beta_orient) + av_s_fus(eQ)*cos(beta_orient); @@ -337,150 +366,69 @@ FGColumnVector3 FGRotor::rotor::fus_angvel_body2ca( const FGColumnVector3 &pqr) return av_w_fus; } -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -// problem function passed to rk solver - - double FGRotor::rotor::dnuFunction::pFunc(double x, double nu) { - double d_nu; - d_nu = k_sat * (ct_lambda * (k_wor - nu) + k_theta) / - (2.0 * sqrt( mu2 + sqr(k_wor - nu)) ); - d_nu = d_nu * k_flowscale - nu; - return d_nu; - }; //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - // merge params to keep the equation short - void FGRotor::rotor::dnuFunction::update_params(rotor *r, double ct_t01, double fs, double w) { - k_sat = 0.5* r->solidity * r->LiftCurveSlope; - ct_lambda = 1.0/2.0*r->B[2] + 1.0/4.0 * r->mu*r->mu; - k_wor = w/(r->Omega*r->Radius); - k_theta = ct_t01; - mu2 = r->mu * r->mu; - k_flowscale = fs; - }; - - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -// Calculate rotor thrust and inflow-ratio (lambda), this is achieved by -// approximating a solution for the differential equation: -// -// dnu/dt = 1/tau ( Ct / (2*sqrt(mu^2+lambda^2)) - nu ) , /SH79/ eqn(26). -// -// Propper calculation of the inflow-ratio (lambda) is vital for the -// following calculations. Simple implementations (i.e. Newton-Raphson w/o -// checking) tend to oscillate or overshoot in the low speed region, -// therefore a more expensive solver is used. +// The calculation is a bit tricky because thrust depends on induced velocity, +// and vice versa. // -// The flow_scale parameter is used to approximate a reduction of inflow -// if the helicopter is close to the ground, yielding to higher thrust, -// see /TA77/ eqn(10a). Doing the ground effect calculation here seems -// more favorable then to code it in the fdm_config. +// The flow_scale parameter (ranging from 0.5-1.0) is used to approximate a +// reduction of inflow if the helicopter is close to the ground, yielding to +// higher thrust, see /TA77/ eqn(10a). -void FGRotor::rotor::calc_flow_and_thrust( - double dt, double rho, double theta_0, - double Uw, double Ww, double flow_scale) +void FGRotor::calc_flow_and_thrust( double theta_0, double Uw, double Ww, + double flow_scale) { double ct_over_sigma = 0.0; - double ct_l, ct_t0, ct_t1; - double nu_ret = 0.0; + double c0, ct_l, ct_t0, ct_t1; + double mu2; mu = Uw/(Omega*Radius); // /SH79/ eqn(24) - - ct_t0 = (1.0/3.0*B[3] + 1.0/2.0 * TipLossB*mu*mu - 4.0/(9.0*M_PI) * mu*mu*mu )*theta_0; - ct_t1 = (1.0/4.0*B[4] + 1.0/4.0 * B[2]*mu*mu)*BladeTwist; - - // merge params together - flowEquation.update_params(this, ct_t0+ct_t1, flow_scale, Ww); + mu2 = sqr(mu); - nu_ret = rk.evolve(nu, &flowEquation); + ct_t0 = (1.0/3.0*B[3] + 1.0/2.0 * TipLossB*mu2 - 4.0/(9.0*M_PI) * mu*mu2 ) * theta_0; + ct_t1 = (1.0/4.0*B[4] + 1.0/4.0 * B[2]*mu2) * BladeTwist; - if (rk.getStatus() != FGRungeKutta::eNoError) { // never observed so far - cerr << "# IEHHHH [" << flags << "]: Solver Error - resetting!" << endl; - rk.clearStatus(); - nu_ret = nu; // use old value and keep fingers crossed. - } + ct_l = (1.0/2.0*B[2] + 1.0/4.0 * mu2) * lambda; // first time - // keep an eye on the solver, but be quiet after a hundred messages - if (reports < 100 && rk.getIterations()>6) { - cerr << "# LOOK [" << flags << "]: Solver took " - << rk.getIterations() << " rounds." << endl; - reports++; - if (reports==100) { - cerr << "# stopped babbling after 100 notifications." << endl; - } - } + c0 = (LiftCurveSlope/2.0)*(ct_l + ct_t0 + ct_t1) * Solidity; + c0 = c0 / ( 2.0 * sqrt( sqr(mu) + sqr(lambda) ) + 1e-15); - // now from nu to lambda, Ct, and Thrust + // replacement for /SH79/ eqn(26). + // ref: dnu/dt = 1/tau ( Ct / (2*sqrt(mu^2+lambda^2)) - nu ) + // taking mu and lambda constant, this integrates to + + nu = flow_scale * ((nu - c0) * exp(-dt/InflowLag) + c0); + + // now from nu to lambda, C_T, and Thrust - nu = nu_ret; lambda = Ww/(Omega*Radius) - nu; // /SH79/ eqn(25) - ct_l = (1.0/2.0*B[2] + 1.0/4.0 * mu*mu)*lambda; + ct_l = (1.0/2.0*B[2] + 1.0/4.0 * mu2) * lambda; + ct_over_sigma = (LiftCurveSlope/2.0)*(ct_l + ct_t0 + ct_t1); // /SH79/ eqn(27) Thrust = BladeNum*BladeChord*Radius*rho*sqr(Omega*Radius) * ct_over_sigma; - Ct = ct_over_sigma * solidity; + C_T = ct_over_sigma * Solidity; v_induced = nu * (Omega*Radius); - if (dump_req && (flags & eMain) ) { - printf("# mu %f : nu %f lambda %f vi %f\n", mu, nu, lambda, v_induced); - } - } -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -// this is the most arcane part in the calculation chain the -// constants used should be reverted to more general parameters. -// otoh: it works also for smaller rotors, sigh! -// See /SH79/ eqn(36), and /BA41/ for a richer set of equations. - -void FGRotor::rotor::calc_torque(double rho, double theta_0) -{ - double Qa0; - double cq_s_m[5], cq_over_sigma; - double l,m,t075; // shortcuts - - t075 = theta_0 + 0.75 * BladeTwist; - - m = mu; - l = lambda; - - cq_s_m[0] = 0.00109 - 0.0036*l - 0.0027*t075 - 1.10*sqr(l) - 0.545*l*t075 + 0.122*sqr(t075); - cq_s_m[2] = ( 0.00109 - 0.0027*t075 - 3.13*sqr(l) - 6.35*l*t075 - 1.93*sqr(t075) ) * sqr(m); - cq_s_m[3] = - 0.133*l*t075 * sqr(m)*m; - cq_s_m[4] = ( - 0.976*sqr(l) - 6.38*l*t075 - 5.26*sqr(t075) ) * sqr(m)*sqr(m); - - cq_over_sigma = cq_s_m[0] + cq_s_m[2] + cq_s_m[3] + cq_s_m[4]; - // guess an a (LiftCurveSlope) is included in eqn above, so check if there is a large - // influence when a_'other-model'/ a_'ch54' diverts from 1.0. - - Qa0 = BladeNum * BladeChord * R[2] * rho * sqr(Omega*Radius); - -// TODO: figure out how to handle negative cq_over_sigma/torque - - Torque = Qa0 * cq_over_sigma; - - return; -} //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // The coning angle doesn't apply for teetering rotors, but calculating // doesn't hurt. /SH79/ eqn(29) -void FGRotor::rotor::calc_coning_angle(double rho, double theta_0) +void FGRotor::calc_coning_angle(double theta_0) { double lock_gamma = LockNumberByRho * rho; - double a0_l = (1.0/6.0 * B[3] + 0.04 * mu*mu*mu) * lambda; - double a0_t0 = (1.0/8.0 * B[4] + 1.0/8.0 * B[2]*mu*mu) * theta_0; - double a0_t1 = (1.0/10.0 * B[5] + 1.0/12.0 * B[3]*mu*mu) * BladeTwist; + double a0_l = (1.0/6.0 + 0.04 * mu*mu*mu) * lambda; + double a0_t0 = (1.0/8.0 + 1.0/8.0 * mu*mu) * theta_0; + double a0_t1 = (1.0/10.0 + 1.0/12.0 * mu*mu) * BladeTwist; a0 = lock_gamma * ( a0_l + a0_t0 + a0_t1); return; } @@ -489,33 +437,33 @@ void FGRotor::rotor::calc_coning_angle(double rho, double theta_0) // Flapping angles relative to control axes /SH79/ eqn(32) -void FGRotor::rotor::calc_flapping_angles( double rho, double theta_0, - const FGColumnVector3 &pqr_fus_w) +void FGRotor::calc_flapping_angles(double theta_0, const FGColumnVector3 &pqr_fus_w) { double lock_gamma = LockNumberByRho * rho; - double mu2_2B2 = sqr(mu)/(2.0*B[2]); + + double mu2_2 = sqr(mu)/2.0; double t075 = theta_0 + 0.75 * BladeTwist; // common approximation for rectangular blades - a_1 = 1.0/(1.0 - mu2_2B2) * ( + a_1 = 1.0/(1.0 - mu2_2) * ( (2.0*lambda + (8.0/3.0)*t075)*mu + pqr_fus_w(eP)/Omega - - 16.0 * pqr_fus_w(eQ)/(B[4]*lock_gamma*Omega) + - 16.0 * pqr_fus_w(eQ)/(lock_gamma*Omega) ); - - b_1 = 1.0/(1.0 + mu2_2B2) * ( + + b_1 = 1.0/(1.0 + mu2_2) * ( (4.0/3.0)*mu*a0 - pqr_fus_w(eQ)/Omega - - 16.0 * pqr_fus_w(eP)/(B[4]*lock_gamma*Omega) + - 16.0 * pqr_fus_w(eP)/(lock_gamma*Omega) ); - + // used in force calc - a_dw = 1.0/(1.0 - mu2_2B2) * ( + a_dw = 1.0/(1.0 - mu2_2) * ( (2.0*lambda + (8.0/3.0)*t075)*mu - - 24.0 * pqr_fus_w(eQ)/(B[4]*lock_gamma*Omega) - * ( 1.0 - ( 0.29 * t075 / (Ct/solidity) ) ) + - 24.0 * pqr_fus_w(eQ)/(lock_gamma*Omega) + * ( 1.0 - ( 0.29 * t075 / (C_T/Solidity) ) ) ); - + return; } @@ -523,20 +471,20 @@ void FGRotor::rotor::calc_flapping_angles( double rho, double theta_0, // /SH79/ eqn(38,39) -void FGRotor::rotor::calc_drag_and_side_forces(double rho, double theta_0) +void FGRotor::calc_drag_and_side_forces(double theta_0) { double cy_over_sigma ; double t075 = theta_0 + 0.75 * BladeTwist; H_drag = Thrust * a_dw; - + cy_over_sigma = ( 0.75*b_1*lambda - 1.5*a0*mu*lambda + 0.25*a_1*b_1*mu - a0*a_1*sqr(mu) + (1.0/6.0)*a0*a_1 - (0.75*mu*a0 - (1.0/3.0)*b_1 - 0.5*sqr(mu)*b_1)*t075 ); cy_over_sigma *= LiftCurveSlope/2.0; - + J_side = BladeNum * BladeChord * Radius * rho * sqr(Omega*Radius) * cy_over_sigma; return; @@ -544,491 +492,257 @@ void FGRotor::rotor::calc_drag_and_side_forces(double rho, double theta_0) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +// Simplified version of /SH79/ eqn(36). Uses an estimate for blade drag +// (a new config parameter to come...). +// From "Bramwell's Helicopter Dynamics" ­ second edition, eqn(3.43) and (3.44) + +void FGRotor::calc_torque(double theta_0) +{ + // estimate blade drag + double delta_dr = 0.009 + 0.3*sqr(6.0*C_T/(LiftCurveSlope*Solidity)); + + Torque = rho * BladeNum * BladeChord * delta_dr * sqr(Omega*Radius) * R[2] * + (1.0+4.5*sqr(mu))/8.0 + - (Thrust*lambda + H_drag*mu)*Radius; + + return; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + // transform rotor forces from control axes to shaft axes, and express // in body axes /SH79/ eqn(40,41) -FGColumnVector3 FGRotor::rotor::body_forces(double a_ic, double b_ic) +FGColumnVector3 FGRotor::body_forces(double a_ic, double b_ic) { - FGColumnVector3 F_s( - - H_drag*cos(beta_orient) - J_side*sin(beta_orient) + Thrust*b_ic, - - H_drag*sin(beta_orient) + J_side*cos(beta_orient) + Thrust*a_ic, - - Thrust); + FGColumnVector3 F_s( + - H_drag*cos(beta_orient) - J_side*sin(beta_orient) + Thrust*b_ic, + - H_drag*sin(beta_orient) + J_side*cos(beta_orient) + Thrust*a_ic, + - Thrust); - if (dump_req && (flags & eMain) ) { - printf("# abß: % f % f % f\n", a_ic, b_ic, beta_orient ); - printf("# HJT: % .2f % .2f % .2f\n", H_drag, J_side, Thrust ); - printf("# F_s: % .2f % .2f % .2f\n", F_s(1), F_s(2), F_s(3) ); - FGColumnVector3 F_h; - F_h = ShaftToBody * F_s; - printf("# F_h: % .2f % .2f % .2f\n", F_h(1), F_h(2), F_h(3) ); - } - - return ShaftToBody * F_s; + return HsrToTbo * F_s; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// rotational sense is handled here -// still a todo: how to get propper values for 'BladeMassMoment' -// here might be a good place to tweak hovering stability, check /AM50/. +// calculates the additional moments due to hinge offset and handles +// torque and sense -FGColumnVector3 FGRotor::rotor::body_moments(FGColumnVector3 F_h, double a_ic, double b_ic) +FGColumnVector3 FGRotor::body_moments(double a_ic, double b_ic) { FGColumnVector3 M_s, M_hub, M_h; - - FGColumnVector3 h_pos(RelDistance_xhub, 0.0, RelHeight_zhub); - - // vermutlich ein biege moment, bzw.widerstands moment ~ d^3 - double M_w_tilde = 0.0 ; - double mf = 0.0 ; - - M_w_tilde = BladeMassMoment; + double mf; // cyclic flapping relative to shaft axes /SH79/ eqn(43) a1s = a_1*cos(beta_orient) + b_1*sin(beta_orient) - b_ic; b1s = b_1*cos(beta_orient) - a_1*sin(beta_orient) + a_ic; - // mind this: no HingeOffset, no additional pitch/roll moments - mf = 0.5 * (HingeOffset+HingeOffset_hover) * BladeNum * Omega*Omega * M_w_tilde; + mf = 0.5 * HingeOffset * BladeNum * Omega*Omega * BladeMassMoment; + M_s(eL) = mf*b1s; M_s(eM) = mf*a1s; - M_s(eN) = Torque; + M_s(eN) = Torque * Sense ; - if (flags & eRotCW) { - M_s(eN) = -M_s(eN); - } - - if (flags & eCoaxial) { - M_s(eN) = 0.0; - } - - M_hub = ShaftToBody * M_s; - - M_h = M_hub + (h_pos * F_h); - - return M_h; -} - - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -// Constructor - -FGRotor::FGRotor(FGFDMExec *exec, Element* rotor_element, int num) - : FGThruster(exec, rotor_element, num) -{ - - FGColumnVector3 location, orientation; - Element *thruster_element; - - PropertyManager = fdmex->GetPropertyManager(); - dt = fdmex->GetDeltaT(); - - /* apply defaults */ - - rho = 0.002356; // just a sane value - - RPM = 0.0; - Sense = 1.0; - tailRotorPresent = false; - - effective_tail_col = 0.001; // just a sane value - - prop_inflow_ratio_lambda = 0.0; - prop_advance_ratio_mu = 0.0; - prop_inflow_ratio_induced_nu = 0.0; - prop_mr_torque = 0.0; - prop_thrust_coefficient = 0.0; - prop_coning_angle = 0.0; - - prop_theta_downwash = prop_phi_downwash = 0.0; - - hover_threshold = 0.0; - hover_scale = 0.0; - - mr.zero(); - tr.zero(); - - // debug stuff - prop_DumpFlag = 0; - - /* configure */ - - Type = ttRotor; - SetTransformType(FGForce::tCustom); - - // get data from parent and 'mount' the rotor system - - thruster_element = rotor_element->GetParent()->FindElement("sense"); - if (thruster_element) { - Sense = thruster_element->GetDataAsNumber() >= 0.0 ? 1.0: -1.0; - } - - thruster_element = rotor_element->GetParent()->FindElement("location"); - if (thruster_element) location = thruster_element->FindElementTripletConvertTo("IN"); - else cerr << "No thruster location found." << endl; - - thruster_element = rotor_element->GetParent()->FindElement("orient"); - if (thruster_element) orientation = thruster_element->FindElementTripletConvertTo("RAD"); - else cerr << "No thruster orientation found." << endl; - - SetLocation(location); - SetAnglesToBody(orientation); - - // get main rotor parameters - mr.parent = rotor_element; - - int flags = eMain; - - string a_val=""; - a_val = rotor_element->GetAttributeValue("variant"); - if ( a_val == "coaxial" ) { - flags += eCoaxial; - cerr << "# found 'coaxial' variant" << endl; - } - - if (Sense<0.0) { - flags += eRotCW; - } - - mr.configure(flags); - - mr.rk.init(0,dt,6); - - // get tail rotor parameters - tr.parent=rotor_element->FindElement("tailrotor"); - if (tr.parent) { - tailRotorPresent = true; - } else { - tailRotorPresent = false; - cerr << "# No tailrotor found, assuming a single rotor." << endl; - } - - if (tailRotorPresent) { - int flags = eTail; - if (Sense<0.0) { - flags += eRotCW; - } - tr.configure(flags, &mr); - tr.rk.init(0,dt,6); - tr.RpmRatio = tr.NominalRPM/mr.NominalRPM; // 'connect' - } - - /* remaining parameters */ - - // ground effect - double c_ground_effect = 0.0; // uh1 ~ 0.28 , larger values increase the effect - ground_effect_exp = 0.0; - ground_effect_shift = 0.0; - - if (rotor_element->FindElement("cgroundeffect")) - c_ground_effect = rotor_element->FindElementValueAsNumber("cgroundeffect"); - - if (rotor_element->FindElement("groundeffectshift")) - ground_effect_shift = rotor_element->FindElementValueAsNumberConvertTo("groundeffectshift","FT"); - - // prepare calculations, see /TA77/ - if (c_ground_effect > 1e-5) { - ground_effect_exp = 1.0 / ( 2.0*mr.Radius * c_ground_effect ); - } else { - ground_effect_exp = 0.0; // disable - } - - // smooth out jumps in hagl reported, otherwise the ground effect - // calculation would cause jumps too. 1Hz seems sufficient. - damp_hagl = Filter(1.0,dt); - - - // misc, experimental - if (rotor_element->FindElement("hoverthreshold")) - hover_threshold = rotor_element->FindElementValueAsNumberConvertTo("hoverthreshold", "FT/SEC"); - - if (rotor_element->FindElement("hoverscale")) - hover_scale = rotor_element->FindElementValueAsNumber("hoverscale"); - - // enable import-export - bind(); - - // unused right now - prop_rotorbrake->setDoubleValue(0.0); - prop_freewheel_factor->setDoubleValue(1.0); - - Debug(0); - -} // Constructor - -//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -FGRotor::~FGRotor() -{ - Debug(1); + return HsrToTbo * M_s; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// mea-culpa - the connection to the engine might be wrong, but the calling -// convention appears to be 'variable' too. -// piston call: -// return Thruster->Calculate((Eng_HP * hptoftlbssec)-Thruster->GetPowerRequired()); -// turbine call: -// Thrust = Thruster->Calculate(Thrust); // allow thruster to modify thrust (i.e. reversing) -// -// Here 'Calculate' takes thrust and estimates the power provided. - -double FGRotor::Calculate(double PowerAvailable) +void FGRotor::CalcStatePart1(void) { - // controls double A_IC; // lateral (roll) control in radians double B_IC; // longitudinal (pitch) control in radians - double theta_col; // main rotor collective pitch in radians - double tail_col; // tail rotor collective in radians + double theta_col; // rotor collective pitch in radians - // state - double h_agl_ft = 0.0; double Vt ; - FGColumnVector3 UVW_h; - FGColumnVector3 PQR_h; + FGColumnVector3 UVW_h, PQR_h; + FGColumnVector3 vHub_ca, avFus_ca; - /* total vehicle velocity including wind effects in feet per second. */ - Vt = fdmex->GetAuxiliary()->GetVt(); + double h_agl_ft, filtered_hagl = 0.0; + double ge_factor = 1.0; - dt = fdmex->GetDeltaT(); // might be variable ? - - dump_req = prop_DumpFlag; - prop_DumpFlag = 0; - - // fetch often used values + // fetch needed values from environment + Vt = fdmex->GetAuxiliary()->GetVt(); // total vehicle velocity including wind + dt = fdmex->GetDeltaT(); rho = fdmex->GetAtmosphere()->GetDensity(); // slugs/ft^3. - UVW_h = fdmex->GetAuxiliary()->GetAeroUVW(); PQR_h = fdmex->GetAuxiliary()->GetAeroPQR(); - - // handle present RPM now, calc omega values. - - if (RPM < mr.MinRPM) { // kludge, otherwise calculations go bananas - RPM = mr.MinRPM; - } - - mr.ActualRPM = RPM; - mr.Omega = (RPM/60.0)*2.0*M_PI; - - if (tailRotorPresent) { - tr.ActualRPM = RPM*tr.RpmRatio; - tr.Omega = (RPM*tr.RpmRatio/60.0)*2.0*M_PI; - } - - // read control inputs - - A_IC = prop_lateral_ctrl->getDoubleValue(); - B_IC = prop_longitudinal_ctrl->getDoubleValue(); - theta_col = prop_collective_ctrl->getDoubleValue(); - tail_col = 0.0; - if (tailRotorPresent) { - tail_col = prop_antitorque_ctrl->getDoubleValue(); - } - - - FGColumnVector3 vHub_ca = mr.hub_vel_body2ca(UVW_h,PQR_h,A_IC,B_IC); - FGColumnVector3 avFus_ca = mr.fus_angvel_body2ca(PQR_h); - - h_agl_ft = fdmex->GetPropagate()->GetDistanceAGL(); + // update InvTransform, the rotor orientation could have been altered + InvTransform = Transform().Transposed(); - double filtered_hagl; - filtered_hagl = damp_hagl.execute( h_agl_ft + ground_effect_shift ); - - // gnuplot> plot [-1:50] 1 - exp((-x/44)/.28) - double ge_factor = 1.0; - if (ground_effect_exp > 1e-5) { - ge_factor -= exp(-filtered_hagl*ground_effect_exp); - } - // clamp - if (ge_factor<0.5) ge_factor=0.5; - - if (dump_req) { - printf("# GE h: %.3f (%.3f) f: %f\n", filtered_hagl, h_agl_ft + ground_effect_shift, ge_factor); + // handle RPM requirements, calc omega. + if (ExternalRPM && ExtRPMsource) { + RPM = ExtRPMsource->getDoubleValue() / GearRatio; } + if (RPM < 1.0) { // kludge, otherwise calculations go bananas + RPM = 1.0; + } - // EXPERIMENTAL: modify rotor for hover, see rotor::body_moments for the consequences - if (hover_threshold > 1e-5 && Vt < hover_threshold) { - double scale = 1.0 - Vt/hover_threshold; - mr.HingeOffset_hover = scale*hover_scale*mr.Radius; - } else { - mr.HingeOffset_hover = 0.0; + Omega = (RPM/60.0)*2.0*M_PI; + + // set control inputs + A_IC = LateralCtrl; + B_IC = LongitudinalCtrl; + theta_col = CollectiveCtrl; + + // ground effect + if (GroundEffectExp > 1e-5) { + if (h_agl_ft<0.0) h_agl_ft = 0.0; // clamp + filtered_hagl = damp_hagl.execute(h_agl_ft) + GroundEffectShift; + // actual/nominal factor avoids absurd scales at startup + ge_factor -= exp(-filtered_hagl*GroundEffectExp) * (RPM / NominalRPM); + if (ge_factor<0.5) ge_factor=0.5; // clamp } // all set, start calculations - /* MAIN ROTOR */ + vHub_ca = hub_vel_body2ca(UVW_h, PQR_h, A_IC, B_IC); - mr.calc_flow_and_thrust(dt, rho, theta_col, vHub_ca(eU), vHub_ca(eW), ge_factor); + avFus_ca = fus_angvel_body2ca(PQR_h); - prop_inflow_ratio_lambda = mr.lambda; - prop_advance_ratio_mu = mr.mu; - prop_inflow_ratio_induced_nu = mr.nu; - prop_thrust_coefficient = mr.Ct; + calc_flow_and_thrust(theta_col, vHub_ca(eU), vHub_ca(eW), ge_factor); - mr.calc_coning_angle(rho, theta_col); - prop_coning_angle = mr.a0; + calc_coning_angle(theta_col); - mr.calc_torque(rho, theta_col); - prop_mr_torque = mr.Torque; + calc_flapping_angles(theta_col, avFus_ca); - mr.calc_flapping_angles(rho, theta_col, avFus_ca); - mr.calc_drag_and_side_forces(rho, theta_col); + calc_drag_and_side_forces(theta_col); - FGColumnVector3 F_h_mr = mr.body_forces(A_IC,B_IC); - FGColumnVector3 M_h_mr = mr.body_moments(F_h_mr, A_IC, B_IC); + calc_torque(theta_col); - // export downwash angles - // theta: positive for downwash moving from +z_h towards +x_h - // phi: positive for downwash moving from +z_h towards -y_h + // Fixme: only valid for a 'decent' rotor + theta_downwash = atan2( - UVW_h(eU), v_induced - UVW_h(eW)); + phi_downwash = atan2( UVW_h(eV), v_induced - UVW_h(eW)); - prop_theta_downwash = atan2( - UVW_h(eU), mr.v_induced - UVW_h(eW)); - prop_phi_downwash = atan2( UVW_h(eV), mr.v_induced - UVW_h(eW)); + vFn = body_forces(A_IC, B_IC); + vMn = Transform() * body_moments(A_IC, B_IC); - mr.force(eX) = F_h_mr(1); - mr.force(eY) = F_h_mr(2); - mr.force(eZ) = F_h_mr(3); - - mr.moment(eL) = M_h_mr(1); - mr.moment(eM) = M_h_mr(2); - mr.moment(eN) = M_h_mr(3); - - /* TAIL ROTOR */ - - FGColumnVector3 F_h_tr(0.0, 0.0, 0.0); - FGColumnVector3 M_h_tr(0.0, 0.0, 0.0); - - if (tailRotorPresent) { - FGColumnVector3 vHub_ca_tr = tr.hub_vel_body2ca(UVW_h,PQR_h); - FGColumnVector3 avFus_ca_tr = tr.fus_angvel_body2ca(PQR_h); - - tr.calc_flow_and_thrust(dt, rho, tail_col, vHub_ca_tr(eU), vHub_ca_tr(eW)); - tr.calc_coning_angle(rho, tail_col); - - // test code for cantered tail rotor, see if it has a notable effect. /SH79/ eqn(47) - if (fabs(tr.CantAngleD3)>1e-5) { - double tan_d3 = tan(tr.CantAngleD3); - double d_t0t; - double ca_dt = dt/12.0; - for (int i = 0; i<12; i++) { - d_t0t = 1/0.1*(tail_col - tr.a0 * tan_d3 - effective_tail_col); - effective_tail_col += d_t0t*ca_dt; - } - } else { - effective_tail_col = tail_col; - } - - tr.calc_torque(rho, effective_tail_col); - tr.calc_flapping_angles(rho, effective_tail_col, avFus_ca_tr); - tr.calc_drag_and_side_forces(rho, effective_tail_col); - - F_h_tr = tr.body_forces(); - M_h_tr = tr.body_moments(F_h_tr); - } - - tr.force(eX) = F_h_tr(1) ; - tr.force(eY) = F_h_tr(2) ; - tr.force(eZ) = F_h_tr(3) ; - tr.moment(eL) = M_h_tr(1) ; - tr.moment(eM) = M_h_tr(2) ; - tr.moment(eN) = M_h_tr(3) ; - -/* - TODO: - check negative mr.Torque conditions - freewheel factor: assure [0..1] just multiply with available power - rotorbrake: just steal from available power - -*/ - - // calculate new RPM, assuming a stiff connection between engine and rotor. - - double engine_hp = PowerAvailable/2.24; // 'undo' force via the estimation factor used in aeromatic - double engine_torque = 550.0*engine_hp/mr.Omega; - double Omega_dot = (engine_torque - mr.Torque) / mr.PolarMoment; - - RPM += ( Omega_dot * dt )/(2.0*M_PI) * 60.0; - - if (0 && dump_req) { - printf("# SENSE : % d % d\n", mr.flags & eRotCW ? -1 : 1, tr.flags & eRotCW ? -1 : 1); - printf("# vi : % f % f\n", mr.v_induced, tr.v_induced); - printf("# a0 a1 b1 : % f % f % f\n", mr.a0, mr.a_1, mr.b_1 ); - printf("# m forces : % f % f % f\n", mr.force(eX), mr.force(eY), mr.force(eZ) ); - printf("# m moments : % f % f % f\n", mr.moment(eL), mr.moment(eM), mr.moment(eN) ); - printf("# t forces : % f % f % f\n", tr.force(eX), tr.force(eY), tr.force(eZ) ); - printf("# t moments : % f % f % f\n", tr.moment(eL), tr.moment(eM), tr.moment(eN) ); - } - - // finally set vFn & vMn - vFn = mr.force + tr.force; - vMn = mr.moment + tr.moment; - - // and just lie here - Thrust = 0.0; - - // return unmodified thrust to the turbine. - // :TK: As far as I can see the return value is unused. - return PowerAvailable; - -} // Calculate +} //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -// FGThruster does return 0.0 (the implicit direct thruster) -// piston CALL: return Thruster->Calculate((Eng_HP * hptoftlbssec)-Thruster->GetPowerRequired()); +void FGRotor::CalcStatePart2(double PowerAvailable) +{ + if (! ExternalRPM) { + // calculate new RPM + double ExcessTorque = PowerAvailable / Omega; + double deltaOmega = ExcessTorque / PolarMoment * dt; + RPM += deltaOmega/(2.0*M_PI) * 60.0; + if (RPM < 0.0) RPM = 0.0; // Engine won't turn backwards + } +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% double FGRotor::GetPowerRequired(void) { - PowerRequired = 0.0; + CalcStatePart1(); + PowerRequired = Torque * Omega; return PowerRequired; } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -bool FGRotor::bind(void) { +double FGRotor::Calculate(double PowerAvailable) +{ + CalcStatePart2(PowerAvailable); + return Thrust; +} + +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +bool FGRotor::BindModel(void) +{ string property_name, base_property_name; base_property_name = CreateIndexedPropertyName("propulsion/engine", EngineNum); - PropertyManager->Tie( base_property_name + "/rotor-rpm", this, &FGRotor::GetRPM ); - PropertyManager->Tie( base_property_name + "/thrust-mr-lbs", &mr.Thrust ); - PropertyManager->Tie( base_property_name + "/vi-mr-fps", &mr.v_induced ); - PropertyManager->Tie( base_property_name + "/a0-mr-rad", &mr.a0 ); - PropertyManager->Tie( base_property_name + "/a1-mr-rad", &mr.a1s ); // s means shaft axes - PropertyManager->Tie( base_property_name + "/b1-mr-rad", &mr.b1s ); - PropertyManager->Tie( base_property_name + "/thrust-tr-lbs", &tr.Thrust ); - PropertyManager->Tie( base_property_name + "/vi-tr-fps", &tr.v_induced ); + property_name = base_property_name + "/rotor-rpm"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetRPM ); - // lambda - PropertyManager->Tie( base_property_name + "/inflow-ratio", &prop_inflow_ratio_lambda ); - // mu - PropertyManager->Tie( base_property_name + "/advance-ratio", &prop_advance_ratio_mu ); - // nu - PropertyManager->Tie( base_property_name + "/induced-inflow-ratio", &prop_inflow_ratio_induced_nu ); + property_name = base_property_name + "/x-engine-rpm"; // used for RPM eXchange + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetEngineRPM ); - PropertyManager->Tie( base_property_name + "/torque-mr-lbsft", &prop_mr_torque ); - PropertyManager->Tie( base_property_name + "/thrust-coefficient", &prop_thrust_coefficient ); - PropertyManager->Tie( base_property_name + "/main-rotor-rpm", &mr.ActualRPM ); - PropertyManager->Tie( base_property_name + "/tail-rotor-rpm", &tr.ActualRPM ); + property_name = base_property_name + "/rotor-thrust-lbs"; // might be redundant - check! + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetThrust ); - // position of the downwash - PropertyManager->Tie( base_property_name + "/theta-downwash-rad", &prop_theta_downwash ); - PropertyManager->Tie( base_property_name + "/phi-downwash-rad", &prop_phi_downwash ); + property_name = base_property_name + "/a0-rad"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetA0 ); - // nodes to use via getValue - prop_collective_ctrl = PropertyManager->GetNode(base_property_name + "/collective-ctrl-rad",true); - prop_lateral_ctrl = PropertyManager->GetNode(base_property_name + "/lateral-ctrl-rad",true); - prop_longitudinal_ctrl = PropertyManager->GetNode(base_property_name + "/longitudinal-ctrl-rad",true); - prop_antitorque_ctrl = PropertyManager->GetNode(base_property_name + "/antitorque-ctrl-rad",true); + property_name = base_property_name + "/a1-rad"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetA1 ); - prop_rotorbrake = PropertyManager->GetNode(base_property_name + "/rotorbrake-hp", true); - prop_freewheel_factor = PropertyManager->GetNode(base_property_name + "/freewheel-factor", true); + property_name = base_property_name + "/b1-rad"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetB1 ); - PropertyManager->Tie( base_property_name + "/dump-flag", &prop_DumpFlag ); + property_name = base_property_name + "/inflow-ratio"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetLambda ); + + property_name = base_property_name + "/advance-ratio"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetMu ); + + property_name = base_property_name + "/induced-inflow-ratio"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetNu ); + + property_name = base_property_name + "/vi-fps"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetVi ); + + property_name = base_property_name + "/thrust-coefficient"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetCT ); + + property_name = base_property_name + "/torque-lbsft"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetTorque ); + + property_name = base_property_name + "/theta-downwash-rad"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetThetaDW ); + + property_name = base_property_name + "/phi-downwash-rad"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetPhiDW ); + + switch (ControlMap) { + case eTailCtrl: + property_name = base_property_name + "/antitorque-ctrl-rad"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetCollectiveCtrl, &FGRotor::SetCollectiveCtrl); + break; + case eTandemCtrl: + property_name = base_property_name + "/tail-collective-ctrl-rad"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetCollectiveCtrl, &FGRotor::SetCollectiveCtrl); + property_name = base_property_name + "/lateral-ctrl-rad"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetLateralCtrl, &FGRotor::SetLateralCtrl); + property_name = base_property_name + "/longitudinal-ctrl-rad"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetLongitudinalCtrl, &FGRotor::SetLongitudinalCtrl); + break; + default: // eMainCtrl + property_name = base_property_name + "/collective-ctrl-rad"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetCollectiveCtrl, &FGRotor::SetCollectiveCtrl); + property_name = base_property_name + "/lateral-ctrl-rad"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetLateralCtrl, &FGRotor::SetLateralCtrl); + property_name = base_property_name + "/longitudinal-ctrl-rad"; + PropertyManager->Tie( property_name.c_str(), this, &FGRotor::GetLongitudinalCtrl, &FGRotor::SetLongitudinalCtrl); + } + + if (ExternalRPM) { + if (RPMdefinition == -1) { + property_name = base_property_name + "/x-rpm-dict"; + ExtRPMsource = PropertyManager->GetNode(property_name, true); + } else if (RPMdefinition >= 0 && RPMdefinition != EngineNum) { + string ipn = CreateIndexedPropertyName("propulsion/engine", RPMdefinition); + property_name = ipn + "/x-engine-rpm"; + ExtRPMsource = PropertyManager->GetNode(property_name, false); + if (! ExtRPMsource) { + cerr << "# Warning: Engine number " << EngineNum << "." << endl; + cerr << "# No 'x-engine-rpm' property found for engine " << RPMdefinition << "." << endl; + cerr << "# Please check order of engine definitons." << endl; + } + } else { + cerr << "# Engine number " << EngineNum; + cerr << ", given ExternalRPM value '" << RPMdefinition << "' unhandled." << endl; + } + } return true; } @@ -1038,7 +752,7 @@ bool FGRotor::bind(void) { string FGRotor::GetThrusterLabels(int id, string delimeter) { - std::ostringstream buf; + ostringstream buf; buf << Name << " RPM (engine " << id << ")"; @@ -1050,11 +764,13 @@ string FGRotor::GetThrusterLabels(int id, string delimeter) string FGRotor::GetThrusterValues(int id, string delimeter) { - std::ostringstream buf; + + ostringstream buf; buf << RPM; return buf.str(); + } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -1078,11 +794,46 @@ string FGRotor::GetThrusterValues(int id, string delimeter) void FGRotor::Debug(int from) { + string ControlMapName; + if (debug_lvl <= 0) return; if (debug_lvl & 1) { // Standard console startup message output if (from == 0) { // Constructor cout << "\n Rotor Name: " << Name << endl; + cout << " Diameter = " << 2.0 * Radius << " ft." << endl; + cout << " Number of Blades = " << BladeNum << endl; + cout << " Gear Ratio = " << GearRatio << endl; + cout << " Sense = " << Sense << endl; + cout << " Nominal RPM = " << NominalRPM << endl; + + if (ExternalRPM) { + if (RPMdefinition == -1) { + cout << " RPM is controlled externally" << endl; + } else { + cout << " RPM source set to engine " << RPMdefinition << endl; + } + } + + cout << " Blade Chord = " << BladeChord << endl; + cout << " Lift Curve Slope = " << LiftCurveSlope << endl; + cout << " Blade Twist = " << BladeTwist << endl; + cout << " Hinge Offset = " << HingeOffset << endl; + cout << " Blade Flapping Moment = " << BladeFlappingMoment << endl; + cout << " Blade Mass Moment = " << BladeMassMoment << endl; + cout << " Polar Moment = " << PolarMoment << endl; + cout << " Inflow Lag = " << InflowLag << endl; + cout << " Tip Loss = " << TipLossB << endl; + cout << " Lock Number = " << LockNumberByRho * 0.002356 << " (SL)" << endl; + cout << " Solidity = " << Solidity << endl; + + switch (ControlMap) { + case eTailCtrl: ControlMapName = "Tail Rotor"; break; + case eTandemCtrl: ControlMapName = "Tandem Rotor"; break; + default: ControlMapName = "Main Rotor"; + } + cout << " Control Mapping = " << ControlMapName << endl; + } } if (debug_lvl & 2 ) { // Instantiation/Destruction notification @@ -1101,7 +852,9 @@ void FGRotor::Debug(int from) cout << IdHdr << endl; } } + } + } // namespace JSBSim diff --git a/src/FDM/JSBSim/models/propulsion/FGRotor.h b/src/FDM/JSBSim/models/propulsion/FGRotor.h index 0663fba46..9892baa37 100644 --- a/src/FDM/JSBSim/models/propulsion/FGRotor.h +++ b/src/FDM/JSBSim/models/propulsion/FGRotor.h @@ -4,7 +4,7 @@ Author: T. Kreitler Date started: 08/24/00 - ------------- Copyright (C) 2010 T. Kreitler ------------- + ------------- Copyright (C) 2010 T. Kreitler (t.kreitler@web.de) ------------- This program 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 @@ -26,6 +26,7 @@ HISTORY -------------------------------------------------------------------------------- 01/01/10 T.Kreitler test implementation +01/10/11 T.Kreitler changed to single rotor model %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SENTRY @@ -39,14 +40,12 @@ INCLUDES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #include "FGThruster.h" -#include "math/FGTable.h" -#include "math/FGRungeKutta.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -#define ID_ROTOR "$Id: FGRotor.h,v 1.6 2010/06/02 04:05:13 jberndt Exp $" +#define ID_ROTOR "$Id: FGRotor.h,v 1.8 2011/01/17 22:09:59 jberndt Exp $" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FORWARD DECLARATIONS @@ -58,58 +57,35 @@ namespace JSBSim { CLASS DOCUMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -/** Models a rotor system. The default configuration consists of main and - tail rotor. A practical way to define the positions is to start with an - imaginary gear-box near the cg of the vehicle. - - In this case the location in the thruster definition should be - approximately equal to the cg defined in the fdm_config/mass_balance - section. If the default orientation (roll=pitch=yaw=0) is used the - positions of the rotor hubs are now defined relative to the location - of the thruster (i.e. the cg-centered body coordinate system). +/** Models a helicopter rotor. -

Configuration File Format:

+

Configuration File Format

@code - + {number} {number} - {number} - {number} + {number} {number} - {number} {number} {number} - {number} {number} - {number} - {number} - {number} - {number} - {number} {number} - - {number} - {number} - {number} - {number} - {number} - {number} - {number} - {number} - {number} - {number} - {number} - {number} - {number} - {number} - - {number} + {number} + {number} + {number} + {number} + {number} + + {MAIN|TAIL|TANDEM} + {number} + + {number} {number} // LENGTH means any of the supported units, same for ANGLE and MOMENT. -// Xunit-attributes are a hint for currently unsupported units, so +// Xunit-attributes are a hint for currently unsupported units, so // values must be provided accordingly. @endcode @@ -119,67 +95,87 @@ CLASS DOCUMENTATION Brief description and the symbol frequently found in the literature. 
-    \           - Rotor disk diameter (R).
+    \           - Rotor disk diameter (2x R).
     \          - Number of blades (b).
-    \               - Relative height in body coordinate system, thus usually negative.
-    \               - Relative distance in body coordinate system, close to zero 
-                             for main rotor, and usually negative for the tail rotor. 
+    \          - Ratio of (engine rpm) / (rotor rpm), usually > 1.
     \         - RPM at which the rotor usally operates. 
-    \             - Lowest RPM generated by the code, optional.
     \              - Blade chord, (c).
     \     - Slope of curve of section lift against section angle of attack,
                              per rad (a).
-    \     - Flapping moment of inertia (I_b).
     \              - Blade twist from root to tip, (theta_1).
-    \         - Blade mass moment. (Biege/Widerstands-moment)
+    \        - Rotor flapping-hinge offset (e).
+    \     - Flapping moment of inertia (I_b).
+    \         - Blade mass moment. Mass of a single blade times the blade's
+                             cg-distance from the hub, optional.
+    \        - Moment of inertia for the whole rotor disk, optional.
+    \          - Rotor inflow time constant, sec. Smaller values yield to
+                              quicker responses to control input (defaults to 0.2).
     \      - Tip-loss factor. The Blade fraction that produces lift.
                               Value usually ranges between 0.95 - 1.0, optional (B).
-    \        - Moment of inertia for the whole rotor disk, optional.
-    \          - Rotor inflow time constant, sec.
-    \          - Orientation of the rotor shaft, negative angles define
-                              a 'forward' tilt. Used by main rotor, optional.
-    \        - Rotor flapping-hinge offset (e).
-    
+
+    \         - Defines the control inputs used (see notes).
+    \        - Links the rotor to another rotor, or an user controllable property.
+
     Experimental properties
     
-    \          - Flapping hinge cantangle used by tail rotor, optional.
-    \      - Parameter for exponent in ground effect approximation. Value should
-                              be in the range 0.2 - 0.35, 0.0 disables, optional.
-    \  - Further adjustment of ground effect. 
+    \    - Exponent for ground effect approximation. Values usually range from 0.04
+                            for large rotors to 0.1 for smaller ones. As a rule of thumb the effect 
+                            vanishes at a height 2-3 times the rotor diameter.
+                              formula used: exp ( - groundeffectexp * (height+groundeffectshift) )
+                            Omitting or setting to 0.0 disables the effect calculation.
+    \  - Further adjustment of ground effect, approx. hub height or slightly above.

Notes:

- The behavior of the rotor is controlled/influenced by 5 inputs.
    -
  • The power provided by the engine. This is handled by the regular engine controls.
    • -
  • The collective control input. This is read from the fdm property - propulsion/engine[x]/collective-ctrl-rad.
    • -
  • The lateral cyclic input. Read from - propulsion/engine[x]/lateral-ctrl-rad.
    • -
  • The longitudinal cyclic input. Read from - propulsion/engine[x]/longitudinal-ctrl-rad.
    • -
  • The tail collective (aka antitorque, aka pedal) control input. Read from - propulsion/engine[x]/antitorque-ctrl-rad.
    • +

    - Controls -

    -
+ The behavior of the rotor is controlled/influenced by following inputs.
    +
  • The power provided by the engine. This is handled by the regular engine controls.
    • +
  • The collective control input. This is read from the fdm property + propulsion/engine[x]/collective-ctrl-rad. See below for tail rotor
    • +
  • The lateral cyclic input. Read from + propulsion/engine[x]/lateral-ctrl-rad.
    • +
  • The longitudinal cyclic input. Read from + propulsion/engine[x]/longitudinal-ctrl-rad.
    • +
  • The tail collective (aka antitorque, aka pedal) control input. Read from + propulsion/engine[x]/antitorque-ctrl-rad or + propulsion/engine[x]/tail-collective-ctrl-rad.
    • - In order to keep the rotor speed constant, use of a RPM-Governor system is encouraged. +
- It is possible to use different orientation/locations for the rotor system, but then xhub - and zhub are no longer aligned to the body frame and need proper recalculation. +

- Tail/tandem rotor -

- To model a standalone main rotor just omit the element. If you provide - a plain element all tail rotor parameters are estimated. - - The 'sense' parameter from the thruster is interpreted as follows, sense=1 means - counter clockwise rotation of the main rotor, as viewed from above. This is as a far - as I know more popular than clockwise rotation, which is defined by setting sense to - -1 (to be honest, I'm just 99.9% sure that the orientation is handled properly). - - Concerning coaxial designs: By providing the 'variant' attribute with value 'coaxial' - a Kamov-style rotor is modeled - i.e. the rotor produces no torque. + Providing \ 0 \ the tail rotor's RPM + is linked to to the main (=first, =0) rotor, and specifing + \ TAIL \ tells this rotor to read the + collective input from propulsion/engine[1]/antitorque-ctrl-rad + (The TAIL-map ignores lateral and longitudinal input). The rotor needs to be + attached to a dummy engine, e.g. an 1HP electrical engine. + A tandem rotor is setup analogous. +

- Sense -

+ + The 'sense' parameter from the thruster is interpreted as follows, sense=1 means + counter clockwise rotation of the main rotor, as viewed from above. This is as a far + as I know more popular than clockwise rotation, which is defined by setting sense to + -1. Concerning coaxial designs - by setting 'sense' to zero, a Kamov-style rotor is + modeled (i.e. the rotor produces no torque). + +

- Engine issues -

+ + Currently the rotor can only be driven with piston and electrical engines. An adaption + for the turboprop engine might become available in the future. + In order to keep the rotor speed constant, use of a RPM-Governor system is + encouraged (see examples). + +

- Development hints -

+ + Setting \ -1 \ the rotor's RPM is controlled by + the propulsion/engine[x]/x-rpm-dict property. This feature can be useful + when developing a FDM. +

References:

@@ -196,147 +192,90 @@ CLASS DOCUMENTATION @author Thomas Kreitler - @version $Id: FGRotor.h,v 1.6 2010/06/02 04:05:13 jberndt Exp $ + @version $Id: FGRotor.h,v 1.8 2011/01/17 22:09:59 jberndt Exp $ */ + + /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS DECLARATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -class FGRotor : public FGThruster { - - enum eRotorFlags {eNone=0, eMain=1, eTail=2, eCoaxial=4, eRotCW=8} ; - struct rotor { - - virtual ~rotor(); - void zero(); - void configure(int f, const rotor *xmain = NULL); - - // assist in parameter retrieval - double cnf_elem(const string& ename, double default_val=0.0, const string& unit = "", bool tell=false); - double cnf_elem(const string& ename, double default_val=0.0, bool tell=false); - - // rotor dynamics - void calc_flow_and_thrust(double dt, double rho, double theta_0, double Uw, double Ww, double flow_scale = 1.0); - - void calc_torque(double rho, double theta_0); - void calc_coning_angle(double rho, double theta_0); - void calc_flapping_angles(double rho, double theta_0, const FGColumnVector3 &pqr_fus_w); - void calc_drag_and_side_forces(double rho, double theta_0); - - // transformations - FGColumnVector3 hub_vel_body2ca( const FGColumnVector3 &uvw, const FGColumnVector3 &pqr, - double a_ic = 0.0 , double b_ic = 0.0 ); - FGColumnVector3 fus_angvel_body2ca( const FGColumnVector3 &pqr); - - FGColumnVector3 body_forces(double a_ic = 0.0 , double b_ic = 0.0 ); - FGColumnVector3 body_moments(FGColumnVector3 F_h, double a_ic = 0.0 , double b_ic = 0.0 ); - - // bookkeeping - int flags ; - Element *parent ; - - // used in flow calculation - // FGRK4 rk ; // use this after checking - FGRKFehlberg rk ; - int reports ; - - // configuration parameters - double Radius ; - int BladeNum ; - double RelDistance_xhub ; - double RelShift_yhub ; - double RelHeight_zhub ; - double NominalRPM ; - double MinRPM ; - double BladeChord ; - double LiftCurveSlope ; - double BladeFlappingMoment ; - double BladeTwist ; - double BladeMassMoment ; - double TipLossB ; - double PolarMoment ; - double InflowLag ; - double ShaftTilt ; - double HingeOffset ; - double HingeOffset_hover ; - double CantAngleD3 ; - - double theta_shaft ; - double phi_shaft ; - - // derived parameters - double LockNumberByRho ; - double solidity ; // aka sigma - double RpmRatio ; // main_to_tail, hmm - double R[5] ; // Radius powers - double B[6] ; // TipLossB powers - - FGMatrix33 BodyToShaft ; // [S]T, see /SH79/ eqn(17,18) - FGMatrix33 ShaftToBody ; // [S] - - // dynamic values - double ActualRPM ; - double Omega ; // must be > 0 - double beta_orient ; - double a0 ; // coning angle (rad) - double a_1, b_1, a_dw ; - double a1s, b1s ; // cyclic flapping relative to shaft axes, /SH79/ eqn(43) - double H_drag, J_side ; - - double Torque ; - double Thrust ; - double Ct ; - double lambda ; // inflow ratio - double mu ; // tip-speed ratio - double nu ; // induced inflow ratio - double v_induced ; // always positive [ft/s] - - // results - FGColumnVector3 force ; - FGColumnVector3 moment ; - - - // declare the problem function - class dnuFunction : public FGRungeKuttaProblem { - public: - void update_params(rotor *r, double ct_t01, double fs, double w); - private: - double pFunc(double x, double y); - // some shortcuts - double k_sat, ct_lambda, k_wor, k_theta, mu2, k_flowscale; - } flowEquation; - - - }; +class FGRotor : public FGThruster { + enum eCtrlMapping {eMainCtrl=0, eTailCtrl, eTandemCtrl}; public: - /** Constructor - @param exec pointer to executive structure - @param rotor_element pointer to XML element in the config file - @param num the number of this rotor */ + + /** Constructor for FGRotor. + @param exec a pointer to the main executive object + @param rotor_element a pointer to the thruster config file XML element + @param num the number of this rotor */ FGRotor(FGFDMExec *exec, Element* rotor_element, int num); - /// Destructor + /// Destructor for FGRotor ~FGRotor(); - void SetRPM(double rpm) {RPM = rpm;} + /** Returns the power required by the rotor. Well, to achieve this the rotor + is cycled through the whole machinery, yielding to a new state. + (hmm, sort of a huge side effect) + */ + double GetPowerRequired(void); - /** Calculates forces and moments created by the rotor(s) and updates - vFn,vMn state variables. RPM changes are handled inside, too. - The RPM change is based on estimating the torque provided by the engine. + /** Returns the scalar thrust of the rotor, and adjusts the RPM value. */ + double Calculate(double PowerAvailable); - @param PowerAvailable here this is the thrust (not power) provided by a turbine - @return PowerAvailable */ - double Calculate(double); - double GetRPM(void) const { return RPM; } - double GetDiameter(void) { return mr.Radius*2.0; } + /// Retrieves the RPMs of the rotor. + double GetRPM(void) const { return RPM; } + + // void SetRPM(double rpm) { RPM = rpm; } + + /// Retrieves the RPMs of the Engine, as seen from this rotor. + double GetEngineRPM(void) const { return GearRatio*RPM; } // bit of a hack. + /// Tells the rotor's gear ratio, usually the engine asks for this. + double GetGearRatio(void) { return GearRatio; } + /// Retrieves the thrust of the rotor. + double GetThrust(void) const { return Thrust; } - // Stubs. Right now this rotor-to-engine interface is just a hack. - double GetTorque(void) { return 0.0; /* return mr.Torque;*/ } - double GetPowerRequired(void); + /// Retrieves the rotor's coning angle + double GetA0(void) const { return a0; } + /// Retrieves the longitudinal flapping angle with respect to the rotor shaft + double GetA1(void) const { return a1s; } + /// Retrieves the lateral flapping angle with respect to the rotor shaft + double GetB1(void) const { return b1s; } + + /// Retrieves the inflow ratio + double GetLambda(void) const { return lambda; } + /// Retrieves the tip-speed (aka advance) ratio + double GetMu(void) const { return mu; } + /// Retrieves the induced inflow ratio + double GetNu(void) const { return nu; } + /// Retrieves the induced velocity + double GetVi(void) const { return v_induced; } + /// Retrieves the thrust coefficient + double GetCT(void) const { return C_T; } + /// Retrieves the torque + double GetTorque(void) const { return Torque; } + + /// Downwash angle - currently only valid for a rotor that spins horizontally + double GetThetaDW(void) const { return theta_downwash; } + /// Downwash angle - currently only valid for a rotor that spins horizontally + double GetPhiDW(void) const { return phi_downwash; } + + /// Retrieves the collective control input in radians. + double GetCollectiveCtrl(void) const { return CollectiveCtrl; } + /// Retrieves the lateral control input in radians. + double GetLateralCtrl(void) const { return LateralCtrl; } + /// Retrieves the longitudinal control input in radians. + double GetLongitudinalCtrl(void) const { return LongitudinalCtrl; } + + /// Sets the collective control input in radians. + void SetCollectiveCtrl(double c) { CollectiveCtrl = c; } + /// Sets the lateral control input in radians. + void SetLateralCtrl(double c) { LateralCtrl = c; } + /// Sets the longitudinal control input in radians. + void SetLongitudinalCtrl(double c) { LongitudinalCtrl = c; } // Stubs. Only main rotor RPM is returned string GetThrusterLabels(int id, string delimeter); @@ -344,59 +283,105 @@ public: private: - bool bind(void); + // assist in parameter retrieval + double ConfigValueConv( Element* e, const string& ename, double default_val=0.0, + const string& unit = "", bool tell=false); - double RPM; - double Sense; // default is counter clockwise rotation of the main rotor (viewed from above) - bool tailRotorPresent; + double ConfigValue( Element* e, const string& ename, double default_val=0.0, + bool tell=false); + void Configure(Element* rotor_element); + + // true entry points + void CalcStatePart1(void); + void CalcStatePart2(double PowerAvailable); + + // rotor dynamics + void calc_flow_and_thrust(double theta_0, double Uw, double Ww, double flow_scale = 1.0); + void calc_coning_angle(double theta_0); + void calc_flapping_angles(double theta_0, const FGColumnVector3 &pqr_fus_w); + void calc_drag_and_side_forces(double theta_0); + void calc_torque(double theta_0); + + // transformations + FGColumnVector3 hub_vel_body2ca( const FGColumnVector3 &uvw, const FGColumnVector3 &pqr, + double a_ic = 0.0 , double b_ic = 0.0 ); + FGColumnVector3 fus_angvel_body2ca( const FGColumnVector3 &pqr); + FGColumnVector3 body_forces(double a_ic = 0.0 , double b_ic = 0.0 ); + FGColumnVector3 body_moments(double a_ic = 0.0 , double b_ic = 0.0 ); + + // interface + bool BindModel(void); void Debug(int from); - FGPropertyManager* PropertyManager; - - rotor mr; - rotor tr; - + // environment + double dt; + double rho; Filter damp_hagl; - double rho; - - double effective_tail_col; // /SH79/ eqn(47) + // configuration parameters + double Radius; + int BladeNum; - double ground_effect_exp; - double ground_effect_shift; + double Sense; + double NominalRPM; + int ExternalRPM; + int RPMdefinition; + FGPropertyManager* ExtRPMsource; - double hover_threshold; - double hover_scale; + double BladeChord; + double LiftCurveSlope; + double BladeTwist; + double HingeOffset; + double BladeFlappingMoment; + double BladeMassMoment; + double PolarMoment; + double InflowLag; + double TipLossB; - // fdm imported controls - FGPropertyManager* prop_collective_ctrl; - FGPropertyManager* prop_lateral_ctrl; - FGPropertyManager* prop_longitudinal_ctrl; - FGPropertyManager* prop_antitorque_ctrl; + double GroundEffectExp; + double GroundEffectShift; - FGPropertyManager* prop_freewheel_factor; - FGPropertyManager* prop_rotorbrake; + // derived parameters + double LockNumberByRho; + double Solidity; // aka sigma + double R[5]; // Radius powers + double B[5]; // TipLossB powers - // fdm export ... - double prop_inflow_ratio_lambda; - double prop_advance_ratio_mu; - double prop_inflow_ratio_induced_nu; - double prop_mr_torque; - double prop_coning_angle; + // Some of the calculations require shaft axes. So the + // thruster orientation (Tbo, with b for body) needs to be + // expressed/represented in helicopter shaft coordinates (Hsr). + FGMatrix33 InvTransform; + FGMatrix33 TboToHsr; + FGMatrix33 HsrToTbo; - double prop_theta_downwash; - double prop_phi_downwash; + // dynamic values + double RPM; + double Omega; // must be > 0 + double beta_orient; // rotor orientation angle (rad) + double a0; // coning angle (rad) + double a_1, b_1, a_dw; // flapping angles + double a1s, b1s; // cyclic flapping relative to shaft axes, /SH79/ eqn(43) + double H_drag, J_side; // Forces - double prop_thrust_coefficient; - double prop_lift_coefficient; + double Torque; + double C_T; // rotor thrust coefficient + double lambda; // inflow ratio + double mu; // tip-speed ratio + double nu; // induced inflow ratio + double v_induced; // induced velocity, always positive [ft/s] - double dt; // deltaT doesn't do the thing + double theta_downwash; + double phi_downwash; - // devel/debug stuff - int prop_DumpFlag; // causes 1-time dump on stdout + // control + eCtrlMapping ControlMap; + double CollectiveCtrl; + double LateralCtrl; + double LongitudinalCtrl; }; + } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% #endif From 5268b3c6ede60689b351475fe8cbc0dfd41053b4 Mon Sep 17 00:00:00 2001 From: ThorstenB Date: Sun, 23 Jan 2011 15:24:28 +0100 Subject: [PATCH 10/13] Report stopped MP transmission at higher log level, it's too important to be a "debug only" message. --- src/MultiPlayer/multiplaymgr.cxx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/MultiPlayer/multiplaymgr.cxx b/src/MultiPlayer/multiplaymgr.cxx index 89d2eb85a..7657ad4ee 100644 --- a/src/MultiPlayer/multiplaymgr.cxx +++ b/src/MultiPlayer/multiplaymgr.cxx @@ -561,7 +561,7 @@ FGMultiplayMgr::SendMyPosition(const FGExternalMotionData& motionInfo) { // Current local data is invalid (NaN), so stop MP transmission. // => Be nice to older FG versions (no NaN checks) and don't waste bandwidth. - SG_LOG(SG_NETWORK, SG_DEBUG, "FGMultiplayMgr::SendMyPosition - " + SG_LOG(SG_NETWORK, SG_ALERT, "FGMultiplayMgr::SendMyPosition - " << "Local data is invalid (NaN). Data not transmitted."); return; } From cb82e136ddd853abeaef0c86965ad3d8751f7d7c Mon Sep 17 00:00:00 2001 From: Mathias Froehlich Date: Sun, 23 Jan 2011 20:34:14 +0100 Subject: [PATCH 11/13] Help configure finding the rti libraries. --- configure.ac | 3 +++ 1 file changed, 3 insertions(+) diff --git a/configure.ac b/configure.ac index d7f7a65a6..cd41238ff 100644 --- a/configure.ac +++ b/configure.ac @@ -271,9 +271,11 @@ if test "x$with_rti13" != "xno"; then if test "x$simgear_hla" = "xyes" ; then AC_MSG_CHECKING([for hla libraries]) saved_LIBS="$LIBS" + saved_LDFLAGS="$LDFLAGS" for rti13libs in "-lRTI-NG -lFedTime" "-lRTI-NGd -lFedTimed" ; do if test "x$hla_libs" = "x" ; then LIBS="-lsghla13 -lsghla -lsgxml -lsgstructure -lsgmath -lsgdebug -lsgtiming $rti13libs -lrt $saved_LIBS" + LDFLAGS="$HLA_LDFLAGS $saved_LDFLAGS" AC_TRY_LINK([ #include ],[ @@ -282,6 +284,7 @@ if test "x$with_rti13" != "xno"; then fi done LIBS="$saved_LIBS" + LDFLAGS="$saved_LDFLAGS" AC_MSG_RESULT($hla_libs) fi AC_LANG_POP() From c21ba6cb200e9f22953e903adba2522aaa1ee497 Mon Sep 17 00:00:00 2001 From: Frederic Bouvier Date: Sun, 23 Jan 2011 21:07:24 +0100 Subject: [PATCH 12/13] Cmake: restore its original name to the ephemeris library --- CMakeModules/FindSimGear.cmake | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/CMakeModules/FindSimGear.cmake b/CMakeModules/FindSimGear.cmake index 8a494dde7..88fb177a2 100644 --- a/CMakeModules/FindSimGear.cmake +++ b/CMakeModules/FindSimGear.cmake @@ -111,16 +111,17 @@ if(${SIMGEAR_LIBRARIES} STREQUAL "SIMGEAR_LIBRARIES-NOTFOUND") # on MSVC builds set(thread_lib threads) endif(NOT MSVC) - + # note the order here affects the order Simgear libraries are # linked in, and hence ability to link when using a traditional # linker such as GNU ld on Linux set(comps - ephemeris + ephem environment nasal sky - material tgdb + material + tgdb model screen bucket From b085649f20af7de0383c3af7ca3ccdb4af10d131 Mon Sep 17 00:00:00 2001 From: Torsten Dreyer Date: Sun, 23 Jan 2011 21:51:07 +0100 Subject: [PATCH 13/13] Code cleanup, no functional change --- src/Environment/realwx_ctrl.cxx | 24 ++++++++++++------------ src/Environment/tiedpropertylist.hxx | 2 +- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/Environment/realwx_ctrl.cxx b/src/Environment/realwx_ctrl.cxx index a547ef7e5..98fe6ed3b 100644 --- a/src/Environment/realwx_ctrl.cxx +++ b/src/Environment/realwx_ctrl.cxx @@ -194,18 +194,18 @@ public: class MetarLoadRequest { public: - MetarLoadRequest( const string & stationId ) { - _stationId = stationId; - _proxyHost = fgGetNode("/sim/presets/proxy/host", true)->getStringValue(); - _proxyPort = fgGetNode("/sim/presets/proxy/port", true)->getStringValue(); - _proxyAuth = fgGetNode("/sim/presets/proxy/authentication", true)->getStringValue(); - } - MetarLoadRequest( const MetarLoadRequest & other ) { - _stationId = other._stationId; - _proxyHost = other._proxyAuth; - _proxyPort = other._proxyPort; - _proxyAuth = other._proxyAuth; - } + MetarLoadRequest( const string & stationId ) : + _stationId(stationId), + _proxyHost(fgGetNode("/sim/presets/proxy/host", true)->getStringValue()), + _proxyPort(fgGetNode("/sim/presets/proxy/port", true)->getStringValue()), + _proxyAuth(fgGetNode("/sim/presets/proxy/authentication", true)->getStringValue()) + {} + MetarLoadRequest( const MetarLoadRequest & other ) : + _stationId(other._stationId), + _proxyHost(other._proxyAuth), + _proxyPort(other._proxyPort), + _proxyAuth(other._proxyAuth) + {} string _stationId; string _proxyHost; string _proxyPort; diff --git a/src/Environment/tiedpropertylist.hxx b/src/Environment/tiedpropertylist.hxx index dbb9cc45a..cfdd57c97 100644 --- a/src/Environment/tiedpropertylist.hxx +++ b/src/Environment/tiedpropertylist.hxx @@ -7,7 +7,7 @@ using simgear::PropertyList; class TiedPropertyList : PropertyList { public: TiedPropertyList() {} - TiedPropertyList( SGPropertyNode_ptr root ) { _root = root; } + TiedPropertyList( SGPropertyNode_ptr root ) : _root(root) {} void setRoot( SGPropertyNode_ptr root ) { _root = root; } SGPropertyNode_ptr getRoot() const { return _root; }