[paparazzi-commits] [6298] changed the output

paparazzi-commits
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6298] changed the output

From:	Martin Dieblich
Subject:	[paparazzi-commits] [6298] changed the output
Date:	Thu, 28 Oct 2010 12:21:54 +0000
Revision: 6298
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6298
Author:   mdieblich
Date:     2010-10-28 12:21:53 +0000 (Thu, 28 Oct 2010)
Log Message:
-----------
changed the output

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp     
2010-10-28 11:38:14 UTC (rev 6297)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp     
2010-10-28 12:21:53 UTC (rev 6298)
@@ -46,13 +46,13 @@
        printf("FILTER output will be in ");
        #if FILTER_OUTPUT_IN_NED
                printf("NED\n");
-       #else
+  #else /* FILTER_OUTPUT_IN_ECEF */
                printf("ECEF\n");
-       #endif
+       #endif /* FILTER_OUTPUT_IN_NED / ECEF */
        
        #if UPDATE_WITH_GRAVITY
        printf("the orientation becomes UPDATED with the GRAVITY\n");
-  #endif
+  #endif /* UPDATE_WITH_GRAVITY */
 
   init_ins_state();
   set_reference_direction();
@@ -91,20 +91,20 @@
   uint8_t read_ok;
   #if WITH_GPS
   struct raw_log_entry e = next_GPS(file_descriptor);
-  #else
+  #else /* WITH_GPS */
   struct raw_log_entry e = read_raw_log_entry(file_descriptor, &read_ok);
   pos_0_ecef = Vector3d(4627578.56, 119659.25, 4373248.00);
   pos_cov_0 = Vector3d::Ones()*100;
   speed_0_ecef    = Vector3d::Zero();
   speed_cov_0 = Vector3d::Ones();
-  #endif // WITH_GPS
+  #endif /* WITH_GPS */
   
   #ifdef EKNAV_FROM_LOG_DEBUG
     int imu_ready = 0, 
         mag_ready = 0,
         baro_ready = 0,
         gps_ready = 0;
-  #endif
+  #endif /* EKNAV_FROM_LOG_DEBUG */
   
   for(read_ok = 1; (read_ok) && NOT_ENOUGH_MEASUREMENTS(imu_measurements, 
magnetometer_measurements, baro_measurements, gps_measurements); e = 
read_raw_log_entry(file_descriptor, &read_ok)){
     if(IMU_READY(e.message.valid_sensors)){
@@ -165,7 +165,7 @@
         gps_ready = 1;
       }
     }
-    #endif
+    #endif /* EKNAV_FROM_LOG_DEBUG */
   }
   // setting the covariance
   gravity.weight_of_the_measurement *= imu_measurements;
@@ -177,7 +177,7 @@
   #ifdef EKNAV_FROM_LOG_DEBUG
   DISPLAY_FLOAT_RMAT("     B", attitude_profile_matrix);
   DISPLAY_FLOAT_RMAT("sigmaB", sigmaB);
-  #endif
+  #endif /* EKNAV_FROM_LOG_DEBUG */
   
   //  setting the initial orientation
   q_ned2body = estimated_attitude(attitude_profile_matrix, 1000, 1e-6, sigmaB, 
&sigma_q);
@@ -227,19 +227,15 @@
                // use the gravity as reference
                ins.obs_vector(ins.avg_state.position.normalized(), 
VECT3_AS_VECTOR3D(imu_float.accel), accelerometer_noise.norm());
        }
-  #endif
+  #endif /* UPDATE_WITH_GRAVITY */
   
-  if(BARO_READY(data_valid)){
+   if(BARO_READY(data_valid)){
     ins.obs_baro_report(baro_0_height+imu_baro_height, baro_noise);
-  }
+  }   // comment out multiple lines */
   
   if(GPS_READY(data_valid)){
                ins.obs_gps_pv_report(VECT3_AS_VECTOR3D(imu_ecef_pos)/100, 
VECT3_AS_VECTOR3D(imu_ecef_vel)/100, 10*gps_pos_noise, 10*gps_speed_noise);
-    
-    Matrix<double, 3, 3> ecef2enu;
-    RMAT_TO_EIGENMATRIX(ecef2enu,current_ltp.ltp_of_ecef.m);
-
-       }
+       }   // comment out multiple lines */
 }
 
 
@@ -322,9 +318,10 @@
   ltp_def_from_ecef_d(&current_ltp, &ecef_pos_pprz);
   DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
   QUAT_ENU_FROM_TO_NED(q_ecef2enu, q_ecef2ned);
+//FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c,          _a2b,        _b2c)
+// a = ecef b = ned c = body
+  FLOAT_QUAT_COMP_INV_NORM_SHORTEST(q_ecef2body, q_ecef2ned, q_ned2body);
   
-  FLOAT_QUAT_COMP_NORM_SHORTEST(q_ecef2body, q_ecef2ned, q_ned2body);
-  
   #ifdef EKNAV_FROM_LOG_DEBUG
   printf("Right after initialization:\n");
   DISPLAY_DOUBLE_QUAT("\t ned2body quaternion:", q_ned2body);
@@ -335,7 +332,7 @@
   DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ecef2ned);
   DISPLAY_DOUBLE_QUAT("\tecef2body quaternion:", q_ecef2body);
   DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ecef2body);
-  #endif
+  #endif /* EKNAV_FROM_LOG_DEBUG */
   
   return DOUBLEQUAT_AS_QUATERNIOND(q_ecef2body);
 }
@@ -425,22 +422,30 @@
   int32_t z = pos_ned.z/0.0039;
 
   fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, 
AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z);
-  
+  #if 0
   QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), 
-ins.avg_state.orientation.x(),
                 -ins.avg_state.orientation.y(), 
