Paparazzi UAS  v5.18.0_stable
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 "generated/airframe.h" /* to get NPS_SENSORS_PARAMS */
4 
5 #include "nps_fdm.h"
6 #include "nps_random.h"
7 #include NPS_SENSORS_PARAMS
9 
10 void nps_sensor_accel_init(struct NpsSensorAccel *accel, double time)
11 {
12  FLOAT_VECT3_ZERO(accel->value);
13  accel->min = NPS_ACCEL_MIN;
14  accel->max = NPS_ACCEL_MAX;
16  NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, 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 
NpsSensorAccel::next_update
double next_update
Definition: nps_sensor_accel.h:17
NpsSensorAccel::sensitivity
struct DoubleMat33 sensitivity
Definition: nps_sensor_accel.h:13
NpsSensorAccel::bias
struct DoubleVect3 bias
Definition: nps_sensor_accel.h:16
NpsSensorAccel::neutral
struct DoubleVect3 neutral
Definition: nps_sensor_accel.h:14
NpsFdm::body_accel
struct DoubleVect3 body_accel
acceleration in body frame as measured by an accelerometer (incl.
Definition: nps_fdm.h:87
nps_fdm.h
VECT3_ADD
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
NpsSensorAccel::min
int min
Definition: nps_sensor_accel.h:11
FLOAT_MAT33_DIAG
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
Definition: pprz_algebra_float.h:234
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
fdm
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
Definition: nps_fdm_crrcsim.c:84
nps_random.h
FLOAT_VECT3_ZERO
#define FLOAT_VECT3_ZERO(_v)
Definition: pprz_algebra_float.h:161
VECT3_BOUND_CUBE
#define VECT3_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:224
NpsSensorAccel::noise_std_dev
struct DoubleVect3 noise_std_dev
Definition: nps_sensor_accel.h:15
nps_sensor_accel.h
NpsSensorAccel
Definition: nps_sensor_accel.h:9
nps_sensor_accel_init
void nps_sensor_accel_init(struct NpsSensorAccel *accel, double time)
Definition: nps_sensor_accel.c:10
NpsSensorAccel::data_available
bool data_available
Definition: nps_sensor_accel.h:18
nps_sensor_accel_run_step
void nps_sensor_accel_run_step(struct NpsSensorAccel *accel, double time, struct DoubleRMat *body_to_imu)
Definition: nps_sensor_accel.c:27
MAT33_VECT3_MUL
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:463
DoubleRMat
rotation matrix
Definition: pprz_algebra_double.h:69
NpsSensorAccel::max
int max
Definition: nps_sensor_accel.h:12
body_to_imu
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
NpsSensorAccel::value
struct DoubleVect3 value
Definition: nps_sensor_accel.h:10
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
double_vect3_add_gaussian_noise
void double_vect3_add_gaussian_noise(struct DoubleVect3 *vect, struct DoubleVect3 *std_dev)
Definition: nps_random.c:34