1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 3 Header: FGPropagate.h 4 Author: Jon S. Berndt 5 Date started: 1/5/99 6 7 ------------- Copyright (C) 1999 Jon S. Berndt (jon@jsbsim.org) ------------- 8 9 This program is free software; you can redistribute it and/or modify it under 10 the terms of the GNU Lesser General Public License as published by the Free 11 Software Foundation; either version 2 of the License, or (at your option) any 12 later version. 13 14 This program is distributed in the hope that it will be useful, but WITHOUT 15 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 16 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more 17 details. 18 19 You should have received a copy of the GNU Lesser General Public License along 20 with this program; if not, write to the Free Software Foundation, Inc., 59 21 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 22 23 Further information about the GNU Lesser General Public License can also be 24 found on the world wide web at http://www.gnu.org. 25 26 HISTORY 27 -------------------------------------------------------------------------------- 28 01/05/99 JSB Created 29 30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 31 SENTRY 32 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ 33 34 #ifndef FGPROPAGATE_H 35 #define FGPROPAGATE_H 36 37 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 38 INCLUDES 39 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ 40 41 #include "models/FGModel.h" 42 #include "math/FGLocation.h" 43 #include "math/FGQuaternion.h" 44 45 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 46 FORWARD DECLARATIONS 47 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ 48 49 namespace JSBSim { 50 51 class FGInitialCondition; 52 53 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 54 CLASS DOCUMENTATION 55 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ 56 57 /** Models the EOM and integration/propagation of state. 58 The Equations of Motion (EOM) for JSBSim are integrated to propagate the 59 state of the vehicle given the forces and moments that act on it. The 60 integration accounts for a rotating Earth. 61 62 Integration of rotational and translation position and rate can be 63 customized as needed or frozen by the selection of no integrator. The 64 selection of which integrator to use is done through the setting of 65 the associated property. There are four properties which can be set: 66 67 @code 68 simulation/integrator/rate/rotational 69 simulation/integrator/rate/translational 70 simulation/integrator/position/rotational 71 simulation/integrator/position/translational 72 @endcode 73 74 Each of the integrators listed above can be set to one of the following values: 75 76 @code 77 0: No integrator (Freeze) 78 1: Rectangular Euler 79 2: Trapezoidal 80 3: Adams Bashforth 2 81 4: Adams Bashforth 3 82 5: Adams Bashforth 4 83 @endcode 84 85 @author Jon S. Berndt, Mathias Froehlich, Bertrand Coconnier 86 */ 87 88 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 89 CLASS DECLARATION 90 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ 91 92 class FGPropagate : public FGModel { 93 public: 94 95 /** The current vehicle state vector structure contains the translational and 96 angular position, and the translational and angular velocity. */ 97 struct VehicleState { 98 /** Represents the current location of the vehicle in Earth centered Earth 99 fixed (ECEF) frame. 100 units ft */ 101 FGLocation vLocation; 102 103 /** The velocity vector of the vehicle with respect to the ECEF frame, 104 expressed in the body system. 105 units ft/sec */ 106 FGColumnVector3 vUVW; 107 108 /** The angular velocity vector for the vehicle relative to the ECEF frame, 109 expressed in the body frame. 110 units rad/sec */ 111 FGColumnVector3 vPQR; 112 113 /** The angular velocity vector for the vehicle body frame relative to the 114 ECI frame, expressed in the body frame. 115 units rad/sec */ 116 FGColumnVector3 vPQRi; 117 118 /** The current orientation of the vehicle, that is, the orientation of the 119 body frame relative to the local, NED frame. */ 120 FGQuaternion qAttitudeLocal; 121 122 /** The current orientation of the vehicle, that is, the orientation of the 123 body frame relative to the inertial (ECI) frame. */ 124 FGQuaternion qAttitudeECI; 125 126 FGQuaternion vQtrndot; 127 128 FGColumnVector3 vInertialVelocity; 129 130 FGColumnVector3 vInertialPosition; 131 132 std::deque <FGColumnVector3> dqPQRidot; 133 std::deque <FGColumnVector3> dqUVWidot; 134 std::deque <FGColumnVector3> dqInertialVelocity; 135 std::deque <FGQuaternion> dqQtrndot; 136 }; 137 138 /** Constructor. 139 The constructor initializes several variables, and sets the initial set 140 of integrators to use as follows: 141 - integrator, rotational rate = Adams Bashforth 2 142 - integrator, translational rate = Adams Bashforth 2 143 - integrator, rotational position = Trapezoidal 144 - integrator, translational position = Trapezoidal 145 @param Executive a pointer to the parent executive object */ 146 FGPropagate(FGFDMExec* Executive); 147 148 /// Destructor 149 ~FGPropagate(); 150 151 /// These define the indices use to select the various integrators. 152 enum eIntegrateType {eNone = 0, eRectEuler, eTrapezoidal, eAdamsBashforth2, 153 eAdamsBashforth3, eAdamsBashforth4, eBuss1, eBuss2, eLocalLinearization, eAdamsBashforth5}; 154 155 /** Initializes the FGPropagate class after instantiation and prior to first execution. 156 The base class FGModel::InitModel is called first, initializing pointers to the 157 other FGModel objects (and others). */ 158 bool InitModel(void); 159 160 void InitializeDerivatives(); 161 162 /** Runs the state propagation model; called by the Executive 163 Can pass in a value indicating if the executive is directing the simulation to Hold. 164 @param Holding if true, the executive has been directed to hold the sim from 165 advancing time. Some models may ignore this flag, such as the Input 166 model, which may need to be active to listen on a socket for the 167 "Resume" command to be given. 168 @return false if no error */ 169 bool Run(bool Holding); 170 171 /** Retrieves the velocity vector. 172 The vector returned is represented by an FGColumnVector reference. The vector 173 for the velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector 174 is 1-based, so that the first element can be retrieved using the "()" operator. 175 In other words, vVel(1) is Vnorth. Various convenience enumerators are defined 176 in FGJSBBase. The relevant enumerators for the vector returned by this call are, 177 eNorth=1, eEast=2, eDown=3. 178 units ft/sec 179 @return The vehicle velocity vector with respect to the Earth centered frame, 180 expressed in Local horizontal frame. 181 */ GetVel(void)182 const FGColumnVector3& GetVel(void) const { return vVel; } 183 184 /** Retrieves the body frame vehicle velocity vector. 185 The vector returned is represented by an FGColumnVector reference. The vector 186 for the velocity in Body frame is organized (Vx, Vy, Vz). The vector 187 is 1-based, so that the first element can be retrieved using the "()" operator. 188 In other words, vUVW(1) is Vx. Various convenience enumerators are defined 189 in FGJSBBase. The relevant enumerators for the vector returned by this call are, 190 eX=1, eY=2, eZ=3. 191 units ft/sec 192 @return The body frame vehicle velocity vector in ft/sec. 193 */ GetUVW(void)194 const FGColumnVector3& GetUVW(void) const { return VState.vUVW; } 195 196 /** Retrieves the body angular rates vector, relative to the ECEF frame. 197 Retrieves the body angular rates (p, q, r), which are calculated by integration 198 of the angular acceleration. 199 The vector returned is represented by an FGColumnVector reference. The vector 200 for the angular velocity in Body frame is organized (P, Q, R). The vector 201 is 1-based, so that the first element can be retrieved using the "()" operator. 202 In other words, vPQR(1) is P. Various convenience enumerators are defined 203 in FGJSBBase. The relevant enumerators for the vector returned by this call are, 204 eP=1, eQ=2, eR=3. 205 units rad/sec 206 @return The body frame angular rates in rad/sec. 207 */ GetPQR(void)208 const FGColumnVector3& GetPQR(void) const {return VState.vPQR;} 209 210 /** Retrieves the body angular rates vector, relative to the ECI (inertial) frame. 211 Retrieves the body angular rates (p, q, r), which are calculated by integration 212 of the angular acceleration. 213 The vector returned is represented by an FGColumnVector reference. The vector 214 for the angular velocity in Body frame is organized (P, Q, R). The vector 215 is 1-based, so that the first element can be retrieved using the "()" operator. 216 In other words, vPQR(1) is P. Various convenience enumerators are defined 217 in FGJSBBase. The relevant enumerators for the vector returned by this call are, 218 eP=1, eQ=2, eR=3. 219 units rad/sec 220 @return The body frame inertial angular rates in rad/sec. 221 */ GetPQRi(void)222 const FGColumnVector3& GetPQRi(void) const {return VState.vPQRi;} 223 224 /** Retrieves the time derivative of the body orientation quaternion. 225 Retrieves the time derivative of the body orientation quaternion based on 226 the rate of change of the orientation between the body and the ECI frame. 227 The quaternion returned is represented by an FGQuaternion reference. The 228 quaternion is 1-based, so that the first element can be retrieved using 229 the "()" operator. 230 units rad/sec^2 231 @return The time derivative of the body orientation quaternion. 232 */ GetQuaterniondot(void)233 const FGQuaternion& GetQuaterniondot(void) const {return VState.vQtrndot;} 234 235 /** Retrieves the Euler angles that define the vehicle orientation. 236 Extracts the Euler angles from the quaternion that stores the orientation 237 in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The 238 vector returned is represented by an FGColumnVector reference. The vector 239 for the Euler angles is organized (Phi, Theta, Psi). The vector 240 is 1-based, so that the first element can be retrieved using the "()" operator. 241 In other words, the returned vector item with subscript (1) is Phi. 242 Various convenience enumerators are defined in FGJSBBase. The relevant 243 enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3. 244 units radians 245 @return The Euler angle vector, where the first item in the 246 vector is the angle about the X axis, the second is the 247 angle about the Y axis, and the third item is the angle 248 about the Z axis (Phi, Theta, Psi). 249 */ GetEuler(void)250 const FGColumnVector3& GetEuler(void) const { return VState.qAttitudeLocal.GetEuler(); } 251 252 /** Retrieves the Euler angles (in degrees) that define the vehicle orientation. 253 Extracts the Euler angles from the quaternion that stores the orientation 254 in the Local frame. The order of rotation used is Yaw-Pitch-Roll. The 255 vector returned is represented by an FGColumnVector reference. The vector 256 for the Euler angles is organized (Phi, Theta, Psi). The vector 257 is 1-based, so that the first element can be retrieved using the "()" operator. 258 In other words, the returned vector item with subscript (1) is Phi. 259 Various convenience enumerators are defined in FGJSBBase. The relevant 260 enumerators for the vector returned by this call are, ePhi=1, eTht=2, ePsi=3. 261 units degrees 262 @return The Euler angle vector, where the first item in the 263 vector is the angle about the X axis, the second is the 264 angle about the Y axis, and the third item is the angle 265 about the Z axis (Phi, Theta, Psi). 266 */ 267 FGColumnVector3 GetEulerDeg(void) const; 268 269 /** Retrieves a body frame velocity component. 270 Retrieves a body frame velocity component. The velocity returned is 271 extracted from the vUVW vector (an FGColumnVector). The vector for the 272 velocity in Body frame is organized (Vx, Vy, Vz). The vector is 1-based. 273 In other words, GetUVW(1) returns Vx. Various convenience enumerators 274 are defined in FGJSBBase. The relevant enumerators for the velocity 275 returned by this call are, eX=1, eY=2, eZ=3. 276 units ft/sec 277 @param idx the index of the velocity component desired (1-based). 278 @return The body frame velocity component. 279 */ GetUVW(int idx)280 double GetUVW(int idx) const { return VState.vUVW(idx); } 281 282 /** Retrieves a Local frame velocity component. 283 Retrieves a Local frame velocity component. The velocity returned is 284 extracted from the vVel vector (an FGColumnVector). The vector for the 285 velocity in Local frame is organized (Vnorth, Veast, Vdown). The vector 286 is 1-based. In other words, GetVel(1) returns Vnorth. Various convenience 287 enumerators are defined in FGJSBBase. The relevant enumerators for the 288 velocity returned by this call are, eNorth=1, eEast=2, eDown=3. 289 units ft/sec 290 @param idx the index of the velocity component desired (1-based). 291 @return The body frame velocity component. 292 */ GetVel(int idx)293 double GetVel(int idx) const { return vVel(idx); } 294 295 /** Retrieves the total inertial velocity in ft/sec. 296 */ GetInertialVelocityMagnitude(void)297 double GetInertialVelocityMagnitude(void) const { return VState.vInertialVelocity.Magnitude(); } 298 299 /** Retrieves the total local NED velocity in ft/sec. 300 */ GetNEDVelocityMagnitude(void)301 double GetNEDVelocityMagnitude(void) const { return VState.vUVW.Magnitude(); } 302 303 /** Retrieves the inertial velocity vector in ft/sec. 304 */ GetInertialVelocity(void)305 const FGColumnVector3& GetInertialVelocity(void) const { return VState.vInertialVelocity; } GetInertialVelocity(int i)306 double GetInertialVelocity(int i) const { return VState.vInertialVelocity(i); } 307 308 /** Retrieves the inertial position vector. 309 */ GetInertialPosition(void)310 const FGColumnVector3& GetInertialPosition(void) const { return VState.vInertialPosition; } GetInertialPosition(int i)311 double GetInertialPosition(int i) const { return VState.vInertialPosition(i); } 312 313 /** Calculates and retrieves the velocity vector relative to the earth centered earth fixed (ECEF) frame. 314 */ GetECEFVelocity(void)315 FGColumnVector3 GetECEFVelocity(void) const {return Tb2ec * VState.vUVW; } 316 317 /** Calculates and retrieves the velocity vector relative to the earth centered earth fixed (ECEF) frame 318 for a particular axis. 319 */ GetECEFVelocity(int idx)320 double GetECEFVelocity(int idx) const {return (Tb2ec * VState.vUVW)(idx); } 321 322 /** Returns the current altitude above sea level. 323 This function returns the altitude above sea level. 324 units ft 325 @return The current altitude above sea level in feet. 326 */ GetAltitudeASL(void)327 double GetAltitudeASL(void) const { return VState.vLocation.GetAltitudeASL(); } 328 329 /** Returns the current altitude above sea level. 330 This function returns the altitude above sea level. 331 units meters 332 @return The current altitude above sea level in meters. 333 */ GetAltitudeASLmeters(void)334 double GetAltitudeASLmeters(void) const { return GetAltitudeASL()*fttom;} 335 336 /** Retrieves a body frame angular velocity component relative to the ECEF frame. 337 Retrieves a body frame angular velocity component. The angular velocity 338 returned is extracted from the vPQR vector (an FGColumnVector). The vector 339 for the angular velocity in Body frame is organized (P, Q, R). The vector 340 is 1-based. In other words, GetPQR(1) returns P (roll rate). Various 341 convenience enumerators are defined in FGJSBBase. The relevant enumerators 342 for the angular velocity returned by this call are, eP=1, eQ=2, eR=3. 343 units rad/sec 344 @param axis the index of the angular velocity component desired (1-based). 345 @return The body frame angular velocity component. 346 */ GetPQR(int axis)347 double GetPQR(int axis) const {return VState.vPQR(axis);} 348 349 /** Retrieves a body frame angular velocity component relative to the ECI (inertial) frame. 350 Retrieves a body frame angular velocity component. The angular velocity 351 returned is extracted from the vPQR vector (an FGColumnVector). The vector 352 for the angular velocity in Body frame is organized (P, Q, R). The vector 353 is 1-based. In other words, GetPQR(1) returns P (roll rate). Various 354 convenience enumerators are defined in FGJSBBase. The relevant enumerators 355 for the angular velocity returned by this call are, eP=1, eQ=2, eR=3. 356 units rad/sec 357 @param axis the index of the angular velocity component desired (1-based). 358 @return The body frame angular velocity component. 359 */ GetPQRi(int axis)360 double GetPQRi(int axis) const {return VState.vPQRi(axis);} 361 362 /** Retrieves a vehicle Euler angle component. 363 Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that 364 stores the vehicle orientation relative to the Local frame. The order of 365 rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is 366 Phi. Various convenience enumerators are defined in FGJSBBase. The 367 relevant enumerators for the Euler angle returned by this call are, 368 ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta). 369 units radians 370 @return An Euler angle. 371 */ GetEuler(int axis)372 double GetEuler(int axis) const { return VState.qAttitudeLocal.GetEuler(axis); } 373 374 /** Retrieves a vehicle Euler angle component in degrees. 375 Retrieves an Euler angle (Phi, Theta, or Psi) from the quaternion that 376 stores the vehicle orientation relative to the Local frame. The order of 377 rotations used is Yaw-Pitch-Roll. The Euler angle with subscript (1) is 378 Phi. Various convenience enumerators are defined in FGJSBBase. The 379 relevant enumerators for the Euler angle returned by this call are, 380 ePhi=1, eTht=2, ePsi=3 (e.g. GetEuler(eTht) returns Theta). 381 units degrees 382 @return An Euler angle in degrees. 383 */ GetEulerDeg(int axis)384 double GetEulerDeg(int axis) const { return VState.qAttitudeLocal.GetEuler(axis) * radtodeg; } 385 386 /** Retrieves the cosine of a vehicle Euler angle component. 387 Retrieves the cosine of an Euler angle (Phi, Theta, or Psi) from the 388 quaternion that stores the vehicle orientation relative to the Local frame. 389 The order of rotations used is Yaw-Pitch-Roll. The Euler angle 390 with subscript (1) is Phi. Various convenience enumerators are defined in 391 FGJSBBase. The relevant enumerators for the Euler angle referred to in this 392 call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetCosEuler(eTht) returns cos(theta)). 393 units none 394 @return The cosine of an Euler angle. 395 */ GetCosEuler(int idx)396 double GetCosEuler(int idx) const { return VState.qAttitudeLocal.GetCosEuler(idx); } 397 398 /** Retrieves the sine of a vehicle Euler angle component. 399 Retrieves the sine of an Euler angle (Phi, Theta, or Psi) from the 400 quaternion that stores the vehicle orientation relative to the Local frame. 401 The order of rotations used is Yaw-Pitch-Roll. The Euler angle 402 with subscript (1) is Phi. Various convenience enumerators are defined in 403 FGJSBBase. The relevant enumerators for the Euler angle referred to in this 404 call are, ePhi=1, eTht=2, ePsi=3 (e.g. GetSinEuler(eTht) returns sin(theta)). 405 units none 406 @return The sine of an Euler angle. 407 */ GetSinEuler(int idx)408 double GetSinEuler(int idx) const { return VState.qAttitudeLocal.GetSinEuler(idx); } 409 410 /** Returns the current altitude rate. 411 Returns the current altitude rate (rate of climb). 412 units ft/sec 413 @return The current rate of change in altitude. 414 */ Gethdot(void)415 double Gethdot(void) const { return -vVel(eDown); } 416 417 /** Returns the "constant" LocalTerrainRadius. 418 The LocalTerrainRadius parameter is set by the calling application or set to 419 sea level + terrain elevation if JSBSim is running in standalone mode. 420 units feet 421 @return distance of the local terrain from the center of the earth. 422 */ 423 double GetLocalTerrainRadius(void) const; 424 425 /** Returns the Earth position angle. 426 @return Earth position angle in radians. 427 */ GetEarthPositionAngle(void)428 double GetEarthPositionAngle(void) const { return epa; } 429 430 /** Returns the Earth position angle in degrees. 431 @return Earth position angle in degrees. 432 */ GetEarthPositionAngleDeg(void)433 double GetEarthPositionAngleDeg(void) const { return epa*radtodeg;} 434 GetTerrainVelocity(void)435 const FGColumnVector3& GetTerrainVelocity(void) const { return LocalTerrainVelocity; } GetTerrainAngularVelocity(void)436 const FGColumnVector3& GetTerrainAngularVelocity(void) const { return LocalTerrainAngularVelocity; } 437 void RecomputeLocalTerrainVelocity(); 438 GetTerrainElevation(void)439 double GetTerrainElevation(void) const { return GetLocalTerrainRadius() - VState.vLocation.GetSeaLevelRadius(); } 440 double GetDistanceAGL(void) const; 441 double GetDistanceAGLKm(void) const; GetRadius(void)442 double GetRadius(void) const { 443 if (VState.vLocation.GetRadius() == 0) return 1.0; 444 else return VState.vLocation.GetRadius(); 445 } GetLongitude(void)446 double GetLongitude(void) const { return VState.vLocation.GetLongitude(); } GetLatitude(void)447 double GetLatitude(void) const { return VState.vLocation.GetLatitude(); } 448 GetGeodLatitudeRad(void)449 double GetGeodLatitudeRad(void) const { return VState.vLocation.GetGeodLatitudeRad(); } GetGeodLatitudeDeg(void)450 double GetGeodLatitudeDeg(void) const { return VState.vLocation.GetGeodLatitudeDeg(); } 451 GetGeodeticAltitude(void)452 double GetGeodeticAltitude(void) const { return VState.vLocation.GetGeodAltitude(); } GetGeodeticAltitudeKm(void)453 double GetGeodeticAltitudeKm(void) const { return VState.vLocation.GetGeodAltitude()*0.0003048; } 454 GetLongitudeDeg(void)455 double GetLongitudeDeg(void) const { return VState.vLocation.GetLongitudeDeg(); } GetLatitudeDeg(void)456 double GetLatitudeDeg(void) const { return VState.vLocation.GetLatitudeDeg(); } GetLocation(void)457 const FGLocation& GetLocation(void) const { return VState.vLocation; } GetLocation(int i)458 double GetLocation(int i) const { return VState.vLocation(i); } 459 460 /** Retrieves the local-to-body transformation matrix. 461 The quaternion class, being the means by which the orientation of the 462 vehicle is stored, manages the local-to-body transformation matrix. 463 @return a reference to the local-to-body transformation matrix. */ GetTl2b(void)464 const FGMatrix33& GetTl2b(void) const { return Tl2b; } 465 466 /** Retrieves the body-to-local transformation matrix. 467 The quaternion class, being the means by which the orientation of the 468 vehicle is stored, manages the body-to-local transformation matrix. 469 @return a reference to the body-to-local matrix. */ GetTb2l(void)470 const FGMatrix33& GetTb2l(void) const { return Tb2l; } 471 472 /** Retrieves the ECEF-to-body transformation matrix. 473 @return a reference to the ECEF-to-body transformation matrix. */ GetTec2b(void)474 const FGMatrix33& GetTec2b(void) const { return Tec2b; } 475 476 /** Retrieves the body-to-ECEF transformation matrix. 477 @return a reference to the body-to-ECEF matrix. */ GetTb2ec(void)478 const FGMatrix33& GetTb2ec(void) const { return Tb2ec; } 479 480 /** Retrieves the ECI-to-body transformation matrix. 481 @return a reference to the ECI-to-body transformation matrix. */ GetTi2b(void)482 const FGMatrix33& GetTi2b(void) const { return Ti2b; } 483 484 /** Retrieves the body-to-ECI transformation matrix. 485 @return a reference to the body-to-ECI matrix. */ GetTb2i(void)486 const FGMatrix33& GetTb2i(void) const { return Tb2i; } 487 488 /** Retrieves the ECEF-to-ECI transformation matrix. 489 @return a reference to the ECEF-to-ECI transformation matrix. 490 @see SetEarthPositionAngle */ GetTec2i(void)491 const FGMatrix33& GetTec2i(void) const { return Tec2i; } 492 493 /** Retrieves the ECI-to-ECEF transformation matrix. 494 @return a reference to the ECI-to-ECEF matrix. 495 @see SetEarthPositionAngle */ GetTi2ec(void)496 const FGMatrix33& GetTi2ec(void) const { return Ti2ec; } 497 498 /** Retrieves the ECEF-to-local transformation matrix. 499 Retrieves the ECEF-to-local transformation matrix. Note that the so-called 500 local from is also know as the NED frame (for North, East, Down). 501 @return a reference to the ECEF-to-local matrix. */ GetTec2l(void)502 const FGMatrix33& GetTec2l(void) const { return Tec2l; } 503 504 /** Retrieves the local-to-ECEF transformation matrix. 505 Retrieves the local-to-ECEF transformation matrix. Note that the so-called 506 local from is also know as the NED frame (for North, East, Down). 507 @return a reference to the local-to-ECEF matrix. */ GetTl2ec(void)508 const FGMatrix33& GetTl2ec(void) const { return Tl2ec; } 509 510 /** Retrieves the local-to-inertial transformation matrix. 511 @return a reference to the local-to-inertial transformation matrix. 512 @see SetEarthPositionAngle */ GetTl2i(void)513 const FGMatrix33& GetTl2i(void) const { return Tl2i; } 514 515 /** Retrieves the inertial-to-local transformation matrix. 516 @return a reference to the inertial-to-local matrix. 517 @see SetEarthPositionAngle */ GetTi2l(void)518 const FGMatrix33& GetTi2l(void) const { return Ti2l; } 519 GetVState(void)520 const VehicleState& GetVState(void) const { return VState; } 521 522 void SetVState(const VehicleState& vstate); 523 524 /** Sets the Earth position angle. 525 This is the relative angle around the Z axis of the ECEF frame with 526 respect to the inertial frame. 527 @param EPA Earth position angle in radians. 528 */ SetEarthPositionAngle(double EPA)529 void SetEarthPositionAngle(double EPA) {epa = EPA;} 530 531 void SetInertialOrientation(const FGQuaternion& Qi); 532 void SetInertialVelocity(const FGColumnVector3& Vi); 533 void SetInertialRates(const FGColumnVector3& vRates); 534 535 /** Returns the quaternion that goes from Local to Body. */ GetQuaternion(void)536 const FGQuaternion GetQuaternion(void) const { return VState.qAttitudeLocal; } 537 538 /** Returns the quaternion that goes from ECI to Body. */ GetQuaternionECI(void)539 const FGQuaternion GetQuaternionECI(void) const { return VState.qAttitudeECI; } 540 541 /** Returns the quaternion that goes from ECEF to Body. */ GetQuaternionECEF(void)542 const FGQuaternion GetQuaternionECEF(void) const { return Qec2b; } 543 SetPQR(unsigned int i,double val)544 void SetPQR(unsigned int i, double val) { 545 VState.vPQR(i) = val; 546 VState.vPQRi = VState.vPQR + Ti2b * in.vOmegaPlanet; 547 } 548 SetUVW(unsigned int i,double val)549 void SetUVW(unsigned int i, double val) { 550 VState.vUVW(i) = val; 551 CalculateInertialVelocity(); 552 } 553 554 // SET functions 555 SetLongitude(double lon)556 void SetLongitude(double lon) 557 { 558 VState.vLocation.SetLongitude(lon); 559 UpdateVehicleState(); 560 } SetLongitudeDeg(double lon)561 void SetLongitudeDeg(double lon) { SetLongitude(lon*degtorad); } SetLatitude(double lat)562 void SetLatitude(double lat) 563 { 564 VState.vLocation.SetLatitude(lat); 565 UpdateVehicleState(); 566 } SetLatitudeDeg(double lat)567 void SetLatitudeDeg(double lat) { SetLatitude(lat*degtorad); } SetRadius(double r)568 void SetRadius(double r) 569 { 570 VState.vLocation.SetRadius(r); 571 VState.vInertialPosition = Tec2i * VState.vLocation; 572 } 573 SetAltitudeASL(double altASL)574 void SetAltitudeASL(double altASL) 575 { 576 VState.vLocation.SetAltitudeASL(altASL); 577 UpdateVehicleState(); 578 } SetAltitudeASLmeters(double altASL)579 void SetAltitudeASLmeters(double altASL) { SetAltitudeASL(altASL/fttom); } 580 581 void SetTerrainElevation(double tt); 582 void SetDistanceAGL(double tt); 583 void SetDistanceAGLKm(double tt); 584 585 void SetInitialState(const FGInitialCondition*); 586 void SetLocation(const FGLocation& l); SetLocation(const FGColumnVector3 & lv)587 void SetLocation(const FGColumnVector3& lv) 588 { 589 FGLocation l = FGLocation(lv); 590 SetLocation(l); 591 } SetPosition(const double Lon,const double Lat,const double Radius)592 void SetPosition(const double Lon, const double Lat, const double Radius) 593 { 594 FGLocation l = FGLocation(Lon, Lat, Radius); 595 SetLocation(l); 596 } 597 NudgeBodyLocation(const FGColumnVector3 & deltaLoc)598 void NudgeBodyLocation(const FGColumnVector3& deltaLoc) { 599 VState.vInertialPosition -= Tb2i*deltaLoc; 600 VState.vLocation -= Tb2ec*deltaLoc; 601 } 602 603 /** Sets the property forces/hold-down. This allows to do hard 'hold-down' 604 such as for rockets on a launch pad with engines ignited. 605 @param hd enables the 'hold-down' function if non-zero 606 */ 607 void SetHoldDown(bool hd); 608 609 void DumpState(void); 610 611 struct Inputs { 612 FGColumnVector3 vPQRidot; 613 FGColumnVector3 vUVWidot; 614 FGColumnVector3 vOmegaPlanet; 615 double SemiMajor; 616 double SemiMinor; 617 double DeltaT; 618 } in; 619 620 private: 621 622 // state vector 623 624 struct VehicleState VState; 625 626 FGColumnVector3 vVel; 627 FGMatrix33 Tec2b; 628 FGMatrix33 Tb2ec; 629 FGMatrix33 Tl2b; // local to body frame matrix copy for immediate local use 630 FGMatrix33 Tb2l; // body to local frame matrix copy for immediate local use 631 FGMatrix33 Tl2ec; // local to ECEF matrix copy for immediate local use 632 FGMatrix33 Tec2l; // ECEF to local frame matrix copy for immediate local use 633 FGMatrix33 Tec2i; // ECEF to ECI frame matrix copy for immediate local use 634 FGMatrix33 Ti2ec; // ECI to ECEF frame matrix copy for immediate local use 635 FGMatrix33 Ti2b; // ECI to body frame rotation matrix 636 FGMatrix33 Tb2i; // body to ECI frame rotation matrix 637 FGMatrix33 Ti2l; 638 FGMatrix33 Tl2i; 639 double epa; // Earth Position Angle 640 641 FGQuaternion Qec2b; 642 643 FGColumnVector3 LocalTerrainVelocity, LocalTerrainAngularVelocity; 644 645 eIntegrateType integrator_rotational_rate; 646 eIntegrateType integrator_translational_rate; 647 eIntegrateType integrator_rotational_position; 648 eIntegrateType integrator_translational_position; 649 650 void CalculateInertialVelocity(void); 651 void CalculateUVW(void); 652 void CalculateQuatdot(void); 653 654 void Integrate( FGColumnVector3& Integrand, 655 FGColumnVector3& Val, 656 std::deque <FGColumnVector3>& ValDot, 657 double dt, 658 eIntegrateType integration_type); 659 660 void Integrate( FGQuaternion& Integrand, 661 FGQuaternion& Val, 662 std::deque <FGQuaternion>& ValDot, 663 double dt, 664 eIntegrateType integration_type); 665 666 void UpdateLocationMatrices(void); 667 void UpdateBodyMatrices(void); 668 void UpdateVehicleState(void); 669 670 void WriteStateFile(int num); 671 void bind(void); 672 void Debug(int from); 673 }; 674 } 675 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 676 #endif 677