-ins.avg_state.orientation.z());
   QUAT_ASSIGN(q_ned2enu, 0, M_SQRT1_2, M_SQRT1_2, 0);
   
   FLOAT_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
        FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body);               
// q_enu2body = q_ecef2body * (q_ecef2enu)^*
-  FLOAT_QUAT_COMP(q_ned2body, q_ned2enu, q_enu2body)                           
        // q_ned2body = q_enu2body * q_ned2enu
+  FLOAT_QUAT_COMP(q_ned2body, q_ned2enu, q_enu2body);                          
        // q_ned2body = q_enu2body * q_ned2enu
 
+  #else /* if 0 */
+  QUATERNIOND_AS_DOUBLEQUAT(q_ecef2body, ins.avg_state.orientation);
+  DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
+  FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body);
+  QUAT_ENU_FROM_TO_NED(q_enu2body, q_ned2body);
   
+  #endif /* if 0 */
+  
   struct FloatEulers e;
   FLOAT_EULERS_OF_QUAT(e, q_ned2body);
-       
+  
+  
        #if PRINT_EULER_NED
                printf("EULER % 6.1f % 6.1f % 6.1f\n", e.phi*180*M_1_PI, 
e.theta*180*M_1_PI, e.psi*180*M_1_PI);
-       #endif
+       #endif /* PRINT_EULER_NED */
   fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e.phi, 
e.theta, e.psi);
   fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f 
%f %f\n", time, AC_ID,
                                sqrt(ins.cov( 0, 0)),  sqrt(ins.cov( 1, 1)),  
sqrt(ins.cov( 2, 2)), 
@@ -448,8 +453,8 @@
                                sqrt(ins.cov( 6, 6)),  sqrt(ins.cov( 7, 7)),  
sqrt(ins.cov( 8, 8)), 
                                sqrt(ins.cov( 9, 9)),  sqrt(ins.cov(10,10)),  
sqrt(ins.cov(11,11)));
   fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, 
ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), 
ins.avg_state.gyro_bias(2));
-       
-#else
+
+#else /* FILTER_OUTPUT_IN_ECEF */
   int32_t xdd = 0;
   int32_t ydd = 0;
   int32_t zdd = 0;
@@ -476,5 +481,5 @@
                                sqrt(ins.cov( 6, 6)),  sqrt(ins.cov( 7, 7)),  
sqrt(ins.cov( 8, 8)), 
                                sqrt(ins.cov( 9, 9)),  sqrt(ins.cov(10,10)),  
sqrt(ins.cov(11,11)));
   fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, 
ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), 
ins.avg_state.gyro_bias(2));
-#endif
+#endif /* FILTER_OUTPUT_IN_NED / ECEF */
 }

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp     
2010-10-28 11:38:14 UTC (rev 6297)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp     
2010-10-28 12:21:53 UTC (rev 6298)
@@ -51,9 +51,7 @@
 #define PRINT_GPS 0
 #define PRINT_EULER_NED 0
 
-/** baro-sensor **/
 
-
 /** geodetic **/
 #define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) VECT3_ASSIGN(ref, 
0.51562740288882, -0.05707735220832, 0.85490967783446)
 Vector3d reference_direction;
@@ -90,7 +88,8 @@
 // NOTE: Measured during hovering in the air. Movement in the range of a 1 m³ 
cube with (approx.) max. 0.2 m/s speed.
 /// IMU
 const Vector3d gyroscope_noise      ( 1.0449e-1,  1.1191e-1,  4.5906e-2 );
-const Vector3d gyro_stability_noise ( 1.0000e-6,  1.0000e-6,  1.0000e-6 );
+//const Vector3d gyroscope_noise      ( 1.0000e-1,  1.0000e-1,  1.0000e-1 );
+const Vector3d gyro_stability_noise ( 1.0000e-3,  1.0000e-3,  1.0000e-3 );
 const Vector3d accelerometer_noise  ( 2.5457e+0,  1.8242e+0,  1.5660e+0 );
 const unsigned short imu_frequency  = 512;
 

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h      
2010-10-28 11:38:14 UTC (rev 6297)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h      
2010-10-28 12:21:53 UTC (rev 6298)
@@ -157,7 +157,7 @@
  */
 
 #define VECTOR_AS_VECT3(coords, vector) { coords.x = vector(0); coords.y = 
vector(1); coords.z = vector(2);}
-#define QUATERNIOND_AS_DOUBLEQUAT(doublequat, quaterniond) {doublequat.qi = 
quaterniond.w(); doublequat.qx = quaterniond.x(); doublequat.qy = 
quaterniond.y(); doublequat.qz = quaterniond.z();}
+#define QUATERNIOND_AS_DOUBLEQUAT(doublequat, quaterniond) {(doublequat).qi = 
(quaterniond).w(); (doublequat).qx = (quaterniond).x(); (doublequat).qy = 
(quaterniond).y(); (doublequat).qz = (quaterniond).z();}
 #define PPRZ_LLA_TO_EIGEN_ECEF(lla, ecef){     \
        struct EcefCoor_f ecef_pprz;                                            
        \
        ecef_of_lla_f(&ecef_pprz, &lla);                                        
\
[Prev in Thread]
Current Thread
[Next in Thread]
[paparazzi-commits] [6298] changed the output, Martin Dieblich <=
Prev by Date: [paparazzi-commits] [6297] grouped the ahrs variables into one structure
Next by Date: [paparazzi-commits] [6299] ins.h from modules is different so it is renamed
Previous by thread: [paparazzi-commits] [6297] grouped the ahrs variables into one structure
Next by thread: [paparazzi-commits] [6299] ins.h from modules is different so it is renamed
Index(es):
- Date
- Thread