[paparazzi-commits] [4993] nps: add hmsl to gps

[Felix Ruess]

Revision: 4993
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4993
Author:   flixr
Date:     2010-06-20 23:41:10 +0000 (Sun, 20 Jun 2010)
Log Message:
-----------
nps: add hmsl to gps

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/booz2_gps.h
    paparazzi3/trunk/sw/airborne/math/pprz_geodetic_int.h
    paparazzi3/trunk/sw/simulator/nps/nps_fdm.h
    paparazzi3/trunk/sw/simulator/nps/nps_fdm_jsbsim.c
    paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.c
    paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.h

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_gps.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_gps.h       2010-06-20 22:03:25 UTC 
(rev 4992)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_gps.h       2010-06-20 23:41:10 UTC 
(rev 4993)
@@ -69,8 +69,8 @@
   booz_gps_state.ecef_vel.z = sensors.gps.ecef_vel.z * 100.;
   booz_gps_state.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7;
   booz_gps_state.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7;
-  booz_gps_state.lla_pos.alt = sensors.gps.lla_pos.alt * 100. + NAV_HMSL0;
-  booz_gps_state.hmsl        = sensors.gps.lla_pos.alt * 100.;
+  booz_gps_state.lla_pos.alt = sensors.gps.lla_pos.alt * 100.;
+  booz_gps_state.hmsl        = sensors.gps.hmsl * 100.;
   booz_gps_state.fix = BOOZ2_GPS_FIX_3D;
   booz_gps_available = TRUE;
 }

Modified: paparazzi3/trunk/sw/airborne/math/pprz_geodetic_int.h
===================================================================
--- paparazzi3/trunk/sw/airborne/math/pprz_geodetic_int.h       2010-06-20 
22:03:25 UTC (rev 4992)
+++ paparazzi3/trunk/sw/airborne/math/pprz_geodetic_int.h       2010-06-20 
23:41:10 UTC (rev 4993)
@@ -15,8 +15,8 @@
   int32_t z;
 };
 
-/* lon, lat in radians */
-/* alt in meters       */
+/* lon, lat in radians*1e7  */
+/* alt in centimeters       */
 struct LlaCoor_i {
   int32_t lon;
   int32_t lat;

Modified: paparazzi3/trunk/sw/simulator/nps/nps_fdm.h
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_fdm.h 2010-06-20 22:03:25 UTC (rev 
4992)
+++ paparazzi3/trunk/sw/simulator/nps/nps_fdm.h 2010-06-20 23:41:10 UTC (rev 
4993)
@@ -25,6 +25,7 @@
   struct EcefCoor_d  ecef_pos;
   struct NedCoor_d ltpprz_pos;
   struct LlaCoor_d lla_pos;
+  double hmsl;
 
   /*  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-06-20 22:03:25 UTC 
(rev 4992)
+++ paparazzi3/trunk/sw/simulator/nps/nps_fdm_jsbsim.c  2010-06-20 23:41:10 UTC 
(rev 4993)
@@ -82,6 +82,7 @@
    * position
    */
   jsbsimloc_to_loc(&fdm.ecef_pos,&propagate->GetLocation());
+  fdm.hmsl = propagate->GetAltitudeASLmeters();
 
   /*
    * linear speed and accelerations
@@ -247,9 +248,9 @@
 
 void jsbsimloc_to_lla(LlaCoor_d* fdm_lla, FGLocation* jsb_location) {
 
-  fdm_lla->lat = jsb_location->GetLatitude();
+  fdm_lla->lat = jsb_location->GetGeodLatitudeRad();
   fdm_lla->lon = jsb_location->GetLongitude();
-  fdm_lla->alt = MetersOfFeet(jsb_location->GetGeodAltitude());
+  fdm_lla->alt = MetersOfFeet(jsb_location->GetGeodeticAltitude());
   //  printf("%f\n", jsb_location->GetGeodAltitude());
 }
 #endif

Modified: paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.c  2010-06-20 22:03:25 UTC 
(rev 4992)
+++ paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.c  2010-06-20 23:41:10 UTC 
(rev 4993)
@@ -10,6 +10,7 @@
 void nps_sensor_gps_init(struct NpsSensorGps* gps, double time) {
   FLOAT_VECT3_ZERO(gps->ecef_pos);
   FLOAT_VECT3_ZERO(gps->ecef_vel);
+  gps->hmsl = 0.0;
   gps->pos_latency = NPS_GPS_POS_LATENCY;
   gps->speed_latency = NPS_GPS_SPEED_LATENCY;
   VECT3_ASSIGN(gps->pos_noise_std_dev,
@@ -83,6 +84,8 @@
   /* store that for later and retrieve a previously stored data */
   UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, 
gps->pos_latency, &gps->lla_pos);
 
+  double cur_hmsl_reading = fdm.hmsl;
+  UpdateSensorLatency(time, &cur_hmsl_reading, &gps->hmsl_history, 
gps->pos_latency, &gps->hmsl);
 
   gps->next_update += NPS_GPS_DT;
   gps->data_available = TRUE;

Modified: paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.h
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.h  2010-06-20 22:03:25 UTC 
(rev 4992)
+++ paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.h  2010-06-20 23:41:10 UTC 
(rev 4993)
@@ -14,6 +14,7 @@
   struct EcefCoor_d ecef_pos;
   struct EcefCoor_d ecef_vel;
   struct LlaCoor_d  lla_pos;
+  double hmsl;
   struct DoubleVect3  pos_noise_std_dev;
   struct DoubleVect3  speed_noise_std_dev;
   struct DoubleVect3  pos_bias_initial;
@@ -21,11 +22,12 @@
   struct DoubleVect3  pos_bias_random_walk_value;
   double pos_latency;
   double speed_latency;
+  GSList* hmsl_history;
   GSList* pos_history;
   GSList* lla_history;
   GSList* speed_history;
-  double       next_update;
-  bool_t       data_available;
+  double next_update;
+  bool_t data_available;
 };