Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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.h"
9
10void nps_sensor_gps_init(struct NpsSensorGps *gps, double time)
11{
12 FLOAT_VECT3_ZERO(gps->ecef_pos);
13 FLOAT_VECT3_ZERO(gps->ecef_vel);
14 gps->hmsl = 0.0;
15 gps->pos_latency = NPS_GPS_POS_LATENCY;
16 gps->speed_latency = NPS_GPS_SPEED_LATENCY;
17 VECT3_ASSIGN(gps->pos_noise_std_dev,
18 NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV);
19 VECT3_ASSIGN(gps->speed_noise_std_dev,
20 NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV);
21 VECT3_ASSIGN(gps->pos_bias_initial,
22 NPS_GPS_POS_BIAS_INITIAL_X, NPS_GPS_POS_BIAS_INITIAL_Y, NPS_GPS_POS_BIAS_INITIAL_Z);
23 VECT3_ASSIGN(gps->pos_bias_random_walk_std_dev,
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);
27 FLOAT_VECT3_ZERO(gps->pos_bias_random_walk_value);
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
39void 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 */
64 double_vect3_add_gaussian_noise(&pos_error, &gps->pos_noise_std_dev);
65 /* update random walk bias and add it to error*/
66 double_vect3_update_random_walk(&gps->pos_bias_random_walk_value, &gps->pos_bias_random_walk_std_dev, NPS_GPS_DT, 5.);
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 GpsState gps
global GPS state
Definition gps.c:74
#define FLOAT_VECT3_ZERO(_v)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define VECT3_COPY(_a, _b)
#define VECT3_ADD(_a, _b)
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
void double_vect3_add_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev)
Definition nps_random.c:34
void double_vect3_update_random_walk(struct DoubleVect3 *rw, struct DoubleVect3 *std_dev, double dt, double thau)
Definition nps_random.c:65
void nps_sensor_gps_init(struct NpsSensorGps *gps, double time)
void nps_sensor_gps_run_step(struct NpsSensorGps *gps, double time)
void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading)
void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading)
#define TRUE
Definition std.h:4
#define FALSE
Definition std.h:5