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_sensor_gps.c
Go to the documentation of this file.
1 #include "nps_sensor_gps.h"
2 
3 #include <stdio.h>
4 #include "generated/airframe.h"
5 #include "nps_fdm.h"
6 #include "nps_random.h"
7 #include "nps_sensors_utils.h"
8 #include NPS_SENSORS_PARAMS
9 
10 void nps_sensor_gps_init(struct NpsSensorGps *gps, double time)
11 {
14  gps->hmsl = 0.0;
15  gps->pos_latency = NPS_GPS_POS_LATENCY;
16  gps->speed_latency = NPS_GPS_SPEED_LATENCY;
18  NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV);
20  NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV);
22  NPS_GPS_POS_BIAS_INITIAL_X, NPS_GPS_POS_BIAS_INITIAL_Y, NPS_GPS_POS_BIAS_INITIAL_Z);
24  NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X,
25  NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y,
26  NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z);
28  gps->next_update = time;
29  gps->data_available = FALSE;
30 }
31 
32 /*
33  * WARNING!
34  * noise and bias is currently added in ECEF frame
35  * If noise or bias is not the same for all axes this has to be done in LTP!!
36  *
37  */
38 
39 void nps_sensor_gps_run_step(struct NpsSensorGps *gps, double time)
40 {
41  if (time < gps->next_update) {
42  return;
43  }
44 
45  /*
46  * simulate speed sensor
47  */
48  struct DoubleVect3 cur_speed_reading;
49  VECT3_COPY(cur_speed_reading, fdm.ecef_ecef_vel);
50  /* add a gaussian noise */
51  double_vect3_add_gaussian_noise(&cur_speed_reading, &gps->speed_noise_std_dev);
52 
53  /* store that for later and retrieve a previously stored data */
54  UpdateSensorLatency(time, &cur_speed_reading, &gps->speed_history, gps->speed_latency, &gps->ecef_vel);
55 
56 
57  /*
58  * simulate position sensor
59  */
60  /* compute gps error readings */
61  struct DoubleVect3 pos_error;
62  VECT3_COPY(pos_error, gps->pos_bias_initial);
63  /* add a gaussian noise */
65  /* update random walk bias and add it to error*/
67  VECT3_ADD(pos_error, gps->pos_bias_random_walk_value);
68 
69  /* add error to current pos reading */
70  struct DoubleVect3 cur_pos_reading;
71  VECT3_COPY(cur_pos_reading, fdm.ecef_pos);
72  VECT3_ADD(cur_pos_reading, pos_error);
73 
74  /* store that for later and retrieve a previously stored data */
75  UpdateSensorLatency(time, &cur_pos_reading, &gps->pos_history, gps->pos_latency, &gps->ecef_pos);
76 
77 
78  /*
79  * simulate lla pos
80  */
81  /* convert current ecef reading to lla */
82  struct LlaCoor_d cur_lla_reading;
83  lla_of_ecef_d(&cur_lla_reading, (struct EcefCoor_d *) &cur_pos_reading);
84 
85  /* store that for later and retrieve a previously stored data */
86  UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, gps->pos_latency, &gps->lla_pos);
87 
88  double cur_hmsl_reading = fdm.hmsl;
89  UpdateSensorLatency_Single(time, &cur_hmsl_reading, &gps->hmsl_history, gps->pos_latency, &gps->hmsl);
90 
91  gps->next_update += NPS_GPS_DT;
92 #ifndef NPS_NO_GPS
93  gps->data_available = TRUE;
94 #endif
95 }
96 
struct EcefCoor_d ecef_vel
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
GSList * hmsl_history
double next_update
struct DoubleVect3 pos_bias_random_walk_std_dev
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
struct EcefCoor_d ecef_pos
void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading)
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
GSList * lla_history
void double_vect3_update_random_walk(struct DoubleVect3 *rw, struct DoubleVect3 *std_dev, double dt, double thau)
Definition: nps_random.c:65
vector in Latitude, Longitude and Altitude
void double_vect3_add_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev)
Definition: nps_random.c:34
void nps_sensor_gps_run_step(struct NpsSensorGps *gps, double time)
void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading)
struct DoubleVect3 pos_noise_std_dev
#define FALSE
Definition: std.h:5
double hmsl
Definition: nps_fdm.h:56
struct EcefCoor_d ecef_ecef_vel
velocity in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:64
#define FLOAT_VECT3_ZERO(_v)
#define TRUE
Definition: std.h:4
struct DoubleVect3 pos_bias_initial
struct LlaCoor_d lla_pos
vector in EarthCenteredEarthFixed coordinates
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
double pos_latency
GSList * pos_history
struct DoubleVect3 pos_bias_random_walk_value
GSList * speed_history
double speed_latency
struct EcefCoor_d ecef_pos
Definition: nps_fdm.h:53
void nps_sensor_gps_init(struct NpsSensorGps *gps, double time)
struct GpsState gps
global GPS state
Definition: gps.c:75
struct DoubleVect3 speed_noise_std_dev