[ins] add new ins based on invariant filter

- experimental invariant filter for complet ins
- tested on fixed-wing only, but should work on rotorcraft as well
- generic vector and rung-kutta library
- fix alt init in utm local frame
- settable channel order on mag hmc58xx module

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)

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; }
+  }
+}
 
 
 

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 */

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
+

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();

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};

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

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
+ * <http://www.gnu.org/licenses/>.
+ *
+ */
+
+
+/*
+ * 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 */
+

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;
 }