← Back to branch summary

~ubuntu-branches/ubuntu/precise/flightgear/precise

« back to all changes in this revision

Viewing changes to src/FDM/JSBSim/models/FGPropagate.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Ove Kaaven
  • Date: 2011-01-30 15:46:35 UTC
  • mfrom: (3.1.7 sid)
  • Revision ID: james.westby@ubuntu.com-20110130154635-rlynmg9n5hzxq5xe
Tags: 2.0.0-3
* Recommend fgfs-aircraft-base and fgfs-models-base.
  Closes. #610276.
* Added note about scenery SharedModels.tgz to README.Debian.

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/FDM/JSBSim/models/FGPropagate.cpp
6
6
 Purpose:      Integrate the EOM to determine instantaneous position
7
7
 Called by:    FGFDMExec
8
8
 
9
 
 ------------- Copyright (C) 1999  Jon S. Berndt (jsb@hal-pc.org) -------------
 
9
 ------------- Copyright (C) 1999  Jon S. Berndt (jon@jsbsim.org) -------------
10
10
 
11
11
 This program is free software; you can redistribute it and/or modify it under
12
12
 the terms of the GNU Lesser General Public License as published by the Free Software
54
54
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
55
55
 
56
56
#include <cmath>
57
 
#include <iomanip>
 
57
#include <cstdlib>
 
58
#include <iostream>
58
59
 
59
60
#include "FGPropagate.h"
60
 
#include <FGState.h>
61
 
#include <FGFDMExec.h>
 
61
#include "FGFDMExec.h"
 
62
#include "FGState.h"
62
63
#include "FGAircraft.h"
63
64
#include "FGMassBalance.h"
64
65
#include "FGInertial.h"
65
 
#include <input_output/FGPropertyManager.h>
 
66
#include "input_output/FGPropertyManager.h"
 
67
 
 
68
using namespace std;
66
69
 
67
70
namespace JSBSim {
68
71
 
82
85
  last_vPQRdot.InitMatrix();
83
86
  vPQRdot.InitMatrix();
84
87
  
 
88
  last2_vQtrndot = FGQuaternion(0,0,0);
 
89
  last_vQtrndot = FGQuaternion(0,0,0);
 
90
  vQtrndot = FGQuaternion(0,0,0);
 
91
 
85
92
  last2_vUVWdot.InitMatrix();
86
93
  last_vUVWdot.InitMatrix();
87
94
  vUVWdot.InitMatrix();
93
100
  vOmegaLocal.InitMatrix();
94
101
 
95
102
  integrator_rotational_rate = eAdamsBashforth2;
96
 
  integrator_translational_rate = eAdamsBashforth2;
97
 
