Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nps_sensor_gyro.c
Go to the documentation of this file.
1 #include "nps_sensor_gyro.h"
2 
3 #include "generated/airframe.h"
4 #include "nps_fdm.h"
5 #include NPS_SENSORS_PARAMS
7 #include "nps_random.h"
8 
9 void nps_sensor_gyro_init(struct NpsSensorGyro *gyro, double time)
10 {
11  FLOAT_VECT3_ZERO(gyro->value);
12  gyro->min = NPS_GYRO_MIN;
13  gyro->max = NPS_GYRO_MAX;
15  NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR);
16  VECT3_ASSIGN(gyro->neutral,
17  NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R);
19  NPS_GYRO_NOISE_STD_DEV_P, NPS_GYRO_NOISE_STD_DEV_Q, NPS_GYRO_NOISE_STD_DEV_R);
21  NPS_GYRO_BIAS_INITIAL_P, NPS_GYRO_BIAS_INITIAL_Q, NPS_GYRO_BIAS_INITIAL_R);
23  NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P,
24  NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q,
25  NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R);
27  gyro->next_update = time;
28  gyro->data_available = FALSE;
29 }
30 
31 void nps_sensor_gyro_run_step(struct NpsSensorGyro *gyro, double time, struct DoubleRMat *body_to_imu)
32 {
33 
34  if (time < gyro->next_update) {
35  return;
36  }
37 
38  /* transform body rates to IMU frame */
39  struct DoubleVect3 *rate_body = (struct DoubleVect3 *)(&fdm.body_inertial_rotvel);
40  struct DoubleVect3 rate_imu;
41  MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body);
42  /* compute gyros readings */
43  MAT33_VECT3_MUL(gyro->value, gyro->sensitivity, rate_imu);
44  VECT3_ADD(gyro->value, gyro->neutral);
45  /* compute gyro error readings */
46  struct DoubleVect3 gyro_error;
47  VECT3_COPY(gyro_error, gyro->bias_initial);
50  NPS_GYRO_DT, 5.);
51  VECT3_ADD(gyro_error, gyro->bias_random_walk_value);
52 
53  struct DoubleVect3 gain = {gyro->sensitivity.m[0], gyro->sensitivity.m[4], gyro->sensitivity.m[8]};
54  VECT3_EW_MUL(gyro_error, gyro_error, gain);
55 
56  VECT3_ADD(gyro->value, gyro_error);
57 
58  /* round signal to account for adc discretisation */
60  /* saturate */
61  VECT3_BOUND_CUBE(gyro->value, gyro->min, gyro->max);
62 
63  gyro->next_update += NPS_GYRO_DT;
64  gyro->data_available = TRUE;
65 }
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:146
struct DoubleVect3 bias_initial
#define VECT3_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:223
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
struct DoubleVect3 noise_std_dev
void nps_sensor_gyro_run_step(struct NpsSensorGyro *gyro, double time, struct DoubleRMat *body_to_imu)
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:139
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:124
void double_vect3_update_random_walk(struct DoubleVect3 *rw, struct DoubleVect3 *std_dev, double dt, double thau)
Definition: nps_random.c:65
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
struct DoubleRates body_inertial_rotvel
Definition: nps_fdm.h:101
void double_vect3_add_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev)
Definition: nps_random.c:34
struct DoubleVect3 bias_random_walk_std_dev
#define FALSE
Definition: std.h:5
#define FLOAT_VECT3_ZERO(_v)
void nps_sensor_gyro_init(struct NpsSensorGyro *gyro, double time)
#define TRUE
Definition: std.h:4
rotation matrix
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:462
struct DoubleVect3 bias_random_walk_value
#define DOUBLE_VECT3_ROUND(_v)
struct DoubleMat33 sensitivity
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
double m[3 *3]
struct DoubleVect3 value
#define VECT3_EW_MUL(_vo, _va, _vb)
Definition: pprz_algebra.h:216
Paparazzi fixed point algebra.
struct DoubleVect3 neutral