From a08b8f58538e007074214445f120052969add539 Mon Sep 17 00:00:00 2001
From: Bertrand Coconnier <bcoconni@users.sourceforge.net>
Date: Sun, 28 Aug 2016 12:23:17 +0200
Subject: [PATCH] Fixed the initial location so that geodetic latitude and ASL
 are kept in sync.

---
 src/FDM/JSBSim/JSBSim.cxx                     | 24 ++++++++-----------
 .../initialization/FGInitialCondition.cpp     | 11 +++++++++
 .../initialization/FGInitialCondition.h       |  6 +++++
 3 files changed, 27 insertions(+), 14 deletions(-)

diff --git a/src/FDM/JSBSim/JSBSim.cxx b/src/FDM/JSBSim/JSBSim.cxx
index 167c3c249..745f9ad97 100644
--- a/src/FDM/JSBSim/JSBSim.cxx
+++ b/src/FDM/JSBSim/JSBSim.cxx
@@ -1023,16 +1023,17 @@ void FGJSBsim::set_Latitude(double lat)
   SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::set_Latitude: " << lat );
   SG_LOG(SG_FLIGHT,SG_INFO," cur alt (ft) =  " << alt );
 
-  sgGeodToGeoc( lat, alt * SG_FEET_TO_METER,
-                    &sea_level_radius_meters, &lat_geoc );
+  if (needTrim)
+    fgic->SetGeodLatitudeRadIC( lat );
+  else {
+    sgGeodToGeoc( lat, alt * SG_FEET_TO_METER,
+                  &sea_level_radius_meters, &lat_geoc );
 
-  double sea_level_radius_ft = sea_level_radius_meters * SG_METER_TO_FEET;
-  _set_Sea_level_radius( sea_level_radius_ft );
+    double sea_level_radius_ft = sea_level_radius_meters * SG_METER_TO_FEET;
+    _set_Sea_level_radius( sea_level_radius_ft );
 
-  if (needTrim)
-    fgic->SetLatitudeRadIC( lat_geoc );
-  else
     Propagate->SetLatitude(lat_geoc);
+  }
 
   FGInterface::set_Latitude(lat);
 }
@@ -1055,13 +1056,8 @@ void FGJSBsim::set_Altitude(double alt)
 {
   SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Altitude: " << alt );
 
-  if (needTrim) {
-    FGLocation position = fgic->GetPosition();
-
-    position.SetPositionGeodetic(0.0, position.GetGeodLatitudeRad(), alt);
-    fgic->SetAltitudeASLFtIC(position.GetAltitudeASL());
-//  fgic->SetLatitudeRadIC(position.GetLatitude());
-  }
+  if (needTrim)
+    fgic->SetAltitudeASLFtIC(alt);
   else
     Propagate->SetAltitudeASL(alt);
 
diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp
index c5eea1a2f..d71317ce1 100644
--- a/src/FDM/JSBSim/initialization/FGInitialCondition.cpp
+++ b/src/FDM/JSBSim/initialization/FGInitialCondition.cpp
@@ -739,6 +739,17 @@ void FGInitialCondition::SetAltitudeASLFtIC(double alt)
 
 //******************************************************************************
 
+void FGInitialCondition::SetGeodLatitudeRadIC(double geodLatitude)
+{
+  double h = ComputeGeodAltitude(geodLatitude);
+  double lon = position.GetLongitude();
+
+  position.SetPositionGeodetic(lon, geodLatitude, h);
+  lastLatitudeSet = setgeod;
+}
+
+//******************************************************************************
+
 void FGInitialCondition::SetLatitudeRadIC(double lat)
 {
   double altitude;
diff --git a/src/FDM/JSBSim/initialization/FGInitialCondition.h b/src/FDM/JSBSim/initialization/FGInitialCondition.h
index c5428b7ad..02af89c06 100644
--- a/src/FDM/JSBSim/initialization/FGInitialCondition.h
+++ b/src/FDM/JSBSim/initialization/FGInitialCondition.h
@@ -592,6 +592,12 @@ public:
       @param lat Initial latitude in radians */
   void SetLatitudeRadIC(double lat);
 
+  /** Sets the initial geodetic latitude.
+      This method modifies the geodetic altitude in order to keep the altitude
+      above sea level unchanged.
+      @param glat Initial geodetic latitude in radians */
+  void SetGeodLatitudeRadIC(double glat);
+
   /** Sets the initial longitude.
       @param lon Initial longitude in radians */
   void SetLongitudeRadIC(double lon);
-- 
2.39.5