  integrator_rotational_position = eTrapezoidal;
 
103
  integrator_translational_rate = eTrapezoidal;
 
104
  integrator_rotational_position = eAdamsBashforth2;
98
105
  integrator_translational_position = eTrapezoidal;
99
106
 
100
107
  bind();
114
121
{
115
122
  if (!FGModel::InitModel()) return false;
116
123
 
117
 
  SeaLevelRadius = Inertial->GetRefRadius();          // For initialization ONLY
118
 
  RunwayRadius   = SeaLevelRadius;
 
124
  // For initialization ONLY:
 
125
  SeaLevelRadius = LocalTerrainRadius = Inertial->GetRefRadius();
119
126
 
120
 
  VState.vLocation.SetRadius( SeaLevelRadius + 4.0 ); // Todo Add terrain elevation?
 
127
  VState.vLocation.SetRadius( LocalTerrainRadius + 4.0 );
121
128
  VState.vLocation.SetEllipse(Inertial->GetSemimajor(), Inertial->GetSemiminor());
122
129
  vOmega = FGColumnVector3( 0.0, 0.0, Inertial->omega() ); // Earth rotation vector
123
130
 
125
132
  last_vPQRdot.InitMatrix();
126
133
  vPQRdot.InitMatrix();
127
134
  
 
135
  last2_vQtrndot = FGQuaternion(0,0,0);
 
136
  last_vQtrndot = FGQuaternion(0,0,0);
 
137
  vQtrndot = FGQuaternion(0,0,0);
 
138
 
128
139
  last2_vUVWdot.InitMatrix();
129
140
  last_vUVWdot.InitMatrix();
130
141
  vUVWdot.InitMatrix();
136
147
  vOmegaLocal.InitMatrix();
137
148
 
138
149
  integrator_rotational_rate = eAdamsBashforth2;
139
 
  integrator_translational_rate = eAdamsBashforth2;
140
 
  integrator_rotational_position = eTrapezoidal;
 
150
  integrator_translational_rate = eTrapezoidal;
 
151
  integrator_rotational_position = eAdamsBashforth2;
141
152
  integrator_translational_position = eTrapezoidal;
142
153
 
143
154
  return true;
147
158
 
148
159
void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
149
160
{
150
 
  SeaLevelRadius = FGIC->GetSeaLevelRadiusFtIC();
151
 
  RunwayRadius = SeaLevelRadius;
 
161
  SetSeaLevelRadius(FGIC->GetSeaLevelRadiusFtIC());
 
162
  SetTerrainElevation(FGIC->GetTerrainElevationFtIC());
152
163
 
153
164
  // Set the position lat/lon/radius
154
165
  VState.vLocation.SetPosition( FGIC->GetLongitudeRadIC(),
155
166
                          FGIC->GetLatitudeRadIC(),
156
 
                          FGIC->GetAltitudeFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
 
167
                          FGIC->GetAltitudeASLFtIC() + FGIC->GetSeaLevelRadiusFtIC() );
157
168
 
158
169
  VehicleRadius = GetRadius();
159
170
  radInv = 1.0/VehicleRadius;
179
190
  // Finally, make sure that the quaternion stays normalized.
180
191
  VState.vQtrn.Normalize();
181
192
 
182
 
  // Recompute the RunwayRadius level.
183
 
  RecomputeRunwayRadius();
 
193
  // Recompute the LocalTerrainRadius.
 
194
  RecomputeLocalTerrainRadius();
 
195
 
 
196
  // These local copies of the transformation matrices are for use for
 
197
  // initial conditions only.
 
198
 
 
199
  Tl2b = GetTl2b();           // local to body frame transform
 
200
  Tb2l = Tl2b.Transposed();   // body to local frame transform
 
201
  Tl2ec = GetTl2ec();         // local to ECEF transform
 
202
  Tec2l = Tl2ec.Transposed(); // ECEF to local frame transform
 
203
  Tec2b = Tl2b * Tec2l;       // ECEF to body frame transform
 
204
  Tb2ec = Tec2b.Transposed(); // body to ECEF frame tranform
 
205
  Ti2ec = GetTi2ec();         // ECI to ECEF transform
 
206
  Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
 
207
  Ti2b  = Tec2b*Ti2ec;        // ECI to body frame transform
 
208
  Tb2i  = Ti2b.Transposed();  // body to ECI frame transform
184
209
}
185
210
 
186
211
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
208
233
  if (FGModel::Run()) return true;  // Fast return if we have nothing to do ...
209
234
  if (FDMExec->Holding()) return false;
210
235
 
211
 
  RecomputeRunwayRadius();
 
236
  RunPreFunctions();
 
237
 
 
238
  RecomputeLocalTerrainRadius();
212
239
 
213
240
  // Calculate current aircraft radius from center of planet
214
241
 
319
346
  last2_vLocationDot = last_vLocationDot;
320
347
  last_vLocationDot = vLocationDot;
321
348
 
 
349
  RunPreFunctions();
 
350
 
322
351
  Debug(2);
323
352
  return false;
324
353
}
416
445
 
417
446
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
418
447
 
419
 
void FGPropagate::RecomputeRunwayRadius(void)
 
448
void FGPropagate::RecomputeLocalTerrainRadius(void)
420
449
{
421
 
  // Get the runway radius.
 
450
  double t = State->Getsim_time();
 
451
 
 
452
  // Get the LocalTerrain radius.
422
453
  FGLocation contactloc;
423
454
  FGColumnVector3 dv;
424
 
  FGGroundCallback* gcb = FDMExec->GetGroundCallback();
425
 
  double t = State->Getsim_time();
426
 
  gcb->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
427
 
  RunwayRadius = contactloc.GetRadius();
 
455
  FDMExec->GetGroundCallback()->GetAGLevel(t, VState.vLocation, contactloc, dv, dv);
 
456
  LocalTerrainRadius = contactloc.GetRadius();
428
457
}
429
458
 
430
459
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
431
460
 
432
 
void FGPropagate::SetTerrainElevationASL(double tt)
 
461
void FGPropagate::SetTerrainElevation(double terrainElev)
433
462
{
434
 
  FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(tt+SeaLevelRadius);
 
463
  LocalTerrainRadius = terrainElev + SeaLevelRadius;
 
464
  FDMExec->GetGroundCallback()->SetTerrainGeoCentRadius(LocalTerrainRadius);
435
465
}
436
466
 
437
467
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
438
468
 
439
 
double FGPropagate::GetTerrainElevationASL(void) const
 
469
double FGPropagate::GetTerrainElevation(void) const
440
470
{
441
471
  return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
442
472
}
457
487
 
458
488
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
459
489
 
460
 
void FGPropagate::Seth(double tt)
 
490
void FGPropagate::SetAltitudeASL(double altASL)
461
491
{
462
 
  VState.vLocation.SetRadius( tt + SeaLevelRadius );
 
492
  VState.vLocation.SetRadius( altASL + SeaLevelRadius );
463
493
}
464
494
 
465
495
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
466
496
 
467
 
double FGPropagate::GetRunwayRadius(void) const
 
497
double FGPropagate::GetLocalTerrainRadius(void) const
468
498
{
469
 
  return RunwayRadius;
 
499
  return LocalTerrainRadius;
470
500
}
471
501
 
472
502
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
473
503
 
474
504
double FGPropagate::GetDistanceAGL(void) const
475
505
{
476
 
  return VState.vLocation.GetRadius() - RunwayRadius;
 
506
  return VState.vLocation.GetRadius() - LocalTerrainRadius;
477
507
}
478
508
 
479
509
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
480
510
 
481
511
void FGPropagate::SetDistanceAGL(double tt)
482
512
{
483
 
  VState.vLocation.SetRadius( tt + RunwayRadius );
 
513
  VState.vLocation.SetRadius( tt + LocalTerrainRadius );
484
514
}
485
515
 
486
516
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
503
533
  PropertyManager->Tie("velocities/q-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQR);
504
534
  PropertyManager->Tie("velocities/r-rad_sec", this, eR, (PMF)&FGPropagate::GetPQR);
505
535
 
 
536
  PropertyManager->Tie("velocities/pi-rad_sec", this, eP, (PMF)&FGPropagate::GetPQRi);
 
537
  PropertyManager->Tie("velocities/qi-rad_sec", this, eQ, (PMF)&FGPropagate::GetPQRi);
 
538
  PropertyManager->Tie("velocities/ri-rad_sec", this, eR, (PMF)&FGPropagate::GetPQRi);
 
539
 
506
540
  PropertyManager->Tie("velocities/eci-velocity-mag-fps", this, &FGPropagate::GetInertialVelocityMagnitude);
507
541
 
508
542
  PropertyManager->Tie("accelerations/pdot-rad_sec2", this, eP, (PMF)&FGPropagate::GetPQRdot);
513
547
  PropertyManager->Tie("accelerations/vdot-ft_sec2", this, eV, (PMF)&FGPropagate::GetUVWdot);
514
548
  PropertyManager->Tie("accelerations/wdot-ft_sec2", this, eW, (PMF)&FGPropagate::GetUVWdot);
515
549
 
516
 
  PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::Geth, &FGPropagate::Seth, true);
517
 
  PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::Gethmeters, &FGPropagate::Sethmeters, true);
 
550
  PropertyManager->Tie("position/h-sl-ft", this, &FGPropagate::GetAltitudeASL, &FGPropagate::SetAltitudeASL, true);
 
551
  PropertyManager->Tie("position/h-sl-meters", this, &FGPropagate::GetAltitudeASLmeters, &FGPropagate::SetAltitudeASLmeters, true);
518
552
  PropertyManager->Tie("position/lat-gc-rad", this, &FGPropagate::GetLatitude, &FGPropagate::SetLatitude);
519
553
  PropertyManager->Tie("position/long-gc-rad", this, &FGPropagate::GetLongitude, &FGPropagate::SetLongitude);
520
554
  PropertyManager->Tie("position/lat-gc-deg", this, &FGPropagate::GetLatitudeDeg, &FGPropagate::SetLatitudeDeg);
525
559
  PropertyManager->Tie("position/h-agl-ft", this,  &FGPropagate::GetDistanceAGL, &FGPropagate::SetDistanceAGL);
526
560
  PropertyManager->Tie("position/radius-to-vehicle-ft", this, &FGPropagate::GetRadius);
527
561
  PropertyManager->Tie("position/terrain-elevation-asl-ft", this,
528
 
                          &FGPropagate::GetTerrainElevationASL,
529
 
                          &FGPropagate::SetTerrainElevationASL, false);
 
562
                          &FGPropagate::GetTerrainElevation,
 
563
                          &FGPropagate::SetTerrainElevation, false);
530
564
 
531
 
  PropertyManager->Tie("metrics/runway-radius", this, &FGPropagate::GetRunwayRadius);
 
565
  PropertyManager->Tie("metrics/terrain-radius", this, &FGPropagate::GetLocalTerrainRadius);
532
566
 
533
567
  PropertyManager->Tie("attitude/phi-rad", this, (int)ePhi, (PMF)&FGPropagate::GetEuler);
534
568
  PropertyManager->Tie("attitude/theta-rad", this, (int)eTht, (PMF)&FGPropagate::GetEuler);