[paparazzi-commits] [6156] use geodetic latitude in nps and set initial pos with correct geocentric lat

[Felix Ruess]

Revision: 6156
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6156
Author:   flixr
Date:     2010-10-13 19:53:20 +0000 (Wed, 13 Oct 2010)
Log Message:
-----------
use geodetic latitude in nps and set initial pos with correct geocentric lat

Modified Paths:
--------------
    paparazzi3/trunk/conf/simulator/jsbsim/aircraft/reset00.xml
    paparazzi3/trunk/sw/simulator/nps/nps_fdm_jsbsim.c

Modified: paparazzi3/trunk/conf/simulator/jsbsim/aircraft/reset00.xml
===================================================================
--- paparazzi3/trunk/conf/simulator/jsbsim/aircraft/reset00.xml 2010-10-13 
19:53:12 UTC (rev 6155)
+++ paparazzi3/trunk/conf/simulator/jsbsim/aircraft/reset00.xml 2010-10-13 
19:53:20 UTC (rev 6156)
@@ -6,13 +6,14 @@
   <phi   unit="DEG"> 0.0 </phi>
   <theta unit="DEG"> 0.0 </theta>
   <psi   unit="DEG"> 0.0 </psi>
-<!-- SF geocentric ? -->
-<!--  <latitude  unit="DEG">  37.557 </latitude> -->
-<!-- SF geodetic latitude -->
-  <latitude  unit="DEG">  37.6136 </latitude>
+<!-- latitude in geocentric coordinates!! -->
+  <latitude  unit="DEG">  37.427895 </latitude>
+  <!--<latitude  unit="DEG">  37.6136 </latitude> -->
   <longitude unit="DEG">-122.3569 </longitude>
+<!-- altitude is above ground level AGL -->
 <!--  <altitude  unit="M"> 0.11 </altitude> -->
- <altitude  unit="M"> 2.5 </altitude>
+<!-- altitudeMSL is above sea level ASL -->
+  <altitudeMSL unit="M"> 2.5 </altitudeMSL>
 <!--  <altitude  unit="M">95 </altitude> -->
   <winddir unit="DEG"> 0.0 </winddir>
   <vwind unit="FT/SEC"> 0.0 </vwind>

Modified: paparazzi3/trunk/sw/simulator/nps/nps_fdm_jsbsim.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_fdm_jsbsim.c  2010-10-13 19:53:12 UTC 
(rev 6155)
+++ paparazzi3/trunk/sw/simulator/nps/nps_fdm_jsbsim.c  2010-10-13 19:53:20 UTC 
(rev 6156)
@@ -23,9 +23,11 @@
 static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, const FGLocation* 
jsb_location);
 static void jsbsimquat_to_quat(DoubleQuat* fdm_quat, const FGQuaternion* 
jsb_quat);
 static void jsbsimvec_to_rate(DoubleRates* fdm_rate, const FGColumnVector3* 
jsb_vector);
-static void jsbsimloc_to_lla(LlaCoor_d* fdm_lla, FGPropagate* propagate);
+static void llh_from_jsbsim(LlaCoor_d* fdm_lla, FGPropagate* propagate);
+static void lla_from_jsbsim_geodetic(LlaCoor_d* fdm_lla, FGPropagate* 
propagate);
+static void lla_from_jsbsim_geocentric(LlaCoor_d* fdm_lla, FGPropagate* 
propagate);
 //static void rate_to_vec(DoubleVect3* vector, DoubleRates* rate);
-static void test123(LlaCoor_d* fdm_lla, FGPropagate* propagate);
+//static void test123(LlaCoor_d* fdm_lla, FGPropagate* propagate);
 
 static void init_jsbsim(double dt);
 static void init_ltp(void);
@@ -114,8 +116,8 @@
   ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, &ltpdef, &fdm.ecef_ecef_vel);
   ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, &ltpdef, &fdm.ecef_ecef_accel);
 
-  /* lla */
-  jsbsimloc_to_lla(&fdm.lla_pos, propagate);
+  /* llh */
+  llh_from_jsbsim(&fdm.lla_pos, propagate);
   //test123(&fdm.lla_pos, propagate);
 
 
@@ -139,18 +141,6 @@
 
 }
 
-static void test123(LlaCoor_d* fdm_lla, FGPropagate* propagate) {
-
-  fdm_lla->lat = propagate->GetLatitude();
-  fdm_lla->lon = propagate->GetLongitude();
-
-  //  fdm_lla->lat = propagate->GetGeodLatitudeRad();
-
-  // FIXME
-  fdm_lla->alt = MetersOfFeet(propagate->GetDistanceAGL());
-
-}
-
 static void init_jsbsim(double dt) {
 
   char buf[1024];
@@ -241,6 +231,29 @@
 
 }
 
+//longitude and geodetic latitude in rad and height above sea level in m
+void llh_from_jsbsim(LlaCoor_d* fdm_lla, FGPropagate* propagate) {
+  fdm_lla->lat = propagate->GetGeodLatitudeRad();
+  fdm_lla->lon = propagate->GetLongitude();
+  fdm_lla->alt = MetersOfFeet(propagate->GetAltitudeASLmeters());
+  //printf("geodetic alt: %f\n", 
MetersOfFeet(propagate->GetGeodeticAltitude()));
+  //printf("ground alt: %f\n", MetersOfFeet(propagate->GetDistanceAGL()));
+  //printf("ASL alt: %f\n", MetersOfFeet(propagate->GetAltitudeASLmeters()));
+}
+
+void lla_from_jsbsim_geocentric(LlaCoor_d* fdm_lla, FGPropagate* propagate) {
+  fdm_lla->lat = propagate->GetLatitude();
+  fdm_lla->lon = propagate->GetLongitude();
+  fdm_lla->alt = MetersOfFeet(propagate->GetRadius());
+}
+
+void lla_from_jsbsim_geodetic(LlaCoor_d* fdm_lla, FGPropagate* propagate) {
+  fdm_lla->lat = propagate->GetGeodLatitudeRad();
+  fdm_lla->lon = propagate->GetLongitude();
+  fdm_lla->alt = MetersOfFeet(propagate->GetGeodeticAltitude());
+}
+
+
 #if 0
 static void rate_to_vec(DoubleVect3* vector, DoubleRates* rate) {
 
@@ -249,16 +262,16 @@
   vector->z = rate->r;
 
 }
-#endif
 
-void jsbsimloc_to_lla(LlaCoor_d* fdm_lla, FGPropagate* propagate) {
+static void test123(LlaCoor_d* fdm_lla, FGPropagate* propagate) {
 
-  fdm_lla->lat = propagate->GetGeodLatitudeRad();
+  fdm_lla->lat = propagate->GetLatitude();
   fdm_lla->lon = propagate->GetLongitude();
+
+  //  fdm_lla->lat = propagate->GetGeodLatitudeRad();
+
+  // FIXME
   fdm_lla->alt = MetersOfFeet(propagate->GetDistanceAGL());
-  //printf("geodetic alt: %f\n", 
MetersOfFeet(propagate->GetGeodeticAltitude()));
-  //printf("ground alt: %f\n", MetersOfFeet(propagate->GetDistanceAGL()));
-  //printf("ASL alt: %f\n", MetersOfFeet(propagate->GetAltitudeASLmeters()));
 
 }
-
+#endif