-
Notifications
You must be signed in to change notification settings - Fork 450
/
FGAccelerations.cpp
428 lines (350 loc) · 16.4 KB
/
FGAccelerations.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Module: FGAccelerations.cpp
Author: Jon S. Berndt
Date started: 07/12/11
Purpose: Calculates derivatives of rotational and translational rates, and
of the attitude quaternion.
Called by: FGFDMExec
------------- Copyright (C) 2011 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 Foundation; either version 2 of the License, or (at your option) any
later version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
details.
You should have received a copy of the GNU Lesser General Public License along
with this program; if not, write to the Free Software Foundation, Inc., 59
Temple Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU Lesser General Public License can also be
found on the world wide web at http://www.gnu.org.
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
This class encapsulates the calculation of the derivatives of the state vectors
UVW and PQR - the translational and rotational rates relative to the planet
fixed frame. The derivatives relative to the inertial frame are also calculated
as a side effect. Also, the derivative of the attitude quaterion is also
calculated.
HISTORY
--------------------------------------------------------------------------------
07/12/11 JSB Created
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
COMMENTS, REFERENCES, and NOTES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[1] Stevens and Lewis, "Aircraft Control and Simulation", Second edition (2004)
Wiley
[2] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
NASA-Ames", NASA CR-2497, January 1975
[3] Erin Catto, "Iterative Dynamics with Temporal Coherence", February 22, 2005
[4] Mark Harris and Robert Lyle, "Spacecraft Gravitational Torques",
NASA SP-8024, May 1969
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#include "FGAccelerations.h"
#include "FGFDMExec.h"
using namespace std;
namespace JSBSim {
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
FGAccelerations::FGAccelerations(FGFDMExec* fdmex)
: FGModel(fdmex)
{
Debug(0);
Name = "FGAccelerations";
gravTorque = false;
vPQRidot.InitMatrix();
vUVWidot.InitMatrix();
vUVWdot.InitMatrix();
vBodyAccel.InitMatrix();
bind();
Debug(0);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FGAccelerations::~FGAccelerations(void)
{
Debug(1);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGAccelerations::InitModel(void)
{
if (!FGModel::InitModel()) return false;
vPQRidot.InitMatrix();
vUVWidot.InitMatrix();
vUVWdot.InitMatrix();
vBodyAccel.InitMatrix();
return true;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/*
Purpose: Called on a schedule to calculate derivatives.
*/
bool FGAccelerations::Run(bool Holding)
{
if (FGModel::Run(Holding)) return true; // Fast return if we have nothing to do ...
if (Holding) return false;
CalculatePQRdot(); // Angular rate derivative
CalculateUVWdot(); // Translational rate derivative
if (!FDMExec->GetHoldDown())
CalculateFrictionForces(in.DeltaT * rate); // Update rate derivatives with friction forces
Debug(2);
return false;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Compute body frame rotational accelerations based on the current body moments
//
// vPQRdot is the derivative of the absolute angular velocity of the vehicle
// (body rate with respect to the ECEF frame), expressed in the body frame,
// where the derivative is taken in the body frame.
// J is the inertia matrix
// Jinv is the inverse inertia matrix
// vMoments is the moment vector in the body frame
// in.vPQRi is the total inertial angular velocity of the vehicle
// expressed in the body frame.
// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
// Second edition (2004), eqn 1.5-16e (page 50)
void FGAccelerations::CalculatePQRdot(void)
{
if (gravTorque) {
// Compute the gravitational torque
// Reference: See Harris and Lyle "Spacecraft Gravitational Torques",
// NASA SP-8024 (1969) eqn (2) (page 7)
FGColumnVector3 R = in.Ti2b * in.vInertialPosition;
double invRadius = 1.0 / R.Magnitude();
R *= invRadius;
in.Moment += (3.0 * in.vGravAccel.Magnitude() * invRadius) * (R * (in.J * R));
}
// Compute body frame rotational accelerations based on the current body
// moments and the total inertial angular velocity expressed in the body
// frame.
// if (HoldDown && !FDMExec->GetTrimStatus()) {
if (FDMExec->GetHoldDown()) {
// The rotational acceleration in ECI is calculated so that the rotational
// acceleration is zero in the body frame.
vPQRdot.InitMatrix();
vPQRidot = in.vPQRi * (in.Ti2b * in.vOmegaPlanet);
}
else {
vPQRidot = in.Jinv * (in.Moment - in.vPQRi * (in.J * in.vPQRi));
vPQRdot = vPQRidot - in.vPQRi * (in.Ti2b * in.vOmegaPlanet);
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// This set of calculations results in the body and inertial frame accelerations
// being computed.
// Compute body and inertial frames accelerations based on the current body
// forces including centripetal and Coriolis accelerations for the former.
// in.vOmegaPlanet is the Earth angular rate - expressed in the inertial frame -
// so it has to be transformed to the body frame. More completely,
// in.vOmegaPlanet is the rate of the ECEF frame relative to the Inertial
// frame (ECI), expressed in the Inertial frame.
// in.Force is the total force on the vehicle in the body frame.
// in.vPQR is the vehicle body rate relative to the ECEF frame, expressed
// in the body frame.
// in.vUVW is the vehicle velocity relative to the ECEF frame, expressed
// in the body frame.
// Reference: See Stevens and Lewis, "Aircraft Control and Simulation",
// Second edition (2004), eqns 1.5-13 (pg 48) and 1.5-16d (page 50)
void FGAccelerations::CalculateUVWdot(void)
{
if (FDMExec->GetHoldDown() && !FDMExec->GetTrimStatus())
vBodyAccel.InitMatrix();
else
vBodyAccel = in.Force / in.Mass;
vUVWdot = vBodyAccel - (in.vPQR + 2.0 * (in.Ti2b * in.vOmegaPlanet)) * in.vUVW;
// Include Centripetal acceleration.
vUVWdot -= in.Ti2b * (in.vOmegaPlanet * (in.vOmegaPlanet * in.vInertialPosition));
if (FDMExec->GetHoldDown()) {
// The acceleration in ECI is calculated so that the acceleration is zero
// in the body frame.
vUVWidot = in.vOmegaPlanet * (in.vOmegaPlanet * in.vInertialPosition);
vUVWdot.InitMatrix();
}
else {
vUVWdot += in.Tec2b * in.vGravAccel;
vUVWidot = in.Tb2i * vBodyAccel + in.Tec2i * in.vGravAccel;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGAccelerations::SetHoldDown(bool hd)
{
if (hd) {
vUVWidot = in.vOmegaPlanet * (in.vOmegaPlanet * in.vInertialPosition);
vUVWdot.InitMatrix();
vPQRidot = in.vPQRi * (in.Ti2b * in.vOmegaPlanet);
vPQRdot.InitMatrix();
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// Computes the contact forces just before integrating the EOM.
// This routine is using Lagrange multipliers and the projected Gauss-Seidel
// (PGS) method.
// Reference: See Erin Catto, "Iterative Dynamics with Temporal Coherence",
// February 22, 2005
// In JSBSim there is only one rigid body (the aircraft) and there can be
// multiple points of contact between the aircraft and the ground. As a
// consequence our matrix Jac*M^-1*Jac^T is not sparse and the algorithm
// described in Catto's paper has been adapted accordingly.
// The friction forces are resolved in the body frame relative to the origin
// (Earth center).
void FGAccelerations::CalculateFrictionForces(double dt)
{
vector<LagrangeMultiplier*>& multipliers = *in.MultipliersList;
size_t n = multipliers.size();
vFrictionForces.InitMatrix();
vFrictionMoments.InitMatrix();
// If no gears are in contact with the ground then return
if (!n) return;
vector<double> a(n*n); // Will contain Jac*M^-1*Jac^T
vector<double> rhs(n);
// Assemble the linear system of equations
for (unsigned int i=0; i < n; i++) {
FGColumnVector3 U = multipliers[i]->ForceJacobian;
FGColumnVector3 r = multipliers[i]->LeverArm;
FGColumnVector3 v1 = U / in.Mass;
FGColumnVector3 v2 = in.Jinv * (r*U); // Should be J^-T but J is symmetric and so is J^-1
for (unsigned int j=0; j < i; j++)
a[i*n+j] = a[j*n+i]; // Takes advantage of the symmetry of Jac^T*M^-1*Jac
for (unsigned int j=i; j < n; j++) {
U = multipliers[j]->ForceJacobian;
r = multipliers[j]->LeverArm;
a[i*n+j] = DotProduct(U, v1 + v2*r);
}
}
// Assemble the RHS member
// Translation
FGColumnVector3 vdot = vUVWdot;
if (dt > 0.) // Zeroes out the relative movement between the aircraft and the ground
vdot += (in.vUVW - in.Tec2b * in.TerrainVelocity) / dt;
// Rotation
FGColumnVector3 wdot = vPQRdot;
if (dt > 0.) // Zeroes out the relative movement between the aircraft and the ground
wdot += (in.vPQR - in.Tec2b * in.TerrainAngularVel) / 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 'rhs' by a[i,i]. This is in order to save
// a division computation at each iteration of Gauss-Seidel.
for (unsigned int i=0; i < n; i++) {
double d = a[i*n+i];
FGColumnVector3 U = multipliers[i]->ForceJacobian;
FGColumnVector3 r = multipliers[i]->LeverArm;
rhs[i] = -DotProduct(U, vdot + wdot*r)/d;
for (unsigned int j=0; j < n; j++)
a[i*n+j] /= d;
}
// Resolve the Lagrange multipliers with the projected Gauss-Seidel method
for (int iter=0; iter < 50; iter++) {
double norm = 0.;
for (unsigned int i=0; i < n; i++) {
double lambda0 = multipliers[i]->value;
double dlambda = rhs[i];
for (unsigned int j=0; j < n; j++)
dlambda -= a[i*n+j]*multipliers[j]->value;
multipliers[i]->value = Constrain(multipliers[i]->Min, lambda0+dlambda, multipliers[i]->Max);
dlambda = multipliers[i]->value - lambda0;
norm += fabs(dlambda);
}
if (norm < 1E-5) break;
}
// Calculate the total friction forces and moments
for (unsigned int i=0; i< n; i++) {
double lambda = multipliers[i]->value;
FGColumnVector3 U = multipliers[i]->ForceJacobian;
FGColumnVector3 r = multipliers[i]->LeverArm;
FGColumnVector3 F = lambda * U;
vFrictionForces += F;
vFrictionMoments += r * F;
}
FGColumnVector3 accel = vFrictionForces / in.Mass;
FGColumnVector3 omegadot = in.Jinv * vFrictionMoments;
vBodyAccel += accel;
vUVWdot += accel;
vUVWidot += in.Tb2i * accel;
vPQRdot += omegadot;
vPQRidot += omegadot;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGAccelerations::InitializeDerivatives(void)
{
// Make an initial run and set past values
CalculatePQRdot(); // Angular rate derivative
CalculateUVWdot(); // Translational rate derivative
CalculateFrictionForces(0.); // Update rate derivatives with friction forces
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGAccelerations::bind(void)
{
using PMF = double (FGAccelerations::*)(int) const;
PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGAccelerations::GetPQRdot);
PropertyManager->Tie("accelerations/qdot-rad_sec2", this, eQ, (PMF)&FGAccelerations::GetPQRdot);
PropertyManager->Tie("accelerations/rdot-rad_sec2", this, eR, (PMF)&FGAccelerations::GetPQRdot);
PropertyManager->Tie("accelerations/pidot-rad_sec2", this, eP, (PMF)&FGAccelerations::GetPQRidot);
PropertyManager->Tie("accelerations/qidot-rad_sec2", this, eQ, (PMF)&FGAccelerations::GetPQRidot);
PropertyManager->Tie("accelerations/ridot-rad_sec2", this, eR, (PMF)&FGAccelerations::GetPQRidot);
PropertyManager->Tie("accelerations/udot-ft_sec2", this, eU, (PMF)&FGAccelerations::GetUVWdot);
PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGAccelerations::GetUVWdot);
PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGAccelerations::GetUVWdot);
PropertyManager->Tie("accelerations/uidot-ft_sec2", this, eU, (PMF)&FGAccelerations::GetUVWidot);
PropertyManager->Tie("accelerations/vidot-ft_sec2", this, eV, (PMF)&FGAccelerations::GetUVWidot);
PropertyManager->Tie("accelerations/widot-ft_sec2", this, eW, (PMF)&FGAccelerations::GetUVWidot);
PropertyManager->Tie("accelerations/gravity-ft_sec2", this, &FGAccelerations::GetGravAccelMagnitude);
PropertyManager->Tie("simulation/gravitational-torque", &gravTorque);
PropertyManager->Tie("forces/fbx-weight-lbs", this, eX, (PMF)&FGAccelerations::GetWeight);
PropertyManager->Tie("forces/fby-weight-lbs", this, eY, (PMF)&FGAccelerations::GetWeight);
PropertyManager->Tie("forces/fbz-weight-lbs", this, eZ, (PMF)&FGAccelerations::GetWeight);
PropertyManager->Tie("forces/fbx-total-lbs", this, eX, (PMF)&FGAccelerations::GetForces);
PropertyManager->Tie("forces/fby-total-lbs", this, eY, (PMF)&FGAccelerations::GetForces);
PropertyManager->Tie("forces/fbz-total-lbs", this, eZ, (PMF)&FGAccelerations::GetForces);
PropertyManager->Tie("moments/l-total-lbsft", this, eL, (PMF)&FGAccelerations::GetMoments);
PropertyManager->Tie("moments/m-total-lbsft", this, eM, (PMF)&FGAccelerations::GetMoments);
PropertyManager->Tie("moments/n-total-lbsft", this, eN, (PMF)&FGAccelerations::GetMoments);
PropertyManager->Tie("moments/l-gear-lbsft", this, eL, (PMF)&FGAccelerations::GetGroundMoments);
PropertyManager->Tie("moments/m-gear-lbsft", this, eM, (PMF)&FGAccelerations::GetGroundMoments);
PropertyManager->Tie("moments/n-gear-lbsft", this, eN, (PMF)&FGAccelerations::GetGroundMoments);
PropertyManager->Tie("forces/fbx-gear-lbs", this, eX, (PMF)&FGAccelerations::GetGroundForces);
PropertyManager->Tie("forces/fby-gear-lbs", this, eY, (PMF)&FGAccelerations::GetGroundForces);
PropertyManager->Tie("forces/fbz-gear-lbs", this, eZ, (PMF)&FGAccelerations::GetGroundForces);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// 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 FGAccelerations::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: FGAccelerations" << endl;
if (from == 1) cout << "Destroyed: FGAccelerations" << endl;
}
if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
}
if (debug_lvl & 8 && from == 2) { // Runtime state variables
}
if (debug_lvl & 16) { // Sanity checking
}
if (debug_lvl & 64) {
if (from == 0) { // Constructor
}
}
}
}