Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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.h"
7
8void 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_SIGN_X * NPS_MAG_SENSITIVITY_XX,
15 NPS_MAG_SIGN_Y * NPS_MAG_SENSITIVITY_YY,
16 NPS_MAG_SIGN_Z * NPS_MAG_SENSITIVITY_ZZ);
18 NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z);
20 NPS_MAG_NOISE_STD_DEV_X, NPS_MAG_NOISE_STD_DEV_Y, NPS_MAG_NOISE_STD_DEV_Z);
21 struct DoubleEulers imu_to_sensor_eulers =
22 { NPS_MAG_IMU_TO_SENSOR_PHI, NPS_MAG_IMU_TO_SENSOR_THETA, NPS_MAG_IMU_TO_SENSOR_PSI };
23 double_rmat_of_eulers(&(mag->imu_to_sensor_rmat), &imu_to_sensor_eulers);
24 mag->next_update = time;
25 mag->data_available = FALSE;
26}
27
28void nps_sensor_mag_run_step(struct NpsSensorMag *mag, double time, struct DoubleRMat *body_to_imu)
29{
30
31 if (time < mag->next_update) {
32 return;
33 }
34
35 /* transform magnetic field to body frame */
36 struct DoubleVect3 h_body;
37 double_quat_vmult(&h_body, &fdm.ltp_to_body_quat, &fdm.ltp_h);
38
39 /* transform to imu frame */
40 struct DoubleVect3 h_imu;
41 MAT33_VECT3_MUL(h_imu, *body_to_imu, h_body);
42
43 /* transform to sensor frame */
44 struct DoubleVect3 h_sensor;
45 MAT33_VECT3_MUL(h_sensor, mag->imu_to_sensor_rmat, h_imu);
46
47 /* compute magnetometer reading */
48 MAT33_VECT3_MUL(mag->value, mag->sensitivity, h_sensor);
49 VECT3_ADD(mag->value, mag->neutral);
50 /* FIXME: ADD error reading */
51
52 /* round signal to account for adc discretisation */
54 /* saturate */
55 VECT3_BOUND_CUBE(mag->value, mag->min, mag->max);
56
57 mag->next_update += NPS_MAG_DT;
58 mag->data_available = TRUE;
59}
60
void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
static void double_rmat_of_eulers(struct DoubleRMat *rm, struct DoubleEulers *e)
#define DOUBLE_VECT3_ROUND(_v)
rotation matrix
#define FLOAT_MAT33_DIAG(_m, _d00, _d11, _d22)
#define VECT3_BOUND_CUBE(_v, _min, _max)
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define VECT3_ADD(_a, _b)
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
void nps_sensor_mag_init(struct NpsSensorMag *mag, double time)
void nps_sensor_mag_run_step(struct NpsSensorMag *mag, double time, struct DoubleRMat *body_to_imu)
struct DoubleVect3 neutral
struct DoubleMat33 sensitivity
struct DoubleVect3 value
struct DoubleVect3 noise_std_dev
struct DoubleRMat imu_to_sensor_rmat
Paparazzi fixed point algebra.
#define TRUE
Definition std.h:4
#define FALSE
Definition std.h:5