[paparazzi-commits] [5920] fixed bug, which results from awful pointer conversions.

[Martin Dieblich]

Revision: 5920
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5920
Author:   mdieblich
Date:     2010-09-22 16:21:07 +0000 (Wed, 22 Sep 2010)
Log Message:
-----------
fixed bug, which results from awful pointer conversions.

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

Modified: paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.c  2010-09-22 15:36:55 UTC 
(rev 5919)
+++ paparazzi3/trunk/sw/simulator/nps/nps_sensor_gps.c  2010-09-22 16:21:07 UTC 
(rev 5920)
@@ -85,7 +85,7 @@
   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);
+  UpdateSensorLatency_Single(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_sensors_utils.c
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_sensors_utils.c       2010-09-22 
15:36:55 UTC (rev 5919)
+++ paparazzi3/trunk/sw/simulator/nps/nps_sensors_utils.c       2010-09-22 
16:21:07 UTC (rev 5920)
@@ -23,3 +23,25 @@
   //g_memmove((gpointer)sensor_reading, (gpointer)((struct 
BoozDatedSensor*)last->data)->value, sizeof(struct DoubleVect3));
   VECT3_COPY(*((struct DoubleVect3*)sensor_reading), *((struct 
BoozDatedSensor*)last->data)->value);
 }
+
+void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList 
**history, double latency, gpointer sensor_reading) {
+  /* add new reading */
+  struct BoozDatedSensor_Single* cur_read = g_new(struct 
BoozDatedSensor_Single, 1);
+  cur_read->time = time;
+  cur_read->value = (double*) g_memdup(cur_reading, sizeof(double));
+  *history = g_slist_prepend(*history, cur_read);
+  /* remove old readings */
+  GSList* last =  g_slist_last(*history);
+  while (last && ((struct BoozDatedSensor_Single*)last->data)->time < time - 
latency) {
+    *history = g_slist_remove_link(*history, last);
+       g_free(((struct BoozDatedSensor_Single*)last->data)->value);
+       g_free((struct BoozDatedSensor_Single*)last->data);
+    g_slist_free_1(last);
+    last = g_slist_last(*history);
+  }
+
+  /* update sensor        */
+  //g_memmove((gpointer)sensor_reading, (gpointer)((struct 
BoozDatedSensor*)last->data)->value, sizeof(struct DoubleVect3));
+  *((double*)sensor_reading) = *(((struct 
BoozDatedSensor_Single*)last->data)->value);
+
+}

Modified: paparazzi3/trunk/sw/simulator/nps/nps_sensors_utils.h
===================================================================
--- paparazzi3/trunk/sw/simulator/nps/nps_sensors_utils.h       2010-09-22 
15:36:55 UTC (rev 5919)
+++ paparazzi3/trunk/sw/simulator/nps/nps_sensors_utils.h       2010-09-22 
16:21:07 UTC (rev 5920)
@@ -9,8 +9,17 @@
   double time;
 };
 
+struct BoozDatedSensor_Single {
+  double* value;
+  double time;
+};
+
 /* cur_reading and sensor_reading must be of a type that can be cast to 
DoubleVect3* */
 extern void UpdateSensorLatency(double time, gpointer cur_reading, GSList 
**history,
                                                                double latency, 
gpointer sensor_reading);
 
+/* ...and the same for single double values */
+extern void UpdateSensorLatency_Single(double time, gpointer cur_reading, 
GSList **history,
+                                                               double latency, 
gpointer sensor_reading);
+
 #endif /* NPS_SENSORS_UTILS_H */