Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
void nps_sensor_mag_run_step(struct NpsSensorMag *mag, double time, struct DoubleRMat *body_to_imu)
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
struct DoubleVect3 noise_std_dev
#define VECT3_BOUND_CUBE(_v, _min, _max)
Definition: pprz_algebra.h:224
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
euler angles
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
void nps_sensor_mag_init(struct NpsSensorMag *mag, double time)
Definition: nps_sensor_mag.c:8
#define FALSE
Definition: std.h:5
#define TRUE
Definition: std.h:4
static void double_rmat_of_eulers(struct DoubleRMat *rm, struct DoubleEulers *e)
void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
struct DoubleVect3 value
struct DoubleRMat imu_to_sensor_rmat
double next_update
rotation matrix
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:463
#define DOUBLE_VECT3_ROUND(_v)
struct DoubleMat33 sensitivity
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
struct DoubleVect3 neutral
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
Paparazzi fixed point algebra.
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105