[paparazzi-commits] [4416] fix lla_pos in nps

[Felix Ruess]

Revision: 4416
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4416
Author:   flixr
Date:     2010-01-08 18:03:45 +0000 (Fri, 08 Jan 2010)
Log Message:
-----------
fix lla_pos in nps

Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.c
    paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.h

Modified: paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.c  2010-01-08 08:59:34 UTC 
(rev 4415)
+++ paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.c  2010-01-08 18:03:45 UTC 
(rev 4416)
@@ -73,6 +73,17 @@
   UpdateSensorLatency(time, &cur_pos_reading, &gps->pos_history, 
gps->pos_latency, &gps->ecef_pos);
 
 
+  /*
+   * simulate lla pos
+   */
+  /* convert current ecef reading to lla */
+  struct LlaCoor_d cur_lla_reading;
+  lla_of_ecef_d(&cur_lla_reading, (EcefCoor_d*) &cur_pos_reading);
+
+  /* store that for later and retrieve a previously stored data */
+  UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, 
gps->pos_latency, &gps->lla_pos);
+
+
   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-01-08 08:59:34 UTC 
(rev 4415)
+++ paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.h  2010-01-08 18:03:45 UTC 
(rev 4416)
@@ -13,6 +13,7 @@
 struct NpsSensorGps {
   struct EcefCoor_d ecef_pos;
   struct EcefCoor_d ecef_vel;
+  struct LlaCoor_d  lla_pos;
   struct DoubleVect3  pos_noise_std_dev;
   struct DoubleVect3  speed_noise_std_dev;
   struct DoubleVect3  pos_bias_initial;
@@ -21,6 +22,7 @@
   double pos_latency;
   double speed_latency;
   GSList* pos_history;
+  GSList* lla_history;
   GSList* speed_history;
   double       next_update;
   bool_t       data_available;