[test] update some mostly unused ahrs tests

paparazzi · Sep 8, 2014 · 4a5717a · 4a5717a
1 parent a77dc5a
commit 4a5717a
Show file tree

Hide file tree

Showing 4 changed files with 45 additions and 21 deletions.
diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c
@@ -24,7 +24,10 @@
  * Inertial Measurement Unit interface.
  */
 
+#ifdef BOARD_CONFIG
 #include BOARD_CONFIG
+#endif
+
 #include "subsystems/imu.h"
 #include "state.h"
 

diff --git a/sw/airborne/test/ahrs/Makefile b/sw/airborne/test/ahrs/Makefile
@@ -10,8 +10,8 @@ CFLAGS +=
 
 # imu wants airframe to fetch its neutrals
 # ahrs wants airframe to fetch IMU_BODY_TO_IMU_ANGLES
-CFLAGS += -I../../../../var/ahrs_test
-
+# have a fake one in ./generated/airframe.h
+CFLAGS += -I.
 
 #CFLAGS += -DIMU_BODY_TO_IMU_PHI=0 -DIMU_BODY_TO_IMU_THETA=0 -DIMU_BODY_TO_IMU_PSI=0
 # toulouse 0.51562740288882, -0.05707735220832, 0.85490967783446
@@ -25,19 +25,31 @@ CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.
 CFLAGS += -DLOW_NOISE_THRESHOLD=500000
 CFLAGS += -DLOW_NOISE_TIME=2
 
+FREQUENCY ?= 512
+
+CFLAGS += -DPERIODIC_FREQUENCY=$(FREQUENCY)
+CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(FREQUENCY)
+CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(FREQUENCY)
+CFLAGS += -DAHRS_MAG_CORRECT_FREQUENCY=50
+
 SRCS= run_ahrs_on_flight_log.c                    \
 	  ../../math/pprz_trig_int.c                  \
 	  ../../subsystems/ahrs.c                     \
 	  ../../subsystems/ahrs/ahrs_aligner.c        \
-	  ../../subsystems/imu.c
+	  ../../subsystems/imu.c                      \
+      ../../state.c                               \
+      ../../math/pprz_orientation_conversion.c    \
+      ../../math/pprz_geodetic_double.c           \
+      ../../math/pprz_geodetic_float.c            \
+      ../../math/pprz_geodetic_int.c              \
 
 all: run_ahrs_fcr_on_flight_log run_ahrs_ice_on_flight_log
 
 #run_ahrs_flq_on_flight_log: ../../subsystems/ahrs/ahrs_float_lkf_quat.c $(SRCS)
 #	$(CC) -DAHRS_TYPE=AHRS_TYPE_FLQ $(CFLAGS) -o $@ $^ $(LDFLAGS)
 
 run_ahrs_fcr_on_flight_log: ../../subsystems/ahrs/ahrs_float_cmpl.c $(SRCS)
-	$(CC) -DAHRS_TYPE=AHRS_TYPE_FCR $(CFLAGS) -o $@ $^ $(LDFLAGS)
+	$(CC) -DAHRS_TYPE=AHRS_TYPE_FCR -DAHRS_PROPAGATE_QUAT $(CFLAGS) -o $@ $^ $(LDFLAGS)
 
 run_ahrs_ice_on_flight_log: ../../subsystems/ahrs/ahrs_int_cmpl_euler.c $(SRCS)
 	$(CC) -DAHRS_TYPE=AHRS_TYPE_ICE $(CFLAGS) -o $@ $^ $(LDFLAGS)
@@ -58,12 +70,7 @@ MAG_UPDATE_YAW_ONLY ?= 0
 DISABLE_MAG_UPDATE ?= 0
 USE_GPS_HEADING ?= 0
 
-FREQENCY ?= 512
-
-
 AHRS_CFLAGS += -DAHRS_TYPE=$(AHRS_TYPE)
-AHRS_CFLAGS += -DPERIODIC_FREQUENCY=$(FREQUENCY)
-AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(FREQUENCY)
 
 ifeq ($(PROPAGATE_LOW_PASS_RATES), 1)
 AHRS_CFLAGS += -DAHRS_PROPAGATE_LOW_PASS_RATES

diff --git a/sw/airborne/test/ahrs/generated/airframe.h b/sw/airborne/test/ahrs/generated/airframe.h
@@ -0,0 +1,12 @@
+/* fake generated airframe file */
+
+#ifndef AIRFRAME_H
+#define AIRFRAME_H
+
+#define SECTION_IMU 1
+#define IMU_BODY_TO_IMU_PHI 0.
+#define IMU_BODY_TO_IMU_THETA 0.
+#define IMU_BODY_TO_IMU_PSI 0.
+
+
+#endif // AIRFRAME_H
diff --git a/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c b/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c
@@ -155,35 +155,37 @@ static void feed_imu(int i) {
 #if   defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_FLQ
 static void store_filter_output(int i) {
 #ifdef OUTPUT_IN_BODY_FRAME
-  QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_body_quat);
-  RATES_COPY(output[i].rate_est, ahrs_float.body_rate);
+  QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_body_quat);
+  RATES_COPY(output[i].rate_est, ahrs_impl.body_rate);
 #else
-  QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_imu_quat);
-  RATES_COPY(output[i].rate_est, ahrs_float.imu_rate);
+  QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_imu_quat);
+  RATES_COPY(output[i].rate_est, ahrs_impl.imu_rate);
 #endif /* OUTPUT_IN_BODY_FRAME */
   RATES_COPY(output[i].bias_est, ahrs_impl.gyro_bias);
   memcpy(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
 }
 #elif defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_FCR
 static void store_filter_output(int i) {
 #ifdef OUTPUT_IN_BODY_FRAME
-  QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_body_quat);
-  RATES_COPY(output[i].rate_est, ahrs_float.body_rate);
+  QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_body_quat);
+  RATES_COPY(output[i].rate_est, ahrs_impl.body_rate);
 #else
-  QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_imu_quat);
-  RATES_COPY(output[i].rate_est, ahrs_float.imu_rate);
+  QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_imu_quat);
+  RATES_COPY(output[i].rate_est, ahrs_impl.imu_rate);
 #endif /* OUTPUT_IN_BODY_FRAME */
   RATES_COPY(output[i].bias_est, ahrs_impl.gyro_bias);
   //  memcpy(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
 }
 #elif defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_ICE
 static void store_filter_output(int i) {
 #ifdef OUTPUT_IN_BODY_FRAME
-  QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs.ltp_to_body_quat);
-  RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs.body_rate);
+  QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs_impl.ltp_to_body_quat);
+  RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs_impl.body_rate);
 #else
-  QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs.ltp_to_imu_quat);
-  RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs.imu_rate);
+  struct FloatEulers eul_f;
+  EULERS_FLOAT_OF_BFP(eul_f, ahrs_impl.ltp_to_imu_euler);
+  FLOAT_QUAT_OF_EULERS(output[i].quat_est, eul_f);
+  RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs_impl.imu_rate);
 #endif /* OUTPUT_IN_BODY_FRAME */
   RATES_ASSIGN(output[i].bias_est, 0., 0., 0.);
   //  memset(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));