From cc9b817f0e20a879cd23ce7f8014b2dc6a5d493e Mon Sep 17 00:00:00 2001
From: James Turner <jmt@Dulux.local>
Date: Wed, 16 Jun 2010 18:02:41 +0100
Subject: [PATCH 1/2] Extend SGSubsystemGroup, to allow running a fixed dt
 time, internally

---
 simgear/structure/subsystem_mgr.cxx | 45 +++++++++++++++++++++--------
 simgear/structure/subsystem_mgr.hxx |  8 +++++
 2 files changed, 41 insertions(+), 12 deletions(-)

diff --git a/simgear/structure/subsystem_mgr.cxx b/simgear/structure/subsystem_mgr.cxx
index ee9c15d1..26f026cd 100644
--- a/simgear/structure/subsystem_mgr.cxx
+++ b/simgear/structure/subsystem_mgr.cxx
@@ -104,7 +104,9 @@ void SGSubsystem::stamp(const string& name)
 // Implementation of SGSubsystemGroup.
 ////////////////////////////////////////////////////////////////////////
 
-SGSubsystemGroup::SGSubsystemGroup ()
+SGSubsystemGroup::SGSubsystemGroup () :
+  _fixedUpdateTime(-1.0),
+  _updateTimeRemainder(0.0)
 {
 }
 
@@ -157,18 +159,31 @@ SGSubsystemGroup::unbind ()
 void
 SGSubsystemGroup::update (double delta_time_sec)
 {
-    for (unsigned int i = 0; i < _members.size(); i++)
-    {
-         SGTimeStamp timeStamp = SGTimeStamp::now();
-         _members[i]->update(delta_time_sec); // indirect call
-         timeStamp = timeStamp - SGTimeStamp::now();
-         double b = timeStamp.toUSecs();
-         _members[i]->updateExecutionTime(b);
-         double threshold = _members[i]->getTimeWarningThreshold();
-         if (( b > threshold ) && (b > 10000)) {
-             _members[i]->printTimingInformation(b);
-         }
+    int loopCount = 1;
+    // if dt == 0.0, we are paused, so we need to run one iteration
+    // of our members; if we have a fixed update time, we compute a
+    // loop count, and locally adjust dt
+    if ((delta_time_sec > 0.0) && (_fixedUpdateTime > 0.0)) {
+      double localDelta = delta_time_sec + _updateTimeRemainder;
+      loopCount = SGMiscd::roundToInt(localDelta / _fixedUpdateTime);
+      _updateTimeRemainder = delta_time_sec - (loopCount * _fixedUpdateTime);
+      delta_time_sec = _fixedUpdateTime;
     }
+
+    while (loopCount-- > 0) {
+      for (unsigned int i = 0; i < _members.size(); i++)
+      {
+           SGTimeStamp timeStamp = SGTimeStamp::now();
+           _members[i]->update(delta_time_sec); // indirect call
+           timeStamp = timeStamp - SGTimeStamp::now();
+           double b = timeStamp.toUSecs();
+           _members[i]->updateExecutionTime(b);
+           double threshold = _members[i]->getTimeWarningThreshold();
+           if (( b > threshold ) && (b > 10000)) {
+               _members[i]->printTimingInformation(b);
+           }
+      }
+    } // of multiple update loop
 }
 
 void 
@@ -233,6 +248,12 @@ SGSubsystemGroup::remove_subsystem (const string &name)
     }
 }
 
+void
+SGSubsystemGroup::set_fixed_update_time(double dt)
+{
+  _fixedUpdateTime = dt;
+}
+
 void
 SGSubsystemGroup::Member::printTimingStatistics ()
 {
diff --git a/simgear/structure/subsystem_mgr.hxx b/simgear/structure/subsystem_mgr.hxx
index a751d306..87654d9c 100644
--- a/simgear/structure/subsystem_mgr.hxx
+++ b/simgear/structure/subsystem_mgr.hxx
@@ -317,6 +317,10 @@ public:
 
     void collectDebugTiming(bool collect);
 
+    /**
+     * 
+     */
+    void set_fixed_update_time(double fixed_dt);
 private:
 
     class Member {
@@ -345,6 +349,9 @@ private:
     Member * get_member (const string &name, bool create = false);
 
     vector<Member *> _members;
+    
+    double _fixedUpdateTime;
+    double _updateTimeRemainder;
 };
 
 
@@ -376,6 +383,7 @@ public:
     enum GroupType {
         INIT = 0,
         GENERAL,
+        FDM,  ///< flight model, autopilot, instruments that run coupled
         MAX_GROUPS
     };
 

From b846e33ee398025f179d4357345a5be7587aa728 Mon Sep 17 00:00:00 2001
From: James Turner <jmt@dulux.home>
Date: Thu, 17 Jun 2010 21:39:10 +0100
Subject: [PATCH 2/2] Extend Magvar interface to use SGGeod.

---
 simgear/magvar/magvar.cxx | 9 ++++++++-
 simgear/magvar/magvar.hxx | 4 ++++
 2 files changed, 12 insertions(+), 1 deletion(-)

diff --git a/simgear/magvar/magvar.cxx b/simgear/magvar/magvar.cxx
index 095c9718..2744e20e 100644
--- a/simgear/magvar/magvar.cxx
+++ b/simgear/magvar/magvar.cxx
@@ -50,7 +50,7 @@ void SGMagVar::update( double lon, double lat, double alt_m, double jd ) {
 
 void SGMagVar::update( const SGGeod& geod, double jd ) {
 
-  update(geod.getLongitudeDeg(), geod.getLatitudeDeg(),
+  update(geod.getLongitudeRad(), geod.getLatitudeRad(),
     geod.getElevationM(), jd);
 }
 
@@ -62,3 +62,10 @@ double sgGetMagVar( double lon, double lat, double alt_m, double jd ) {
     double field[6];
     return calc_magvar( lat, lon, alt_m / 1000.0, (long)jd, field );
 }
+
+double sgGetMagVar( const SGGeod& pos, double jd )
+{
+  return sgGetMagVar(pos.getLongitudeRad(), pos.getLatitudeRad(), 
+    pos.getElevationM(), jd);
+}
+
diff --git a/simgear/magvar/magvar.hxx b/simgear/magvar/magvar.hxx
index 6a97e5ea..605bece7 100644
--- a/simgear/magvar/magvar.hxx
+++ b/simgear/magvar/magvar.hxx
@@ -103,5 +103,9 @@ public:
  */
 double sgGetMagVar( double lon, double lat, double alt_m, double jd );
 
+/**
+ * overload version of the above to take a SGGeod
+ */
+double sgGetMagVar( const SGGeod& pos, double jd );
 
 #endif // _MAGVAR_HXX