Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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.h"
7#include "nps_random.h"
8
9void nps_sensor_gyro_init(struct NpsSensorGyro *gyro, double time)
10{
12 gyro->min = NPS_GYRO_MIN;
13 gyro->max = NPS_GYRO_MAX;
15 NPS_GYRO_SIGN_P * NPS_GYRO_SENSITIVITY_PP,
16 NPS_GYRO_SIGN_Q * NPS_GYRO_SENSITIVITY_QQ,
17 NPS_GYRO_SIGN_R * NPS_GYRO_SENSITIVITY_RR);
19 NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R);
21 NPS_GYRO_NOISE_STD_DEV_P, NPS_GYRO_NOISE_STD_DEV_Q, NPS_GYRO_NOISE_STD_DEV_R);
23 NPS_GYRO_BIAS_INITIAL_P, NPS_GYRO_BIAS_INITIAL_Q, NPS_GYRO_BIAS_INITIAL_R);
25 NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P,
26 NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q,
27 NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R);
29 gyro->next_update = time;
30 gyro->data_available = FALSE;
31}
32
33void nps_sensor_gyro_run_step(struct NpsSensorGyro *gyro, double time, struct DoubleRMat *body_to_imu)
34{
35
36 if (time < gyro->next_update) {
37 return;
38 }
39
40 /* transform body rates to IMU frame */
41 struct DoubleVect3 *rate_body = (struct DoubleVect3 *)(&fdm.body_inertial_rotvel);
42 struct DoubleVect3 rate_imu;
43 MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body);
44 /* compute gyros readings */
45 MAT33_VECT3_MUL(gyro->value, gyro->sensitivity, rate_imu);
46 VECT3_ADD(gyro->value, gyro->neutral);
47 /* compute gyro error readings */
48 struct DoubleVect3 gyro_error;
49 VECT3_COPY(gyro_error, gyro->bias_initial);
52 NPS_GYRO_DT, 5.);
53 VECT3_ADD(gyro_error, gyro->bias_random_walk_value);
54
55 struct DoubleVect3 gain = {gyro->sensitivity.m[0], gyro->sensitivity.m[4], gyro->sensitivity.m[8]};
56 VECT3_EW_MUL(gyro_error, gyro_error, gain);
57
58 VECT3_ADD(gyro->value, gyro_error);
59
60 /* round signal to account for adc discretisation */
62 /* saturate */
63 VECT3_BOUND_CUBE(gyro->value, gyro->min, gyro->max);
64
65 gyro->next_update += NPS_GYRO_DT;
66 gyro->data_available = TRUE;
67}
#define DOUBLE_VECT3_ROUND(_v)
rotation matrix
#define FLOAT_VECT3_ZERO(_v)
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
#define VECT3_BOUND_CUBE(_v, _min, _max)
#define VECT3_EW_MUL(_vo, _va, _vb)
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define VECT3_COPY(_a, _b)
#define VECT3_ADD(_a, _b)
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_gyro_init(struct NpsSensorGyro *gyro, double time)
void nps_sensor_gyro_run_step(struct NpsSensorGyro *gyro, double time, struct DoubleRMat *body_to_imu)
struct DoubleVect3 bias_random_walk_value
struct DoubleVect3 noise_std_dev
struct DoubleMat33 sensitivity
struct DoubleVect3 bias_initial
struct DoubleVect3 neutral
struct DoubleVect3 bias_random_walk_std_dev
struct DoubleVect3 value
Paparazzi fixed point algebra.
#define TRUE
Definition std.h:4
#define FALSE
Definition std.h:5