Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 }
NpsSensorGyro::min
int min
Definition: nps_sensor_gyro.h:11
NpsSensorGyro
Definition: nps_sensor_gyro.h:9
nps_fdm.h
VECT3_ADD
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
FLOAT_MAT33_DIAG
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
Definition: pprz_algebra_float.h:234
double_vect3_update_random_walk
void double_vect3_update_random_walk(struct DoubleVect3 *rw, struct DoubleVect3 *std_dev, double dt, double thau)
Definition: nps_random.c:65
pprz_algebra_int.h
Paparazzi fixed point algebra.
DOUBLE_VECT3_ROUND
#define DOUBLE_VECT3_ROUND(_v)
Definition: pprz_algebra_double.h:91
VECT3_EW_MUL
#define VECT3_EW_MUL(_vo, _va, _vb)
Definition: pprz_algebra.h:217
NpsSensorGyro::bias_random_walk_value
struct DoubleVect3 bias_random_walk_value
Definition: nps_sensor_gyro.h:18
fdm
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
Definition: nps_fdm_crrcsim.c:84
nps_sensor_gyro_run_step
void nps_sensor_gyro_run_step(struct NpsSensorGyro *gyro, double time, struct DoubleRMat *body_to_imu)
Definition: nps_sensor_gyro.c:31
NpsSensorGyro::data_available
bool data_available
Definition: nps_sensor_gyro.h:20
nps_random.h
NpsSensorGyro::value
struct DoubleVect3 value
Definition: nps_sensor_gyro.h:10
FLOAT_VECT3_ZERO
#define FLOAT_VECT3_ZERO(_v)
Definition: pprz_algebra_float.h:161
NpsSensorGyro::sensitivity
struct DoubleMat33 sensitivity
Definition: nps_sensor_gyro.h:13
VECT3_BOUND_CUBE
#define VECT3_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:224
nps_sensor_gyro_init
void nps_sensor_gyro_init(struct NpsSensorGyro *gyro, double time)
Definition: nps_sensor_gyro.c:9
NpsSensorGyro::bias_random_walk_std_dev
struct DoubleVect3 bias_random_walk_std_dev
Definition: nps_sensor_gyro.h:17
nps_sensor_gyro.h
NpsSensorGyro::noise_std_dev
struct DoubleVect3 noise_std_dev
Definition: nps_sensor_gyro.h:15
NpsSensorGyro::neutral
struct DoubleVect3 neutral
Definition: nps_sensor_gyro.h:14
MAT33_VECT3_MUL
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:463
DoubleRMat
rotation matrix
Definition: pprz_algebra_double.h:69
body_to_imu
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
NpsSensorGyro::bias_initial
struct DoubleVect3 bias_initial
Definition: nps_sensor_gyro.h:16
NpsSensorGyro::max
int max
Definition: nps_sensor_gyro.h:12
NpsSensorGyro::next_update
double next_update
Definition: nps_sensor_gyro.h:19
DoubleVect3
Definition: pprz_algebra_double.h:46
DoubleMat33::m
double m[3 *3]
Definition: pprz_algebra_double.h:63
FALSE
#define FALSE
Definition: std.h:5
VECT3_ASSIGN
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
TRUE
#define TRUE
Definition: std.h:4
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
NpsFdm::body_inertial_rotvel
struct DoubleRates body_inertial_rotvel
Definition: nps_fdm.h:101
double_vect3_add_gaussian_noise
void double_vect3_add_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev)
Definition: nps_random.c:34