Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
8void 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
27void 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 */
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
#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 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