Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
nps_sensors_utils.c
Go to the documentation of this file.
1 #include "nps_sensors_utils.h"
2 
3 //#include <string.h>
4 #include "math/pprz_algebra.h"
5 
6 // Compatibility with older glib
7 #if !GLIB_CHECK_VERSION(2, 68, 0)
8 #define g_memdup2 g_memdup
9 #endif
10 
11 void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading)
12 {
13  /* add new reading */
14  struct BoozDatedSensor *cur_read = g_new(struct BoozDatedSensor, 1);
15  cur_read->time = time;
16  cur_read->value = (struct DoubleVect3 *) g_memdup2(cur_reading, sizeof(struct DoubleVect3));
17  *history = g_slist_prepend(*history, cur_read);
18  /* remove old readings */
19  GSList *last = g_slist_last(*history);
20  while (last && ((struct BoozDatedSensor *)last->data)->time < time - latency) {
21  *history = g_slist_remove_link(*history, last);
22  g_free(((struct BoozDatedSensor *)last->data)->value);
23  g_free((struct BoozDatedSensor *)last->data);
24  g_slist_free_1(last);
25  last = g_slist_last(*history);
26  }
27  /* update sensor */
28  //g_memmove((gpointer)sensor_reading, (gpointer)((struct BoozDatedSensor*)last->data)->value, sizeof(struct DoubleVect3));
29  VECT3_COPY(*((struct DoubleVect3 *)sensor_reading), *((struct BoozDatedSensor *)last->data)->value);
30 }
31 
32 void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **history, double latency,
33  gpointer sensor_reading)
34 {
35  /* add new reading */
36  struct BoozDatedSensor_Single *cur_read = g_new(struct BoozDatedSensor_Single, 1);
37  cur_read->time = time;
38  cur_read->value = (double *) g_memdup2(cur_reading, sizeof(double));
39  *history = g_slist_prepend(*history, cur_read);
40  /* remove old readings */
41  GSList *last = g_slist_last(*history);
42  while (last && ((struct BoozDatedSensor_Single *)last->data)->time < time - latency) {
43  *history = g_slist_remove_link(*history, last);
44  g_free(((struct BoozDatedSensor_Single *)last->data)->value);
45  g_free((struct BoozDatedSensor_Single *)last->data);
46  g_slist_free_1(last);
47  last = g_slist_last(*history);
48  }
49 
50  /* update sensor */
51  //g_memmove((gpointer)sensor_reading, (gpointer)((struct BoozDatedSensor*)last->data)->value, sizeof(struct DoubleVect3));
52  *((double *)sensor_reading) = *(((struct BoozDatedSensor_Single *)last->data)->value);
53 
54 }
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading)
#define g_memdup2
void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading)
struct DoubleVect3 * value
Paparazzi generic algebra macros.
uint32_t last
Definition: rng_arch.c:31