Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
nps_sensor_mag.c
Go to the documentation of this file.
1 #include "nps_sensor_mag.h"
2 
3 #include "generated/airframe.h"
4 #include "nps_fdm.h"
5 #include NPS_SENSORS_PARAMS
7 
8 void nps_sensor_mag_init(struct NpsSensorMag *mag, double time)
9 {
10  VECT3_ASSIGN(mag->value, 0., 0., 0.);
11  mag->min = NPS_MAG_MIN;
12  mag->max = NPS_MAG_MAX;
14  NPS_MAG_SENSITIVITY_XX, NPS_MAG_SENSITIVITY_YY, NPS_MAG_SENSITIVITY_ZZ);
15  VECT3_ASSIGN(mag->neutral,
16  NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z);
18  NPS_MAG_NOISE_STD_DEV_X, NPS_MAG_NOISE_STD_DEV_Y, NPS_MAG_NOISE_STD_DEV_Z);
19  struct DoubleEulers imu_to_sensor_eulers =
20  { NPS_MAG_IMU_TO_SENSOR_PHI, NPS_MAG_IMU_TO_SENSOR_THETA, NPS_MAG_IMU_TO_SENSOR_PSI };
21  double_rmat_of_eulers(&(mag->imu_to_sensor_rmat), &imu_to_sensor_eulers);
22  mag->next_update = time;
23  mag->data_available = FALSE;
24 }
25 
26 void nps_sensor_mag_run_step(struct NpsSensorMag *mag, double time, struct DoubleRMat *body_to_imu)
27 {
28 
29  if (time < mag->next_update) {
30  return;
31  }
32 
33  /* transform magnetic field to body frame */
34  struct DoubleVect3 h_body;
36 
37  /* transform to imu frame */
38  struct DoubleVect3 h_imu;
39  MAT33_VECT3_MUL(h_imu, *body_to_imu, h_body);
40 
41  /* transform to sensor frame */
42  struct DoubleVect3 h_sensor;
43  MAT33_VECT3_MUL(h_sensor, mag->imu_to_sensor_rmat, h_imu);
44 
45  /* compute magnetometer reading */
46  MAT33_VECT3_MUL(mag->value, mag->sensitivity, h_sensor);
47  VECT3_ADD(mag->value, mag->neutral);
48  /* FIXME: ADD error reading */
49 
50  /* round signal to account for adc discretisation */
52  /* saturate */
53  VECT3_BOUND_CUBE(mag->value, mag->min, mag->max);
54 
55  mag->next_update += NPS_MAG_DT;
56  mag->data_available = TRUE;
57 }
58 
double_rmat_of_eulers
static void double_rmat_of_eulers(struct DoubleRMat *rm, struct DoubleEulers *e)
Definition: pprz_algebra_double.h:190
NpsFdm::ltp_h
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105
NpsSensorMag::min
int min
Definition: nps_sensor_mag.h:11
nps_fdm.h
NpsSensorMag
Definition: nps_sensor_mag.h:9
VECT3_ADD
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
NpsFdm::ltp_to_body_quat
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
NpsSensorMag::value
struct DoubleVect3 value
Definition: nps_sensor_mag.h:10
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
NpsSensorMag::next_update
double next_update
Definition: nps_sensor_mag.h:17
fdm
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
Definition: nps_fdm_crrcsim.c:84
nps_sensor_mag_init
void nps_sensor_mag_init(struct NpsSensorMag *mag, double time)
Definition: nps_sensor_mag.c:8
NpsSensorMag::data_available
bool data_available
Definition: nps_sensor_mag.h:18
double_quat_vmult
void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
Definition: pprz_algebra_double.c:90
VECT3_BOUND_CUBE
#define VECT3_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:224
nps_sensor_mag.h
NpsSensorMag::imu_to_sensor_rmat
struct DoubleRMat imu_to_sensor_rmat
Definition: nps_sensor_mag.h:16
NpsSensorMag::noise_std_dev
struct DoubleVect3 noise_std_dev
Definition: nps_sensor_mag.h:15
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
DoubleVect3
Definition: pprz_algebra_double.h:46
NpsSensorMag::max
int max
Definition: nps_sensor_mag.h:12
NpsSensorMag::neutral
struct DoubleVect3 neutral
Definition: nps_sensor_mag.h:14
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
nps_sensor_mag_run_step
void nps_sensor_mag_run_step(struct NpsSensorMag *mag, double time, struct DoubleRMat *body_to_imu)
Definition: nps_sensor_mag.c:26
NpsSensorMag::sensitivity
struct DoubleMat33 sensitivity
Definition: nps_sensor_mag.h:13
DoubleEulers
euler angles
Definition: pprz_algebra_double.h:76