ahrs float_cmpl: compute fixed point representations as well

paparazzi · Feb 20, 2012 · f1656ef · f1656ef
1 parent 7379b9a
commit f1656ef
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 0 deletions.
diff --git a/sw/airborne/subsystems/ahrs.h b/sw/airborne/subsystems/ahrs.h
@@ -93,6 +93,13 @@ extern float ahrs_mag_offset;
     RATES_BFP_OF_REAL(ahrs.body_rate, ahrs_float.body_rate);                   \
   }
 
+#define AHRS_IMU_INT_OF_FLOAT() {                                       \
+    QUAT_BFP_OF_REAL(ahrs.ltp_to_imu_quat, ahrs_float.ltp_to_imu_quat); \
+    EULERS_BFP_OF_REAL(ahrs.ltp_to_imu_euler, ahrs_float.ltp_to_imu_euler); \
+    RMAT_BFP_OF_REAL(ahrs.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat); \
+    RATES_BFP_OF_REAL(ahrs.imu_rate, ahrs_float.imu_rate);            \
+  }
+
 /** AHRS initialization. Called at startup.
  *  Needs to be implemented by each AHRS algorithm.
  */

diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c
@@ -96,6 +96,10 @@ void ahrs_align(void) {
   /* Compute initial body orientation */
   compute_body_orientation_and_rates();
 
+  /* compute fixed point representations */
+  AHRS_INT_OF_FLOAT();
+  AHRS_IMU_INT_OF_FLOAT();
+
   /* used averaged gyro as initial value for bias */
   struct Int32Rates bias0;
   RATES_COPY(bias0, ahrs_aligner.lp_gyro);
@@ -129,17 +133,22 @@ void ahrs_propagate(void) {
 
   const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
 #ifdef AHRS_PROPAGATE_RMAT
+#pragma message "AHRS: propagation using rotation matrix representation"
   FLOAT_RMAT_INTEGRATE_FI(ahrs_float.ltp_to_imu_rmat, omega, dt );
   float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat);
   compute_imu_quat_and_euler_from_rmat();
 #endif
 #ifdef AHRS_PROPAGATE_QUAT
+#pragma message "AHRS: propagation using quaternion representation"
   FLOAT_QUAT_INTEGRATE(ahrs_float.ltp_to_imu_quat, omega, dt);
   FLOAT_QUAT_NORMALIZE(ahrs_float.ltp_to_imu_quat);
   compute_imu_rmat_and_euler_from_quat();
 #endif
   compute_body_orientation_and_rates();
 
+  /* compute fixed point representations */
+  AHRS_INT_OF_FLOAT();
+  AHRS_IMU_INT_OF_FLOAT();
 }
 
 void ahrs_update_accel(void) {