[paparazzi-commits] [6340] added an initialisation function for imu_float

[antoine drouin]

Revision: 6340
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6340
Author:   poine
Date:     2010-11-04 15:50:58 +0000 (Thu, 04 Nov 2010)
Log Message:
-----------
added an initialisation function for imu_float

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/subsystems/imu.c
    paparazzi3/trunk/sw/airborne/subsystems/imu.h

Modified: paparazzi3/trunk/sw/airborne/subsystems/imu.c
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu.c       2010-11-04 15:48:07 UTC 
(rev 6339)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu.c       2010-11-04 15:50:58 UTC 
(rev 6340)
@@ -1,7 +1,7 @@
 /*
  * $Id$
  *
- * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ * Copyright (C) 2008-2010 The Paparazzi Team
  *
  * This file is part of paparazzi.
  *
@@ -38,7 +38,7 @@
     Compute quaternion and rotation matrix
     for conversions between body and imu frame
   */
-#ifdef IMU_BODY_TO_IMU_PHI
+#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined  
IMU_BODY_TO_IMU_PSI
   struct Int32Eulers body_to_imu_eulers =
     { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI),
       ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA),
@@ -46,7 +46,31 @@
   INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers);
   INT32_QUAT_NORMALISE(imu.body_to_imu_quat);
   INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers);
+#else
+  INT32_QUAT_ZERO(imu.body_to_imu_quat);
+  INT32_RMAT_ZERO(imu.body_to_imu_rmat);
 #endif
 
   imu_impl_init();
 }
+
+
+void imu_float_init(struct ImuFloat* imuf) {
+
+  /*
+    Compute quaternion and rotation matrix
+    for conversions between body and imu frame
+  */
+#if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined  
IMU_BODY_TO_IMU_PSI
+  EULERS_ASSIGN(imuf->body_to_imu_eulers,
+               IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, 
IMU_BODY_TO_IMU_PSI);
+  FLOAT_QUAT_OF_EULERS(imuf->body_to_imu_quat, imuf->body_to_imu_eulers);
+  FLOAT_QUAT_NORMALISE(imuf->body_to_imu_quat);
+  FLOAT_RMAT_OF_EULERS(imuf->body_to_imu_rmat, imuf->body_to_imu_eulers);
+#else
+  EULERS_ASSIGN(imuf->body_to_imu_eulers, 0., 0., 0.);
+  FLOAT_QUAT_ZERO(imuf->body_to_imu_quat);
+  FLOAT_RMAT_ZERO(imuf->body_to_imu_rmat);
+#endif
+
+}

Modified: paparazzi3/trunk/sw/airborne/subsystems/imu.h
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/imu.h       2010-11-04 15:48:07 UTC 
(rev 6339)
+++ paparazzi3/trunk/sw/airborne/subsystems/imu.h       2010-11-04 15:50:58 UTC 
(rev 6340)
@@ -59,6 +59,8 @@
   uint32_t sample_count;
 };
 
+extern void imu_float_init(struct ImuFloat* imuf);
+
 /* underlying hardware */
 #ifdef IMU_TYPE_H
 #include IMU_TYPE_H