diff --git a/sw/airborne/test/ahrs/Makefile b/sw/airborne/test/ahrs/Makefile new file mode 100644 index 0000000000..196de6300d --- /dev/null +++ b/sw/airborne/test/ahrs/Makefile @@ -0,0 +1,38 @@ + + +CC = gcc +CFLAGS = -std=c99 -I.. -I../.. -I../../../include -I../../booz -I../../../booz -Wall +LDFLAGS = -lm + +# imu wants airframe to fetch its neutrals +# ahrs wants airframe to fetch IMU_BODY_TO_IMU_ANGLES +CFLAGS += -I../../../../var/BOOZ2_A7 + + +#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 +CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 -DAHRS_H_Z=0.85490967783446 + +#CFLAGS += -DOUTPUT_IN_BODY_FRAME +CFLAGS += -DENABLE_MAG_UPDATE +CFLAGS += -DENABLE_ACCEL_UPDATE + +SRCS= run_ahrs_on_flight_log.c \ + ../../math/pprz_trig_int.c \ + ../../subsystems/ahrs.c \ + ../../subsystems/ahrs/ahrs_aligner.c \ + ../../subsystems/imu.c + +all: run_ahrs_flq_on_flight_log 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_rmat.c $(SRCS) + $(CC) -DAHRS_TYPE=AHRS_TYPE_FCR $(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) + +clean: + rm -f *~ run_ahrs_*_on_flight_log diff --git a/sw/airborne/test/ahrs/plot/algebra_common.m b/sw/airborne/test/ahrs/plot/algebra_common.m new file mode 100644 index 0000000000..c798644a2d --- /dev/null +++ b/sw/airborne/test/ahrs/plot/algebra_common.m @@ -0,0 +1,5 @@ +Q_QI = 1; +Q_QX = 2; +Q_QY = 3; +Q_QZ = 4; +Q_SIZE = 4; diff --git a/sw/airborne/test/ahrs/plot/compare_ahrs_on_flight_log.m b/sw/airborne/test/ahrs/plot/compare_ahrs_on_flight_log.m new file mode 100644 index 0000000000..3c5b8bf78f --- /dev/null +++ b/sw/airborne/test/ahrs/plot/compare_ahrs_on_flight_log.m @@ -0,0 +1,122 @@ +clear(); + +flt1 = 'flq'; +%flt2 = 'ice'; +flt2 = 'fcr'; + +filename1 = ["out_" flt1 ".txt"]; +filename2 = ["out_" flt2 ".txt"]; + +trim_log=true;%false; +time_start=7; +time_stop=25; + +plot_true_state = true; +true_state_filename="/home/poine/work/savannah/paparazzi3.broken_stm32_deletion/trunk/sw/simulator/scilab/q6d/data/stop_stop_fdm.txt" + +true_dat = load(true_state_filename); +dat1 = load(filename1); +dat2 = load(filename2); +% extract the part of interrest +if (trim_log) + idx_roi=find(dat1(:,1)>time_start & dat1(:,1)time_start & dat2(:,1)time_start & true_dat(:,1)time_start & dat(:,1) pi/2) + cnt=cnt-1; + elseif (dif <-pi/2) + cnt=cnt+1; + end + unwraped(i) = wraped(i)+2*pi*cnt; + end +endfunction \ No newline at end of file diff --git a/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c b/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c new file mode 100644 index 0000000000..f3b16af416 --- /dev/null +++ b/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c @@ -0,0 +1,214 @@ + + + +#include +#include +#include +#include + +#include "std.h" + +#include "math/pprz_algebra_float.h" +#include "math/pprz_algebra_double.h" +#include "math/pprz_algebra_int.h" +#include "pprz_algebra_print.h" + +#include "subsystems/ahrs.h" +#include "subsystems/ahrs/ahrs_aligner.h" +#include "subsystems/imu.h" + +#define AHRS_TYPE_FLG 0 +#define AHRS_TYPE_FCR 1 +#define AHRS_TYPE_ICE 2 + +#if defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_FLQ +#include "subsystems/ahrs/ahrs_float_lkf_quat.h" +#define OUT_FILE "./out_flq.txt" +#elif defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_FCR +#include "subsystems/ahrs/ahrs_float_cmpl_rmat.h" +#define OUT_FILE "./out_fcr.txt" +#elif defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_ICE +#include "subsystems/ahrs/ahrs_int_cmpl_euler.h" +#define OUT_FILE "./out_ice.txt" +#endif + + +#define MAX_SAMPLE 1000000 +//#define MAX_SAMPLE 5000 +struct test_sample { + double time; + uint8_t flag; + struct DoubleQuat quat_true; + struct DoubleRates omega_true; + struct DoubleRates gyro; + struct DoubleVect3 accel; + struct DoubleVect3 mag; + struct DoubleVect3 gps_pecef; + struct DoubleVect3 gps_vecef; + double baro; +}; +static struct test_sample samples[MAX_SAMPLE]; +static int nb_samples; + +struct test_output { + struct FloatQuat quat_est; + struct FloatRates bias_est; + struct FloatRates rate_est; + float P[6][6]; +}; +static struct test_output output[MAX_SAMPLE]; + + + +//#define IN_FILE "../../fms/libeknav/flight9_spin_circle.ascii" +//#define IN_FILE "../../fms/libeknav/flight9_2climbs.ascii" +#define IN_FILE "/home/poine/work/savannah/paparazzi3.broken_stm32_deletion/trunk/sw/simulator/scilab/q6d/data/stop_stop_sensors.txt" + + +static void read_ascii_flight_log(const char* filename); +static void feed_imu(int i); +static void store_filter_output(int i); +static void dump_output(const char* filename); + +/* from fms_autopilot_msg.h */ +#define VI_IMU_DATA_VALID 0 +#define VI_MAG_DATA_VALID 1 +#define IMU_AVAILABLE(_flag) (_flag & (1<time, &s->flag, + &s->gyro.p, &s->gyro.q, &s->gyro.r, + &s->accel.x, &s->accel.y, &s->accel.z, + &s->mag.x, &s->mag.y, &s->mag.z, + &s->gps_pecef.x, &s->gps_pecef.y, &s->gps_pecef.z, + &s->gps_vecef.x, &s->gps_vecef.y, &s->gps_vecef.z, + &s->baro); + nb_samples++; + } + while (ret == 18 && nb_samples < MAX_SAMPLE); + nb_samples--; + fclose(fd); + printf("read %d points in file %s\n", nb_samples, filename); +} + + +/* + * + */ +static void feed_imu(int i) { + if (i>0) { + RATES_COPY(imu.gyro_prev, imu.gyro); + } + else { + RATES_BFP_OF_REAL(imu.gyro_prev, samples[0].gyro); + } + RATES_BFP_OF_REAL(imu.gyro, samples[i].gyro); + ACCELS_BFP_OF_REAL(imu.accel, samples[i].accel); + MAGS_BFP_OF_REAL(imu.mag, samples[i].mag); +} + +/* + * + */ +#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); +#else + QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_imu_quat); + RATES_COPY(output[i].rate_est, ahrs_float.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); +#else + QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_imu_quat); + RATES_COPY(output[i].rate_est, ahrs_float.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); +#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); +#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)); +} +#endif + +/* + * + */ +static void dump_output(const char* filename) { + FILE* fd = fopen(filename, "w"); + int i; + for (i=0; i