1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 Module: FGAccelerations.cpp
6 Purpose: Calculates derivatives of rotational and translational rates, and
7 of the attitude quaternion.
10 ------------- Copyright (C) 2011 Jon S. Berndt (jon@jsbsim.org) -------------
12 This program is free software; you can redistribute it and/or modify it under
13 the terms of the GNU Lesser General Public License as published by the Free Software
14 Foundation; either version 2 of the License, or (at your option) any later
17 This program is distributed in the hope that it will be useful, but WITHOUT
18 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
19 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
22 You should have received a copy of the GNU Lesser General Public License along with
23 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
24 Place - Suite 330, Boston, MA 02111-1307, USA.
26 Further information about the GNU Lesser General Public License can also be found on
27 the world wide web at http://www.gnu.org.
29 FUNCTIONAL DESCRIPTION
30 --------------------------------------------------------------------------------
31 This class encapsulates the calculation of the derivatives of the state vectors
32 UVW and PQR - the translational and rotational rates relative to the planet
33 fixed frame. The derivatives relative to the inertial frame are also calculated
34 as a side effect. Also, the derivative of the attitude quaterion is also calculated.
37 --------------------------------------------------------------------------------
40 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
41 COMMENTS, REFERENCES, and NOTES
42 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
43 [1] Stevens and Lewis, "Aircraft Control and Simulation", Second edition (2004)
45 [2] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
46 NASA-Ames", NASA CR-2497, January 1975
47 [3] Erin Catto, "Iterative Dynamics with Temporal Coherence", February 22, 2005
48 [4] Mark Harris and Robert Lyle, "Spacecraft Gravitational Torques",
49 NASA SP-8024, May 1969
51 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
53 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
55 #include "FGAccelerations.h"
56 #include "FGFDMExec.h"
57 #include "input_output/FGPropertyManager.h"
63 static const char *IdSrc = "$Id: FGAccelerations.cpp,v 1.13 2012/02/18 19:11:37 bcoconni Exp $";
64 static const char *IdHdr = ID_ACCELERATIONS;
66 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
68 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
70 FGAccelerations::FGAccelerations(FGFDMExec* fdmex)
74 Name = "FGAccelerations";
78 vPQRidot.InitMatrix();
79 vUVWidot.InitMatrix();
80 vGravAccel.InitMatrix();
81 vBodyAccel.InitMatrix();
82 vQtrndot = FGQuaternion(0,0,0);
88 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
90 FGAccelerations::~FGAccelerations(void)
95 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
97 bool FGAccelerations::InitModel(void)
99 vPQRidot.InitMatrix();
100 vUVWidot.InitMatrix();
101 vGravAccel.InitMatrix();
102 vBodyAccel.InitMatrix();
103 vQtrndot = FGQuaternion(0,0,0);
108 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
110 Purpose: Called on a schedule to calculate derivatives.
113 bool FGAccelerations::Run(bool Holding)
115 if (FGModel::Run(Holding)) return true; // Fast return if we have nothing to do ...
116 if (Holding) return false;
118 CalculatePQRdot(); // Angular rate derivative
119 CalculateUVWdot(); // Translational rate derivative
120 CalculateQuatdot(); // Angular orientation derivative
122 ResolveFrictionForces(in.DeltaT * rate); // Update rate derivatives with friction forces
128 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
129 // Compute body frame rotational accelerations based on the current body moments
131 // vPQRdot is the derivative of the absolute angular velocity of the vehicle
132 // (body rate with respect to the ECEF frame), expressed in the body frame,
133 // where the derivative is taken in the body frame.
134 // J is the inertia matrix
135 // Jinv is the inverse inertia matrix
136 // vMoments is the moment vector in the body frame
137 // in.vPQRi is the total inertial angular velocity of the vehicle
138 // expressed in the body frame.
139 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
140 // Second edition (2004), eqn 1.5-16e (page 50)
142 void FGAccelerations::CalculatePQRdot(void)
145 // Compute the gravitational torque
146 // Reference: See Harris and Lyle "Spacecraft Gravitational Torques",
147 // NASA SP-8024 (1969) eqn (2) (page 7)
148 FGColumnVector3 R = in.Ti2b * in.vInertialPosition;
149 double invRadius = 1.0 / R.Magnitude();
151 in.Moment += (3.0 * in.GAccel * invRadius) * (R * (in.J * R));
154 // Compute body frame rotational accelerations based on the current body
155 // moments and the total inertial angular velocity expressed in the body
158 vPQRidot = in.Jinv * (in.Moment - in.vPQRi * (in.J * in.vPQRi));
159 vPQRdot = vPQRidot - in.vPQRi * (in.Ti2b * in.vOmegaPlanet);
162 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
163 // Compute the quaternion orientation derivative
165 // vQtrndot is the quaternion derivative.
166 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
167 // Second edition (2004), eqn 1.5-16b (page 50)
169 void FGAccelerations::CalculateQuatdot(void)
171 // Compute quaternion orientation derivative on current body rates
172 vQtrndot = in.qAttitudeECI.GetQDot(in.vPQRi);
175 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
176 // This set of calculations results in the body and inertial frame accelerations
178 // Compute body and inertial frames accelerations based on the current body
179 // forces including centripetal and Coriolis accelerations for the former.
180 // in.vOmegaPlanet is the Earth angular rate - expressed in the inertial frame -
181 // so it has to be transformed to the body frame. More completely,
182 // in.vOmegaPlanet is the rate of the ECEF frame relative to the Inertial
183 // frame (ECI), expressed in the Inertial frame.
184 // in.Force is the total force on the vehicle in the body frame.
185 // in.vPQR is the vehicle body rate relative to the ECEF frame, expressed
186 // in the body frame.
187 // in.vUVW is the vehicle velocity relative to the ECEF frame, expressed
188 // in the body frame.
189 // Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
190 // Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
192 void FGAccelerations::CalculateUVWdot(void)
194 vBodyAccel = in.Force / in.Mass;
196 vUVWdot = vBodyAccel - (in.vPQR + 2.0 * (in.Ti2b * in.vOmegaPlanet)) * in.vUVW;
198 // Include Centripetal acceleration.
199 vUVWdot -= in.Ti2b * (in.vOmegaPlanet * (in.vOmegaPlanet * in.vInertialPosition));
201 // Include Gravitation accel
205 double radius = in.vInertialPosition.Magnitude();
206 vGravAccel = -(in.GAccel / radius) * in.vInertialPosition;
210 vGravAccel = in.Tec2i * in.J2Grav;
214 vUVWdot += in.Ti2b * vGravAccel;
215 vUVWidot = in.Tb2i * vBodyAccel + vGravAccel;
218 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
219 // Resolves the contact forces just before integrating the EOM.
220 // This routine is using Lagrange multipliers and the projected Gauss-Seidel
222 // Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
224 // In JSBSim there is only one rigid body (the aircraft) and there can be
225 // multiple points of contact between the aircraft and the ground. As a
226 // consequence our matrix Jac*M^-1*Jac^T is not sparse and the algorithm
227 // described in Catto's paper has been adapted accordingly.
228 // The friction forces are resolved in the body frame relative to the origin
231 void FGAccelerations::ResolveFrictionForces(double dt)
233 const double invMass = 1.0 / in.Mass;
234 const FGMatrix33& Jinv = in.Jinv;
235 FGColumnVector3 vdot, wdot;
236 vector<LagrangeMultiplier*>& multipliers = *in.MultipliersList;
237 int n = multipliers.size();
239 vFrictionForces.InitMatrix();
240 vFrictionMoments.InitMatrix();
242 // If no gears are in contact with the ground then return
245 vector<double> a(n*n); // Will contain Jac*M^-1*Jac^T
246 vector<double> rhs(n);
248 // Assemble the linear system of equations
249 for (int i=0; i < n; i++) {
250 FGColumnVector3 v1 = invMass * multipliers[i]->ForceJacobian;
251 FGColumnVector3 v2 = Jinv * multipliers[i]->MomentJacobian; // Should be J^-T but J is symmetric and so is J^-1
253 for (int j=0; j < i; j++)
254 a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of Jac^T*M^-1*Jac
255 for (int j=i; j < n; j++)
256 a[i*n+j] = DotProduct(v1, multipliers[j]->ForceJacobian)
257 + DotProduct(v2, multipliers[j]->MomentJacobian);
260 // Assemble the RHS member
264 if (dt > 0.) // Zeroes out the relative movement between the aircraft and the ground
265 vdot += (in.vUVW - in.Tec2b * in.TerrainVelocity) / dt;
269 if (dt > 0.) // Zeroes out the relative movement between the aircraft and the ground
270 wdot += (in.vPQR - in.Tec2b * in.TerrainAngularVel) / dt;
272 // Prepare the linear system for the Gauss-Seidel algorithm :
273 // 1. Compute the right hand side member 'rhs'
274 // 2. Divide every line of 'a' and 'rhs' by a[i,i]. This is in order to save
275 // a division computation at each iteration of Gauss-Seidel.
276 for (int i=0; i < n; i++) {
277 double d = 1.0 / a[i*n+i];
279 rhs[i] = -(DotProduct(multipliers[i]->ForceJacobian, vdot)
280 +DotProduct(multipliers[i]->MomentJacobian, wdot))*d;
281 for (int j=0; j < n; j++)
285 // Resolve the Lagrange multipliers with the projected Gauss-Seidel method
286 for (int iter=0; iter < 50; iter++) {
289 for (int i=0; i < n; i++) {
290 double lambda0 = multipliers[i]->value;
291 double dlambda = rhs[i];
293 for (int j=0; j < n; j++)
294 dlambda -= a[i*n+j]*multipliers[j]->value;
296 multipliers[i]->value = Constrain(multipliers[i]->Min, lambda0+dlambda, multipliers[i]->Max);
297 dlambda = multipliers[i]->value - lambda0;
299 norm += fabs(dlambda);
302 if (norm < 1E-5) break;
305 // Calculate the total friction forces and moments
307 for (int i=0; i< n; i++) {
308 double lambda = multipliers[i]->value;
309 vFrictionForces += lambda * multipliers[i]->ForceJacobian;
310 vFrictionMoments += lambda * multipliers[i]->MomentJacobian;
313 FGColumnVector3 accel = invMass * vFrictionForces;
314 FGColumnVector3 omegadot = Jinv * vFrictionMoments;
318 vUVWidot += in.Tb2i * accel;
320 vPQRidot += omegadot;
323 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
325 void FGAccelerations::InitializeDerivatives(void)
327 // Make an initial run and set past values
328 CalculatePQRdot(); // Angular rate derivative
329 CalculateUVWdot(); // Translational rate derivative
330 CalculateQuatdot(); // Angular orientation derivative
331 ResolveFrictionForces(0.); // Update rate derivatives with friction forces
334 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
336 void FGAccelerations::bind(void)
338 typedef double (FGAccelerations::*PMF)(int) const;
340 PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGAccelerations::GetPQRdot);
341 PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGAccelerations::GetPQRdot);
342 PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGAccelerations::GetPQRdot);
344 PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGAccelerations::GetUVWdot);
345 PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGAccelerations::GetUVWdot);
346 PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGAccelerations::GetUVWdot);
348 PropertyManager->Tie("simulation/gravity-model", &gravType);
349 PropertyManager->Tie("simulation/gravitational-torque", &gravTorque);
351 PropertyManager->Tie("forces/fbx-total-lbs", this, eX, (PMF)&FGAccelerations::GetForces);
352 PropertyManager->Tie("forces/fby-total-lbs", this, eY, (PMF)&FGAccelerations::GetForces);
353 PropertyManager->Tie("forces/fbz-total-lbs", this, eZ, (PMF)&FGAccelerations::GetForces);
354 PropertyManager->Tie("moments/l-total-lbsft", this, eL, (PMF)&FGAccelerations::GetMoments);
355 PropertyManager->Tie("moments/m-total-lbsft", this, eM, (PMF)&FGAccelerations::GetMoments);
356 PropertyManager->Tie("moments/n-total-lbsft", this, eN, (PMF)&FGAccelerations::GetMoments);
358 PropertyManager->Tie("moments/l-gear-lbsft", this, eL, (PMF)&FGAccelerations::GetGroundMoments);
359 PropertyManager->Tie("moments/m-gear-lbsft", this, eM, (PMF)&FGAccelerations::GetGroundMoments);
360 PropertyManager->Tie("moments/n-gear-lbsft", this, eN, (PMF)&FGAccelerations::GetGroundMoments);
361 PropertyManager->Tie("forces/fbx-gear-lbs", this, eX, (PMF)&FGAccelerations::GetGroundForces);
362 PropertyManager->Tie("forces/fby-gear-lbs", this, eY, (PMF)&FGAccelerations::GetGroundForces);
363 PropertyManager->Tie("forces/fbz-gear-lbs", this, eZ, (PMF)&FGAccelerations::GetGroundForces);
366 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
367 // The bitmasked value choices are as follows:
368 // unset: In this case (the default) JSBSim would only print
369 // out the normally expected messages, essentially echoing
370 // the config files as they are read. If the environment
371 // variable is not set, debug_lvl is set to 1 internally
372 // 0: This requests JSBSim not to output any messages
374 // 1: This value explicity requests the normal JSBSim
376 // 2: This value asks for a message to be printed out when
377 // a class is instantiated
378 // 4: When this value is set, a message is displayed when a
379 // FGModel object executes its Run() method
380 // 8: When this value is set, various runtime state variables
381 // are printed out periodically
382 // 16: When set various parameters are sanity checked and
383 // a message is printed out when they go out of bounds
385 void FGAccelerations::Debug(int from)
387 if (debug_lvl <= 0) return;
389 if (debug_lvl & 1) { // Standard console startup message output
390 if (from == 0) { // Constructor
394 if (debug_lvl & 2 ) { // Instantiation/Destruction notification
395 if (from == 0) cout << "Instantiated: FGAccelerations" << endl;
396 if (from == 1) cout << "Destroyed: FGAccelerations" << endl;
398 if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
400 if (debug_lvl & 8 && from == 2) { // Runtime state variables
402 if (debug_lvl & 16) { // Sanity checking
404 if (debug_lvl & 64) {
405 if (from == 0) { // Constructor
406 cout << IdSrc << endl;
407 cout << IdHdr << endl;