Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 20:36:06 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[test] update some mostly unused ahrs tests

Browse Source
This commit is contained in:
Felix Ruess
2014-09-08 16:01:02 +02:00
parent df023584f9
commit 38873ac4fc
4 changed files with 45 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/imu.c
+3
View File
@@ -24,7 +24,10 @@
* Inertial Measurement Unit interface.
*/
#ifdef BOARD_CONFIG
#include BOARD_CONFIG
#endif
#include "subsystems/imu.h"
#include "state.h"
sw/airborne/test/ahrs/Makefile
+16 -9
View File
@@ -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,11 +25,23 @@ 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
@@ -37,7 +49,7 @@ all: run_ahrs_fcr_on_flight_log run_ahrs_ice_on_flight_log
# $(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
sw/airborne/test/ahrs/generated/airframe.h
+12
View File
@@ -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
sw/airborne/test/ahrs/run_ahrs_on_flight_log.c
+14 -12
View File
@@ -155,11 +155,11 @@ 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));
@@ -167,11 +167,11 @@ static void store_filter_output(int i) {
#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));
@@ -179,11 +179,13 @@ static void store_filter_output(int i) {
#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));