[paparazzi-commits] [6098] Fixed bug in the algebra-doc, added "synthetic mag mode" to test_libeknav4.cpp to feed only known data to it.

[Martin Dieblich]

Revision: 6098
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6098
Author:   mdieblich
Date:     2010-10-07 18:26:02 +0000 (Thu, 07 Oct 2010)
Log Message:
-----------
Fixed bug in the algebra-doc, added "synthetic mag mode" to test_libeknav4.cpp 
to feed only known data to it.

Modified Paths:
--------------
    paparazzi3/trunk/doc/pprz_algebra/quaternion.tex
    paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.hpp

Modified: paparazzi3/trunk/doc/pprz_algebra/quaternion.tex
===================================================================
--- paparazzi3/trunk/doc/pprz_algebra/quaternion.tex    2010-10-07 14:45:09 UTC 
(rev 6097)
+++ paparazzi3/trunk/doc/pprz_algebra/quaternion.tex    2010-10-07 18:26:02 UTC 
(rev 6098)
@@ -27,8 +27,8 @@
 \inHfile{FLOAT\_QUAT\_ZERO(q)}{pprz\_algebra\_float}
 
 \subsubsection*{$\quat{} = \transp{(\quat i,\quat x,\quat y,\quat z)}$}
+\textit{i} is the real part of the quaternion.
 \begin{equation}
-\textit{i} is the real part of the quaternion.
  \quat a = \begin{pmatrix}i\\ x \\ y \\ z \end{pmatrix}
 \end{equation}
 \inHfile{QUAT\_ASSIGN(q, i, x, y, z)}{pprz\_algebra}
@@ -212,4 +212,4 @@
 \end{pmatrix}
 \multiplication \quat{}
 \end{equation}
-\inHfile{FLOAT\_QUAT\_DERIVATIVE\_LAGRANGE(qd, r, q)}{pprz\_algebra\_float}
\ No newline at end of file
+\inHfile{FLOAT\_QUAT\_DERIVATIVE\_LAGRANGE(qd, r, q)}{pprz\_algebra\_float}

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp       
2010-10-07 14:45:09 UTC (rev 6097)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp       
2010-10-07 18:26:02 UTC (rev 6098)
@@ -3,7 +3,6 @@
 
 #include <stdlib.h>
 
- 
 struct timespec start, prev;
 FILE* ins_logfile;             // note: initilaized in init_ins_state
 unsigned int counter;
@@ -142,16 +141,28 @@
   clock_gettime(TIMER, &now);
   
   double dt_imu_freq = 0.001953125; //  1/512; // doesn't work?
-  
+  #if SYNTHETIC_MAG_MODE
+  ins.predict(Vector3d::Zero(), Vector3d(0,0,-GRAVITY), dt_imu_freq);
+  #else
   ins.predict(RATES_AS_VECTOR3D(imu_float.gyro), 
VECT3_AS_VECTOR3D(imu_float.accel), dt_imu_freq);
+  #endif
   
   if(MAG_READY(data_valid)){
+               #if SYNTHETIC_MAG_MODE
+               DoubleVect3 ref_dir_ned;
+               EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned);
+               std::cout << "MAG-UPDATE\t";
+               ins.obs_vector(reference_direction, 
VECT3_AS_VECTOR3D(ref_dir_ned), mag_noise);
+               //ins.obs_vector(reference_direction, reference_direction, 
mag_noise);
+               #else
                ins.obs_vector(reference_direction, 
VECT3_AS_VECTOR3D(imu_float.mag), mag_noise);
+               #endif
        }
   
   #if UPDATE_WITH_GRAVITY
   if(CLOSE_TO_GRAVITY(imu_float.accel)){
                // use the gravity as reference
+               std::cout << "GRAV-UPDATE\t";
                ins.obs_vector(ins.avg_state.position.normalized(), 
VECT3_AS_VECTOR3D(imu_float.accel), 1.0392e-3);
        }
   #endif
@@ -162,7 +173,6 @@
   
   print_estimator_state(absTime(time_diff(now, start)));
   
-  
 }
 #endif
 
@@ -196,6 +206,11 @@
        LLA_ASSIGN(pos_0_lla, TOULOUSE_LATTITUDE, TOULOUSE_LONGITUDE, 
TOULOUSE_HEIGHT)
        PPRZ_LLA_TO_EIGEN_ECEF(pos_0_lla, pos_0_ecef);
        
+       #if SYNTHETIC_MAG_MODE
+       //pos_0_ecef = Vector3d(4627511.37,119627.69,4373302.43);               
        // measured at ??
+       pos_0_ecef = Vector3d(4625562,115469,4375209);                  
+       #endif
+       
        printf("Starting position\t%f\t%f\t%f\n", pos_0_ecef(0), pos_0_ecef(1), 
pos_0_ecef(2));
        
        ins.avg_state.position    = pos_0_ecef;

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.hpp       
2010-10-07 14:45:09 UTC (rev 6097)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.hpp       
2010-10-07 18:26:02 UTC (rev 6098)
@@ -37,7 +37,7 @@
 /* constants */
 /** Compilation-control **/
 #define RUN_FILTER 1
-#define UPDATE_WITH_GRAVITY 0
+#define UPDATE_WITH_GRAVITY 1
 #define SYNTHETIC_MAG_MODE 1
 #define FILTER_OUTPUT_IN_NED 1