diff --git a/conf/airframes/ENAC/fixed-wing/jp.xml b/conf/airframes/ENAC/fixed-wing/jp.xml
new file mode 100644
index 0000000000..42d7781e9e
--- /dev/null
+++ b/conf/airframes/ENAC/fixed-wing/jp.xml
@@ -0,0 +1,274 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/firmwares/subsystems/fixedwing/ins_float_invariant.makefile b/conf/firmwares/subsystems/fixedwing/ins_float_invariant.makefile
new file mode 100644
index 0000000000..4b4aa81190
--- /dev/null
+++ b/conf/firmwares/subsystems/fixedwing/ins_float_invariant.makefile
@@ -0,0 +1,57 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+# attitude and speed estimation for fixedwings via invariant filter
+
+INS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ins/ins_float_invariant.h\"
+INS_CFLAGS += -DUSE_AHRS_ALIGNER
+INS_CFLAGS += -DUSE_AHRS
+INS_CFLAGS += -DINS_UPDATE_FW_ESTIMATOR
+
+INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
+INS_SRCS += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
+INS_SRCS += $(SRC_SUBSYSTEMS)/ins.c
+INS_SRCS += $(SRC_SUBSYSTEMS)/ins/ins_float_invariant.c
+
+
+ifneq ($(AHRS_ALIGNER_LED),none)
+ INS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
+endif
+
+ifdef AHRS_PROPAGATE_FREQUENCY
+else
+ AHRS_PROPAGATE_FREQUENCY = 125
+endif
+
+ifdef AHRS_CORRECT_FREQUENCY
+else
+ AHRS_CORRECT_FREQUENCY = 125
+endif
+
+INS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
+INS_CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
+
+ap.CFLAGS += $(INS_CFLAGS)
+ap.srcs += $(INS_SRCS)
+
+#
+# NPS uses the real algorithm
+#
+nps.CFLAGS += $(INS_CFLAGS)
+nps.srcs += $(INS_SRCS)
+
+
+#
+# Simple simulation of the AHRS result
+#
+ahrssim_CFLAGS = -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_sim.h\"
+ahrssim_CFLAGS += -DUSE_AHRS -DAHRS_UPDATE_FW_ESTIMATOR
+
+ahrssim_srcs = $(SRC_SUBSYSTEMS)/ahrs.c
+ahrssim_srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_sim.c
+
+sim.CFLAGS += $(ahrssim_CFLAGS)
+sim.srcs += $(ahrssim_srcs)
+
+jsbsim.CFLAGS += $(ahrssim_CFLAGS)
+jsbsim.srcs += $(ahrssim_srcs)
+
diff --git a/conf/firmwares/subsystems/shared/imu_apogee.makefile b/conf/firmwares/subsystems/shared/imu_apogee.makefile
index c3e9e4da9b..6b389e35bb 100644
--- a/conf/firmwares/subsystems/shared/imu_apogee.makefile
+++ b/conf/firmwares/subsystems/shared/imu_apogee.makefile
@@ -26,3 +26,7 @@ ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
ap.CFLAGS += $(IMU_APOGEE_CFLAGS)
ap.srcs += $(IMU_APOGEE_SRCS)
+#
+# Simulator
+#
+include $(CFG_SHARED)/imu_nps.makefile
diff --git a/conf/messages.xml b/conf/messages.xml
index 5fc0c2dfa8..7b84f211e8 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -653,7 +653,26 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/mag_hmc58xx.xml b/conf/modules/mag_hmc58xx.xml
index 609f9da8ea..3c033a6f16 100644
--- a/conf/modules/mag_hmc58xx.xml
+++ b/conf/modules/mag_hmc58xx.xml
@@ -16,7 +16,7 @@
-
+
diff --git a/conf/settings/estimation/ins_float_invariant.xml b/conf/settings/estimation/ins_float_invariant.xml
new file mode 100644
index 0000000000..e7958d7a18
--- /dev/null
+++ b/conf/settings/estimation/ins_float_invariant.xml
@@ -0,0 +1,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/simulator/nps/nps_sensors_params_invariant.h b/conf/simulator/nps/nps_sensors_params_invariant.h
new file mode 100644
index 0000000000..5b23ac6a90
--- /dev/null
+++ b/conf/simulator/nps/nps_sensors_params_invariant.h
@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2013 Felix Ruess, Gautier Hattenberger, JP Condomines
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef NPS_SENSORS_PARAMS_H
+#define NPS_SENSORS_PARAMS_H
+
+#include "generated/airframe.h"
+#include "subsystems/imu.h"
+
+
+#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI
+#define NPS_BODY_TO_IMU_THETA IMU_BODY_TO_IMU_THETA
+#define NPS_BODY_TO_IMU_PSI IMU_BODY_TO_IMU_PSI
+
+/*
+ * Accelerometer
+ */
+/* MPU6050 has 16bit resolution */
+#define NPS_ACCEL_MIN -32767
+#define NPS_ACCEL_MAX 32767
+/* ms-2 */
+/* aka 2^10/ACCEL_X_SENS */
+#define NPS_ACCEL_SENSITIVITY_XX ACCEL_BFP_OF_REAL(1./IMU_ACCEL_X_SENS)
+#define NPS_ACCEL_SENSITIVITY_YY ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Y_SENS)
+#define NPS_ACCEL_SENSITIVITY_ZZ ACCEL_BFP_OF_REAL(1./IMU_ACCEL_Z_SENS)
+
+#define NPS_ACCEL_NEUTRAL_X IMU_ACCEL_X_NEUTRAL
+#define NPS_ACCEL_NEUTRAL_Y IMU_ACCEL_Y_NEUTRAL
+#define NPS_ACCEL_NEUTRAL_Z IMU_ACCEL_Z_NEUTRAL
+/* m2s-4 */
+#define NPS_ACCEL_NOISE_STD_DEV_X 5.e-2
+#define NPS_ACCEL_NOISE_STD_DEV_Y 5.e-2
+#define NPS_ACCEL_NOISE_STD_DEV_Z 5.e-2
+/* ms-2 */
+#define NPS_ACCEL_BIAS_X 0
+#define NPS_ACCEL_BIAS_Y 0
+#define NPS_ACCEL_BIAS_Z 0
+/* s */
+#define NPS_ACCEL_DT (1./125.)
+
+
+
+/*
+ * Gyrometer
+ */
+/* MPU6050 has 16 bit resolution */
+#define NPS_GYRO_MIN -32767
+#define NPS_GYRO_MAX 32767
+
+/* 2^12/GYRO_X_SENS */
+#define NPS_GYRO_SENSITIVITY_PP RATE_BFP_OF_REAL(1./IMU_GYRO_P_SENS)
+#define NPS_GYRO_SENSITIVITY_QQ RATE_BFP_OF_REAL(1./IMU_GYRO_Q_SENS)
+#define NPS_GYRO_SENSITIVITY_RR RATE_BFP_OF_REAL(1./IMU_GYRO_R_SENS)
+
+#define NPS_GYRO_NEUTRAL_P IMU_GYRO_P_NEUTRAL
+#define NPS_GYRO_NEUTRAL_Q IMU_GYRO_Q_NEUTRAL
+#define NPS_GYRO_NEUTRAL_R IMU_GYRO_R_NEUTRAL
+
+#define NPS_GYRO_NOISE_STD_DEV_P RadOfDeg(0.)
+#define NPS_GYRO_NOISE_STD_DEV_Q RadOfDeg(0.)
+#define NPS_GYRO_NOISE_STD_DEV_R RadOfDeg(0.)
+
+#define NPS_GYRO_BIAS_INITIAL_P RadOfDeg( 0.0)
+#define NPS_GYRO_BIAS_INITIAL_Q RadOfDeg( 0.0)
+#define NPS_GYRO_BIAS_INITIAL_R RadOfDeg( 0.0)
+
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.5)
+#define NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.5)
+/* s */
+#define NPS_GYRO_DT (1./125.)
+
+
+
+/*
+ * Magnetometer
+ */
+ /* HMC5883 has 12 bit resolution */
+#define NPS_MAG_MIN -2047
+#define NPS_MAG_MAX 2047
+
+#define NPS_MAG_IMU_TO_SENSOR_PHI 0.
+#define NPS_MAG_IMU_TO_SENSOR_THETA 0.
+#define NPS_MAG_IMU_TO_SENSOR_PSI 0.
+
+#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS)
+#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS)
+#define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS)
+
+#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL
+#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL
+#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL
+
+#define NPS_MAG_NOISE_STD_DEV_X 2e-3
+#define NPS_MAG_NOISE_STD_DEV_Y 2e-3
+#define NPS_MAG_NOISE_STD_DEV_Z 2e-3
+
+#define NPS_MAG_DT (1./60.)
+
+
+/*
+ * Range meter
+ */
+#define BSM_RANGEMETER_RESOLUTION (1024)
+#define BSM_RANGEMETER_SENSITIVITY (1024. / 12.)
+#define BSM_RANGEMETER_MAX_RANGE (6. * BSM_RANGEMETER_SENSITIVITY)
+#define BSM_RANGEMETER_DT (1./20.)
+
+
+/*
+ * Barometer
+ */
+/* m */
+/* aka 2^8/INS_BARO_SENS */
+#define NPS_BARO_QNH 1013.25
+#define NPS_BARO_SENSITIVITY 4.0
+#define NPS_BARO_DT (1./5.)
+#define NPS_BARO_NOISE_STD_DEV 5.e-2
+
+/*
+ * GPS
+ */
+
+#ifndef GPS_PERFECT
+#define GPS_PERFECT 1
+#endif
+
+#if GPS_PERFECT
+
+#define NPS_GPS_SPEED_NOISE_STD_DEV 0.
+#define NPS_GPS_SPEED_LATENCY 0.
+#define NPS_GPS_POS_NOISE_STD_DEV 0.001
+#define NPS_GPS_POS_BIAS_INITIAL_X 0.
+#define NPS_GPS_POS_BIAS_INITIAL_Y 0.
+#define NPS_GPS_POS_BIAS_INITIAL_Z 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 0.
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 0.
+#define NPS_GPS_POS_LATENCY 0.
+
+#else
+
+#define NPS_GPS_SPEED_NOISE_STD_DEV 0.5
+#define NPS_GPS_SPEED_LATENCY 0.2
+#define NPS_GPS_POS_NOISE_STD_DEV 2
+#define NPS_GPS_POS_BIAS_INITIAL_X 0e-1
+#define NPS_GPS_POS_BIAS_INITIAL_Y -0e-1
+#define NPS_GPS_POS_BIAS_INITIAL_Z -0e-1
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X 1e-3
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-3
+#define NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-3
+#define NPS_GPS_POS_LATENCY 0.2
+
+#endif /* GPS_PERFECT */
+
+#define NPS_GPS_DT (1./5.)
+
+#endif /* NPS_SENSORS_PARAMS_H */
+
diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h
index 25439b28c5..f0680fb845 100644
--- a/sw/airborne/math/pprz_algebra.h
+++ b/sw/airborne/math/pprz_algebra.h
@@ -29,6 +29,13 @@
#define SQUARE(_a) ((_a)*(_a))
+//
+//
+// Vector algebra
+//
+//
+
+
/*
* Dimension 2 vectors
*/
@@ -87,7 +94,10 @@
(_v).y = (_v).y < _min ? _min : (_v).y > _max ? _max : (_v).y; \
}
-
+/* _vo=v1*v2 */
+#define VECT2_DOT_PRODUCT(_so, _v1, _v2) { \
+ (_so) = (_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z; \
+}
/*
* Dimension 3 vectors
@@ -100,6 +110,14 @@
(_a).z = (_z); \
}
+/* a = a * b */
+
+#define VECT3_MUL( _v1, _v2){ \
+ (_v1).x = (_v1).x * (_v2).x; \
+ (_v1).y = (_v1).y * (_v2).y; \
+ (_v1).z = (_v1).z * (_v2).z; \
+}
+
/* a = {abs(x), abs(y), abs(z)} */
#define VECT3_ASSIGN_ABS(_a, _x, _y, _z) { \
(_a).x = ABS(_x); \
@@ -218,6 +236,10 @@
(_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
}
+#define VECT3_DOT_PRODUCT(_so, _v1, _v2) { \
+ (_so) = (_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z; \
+}
+
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2) { \
(_vo).x = (_r1).q*(_v2).z - (_r1).r*(_v2).y; \
(_vo).y = (_r1).r*(_v2).x - (_r1).p*(_v2).z; \
@@ -225,12 +247,13 @@
}
+//
+//
+// Euler angles
+//
+//
-/*
- * Euler angles
- */
-
#define EULERS_COPY(_a, _b) { \
(_a).phi = (_b).phi; \
(_a).theta = (_b).theta; \
@@ -286,9 +309,11 @@
(_v).psi = (_v).psi < (_min) ? (_min) : (_v).psi > (_max) ? (_max) : (_v).psi; \
}
-/*
- * Rates
- */
+//
+//
+// Rates
+//
+//
/* ra = {p, q, r} */
#define RATES_ASSIGN(_ra, _p, _q, _r) { \
@@ -376,6 +401,13 @@
+//
+//
+// Matrix
+//
+//
+
+
/*
* 3x3 matrices
*/
@@ -456,9 +488,11 @@
//
//
-// Quaternions
+// Quaternion algebras
//
//
+
+/* _q = [_i _x _y _z] */
#define QUAT_ASSIGN(_q, _i, _x, _y, _z) { \
(_q).qi = (_i); \
(_q).qx = (_x); \
@@ -466,6 +500,7 @@
(_q).qz = (_z); \
}
+/* _qc = _qa - _qc */
#define QUAT_DIFF(_qc, _qa, _qb) { \
(_qc).qi = (_qa).qi - (_qb).qi; \
(_qc).qx = (_qa).qx - (_qb).qx; \
@@ -473,6 +508,7 @@
(_qc).qz = (_qa).qz - (_qb).qz; \
}
+/* _qo = _qi */
#define QUAT_COPY(_qo, _qi) { \
(_qo).qi = (_qi).qi; \
(_qo).qx = (_qi).qx; \
@@ -487,7 +523,7 @@
(b).qz = -(a).qz; \
}
-
+/* _qo = _qi * _s */
#define QUAT_SMUL(_qo, _qi, _s) { \
(_qo).qi = (_qi).qi * (_s); \
(_qo).qx = (_qi).qx * (_s); \
@@ -495,6 +531,7 @@
(_qo).qz = (_qi).qz * (_s); \
}
+/* _qo = _qo + _qi */
#define QUAT_ADD(_qo, _qi) { \
(_qo).qi += (_qi).qi; \
(_qo).qx += (_qi).qx; \
@@ -502,6 +539,7 @@
(_qo).qz += (_qi).qz; \
}
+/* _qo = [qi -qx -qy -qz] */
#define QUAT_INVERT(_qo, _qi) { \
(_qo).qi = (_qi).qi; \
(_qo).qx = -(_qi).qx; \
@@ -509,10 +547,27 @@
(_qo).qz = -(_qi).qz; \
}
+/* _vo=[ qx qy qz] */
+#define QUAT_EXTRACT_Q(_vo, _qi) { \
+ (_vo).x=(_qi).qx; \
+ (_vo).y=(_qi).qy; \
+ (_vo).z=(_qi).qz; \
+}
+
+/* _qo = _qo / _s */
+#define QUAT_SDIV(_qo, _qi, _s) { \
+ (_qo).qi = (_qi).qi / _s; \
+ (_qo).qx = (_qi).qx / _s; \
+ (_qo).qy = (_qi).qy / _s; \
+ (_qo).qz = (_qi).qz / _s; \
+ }
+
+//
+//
+// Rotation Matrices
+//
+//
-/*
- * Rotation Matrices
- */
/* accessor : row and col range from 0 to 2 */
#define RMAT_ELMT(_rm, _row, _col) MAT33_ELMT(_rm, _row, _col)
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index 799087f1f6..9b82a7a9a9 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -95,6 +95,12 @@ struct FloatRates {
while (_a < -M_PI) _a += (2.*M_PI); \
}
+//
+//
+// Vector algebra
+//
+//
+
/*
* Dimension 2 Vectors
@@ -137,6 +143,10 @@ struct FloatRates {
* Dimension 3 Vectors
*/
+#define FLOAT_VECT3_SUM(_a, _b, _c) VECT3_SUM(_a, _b, _c)
+#define FLOAT_VECT3_SDIV(_a, _b, _s) VECT3_SDIV(_a, _b, _s)
+#define FLOAT_VECT3_COPY(_a, _b) VECT3_COPY(_a, _b)
+
#define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.)
#define FLOAT_VECT3_ASSIGN(_a, _x, _y, _z) VECT3_ASSIGN(_a, _x, _y, _z)
@@ -147,14 +157,18 @@ struct FloatRates {
/* a -= b */
#define FLOAT_VECT3_SUB(_a, _b) VECT3_SUB(_a, _b)
+#define FLOAT_VECT3_ADD(_a, _b) VECT3_ADD(_a, _b)
+
/* _vo = _vi * _s */
#define FLOAT_VECT3_SMUL(_vo, _vi, _s) VECT3_SMUL(_vo, _vi, _s)
+#define FLOAT_VECT3_MUL(_v1, _v2) VECT3_MUL(_v1, _v2)
+
#define FLOAT_VECT3_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)
#define FLOAT_VECT3_NORM(_v) (sqrtf(FLOAT_VECT3_NORM2(_v)))
-#define FLOAT_VECT3_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z)
+#define FLOAT_VECT3_DOT_PRODUCT( _v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z)
#define FLOAT_VECT3_CROSS_PRODUCT(_vo, _v1, _v2) { \
(_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \
@@ -243,9 +257,12 @@ struct FloatRates {
}
-/*
- * Rotation Matrices
- */
+//
+//
+// Rotation Matrices
+//
+//
+
/* initialises a matrix to identity */
#define FLOAT_RMAT_ZERO(_rm) FLOAT_MAT33_DIAG(_rm, 1., 1., 1.)
@@ -456,36 +473,48 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
}
-
-/*
- * Quaternions
- */
+//
+//
+// Quaternion algebras
+//
+//
#define FLOAT_QUAT_ZERO(_q) QUAT_ASSIGN(_q, 1., 0., 0., 0.)
+#define FLOAT_QUAT_ASSIGN(_qi, _i, _x, _y, _z) QUAT_ASSIGN(_qi, _i, _x, _y, _z)
+
/* _q += _qa */
#define FLOAT_QUAT_ADD(_qo, _qi) QUAT_ADD(_qo, _qi)
/* _qo = _qi * _s */
#define FLOAT_QUAT_SMUL(_qo, _qi, _s) QUAT_SMUL(_qo, _qi, _s)
+/* _qo = _qo / _s */
+#define FLOAT_QUAT_SDIV( _qo, _qi, _s) QUAT_SDIV(_qo, _qi, _s)
+
+/* */
#define FLOAT_QUAT_EXPLEMENTARY(b,a) QUAT_EXPLEMENTARY(b,a)
+/* */
#define FLOAT_QUAT_COPY(_qo, _qi) QUAT_COPY(_qo, _qi)
#define FLOAT_QUAT_NORM(_q) (sqrtf(SQUARE((_q).qi) + SQUARE((_q).qx)+ \
SQUARE((_q).qy) + SQUARE((_q).qz)))
-#define FLOAT_QUAT_NORMALIZE(_q) { \
- float norm = FLOAT_QUAT_NORM(_q); \
- if (norm > FLT_MIN) { \
- (_q).qi = (_q).qi / norm; \
- (_q).qx = (_q).qx / norm; \
- (_q).qy = (_q).qy / norm; \
- (_q).qz = (_q).qz / norm; \
- } \
+#define FLOAT_QUAT_NORMALIZE(_q) { \
+ float qnorm = FLOAT_QUAT_NORM(_q); \
+ if (qnorm > FLT_MIN) { \
+ (_q).qi = (_q).qi / qnorm; \
+ (_q).qx = (_q).qx / qnorm; \
+ (_q).qy = (_q).qy / qnorm; \
+ (_q).qz = (_q).qz / qnorm; \
+ } \
}
+/* */
+#define FLOAT_QUAT_EXTRACT(_vo, _qi) QUAT_EXTRACT_Q(_vo, _qi)
+
+/* Be careful : after invert make a normalization */
#define FLOAT_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
#define FLOAT_QUAT_WRAP_SHORTEST(_q) { \
@@ -493,6 +522,101 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
QUAT_EXPLEMENTARY(_q,_q); \
}
+/*
+ *
+ * Rotation Matrix using quaternions
+ *
+ */
+
+ /*
+ * The (non commutative) quaternion product * then reads
+ *
+ * [ p0.q0 - p.q ]
+ * p * q = [ ]
+ * [ p0.q + q0.p + p x q ]
+ *
+ */
+
+ /* (qi)-1 * vi * qi represents R_q of n->b on vectors vi
+ *
+ * "FLOAT_QUAT_EXTRACT : Extracted of the vector part"
+ */
+
+#define FLOAT_QUAT_RMAT_N2B(_n2b, _qi, _vi){ \
+ \
+ struct FloatQuat quatinv; \
+ struct FloatVect3 quat3, v1, v2; \
+ float qi; \
+ \
+ FLOAT_QUAT_INVERT(quatinv, _qi); \
+ FLOAT_QUAT_NORMALIZE(quatinv); \
+ \
+ FLOAT_QUAT_EXTRACT(quat3, quatinv); \
+ qi = - FLOAT_VECT3_DOT_PRODUCT(quat3, _vi); \
+ FLOAT_VECT3_CROSS_PRODUCT(v1, quat3, _vi); \
+ FLOAT_VECT3_SMUL(v2, _vi, (quatinv.qi)) ; \
+ FLOAT_VECT3_ADD(v2, v1); \
+ \
+ FLOAT_QUAT_EXTRACT(quat3, _qi); \
+ FLOAT_VECT3_CROSS_PRODUCT(_n2b, v2, quat3); \
+ FLOAT_VECT3_SMUL(v1, v2, (_qi).qi); \
+ FLOAT_VECT3_ADD(_n2b,v1); \
+ FLOAT_VECT3_SMUL(v1, quat3, qi); \
+ FLOAT_VECT3_ADD(_n2b,v1); \
+}
+
+ /*
+ * qi * vi * (qi)-1 represents R_q of b->n on vectors vi
+ */
+#define FLOAT_QUAT_RMAT_B2N(_b2n,_qi,_vi){ \
+ \
+ struct FloatQuat _quatinv; \
+ \
+ \
+ FLOAT_QUAT_INVERT(_quatinv, _qi); \
+ FLOAT_QUAT_NORMALIZE(_quatinv); \
+ \
+ FLOAT_QUAT_RMAT_N2B(_b2n, _quatinv, _vi); \
+}
+
+ /* Right multiplication by a quaternion
+ *
+ * vi * qi
+ *
+ */
+#define FLOAT_QUAT_VMUL_RIGHT(_mright,_qi,_vi){ \
+ \
+ struct FloatVect3 quat3, v1, v2; \
+ float qi; \
+ \
+ FLOAT_QUAT_EXTRACT(quat3, _qi); \
+ qi = - FLOAT_VECT3_DOT_PRODUCT(_vi, quat3); \
+ FLOAT_VECT3_CROSS_PRODUCT(v1, _vi, quat3); \
+ FLOAT_VECT3_SMUL(v2, _vi, (_qi.qi)); \
+ FLOAT_VECT3_ADD(v2, v1); \
+ FLOAT_QUAT_ASSIGN(_mright, qi, v2.x, v2.y, v2.z);\
+}
+
+
+ /* Left multiplication by a quaternion
+ *
+ * qi * vi
+ *
+ */
+#define FLOAT_QUAT_VMUL_LEFT(_mleft,_qi,_vi){ \
+ \
+ struct FloatVect3 quat3, v1, v2; \
+ float qi; \
+ \
+ FLOAT_QUAT_EXTRACT(quat3, _qi); \
+ qi = - FLOAT_VECT3_DOT_PRODUCT(quat3, _vi); \
+ FLOAT_VECT3_CROSS_PRODUCT(v1, quat3, _vi); \
+ FLOAT_VECT3_SMUL(v2, _vi, (_qi.qi)); \
+ FLOAT_VECT3_ADD(v2, v1); \
+ FLOAT_QUAT_ASSIGN(_mleft, qi, v2.x, v2.y, v2.z);\
+}
+
+
/* _a2c = _a2b comp _b2c , aka _a2c = _a2b * _b2c */
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c) { \
FLOAT_QUAT_COMP(_a2c, _a2b, _b2c); \
@@ -710,9 +834,12 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
}
-/*
- * Euler angles
- */
+
+//
+//
+// Euler angles
+//
+//
#define FLOAT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0., 0., 0.);
@@ -755,6 +882,67 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
}
+//
+//
+// Generic vector algebra
+//
+//
+
+/** a = 0 */
+static inline void float_vect_zero(float * a, const int n) {
+ int i;
+ for (i = 0; i < n; i++) { a[i] = 0.; }
+}
+
+/** a = b */
+static inline void float_vect_copy(float * a, const float * b, const int n) {
+ int i;
+ for (i = 0; i < n; i++) { a[i] = b[i]; }
+}
+
+/** o = a + b */
+static inline void float_vect_sum(float * o, const float * a, const float * b, const int n) {
+ int i;
+ for (i = 0; i < n; i++) { o[i] = a[i] + b[i]; }
+}
+
+/** o = a - b */
+static inline void float_vect_diff(float * o, const float * a, const float * b, const int n) {
+ int i;
+ for (i = 0; i < n; i++) { o[i] = a[i] - b[i]; }
+}
+
+/** o = a * b (element wise) */
+static inline void float_vect_mul(float * o, const float * a, const float * b, const int n) {
+ int i;
+ for (i = 0; i < n; i++) { o[i] = a[i] * b[i]; }
+}
+
+/** a += b */
+static inline void float_vect_add(float * a, const float * b, const int n) {
+ int i;
+ for (i = 0; i < n; i++) { a[i] += b[i]; }
+}
+
+/** a -= b */
+static inline void float_vect_sub(float * a, const float * b, const int n) {
+ int i;
+ for (i = 0; i < n; i++) { a[i] -= b[i]; }
+}
+
+/** o = a * s */
+static inline void float_vect_smul(float * o, const float * a, const float s, const int n) {
+ int i;
+ for (i = 0; i < n; i++) { o[i] = a[i] * s; }
+}
+
+/** o = a / s */
+static inline void float_vect_sdiv(float * o, const float * a, const float s, const int n) {
+ int i;
+ if ( fabs(s) > 1e-5 ) {
+ for (i = 0; i < n; i++) { o[i] = a[i] / s; }
+ }
+}
diff --git a/sw/airborne/math/pprz_geodetic.h b/sw/airborne/math/pprz_geodetic.h
index b6abfee5e0..a6af9b02f6 100644
--- a/sw/airborne/math/pprz_geodetic.h
+++ b/sw/airborne/math/pprz_geodetic.h
@@ -47,5 +47,17 @@
(_pos).z = -(_utm1).alt + (_utm2).alt; \
}
+#define UTM_OF_ENU_ADD(_utm, _pos, _utm0) { \
+ (_utm).east = (_utm0).east + (_pos).x; \
+ (_utm).north = (_utm0).north + (_pos).y; \
+ (_utm).alt = (_utm0).alt + (_pos).z; \
+}
+
+#define UTM_OF_NED_ADD(_utm, _pos, _utm0) { \
+ (_utm).east = (_utm0).east + (_pos).y; \
+ (_utm).north = (_utm0).north + (_pos).x; \
+ (_utm).alt = (_utm0).alt - (_pos).z; \
+}
+
#endif /* PPRZ_GEODETIC_H */
diff --git a/sw/airborne/math/pprz_rk_float.h b/sw/airborne/math/pprz_rk_float.h
new file mode 100644
index 0000000000..944c6c25f3
--- /dev/null
+++ b/sw/airborne/math/pprz_rk_float.h
@@ -0,0 +1,165 @@
+/*
+ * Copyright (C) 2013 Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/*
+ * Runge-Kutta library (float version)
+ */
+
+#ifndef PPRZ_RK_FLOAT_H
+#define PPRZ_RK_FLOAT_H
+
+#include "math/pprz_algebra_float.h"
+
+/** First-Order Runge-Kutta
+ *
+ * aka RK1, aka the euler method
+ *
+ * considering x' = f(x,u)
+ * with x = x0 the initial state
+ * and u the command vector
+ *
+ * x_new = x + dt * f(x, u)
+ * is the integrated state vector x based on model f under command u
+ *
+ * @param xo output integrated state
+ * @param x initial state
+ * @param n state dimension
+ * @param u command vector
+ * @param m command dimension
+ * @param f model function
+ * @param dt integration step
+ */
+static inline void runge_kutta_1_float(
+ float * xo,
+ const float * x, const int n,
+ const float * u, const int m,
+ void (*f)(float * o, const float * x, const int n, const float * u, const int m),
+ const float dt)
+{
+ float dx[n];
+
+ f(dx, x, n, u, m);
+ float_vect_smul(xo, dx, dt, n);
+ float_vect_add(xo, x, n);
+}
+
+/** Second-Order Runge-Kutta
+ *
+ * aka RK2, aka the mid-point method
+ *
+ * considering x' = f(x,u)
+ * with x = x0 the initial state
+ * and u the command vector
+ *
+ * mid_point = x + (dt/2)*f(x, u)
+ * x_new = x + dt * f(mid_point, u)
+ * is the integrated state vector x based on model f under command u
+ *
+ * @param xo output integrated state
+ * @param x initial state
+ * @param n state dimension
+ * @param u command vector
+ * @param m command dimension
+ * @param f model function
+ * @param dt integration step
+ */
+static inline void runge_kutta_2_float(
+ float * xo,
+ const float * x, const int n,
+ const float * u, const int m,
+ void (*f)(float * o, const float * x, const int n, const float * u, const int m),
+ const float dt)
+{
+ float mid_point[n];
+
+ // mid_point = x + (dt/2)*f(x, u)
+ f(mid_point, x, n, u, m);
+ float_vect_smul(mid_point, mid_point, dt/2., n);
+ float_vect_add(mid_point, x, n);
+ // xo = x + dt * f(mid_point, u)
+ f(xo, mid_point, n, u, m);
+ float_vect_smul(xo, xo, dt, n);
+ float_vect_add(xo, x, n);
+}
+
+/** Fourth-Order Runge-Kutta
+ *
+ * aka RK4, aka 'the' Runge-Kutta
+ *
+ * considering x' = f(x,u)
+ * with x = x0 the initial state
+ * and u the command vector
+ *
+ * k1 = f(x, u)
+ * k2 = f(x + dt * (k1 / 2), u)
+ * k3 = f(x + dt * (k2 / 2), u)
+ * k4 = f(x + dt * k3, u)
+ *
+ * x_new = x + (dt / 6) * (k1 + 2 * (k2 + k3) + k4)
+ * is the integrated state vector x based on model f under command u
+ *
+ * @param xo output integrated state
+ * @param x initial state
+ * @param n state dimension
+ * @param u command vector
+ * @param m command dimension
+ * @param f model function
+ * @param dt integration step
+ */
+static inline void runge_kutta_4_float(
+ float * xo,
+ const float * x, const int n,
+ const float * u, const int m,
+ void (*f)(float * o, const float * x, const int n, const float * u, const int m),
+ const float dt)
+{
+ float k1[n], k2[n], k3[n], k4[n], ktmp[n];
+
+ // k1 = f(x, u)
+ f(k1, x, n, u, m);
+
+ // k2 = f(x + dt * (k1 / 2), u)
+ float_vect_smul(ktmp, k1, dt / 2., n);
+ float_vect_add(ktmp, x, n);
+ f(k2, ktmp, n, u, m);
+
+ // k3 = f(x + dt * (k2 / 2), u)
+ float_vect_smul(ktmp, k2, dt / 2., n);
+ float_vect_add(ktmp, x, n);
+ f(k3, ktmp, n, u, m);
+
+ // k4 = f(x + dt * k3, u)
+ float_vect_smul(ktmp, k3, dt, n);
+ float_vect_add(ktmp, x, n);
+ f(k4, ktmp, n, u, m);
+
+ // xo = x + (dt / 6) * (k1 + 2 * (k2 + k3) + k4)
+ float_vect_add(k2, k3, n);
+ float_vect_smul(k2, k2, 2., n);
+ float_vect_add(k1, k2, n);
+ float_vect_add(k1, k4, n);
+ float_vect_smul(k1, k1, dt / 6., n);
+ float_vect_sum(xo, x, k1, n);
+}
+
+#endif
+
diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c
index 5eb5ec11ba..dfdc0dcc0d 100644
--- a/sw/airborne/modules/sensors/mag_hmc58xx.c
+++ b/sw/airborne/modules/sensors/mag_hmc58xx.c
@@ -30,6 +30,25 @@
#include "messages.h"
#include "subsystems/datalink/downlink.h"
+#if MODULE_HMC58XX_UPDATE_AHRS
+#include "subsystems/imu.h"
+#include "subsystems/ahrs.h"
+
+#ifndef HMC58XX_CHAN_X
+#define HMC58XX_CHAN_X 0
+#endif
+#ifndef HMC58XX_CHAN_Y
+#define HMC58XX_CHAN_Y 1
+#endif
+#ifndef HMC58XX_CHAN_Z
+#define HMC58XX_CHAN_Z 2
+#endif
+
+#endif
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
struct Hmc58xx mag_hmc58xx;
@@ -43,6 +62,25 @@ void mag_hmc58xx_module_periodic(void) {
void mag_hmc58xx_module_event(void) {
hmc58xx_event(&mag_hmc58xx);
+#if MODULE_HMC58XX_UPDATE_AHRS
+ if (mag_hmc58xx.data_available) {
+ // set channel order
+ struct Int32Vect3 mag = {
+ (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_X]),
+ (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Y]),
+ (int32_t)(mag_hmc58xx.data.value[HMC58XX_CHAN_Z])
+ };
+ // unscaled vector
+ VECT3_COPY(imu.mag_unscaled, mag);
+ // scale vector
+ ImuScaleMag(imu);
+ // update ahrs
+ if (ahrs.status == AHRS_RUNNING) {
+ ahrs_update_mag();
+ }
+ mag_hmc58xx.data_available = FALSE;
+ }
+#endif
#if MODULE_HMC58XX_SYNC_SEND
if (mag_hmc58xx.data_available) {
mag_hmc58xx_report();
diff --git a/sw/airborne/state.c b/sw/airborne/state.c
index d2130dd928..c00dc4a75a 100644
--- a/sw/airborne/state.c
+++ b/sw/airborne/state.c
@@ -337,6 +337,24 @@ void stateCalcPositionUtm_f(void) {
SetBit(state.pos_status, POS_LLA_F);
utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f);
}
+ else if (state.utm_initialized_f) {
+ if (bit_is_set(state.pos_status, POS_ENU_F)) {
+ UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_ENU_I)) {
+ ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i);
+ SetBit(state.pos_status, POS_ENU_F);
+ UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_F)) {
+ UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
+ }
+ else if (bit_is_set(state.pos_status, POS_NED_I)) {
+ NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
+ SetBit(state.pos_status, POS_NED_F);
+ UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f);
+ }
+ }
else {
/* could not get this representation, set errno */
//struct EcefCoor_f _ecef_zero = {0.0f};
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
index 9aed07595d..4ac1f29a8f 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_utils.h
@@ -30,6 +30,7 @@
#ifndef AHRS_FLOAT_UTILS_H
#define AHRS_FLOAT_UTILS_H
+#include "math/pprz_algebra_float.h"
#include "subsystems/ahrs/ahrs_magnetic_field_model.h"
#include "std.h" // for ABS
diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c
new file mode 100644
index 0000000000..1a675801c4
--- /dev/null
+++ b/sw/airborne/subsystems/ins/ins_float_invariant.c
@@ -0,0 +1,568 @@
+/*
+ * Copyright (C) 2012-2013 Jean-Philippe Condomines, Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ *
+ */
+
+
+/**
+ * @file subsystems/ins/ins_float_invariant.c
+ * @author Jean-Philippe Condomines
+ *
+ * INS using invariant filter.
+ *
+ * Only for fixedwing currenctly
+ *
+ */
+
+#include "subsystems/ins/ins_float_invariant.h"
+
+#include "subsystems/ahrs/ahrs_int_utils.h"
+#include "subsystems/ahrs/ahrs_aligner.h"
+#include "subsystems/ahrs.h"
+
+#include "subsystems/ins.h"
+#include "subsystems/gps.h"
+#include "subsystems/imu.h"
+
+#include "generated/airframe.h"
+#include "generated/flight_plan.h"
+#include "subsystems/nav.h"
+
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_algebra_int.h"
+#include "math/pprz_rk_float.h"
+#include "math/pprz_isa.h"
+
+#include "subsystems/abi.h"
+#include "state.h"
+
+#include "led.h"
+
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+/*------------- =*= Invariant Observers =*= -------------*
+ *
+ * State vector :
+ *
+ * x = [q0 q1 q2 q3 vx vy vz px py pz wb1 wb2 wb3 hb as]'
+ *
+ * Dynamic model (dim = 15) :
+ *
+ * x_qdot = 0.5 * x_quat * ( x_rates - x_bias );
+ * x_Vdot = A + 1/as (q * am * (q)-1);
+ * x_Xdot = V;
+ * x_bias_dot = 0;
+ * x_asdot = 0;
+ * x_hbdot = 0;
+ *
+ * Observation model (dim = 10):
+ * yv = V;
+ * yx = X;
+ * yh = - hb;
+ * yb = (q)-1 *B * q; (B : magnetometers)
+ *
+ *------------------------------------------------------*/
+
+// Default values for the tuning gains
+// Tuning parameter of speed error on attitude (e-2)
+#ifndef INS_INV_LV
+#define INS_INV_LV 2.
+#endif
+// Tuning parameter of mag error on attitude (e-2)
+#ifndef INS_INV_LB
+#define INS_INV_LB 6.
+#endif
+// Tuning parameter of horizontal speed error on speed
+#ifndef INS_INV_MV
+#define INS_INV_MV 8.
+#endif
+// Tuning parameter of vertical speed error on speed
+#ifndef INS_INV_MVZ
+#define INS_INV_MVZ 15.
+#endif
+// Tuning parameter of baro error on vertical speed
+#ifndef INS_INV_MH
+#define INS_INV_MH 0.2
+#endif
+// Tuning parameter of horizontal position error on position
+#ifndef INS_INV_NX
+#define INS_INV_NX 0.8
+#endif
+// Tuning parameter of vertical position error on position
+#ifndef INS_INV_NXZ
+#define INS_INV_NXZ 0.5
+#endif
+// Tuning parameter of baro error on vertical position
+#ifndef INS_INV_NH
+#define INS_INV_NH 1.2
+#endif
+// Tuning parameter of speed error on gyro biases (e-3)
+#ifndef INS_INV_OV
+#define INS_INV_OV 1.2
+#endif
+// Tuning parameter of mag error on gyro biases (e-3)
+#ifndef INS_INV_OB
+#define INS_INV_OB 1.
+#endif
+// Tuning parameter of speed error on accel biases (e-2)
+#ifndef INS_INV_RV
+#define INS_INV_RV 4.
+#endif
+// Tuning parameter of baro error on accel biases (vertical projection) (e-8)
+#ifndef INS_INV_RH
+#define INS_INV_RH 8.
+#endif
+// Tuning parameter of baro error on baro bias
+#ifndef INS_INV_SH
+#define INS_INV_SH 0.01
+#endif
+
+
+// FIXME this is still needed for fixedwing integration
+#if INS_UPDATE_FW_ESTIMATOR
+// remotely settable
+#ifndef INS_ROLL_NEUTRAL_DEFAULT
+#define INS_ROLL_NEUTRAL_DEFAULT 0.
+#endif
+#ifndef INS_PITCH_NEUTRAL_DEFAULT
+#define INS_PITCH_NEUTRAL_DEFAULT 0.
+#endif
+float ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
+float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
+#endif
+
+struct InsFloatInv ins_impl;
+
+/* integration time step */
+static const float dt = (1./ ((float)AHRS_PROPAGATE_FREQUENCY));
+
+/* earth gravity model */
+static const struct FloatVect3 A = { 0.f, 0.f, 9.81f };
+
+/* earth magnetic model */
+static const struct FloatVect3 B = { (float)(INS_H_X), (float)(INS_H_Y), (float)(INS_H_Z) };
+
+/* barometer */
+bool_t ins_baro_initialised;
+// Baro event on ABI
+#ifndef INS_BARO_ID
+#define INS_BARO_ID BARO_BOARD_SENDER_ID
+#endif
+abi_event baro_ev;
+static void baro_cb(uint8_t sender_id, const float *pressure);
+
+/* error computation */
+static inline void error_output(struct InsFloatInv * _ins);
+
+/* propagation model (called by runge-kutta library) */
+static inline void invariant_model(float * o, const float * x, const int n, const float * u, const int m);
+
+/* init state and measurements */
+static inline void init_invariant_state(void) {
+ // init state
+ FLOAT_QUAT_ZERO(ins_impl.state.quat);
+ FLOAT_RATES_ZERO(ins_impl.state.bias);
+ FLOAT_VECT3_ZERO(ins_impl.state.pos);
+ FLOAT_VECT3_ZERO(ins_impl.state.speed);
+ ins_impl.state.as = 1.;
+ ins_impl.state.hb = 0.;
+
+ // init measures
+ FLOAT_VECT3_ZERO(ins_impl.meas.pos_gps);
+ FLOAT_VECT3_ZERO(ins_impl.meas.speed_gps);
+ ins_impl.meas.baro_alt = 0.;
+
+ // init baro
+ ins_baro_initialised = FALSE;
+}
+
+void ins_init() {
+
+ // init position
+#if INS_UPDATE_FW_ESTIMATOR
+ struct UtmCoor_f utm0;
+ utm0.north = (float)nav_utm_north0;
+ utm0.east = (float)nav_utm_east0;
+ utm0.alt = GROUND_ALT;
+ utm0.zone = nav_utm_zone0;
+ stateSetLocalUtmOrigin_f(&utm0);
+ stateSetPositionUtm_f(&utm0);
+#else
+ struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
+ llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
+ llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
+ /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
+ llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
+ struct EcefCoor_i ecef_nav0;
+ ecef_of_lla_i(&ecef_nav0, &llh_nav0);
+ struct LtpDef_i ltp_def;
+ ltp_def_from_ecef_i(<p_def, &ecef_nav0);
+ ins_impl.ltp_def.hmsl = NAV_ALT0;
+ stateSetLocalOrigin_i(<p_def);
+#endif
+
+ // Bind to BARO_ABS message
+ AbiBindMsgBARO_ABS(INS_BARO_ID, &baro_ev, baro_cb);
+
+ // init state and measurements
+ init_invariant_state();
+
+ // init gains
+ ins_impl.gains.lv = INS_INV_LV;
+ ins_impl.gains.lb = INS_INV_LB;
+ ins_impl.gains.mv = INS_INV_MV;
+ ins_impl.gains.mvz = INS_INV_MVZ;
+ ins_impl.gains.mh = INS_INV_MH;
+ ins_impl.gains.nx = INS_INV_NX;
+ ins_impl.gains.nxz = INS_INV_NXZ;
+ ins_impl.gains.nh = INS_INV_NH;
+ ins_impl.gains.ov = INS_INV_OV;
+ ins_impl.gains.ob = INS_INV_OB;
+ ins_impl.gains.rv = INS_INV_RV;
+ ins_impl.gains.rh = INS_INV_RH;
+ ins_impl.gains.sh = INS_INV_SH;
+
+ ins.status = INS_UNINIT;
+ ins.hf_realign = FALSE;
+ ins.vf_realign = FALSE;
+}
+
+void ins_periodic(void) {}
+
+void ahrs_init(void) {
+ ahrs.status = AHRS_UNINIT;
+}
+
+
+void ahrs_align(void)
+{
+ /* Compute an initial orientation from accel and mag directly as quaternion */
+ ahrs_float_get_quat_from_accel_mag(&ins_impl.state.quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
+
+ /* use average gyro as initial value for bias */
+ struct FloatRates bias0;
+ RATES_COPY(bias0, ahrs_aligner.lp_gyro);
+ RATES_FLOAT_OF_BFP(ins_impl.state.bias, bias0);
+
+ // ins and ahrs are now running
+ ahrs.status = AHRS_RUNNING;
+ ins.status = INS_RUNNING;
+}
+
+void ahrs_propagate(void) {
+ struct FloatRates body_rates;
+ struct FloatEulers eulers;
+
+ // realign all the filter if needed
+ // a complete init cycle is required
+ if (ins.hf_realign || ins.vf_realign) {
+ ins.status = INS_UNINIT;
+ ahrs.status = AHRS_UNINIT;
+ init_invariant_state();
+ ins.hf_realign = FALSE;
+ ins.vf_realign = FALSE;
+ }
+
+ // fill command vector
+ RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, imu.gyro);
+ ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, imu.accel);
+
+ // update correction gains
+ error_output(&ins_impl);
+
+ // propagate model
+ struct inv_state new_state;
+ runge_kutta_4_float((float*)&new_state/*(float*)&ins_impl.state*/,
+ (float*)&ins_impl.state, INV_STATE_DIM,
+ (float*)&ins_impl.cmd, INV_COMMAND_DIM,
+ invariant_model, dt);
+ ins_impl.state = new_state;
+
+ // normalize quaternion
+ FLOAT_QUAT_NORMALIZE(ins_impl.state.quat);
+
+ // set global state
+ FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
+#if INS_UPDATE_FW_ESTIMATOR
+ // Some stupid lines of code for neutrals
+ eulers.phi -= ins_roll_neutral;
+ eulers.theta -= ins_pitch_neutral;
+ stateSetNedToBodyEulers_f(&eulers);
+#else
+ stateSetNedToBodyQuat_f(&ins_impl.state.quat);
+#endif
+ RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias);
+ stateSetBodyRates_f(&body_rates);
+ stateSetPositionNed_f(&ins_impl.state.pos);
+ stateSetSpeedNed_f(&ins_impl.state.speed);
+
+ //------------------------------------------------------------//
+
+ RunOnceEvery(3,{
+ DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice,
+ &ins_impl.state.quat.qi,
+ &eulers.phi,
+ &eulers.theta,
+ &eulers.psi,
+ &ins_impl.state.speed.x,
+ &ins_impl.state.speed.y,
+ &ins_impl.state.speed.z,
+ &ins_impl.state.pos.x,
+ &ins_impl.state.pos.y,
+ &ins_impl.state.pos.z,
+ &ins_impl.state.bias.p,
+ &ins_impl.state.bias.q,
+ &ins_impl.state.bias.r,
+ &ins_impl.state.as,
+ &ins_impl.state.hb,
+ &ins_impl.meas.baro_alt,
+ &ins_impl.meas.pos_gps.z)
+ });
+
+}
+
+void ahrs_update_gps(void) {
+
+ if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING) {
+
+#if INS_UPDATE_FW_ESTIMATOR
+ if (state.utm_initialized_f) {
+ // position (local ned)
+ ins_impl.meas.pos_gps.x = (gps.utm_pos.north / 100.) - state.utm_origin_f.north;
+ ins_impl.meas.pos_gps.y = (gps.utm_pos.east / 100.) - state.utm_origin_f.east;
+ ins_impl.meas.pos_gps.z = state.utm_origin_f.alt - (gps.hmsl / 1000.);
+ // speed
+ ins_impl.meas.speed_gps.x = gps.ned_vel.x / 100.;
+ ins_impl.meas.speed_gps.y = gps.ned_vel.y / 100.;
+ ins_impl.meas.speed_gps.z = gps.ned_vel.z / 100.;
+ }
+#else
+ if (state.ned_initialized_f) {
+ struct NedCoor_f gps_pos_cm_ned;
+ ned_of_ecef_point_f(&gps_pos_cm_ned, &state.ned_origin_f, &gps.ecef_pos);
+ VECT3_SDIV(ins_impl.meas.pos_gps, gps_pos_m_ned, 100.);
+ struct NedCoor_f gps_speed_cm_s_ned;
+ ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &state.ned_origin_f, &gps.ecef_vel);
+ VECT3_SDIV(ins_impl.meas.speed_gps, gps_speed_cm_s_ned, 100.);
+ }
+#endif
+ }
+
+}
+
+void ins_update_gps(void) {}
+
+void ins_update_baro(void) {}
+
+static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) {
+ static float ins_qfe = 101325.0;
+ static float alpha = 10.;
+ static int32_t i = 1;
+ static float baro_moy = 0.;
+ static float baro_prev = 0.;
+
+ if (!ins_baro_initialised) {
+ // try to find a stable qfe
+ // TODO generic function in pprz_isa ?
+ if (i == 1) {
+ baro_moy = *pressure;
+ baro_prev = *pressure;
+ }
+ baro_moy = (baro_moy*(i-1) + *pressure)/i;
+ alpha = (10.*alpha + (baro_moy-baro_prev))/(11.);
+ baro_prev = baro_moy;
+ // test stop condition
+ if (fabs(alpha) < 0.005) {
+ ins_qfe = baro_moy;
+ ins_baro_initialised = TRUE;
+ }
+ if (i == 250) {
+ ins_qfe = *pressure;
+ ins_baro_initialised = TRUE;
+ }
+ i++;
+ }
+ else { /* normal update with baro measurement */
+ ins_impl.meas.baro_alt = -pprz_isa_height_of_pressure(*pressure, ins_qfe); // Z down
+ }
+}
+
+void ahrs_update_accel(void) {
+}
+
+void ahrs_update_mag(void) {
+ MAGS_FLOAT_OF_BFP(ins_impl.meas.mag, imu.mag);
+}
+
+
+/** Compute dynamic mode
+ *
+ * x_dot = evolution_model + (gain_matrix * error)
+ */
+static inline void invariant_model(float * o, const float * x, const int n, const float * u, const int m __attribute__((unused))) {
+
+ const struct inv_state * s = (struct inv_state *)x;
+ const struct inv_command * c = (struct inv_command *)u;
+ struct inv_state s_dot;
+ struct FloatRates rates;
+ struct FloatVect3 tmp_vect;
+ struct FloatQuat tmp_quat;
+ float norm;
+
+ // test accel sensitivity
+ if (fabs(s->as) < 0.1) {
+ // too small, return x_dot = 0 to avoid division by 0
+ float_vect_zero(o, n);
+ // TODO set ins state to error
+ return;
+ }
+
+ /* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
+ RATES_DIFF(rates, c->rates, s->bias);
+ FLOAT_VECT3_ASSIGN(tmp_vect, rates.p, rates.q, rates.r);
+ FLOAT_QUAT_VMUL_LEFT(s_dot.quat, s->quat, tmp_vect);
+ FLOAT_QUAT_SMUL(s_dot.quat, s_dot.quat, 0.5);
+
+ FLOAT_QUAT_VMUL_RIGHT(tmp_quat, s->quat, ins_impl.corr.LE);
+ FLOAT_QUAT_ADD(s_dot.quat, tmp_quat);
+
+ norm = FLOAT_QUAT_NORM(s->quat);
+ norm = 1. - (norm*norm);
+ FLOAT_QUAT_SMUL(tmp_quat, s->quat, norm);
+ FLOAT_QUAT_ADD(s_dot.quat, tmp_quat);
+
+ /* dot_V = A + (1/as) * (q * am * q-1) + ME */
+ FLOAT_QUAT_RMAT_B2N(s_dot.speed, s->quat, c->accel);
+ FLOAT_VECT3_SMUL(s_dot.speed, s_dot.speed, 1. / (s->as));
+ FLOAT_VECT3_ADD(s_dot.speed, A);
+ FLOAT_VECT3_ADD(s_dot.speed, ins_impl.corr.ME);
+
+ /* dot_X = V + NE */
+ FLOAT_VECT3_SUM(s_dot.pos, s->speed, ins_impl.corr.NE);
+
+ /* bias_dot = q-1 * (OE) * q */
+ FLOAT_QUAT_RMAT_N2B(tmp_vect, s->quat, ins_impl.corr.OE);
+ RATES_ASSIGN(s_dot.bias, tmp_vect.x, tmp_vect.y, tmp_vect.z);
+
+ /* as_dot = as * RE */
+ s_dot.as = (s->as) * (ins_impl.corr.RE);
+
+ /* hb_dot = SE */
+ s_dot.hb = ins_impl.corr.SE;
+
+ // set output
+ memcpy(o, &s_dot, n*sizeof(float));
+}
+
+/** Compute correction vectors
+ * E = ( ŷ - y )
+ * LE, ME, NE, OE : ( gain matrix * error )
+ */
+static inline void error_output(struct InsFloatInv * _ins) {
+
+ struct FloatVect3 YBt, I, Ev, Eb, Ex, Itemp, Ebtemp, Evtemp;
+ float Eh;
+ float temp;
+
+ // test accel sensitivity
+ if (fabs(_ins->state.as) < 0.1) {
+ // too small, don't do anything to avoid division by 0
+ return;
+ }
+
+ /* YBt = q * yB * q-1 */
+ FLOAT_QUAT_RMAT_B2N(YBt, _ins->state.quat, _ins->meas.mag);
+
+ FLOAT_QUAT_RMAT_B2N(I, _ins->state.quat, _ins->cmd.accel);
+ FLOAT_VECT3_SMUL(I, I, 1. / (_ins->state.as));
+
+ /*--------- E = ( ŷ - y ) ----------*/
+ /* Eb = ( B - YBt ) */
+ FLOAT_VECT3_DIFF(Eb, B, YBt);
+
+ // pos and speed error only if GPS data are valid
+ if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING
+#if INS_UPDATE_FW_ESTIMATOR
+ && state.utm_initialized_f
+#else
+ && state.ned_initialized_f
+#endif
+ ) {
+ /* Ev = (V - YV) */
+ FLOAT_VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps);
+ /* Ex = (X - YX) */
+ FLOAT_VECT3_DIFF(Ex, _ins->state.pos, _ins->meas.pos_gps);
+ }
+ else {
+ FLOAT_VECT3_ZERO(Ev);
+ FLOAT_VECT3_ZERO(Ex);
+ }
+ /* Eh = < X,e3 > - hb - YH */
+ Eh = _ins->state.pos.z - _ins->state.hb - _ins->meas.baro_alt;
+
+ /*--------------Gains--------------*/
+
+ /**** LvEv + LbEb = -lvIa x Ev + lb < B x Eb, Ia > Ia *****/
+ FLOAT_VECT3_SMUL(Itemp, I, -_ins->gains.lv/100.);
+ FLOAT_VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
+
+ FLOAT_VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
+ temp = FLOAT_VECT3_DOT_PRODUCT(Ebtemp, I);
+ temp = (_ins->gains.lb/100.) * temp;
+
+ FLOAT_VECT3_SMUL(Ebtemp, I, temp);
+ FLOAT_VECT3_ADD(Evtemp, Ebtemp);
+ FLOAT_VECT3_COPY(_ins->corr.LE, Evtemp);
+
+ /***** MvEv + MhEh = -mv * Ev + (-mh * )********/
+ _ins->corr.ME.x = (-_ins->gains.mv) * Ev.x + 0.;
+ _ins->corr.ME.y = (-_ins->gains.mv) * Ev.y + 0.;
+ _ins->corr.ME.z = ((-_ins->gains.mvz) * Ev.z) + ((-_ins->gains.mh) * Eh);
+
+ /****** NxEx + NhEh = -nx * Ex + (-nh * ) ********/
+ _ins->corr.NE.x = (-_ins->gains.nx) * Ex.x + 0.;
+ _ins->corr.NE.y = (-_ins->gains.nx) * Ex.y + 0.;
+ _ins->corr.NE.z = ((-_ins->gains.nxz) * Ex.z) + ((-_ins->gains.nh) * Eh);
+
+ /****** OvEv + ObEb = ovIa x Ev - ob < B x Eb, Ia > Ia ********/
+ FLOAT_VECT3_SMUL(Itemp, I, _ins->gains.ov/1000.);
+ FLOAT_VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev);
+
+ FLOAT_VECT3_CROSS_PRODUCT(Ebtemp, B, Eb);
+ temp = FLOAT_VECT3_DOT_PRODUCT(Ebtemp, I);
+ temp = (-_ins->gains.ob/1000.) * temp;
+
+ FLOAT_VECT3_SMUL(Ebtemp, I, temp);
+ FLOAT_VECT3_ADD(Evtemp, Ebtemp);
+ FLOAT_VECT3_COPY(_ins->corr.OE, Evtemp);
+
+ /* a scalar */
+ /****** RvEv + RhEh = rv < Ia, Ev > + (-rhEh) **************/
+ _ins->corr.RE = ((_ins->gains.rv/100.) * FLOAT_VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh/10000.) * Eh);
+
+ /****** ShEh ******/
+ _ins->corr.SE = (_ins->gains.sh) * Eh;
+
+}
+
diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.h b/sw/airborne/subsystems/ins/ins_float_invariant.h
new file mode 100644
index 0000000000..9a7e5aafd5
--- /dev/null
+++ b/sw/airborne/subsystems/ins/ins_float_invariant.h
@@ -0,0 +1,118 @@
+/*
+ * Copyright (C) 2012-2013 Jean-Philippe Condomines, Gautier Hattenberger
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, see
+ * .
+ *
+ */
+
+
+/*
+ * For more information, please send an email to "jp.condomines@gmail.com"
+*/
+
+#ifndef INS_FLOAT_INVARIANT_H
+#define INS_FLOAT_INVARIANT_H
+
+#include "subsystems/ahrs.h"
+#include "subsystems/ins.h"
+
+/** Invariant filter state dimension
+ */
+#define INV_STATE_DIM 15
+
+/** Invariant filter state
+ */
+struct inv_state {
+ struct FloatQuat quat; ///< Estimated attitude (quaternion)
+ struct FloatRates bias; ///< Estimated gyro biases
+ struct NedCoor_f speed; ///< Estimates speed
+ struct NedCoor_f pos; ///< Estimates position
+ float hb; ///< Estimates barometers bias
+ float as; ///< Estimated accelerometer sensitivity
+//float cs; ///< Estimated magnetic sensitivity
+};
+
+/** Invariant filter measurement vector dimension
+ */
+#define INV_MEASURE_DIM 10
+
+/** Invariant filter measurement vector
+ */
+struct inv_measures {
+ struct NedCoor_f pos_gps; ///< Measured gps position
+ struct NedCoor_f speed_gps; ///< Measured gps speed
+ struct FloatVect3 mag; ///< Measured magnetic field
+ float baro_alt; ///< Measured barometric altitude
+};
+
+/** Invariant filter command vector dimension
+ */
+#define INV_COMMAND_DIM 6
+
+/** Invariant filter command vector
+ */
+struct inv_command {
+ struct FloatRates rates; ///< Input gyro rates
+ struct FloatVect3 accel; ///< Input accelerometers
+};
+
+/** Invariant filter correction gains
+ */
+struct inv_correction_gains {
+ struct FloatVect3 LE; ///< Correction gains on attitude
+ struct FloatVect3 ME; ///< Correction gains on speed
+ struct FloatVect3 NE; ///< Correction gains on position
+ struct FloatVect3 OE; ///< Correction gains on gyro biases
+ float RE; ///< Correction gains on accel bias
+ float SE; ///< Correction gains on barometer bias
+};
+
+/** Invariant filter tuning gains
+ */
+struct inv_gains {
+ float lv; ///< Tuning parameter of speed error on attitude
+ float lb; ///< Tuning parameter of mag error on attitude
+ float mv; ///< Tuning parameter of horizontal speed error on speed
+ float mvz; ///< Tuning parameter of vertical speed error on speed
+ float mh; ///< Tuning parameter of baro error on vertical speed
+ float nx; ///< Tuning parameter of horizontal position error on position
+ float nxz; ///< Tuning parameter of vertical position error on position
+ float nh; ///< Tuning parameter of baro error on vertical position
+ float ov; ///< Tuning parameter of speed error on gyro biases
+ float ob; ///< Tuning parameter of mag error on gyro biases
+ float rv; ///< Tuning parameter of speed error on accel biases
+ float rh; ///< Tuning parameter of baro error on accel biases (vertical projection)
+ float sh; ///< Tuning parameter of baro error on baro bias
+};
+
+/** Invariant filter structure
+ */
+struct InsFloatInv {
+ struct inv_state state; ///< state vector
+ struct inv_measures meas; ///< measurement vector
+ struct inv_command cmd; ///< command vector
+ struct inv_correction_gains corr; ///< correction gains
+ struct inv_gains gains; ///< tuning gains
+};
+
+extern struct InsFloatInv ins_impl;
+
+extern float ins_roll_neutral;
+extern float ins_pitch_neutral;
+
+#endif /* INS_FLOAT_INVARIANT_H */
+
diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c
index 9e2623e3fe..7989779d36 100644
--- a/sw/airborne/subsystems/navigation/common_nav.c
+++ b/sw/airborne/subsystems/navigation/common_nav.c
@@ -113,16 +113,17 @@ unit_t nav_reset_reference( void ) {
nav_utm_north0 = gps.utm_pos.north/100;
#endif
+ previous_ground_alt = ground_alt;
+ ground_alt = gps.hmsl/1000.;
+
// reset state UTM ref
- struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
+ struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, ground_alt, nav_utm_zone0 };
stateSetLocalUtmOrigin_f(&utm0);
// realign INS if needed
ins.hf_realign = TRUE;
ins.vf_realign = TRUE;
- previous_ground_alt = ground_alt;
- ground_alt = gps.hmsl/1000.;
return 0;
}
diff --git a/sw/simulator/nps/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.c
index 7102706eab..1e20c5c376 100644
--- a/sw/simulator/nps/nps_fdm_jsbsim.c
+++ b/sw/simulator/nps/nps_fdm_jsbsim.c
@@ -434,10 +434,15 @@ static void init_ltp(void) {
fdm.ltp_g.z = 9.81;
#ifdef AHRS_H_X
-#pragma message "Using magnetic field as defined in airframe file."
+#pragma message "Using magnetic field as defined in airframe file (AHRS section)."
fdm.ltp_h.x = AHRS_H_X;
fdm.ltp_h.y = AHRS_H_Y;
fdm.ltp_h.z = AHRS_H_Z;
+#elif defined INS_H_X
+#pragma message "Using magnetic field as defined in airframe file (INS section)."
+ fdm.ltp_h.x = INS_H_X;
+ fdm.ltp_h.y = INS_H_Y;
+ fdm.ltp_h.z = INS_H_Z;
#else
fdm.ltp_h.x = 0.4912;
fdm.ltp_h.y = 0.1225;