[paparazzi-commits] [6157] add some debug variables/message for geodetic/ geocentric lla coords in jsbsim

[Felix Ruess]

Revision: 6157
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6157
Author:   flixr
Date:     2010-10-13 19:53:30 +0000 (Wed, 13 Oct 2010)
Log Message:
-----------
add some debug variables/message for geodetic/geocentric lla coords in jsbsim

Modified Paths:
--------------
    paparazzi3/trunk/conf/messages.xml
    paparazzi3/trunk/sw/simulator/nps/nps_fdm.h
    paparazzi3/trunk/sw/simulator/nps/nps_fdm_jsbsim.c
    paparazzi3/trunk/sw/simulator/nps/nps_ivy.c

Modified: paparazzi3/trunk/conf/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml  2010-10-13 19:53:20 UTC (rev 6156)
+++ paparazzi3/trunk/conf/messages.xml  2010-10-13 19:53:30 UTC (rev 6157)
@@ -1597,8 +1597,19 @@
 
  <!--236 is free -->
  <!--237 is free -->
- <!--238 is free -->
 
+  <message name="BOOZ_SIM_POS_LLH" id="238">
+    <field name="pprz_lat"   type="float" unit="rad" alt_unit="deg" 
alt_unit_coef="57.29578"/>
+    <field name="lat_geod"   type="float" unit="rad" alt_unit="deg" 
alt_unit_coef="57.29578"/>
+    <field name="lat_geoc"   type="float" unit="rad" alt_unit="deg" 
alt_unit_coef="57.29578"/>
+    <field name="pprz_lon"   type="float" unit="rad" alt_unit="deg" 
alt_unit_coef="57.29578"/>
+    <field name="lon"        type="float" unit="rad" alt_unit="deg" 
alt_unit_coef="57.29578"/>
+    <field name="pprz_alt"   type="float" unit="m"/>
+    <field name="alt_geod"   type="float" unit="m"/>
+    <field name="agl"        type="float" unit="m"/>
+    <field name="asl"        type="float" unit="m"/>
+  </message>
+
   <message name="BOOZ_SIM_RPMS" id="239">
     <field name="front"    type="float" unit="RPM"/>
     <field name="back"     type="float" unit="RPM"/>

Modified: paparazzi3/trunk/sw/simulator/nps/nps_fdm.h
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_fdm.h 2010-10-13 19:53:20 UTC (rev 
6156)
+++ paparazzi3/trunk/sw/simulator/nps/nps_fdm.h 2010-10-13 19:53:30 UTC (rev 
6157)
@@ -26,6 +26,11 @@
   struct NedCoor_d ltpprz_pos;
   struct LlaCoor_d lla_pos;
   double hmsl;
+  // for debugging
+  struct LlaCoor_d lla_pos_pprz; //lla converted by pprz from ecef
+  struct LlaCoor_d lla_pos_geod; //geodetic lla from jsbsim
+  struct LlaCoor_d lla_pos_geoc; //geocentric lla from jsbsim
+  double agl; //AGL from jsbsim in m
 
   /*  velocity and acceleration wrt inertial frame expressed in ecef frame */
   //  struct EcefCoor_d  ecef_inertial_vel;

Modified: paparazzi3/trunk/sw/simulator/nps/nps_fdm_jsbsim.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_fdm_jsbsim.c  2010-10-13 19:53:20 UTC 
(rev 6156)
+++ paparazzi3/trunk/sw/simulator/nps/nps_fdm_jsbsim.c  2010-10-13 19:53:30 UTC 
(rev 6157)
@@ -120,7 +120,13 @@
   llh_from_jsbsim(&fdm.lla_pos, propagate);
   //test123(&fdm.lla_pos, propagate);
 
+  //for debug
+  lla_from_jsbsim_geodetic(&fdm.lla_pos_geod, propagate);
+  lla_from_jsbsim_geocentric(&fdm.lla_pos_geoc, propagate);
+  lla_of_ecef_d(&fdm.lla_pos_pprz, &fdm.ecef_pos);
+  fdm.agl = MetersOfFeet(propagate->GetDistanceAGL());
 
+
   /*
    * attitude
    */

Modified: paparazzi3/trunk/sw/simulator/nps/nps_ivy.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_ivy.c 2010-10-13 19:53:20 UTC (rev 
6156)
+++ paparazzi3/trunk/sw/simulator/nps/nps_ivy.c 2010-10-13 19:53:30 UTC (rev 
6157)
@@ -122,6 +122,17 @@
             DegOfRad(fdm.ltp_to_body_eulers.phi),
             DegOfRad(fdm.ltp_to_body_eulers.theta),
             DegOfRad(fdm.ltp_to_body_eulers.psi));
+  IvySendMsg("%d BOOZ_SIM_POS_LLH %f %f %f %f %f %f %f %f %f",
+             AC_ID,
+             (fdm.lla_pos_pprz.lat),
+             (fdm.lla_pos_geod.lat),
+             (fdm.lla_pos_geoc.lat),
+             (fdm.lla_pos_pprz.lon),
+             (fdm.lla_pos_geod.lon),
+             (fdm.lla_pos_pprz.alt),
+             (fdm.lla_pos_geod.alt),
+             (fdm.agl),
+             (fdm.hmsl));
   IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f %f %f %f",
             AC_ID,
             (fdm.ltpprz_ecef_accel.x),