Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
nps_sensor_accel.c
Go to the documentation of this file.
1 #include "nps_sensor_accel.h"
2 
3 #include "nps_fdm.h"
4 #include "nps_random.h"
5 #include "nps_sensors.h"
7 
8 void nps_sensor_accel_init(struct NpsSensorAccel *accel, double time)
9 {
10  FLOAT_VECT3_ZERO(accel->value);
11  accel->min = NPS_ACCEL_MIN;
12  accel->max = NPS_ACCEL_MAX;
14  NPS_ACCEL_SIGN_X * NPS_ACCEL_SENSITIVITY_XX,
15  NPS_ACCEL_SIGN_Y * NPS_ACCEL_SENSITIVITY_YY,
16  NPS_ACCEL_SIGN_Z * NPS_ACCEL_SENSITIVITY_ZZ);
17  VECT3_ASSIGN(accel->neutral,
18  NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z);
20  NPS_ACCEL_NOISE_STD_DEV_X, NPS_ACCEL_NOISE_STD_DEV_Y, NPS_ACCEL_NOISE_STD_DEV_Z);
21  VECT3_ASSIGN(accel->bias,
22  NPS_ACCEL_BIAS_X, NPS_ACCEL_BIAS_Y, NPS_ACCEL_BIAS_Z);
23  accel->next_update = time;
24  accel->data_available = FALSE;
25 }
26 
27 void nps_sensor_accel_run_step(struct NpsSensorAccel *accel, double time, struct DoubleRMat *body_to_imu)
28 {
29  if (time < accel->next_update) {
30  return;
31  }
32 
33  /* transform to imu frame */
34  struct DoubleVect3 accelero_imu;
35  MAT33_VECT3_MUL(accelero_imu, *body_to_imu, fdm.body_accel);
36 
37  /* compute accelero readings */
38  MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu);
39  VECT3_ADD(accel->value, accel->neutral);
40 
41  /* Compute sensor error */
42  struct DoubleVect3 accelero_error;
43  /* constant bias */
44  VECT3_COPY(accelero_error, accel->bias);
45  /* white noise */
46  double_vect3_add_gaussian_noise(&accelero_error, &accel->noise_std_dev);
47  /* scale */
48  struct DoubleVect3 gain = {accel->sensitivity.m[0], accel->sensitivity.m[4], accel->sensitivity.m[8]};
49  VECT3_EW_MUL(accelero_error, accelero_error, gain);
50  /* add error */
51  VECT3_ADD(accel->value, accelero_error);
52 
53  /* round signal to account for adc discretisation */
54  DOUBLE_VECT3_ROUND(accel->value);
55  /* saturate */
56  VECT3_BOUND_CUBE(accel->value, accel->min, accel->max);
57 
58  accel->next_update += NPS_ACCEL_DT;
59  accel->data_available = TRUE;
60 }
61 
double m[3 *3]
#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)
Definition: pprz_algebra.h:224
#define VECT3_EW_MUL(_vo, _va, _vb)
Definition: pprz_algebra.h:217
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:463
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
struct DoubleVect3 body_accel
acceleration in body frame as measured by an accelerometer (incl.
Definition: nps_fdm.h:87
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 nps_sensor_accel_init(struct NpsSensorAccel *accel, double time)
void nps_sensor_accel_run_step(struct NpsSensorAccel *accel, double time, struct DoubleRMat *body_to_imu)
struct DoubleVect3 noise_std_dev
struct DoubleVect3 neutral
struct DoubleMat33 sensitivity
struct DoubleVect3 bias
struct DoubleVect3 value
Paparazzi fixed point algebra.
#define TRUE
Definition: std.h:4
#define FALSE
Definition: std.h:5