Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading)
7 {
8  /* add new reading */
9  struct BoozDatedSensor *cur_read = g_new(struct BoozDatedSensor, 1);
10  cur_read->time = time;
11  cur_read->value = (struct DoubleVect3 *) g_memdup(cur_reading, sizeof(struct DoubleVect3));
12  *history = g_slist_prepend(*history, cur_read);
13  /* remove old readings */
14  GSList *last = g_slist_last(*history);
15  while (last && ((struct BoozDatedSensor *)last->data)->time < time - latency) {
16  *history = g_slist_remove_link(*history, last);
17  g_free(((struct BoozDatedSensor *)last->data)->value);
18  g_free((struct BoozDatedSensor *)last->data);
19  g_slist_free_1(last);
20  last = g_slist_last(*history);
21  }
22  /* update sensor */
23  //g_memmove((gpointer)sensor_reading, (gpointer)((struct BoozDatedSensor*)last->data)->value, sizeof(struct DoubleVect3));
24  VECT3_COPY(*((struct DoubleVect3 *)sensor_reading), *((struct BoozDatedSensor *)last->data)->value);
25 }
26 
27 void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **history, double latency,
28  gpointer sensor_reading)
29 {
30  /* add new reading */
31  struct BoozDatedSensor_Single *cur_read = g_new(struct BoozDatedSensor_Single, 1);
32  cur_read->time = time;
33  cur_read->value = (double *) g_memdup(cur_reading, sizeof(double));
34  *history = g_slist_prepend(*history, cur_read);
35  /* remove old readings */
36  GSList *last = g_slist_last(*history);
37  while (last && ((struct BoozDatedSensor_Single *)last->data)->time < time - latency) {
38  *history = g_slist_remove_link(*history, last);
39  g_free(((struct BoozDatedSensor_Single *)last->data)->value);
40  g_free((struct BoozDatedSensor_Single *)last->data);
41  g_slist_free_1(last);
42  last = g_slist_last(*history);
43  }
44 
45  /* update sensor */
46  //g_memmove((gpointer)sensor_reading, (gpointer)((struct BoozDatedSensor*)last->data)->value, sizeof(struct DoubleVect3));
47  *((double *)sensor_reading) = *(((struct BoozDatedSensor_Single *)last->data)->value);
48 
49 }
void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading)
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading)
Paparazzi generic algebra macros.
uint32_t last
Definition: rng_arch.c:31
struct DoubleVect3 * value