From 3fe72e28e3dc419b02f7c2679d0c5af5fc831ed6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 24 Oct 2013 00:07:22 +0200 Subject: [PATCH] [test] update compare_ahrs --- sw/airborne/test/ahrs/Makefile | 41 ++++-------- sw/airborne/test/ahrs/ahrs_utils.py | 82 +++++++++++------------ sw/airborne/test/ahrs/compare_ahrs.py | 76 ++++++++++----------- sw/airborne/test/ahrs/run_ahrs_on_synth.c | 40 ++++++----- 4 files changed, 115 insertions(+), 124 deletions(-) diff --git a/sw/airborne/test/ahrs/Makefile b/sw/airborne/test/ahrs/Makefile index 7904f638e8..036787a701 100644 --- a/sw/airborne/test/ahrs/Makefile +++ b/sw/airborne/test/ahrs/Makefile @@ -3,14 +3,13 @@ Q=@ CC = gcc -CFLAGS = -std=c99 -I.. -I../.. -I../../../include -I../../booz -I../../../booz -Wall +CFLAGS = -std=c99 -I.. -I../.. -I../../../include -Wall LDFLAGS = -lm -lgsl -lgslcblas CFLAGS += # imu wants airframe to fetch its neutrals # ahrs wants airframe to fetch IMU_BODY_TO_IMU_ANGLES -#CFLAGS += -I../../../../var/BOOZ2_A7 CFLAGS += -I../../../../var/ahrs_test @@ -51,28 +50,15 @@ AHRS_TYPE = AHRS_TYPE_ICQ #AHRS_TYPE = AHRS_TYPE_FCR2 #AHRS_TYPE = AHRS_TYPE_FCQ endif -ifndef PROPAGATE_LOW_PASS_RATES -PROPAGATE_LOW_PASS_RATES = 0 -endif -ifndef GRAVITY_UPDATE_NORM_HEURISTIC -GRAVITY_UPDATE_NORM_HEURISTIC = 0 -endif -ifndef GRAVITY_UPDATE_COORDINATED_TURN -GRAVITY_UPDATE_COORDINATED_TURN = 0 -endif -ifndef MAG_UPDATE_YAW_ONLY -MAG_UPDATE_YAW_ONLY = 0 -endif -ifndef DISABLE_MAG_UPDATE -DISABLE_MAG_UPDATE = 0 -endif -ifndef USE_GPS_HEADING -USE_GPS_HEADING = 0 -endif -ifndef FREQUENCY -FREQENCY = 512 -endif +PROPAGATE_LOW_PASS_RATES ?= 0 +GRAVITY_UPDATE_NORM_HEURISTIC ?= 0 +GRAVITY_UPDATE_COORDINATED_TURN ?= 0 +MAG_UPDATE_YAW_ONLY ?= 0 +DISABLE_MAG_UPDATE ?= 0 +USE_GPS_HEADING ?= 0 + +FREQENCY ?= 512 AHRS_CFLAGS += -DAHRS_TYPE=$(AHRS_TYPE) @@ -114,10 +100,6 @@ ifeq ($(AHRS_TYPE), AHRS_TYPE_ICQ) AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat.h\" AHRS_SRCS += ../../subsystems/ahrs/ahrs_int_cmpl_quat.c endif -ifeq ($(AHRS_TYPE), AHRS_TYPE_FLQ) -AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_lkf_quat.h\" -AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_lkf_quat.c -endif ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR) AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_dcm.c @@ -135,9 +117,14 @@ endif RAOS_SRCS = ./ahrs_on_synth.c \ + ../../state.c \ ../../subsystems/imu.c \ ../../subsystems/imu/imu_dummy.c \ ../../math/pprz_trig_int.c \ + ../../math/pprz_orientation_conversion.c \ + ../../math/pprz_geodetic_int.c \ + ../../math/pprz_geodetic_float.c \ + ../../math/pprz_geodetic_double.c \ ../../../simulator/nps/nps_random.c diff --git a/sw/airborne/test/ahrs/ahrs_utils.py b/sw/airborne/test/ahrs/ahrs_utils.py index b6636d2584..60d33d7e9a 100644 --- a/sw/airborne/test/ahrs/ahrs_utils.py +++ b/sw/airborne/test/ahrs/ahrs_utils.py @@ -31,48 +31,45 @@ from array import array import numpy import matplotlib.pyplot as plt + def run_simulation(ahrs_type, build_opt, traj_nb): print "\nBuilding ahrs" - args = ["make", "clean", "run_ahrs_on_synth", "AHRS_TYPE=AHRS_TYPE_"+ahrs_type] + build_opt -# print args + args = ["make", "clean", "run_ahrs_on_synth", "AHRS_TYPE=AHRS_TYPE_" + ahrs_type] + build_opt + # print args p = subprocess.Popen(args=args, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False) outputlines = p.stdout.readlines() p.wait() for i in outputlines: - print " # "+i, + print " # " + i, print print "Running simulation" print " using traj " + str(traj_nb) - p = subprocess.Popen(args=["./run_ahrs_on_synth",str(traj_nb)], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False) + p = subprocess.Popen(args=["./run_ahrs_on_synth", str(traj_nb)], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, + shell=False) outputlines = p.stdout.readlines() p.wait() -# for i in outputlines: -# print " "+i, -# print "\n" + # for i in outputlines: + # print " "+i, + # print "\n" ahrs_data_type = [('time', 'float32'), ('phi_true', 'float32'), ('theta_true', 'float32'), ('psi_true', 'float32'), - ('p_true', 'float32'), ('q_true', 'float32'), ('r_true', 'float32'), - ('bp_true', 'float32'), ('bq_true', 'float32'), ('br_true', 'float32'), + ('p_true', 'float32'), ('q_true', 'float32'), ('r_true', 'float32'), + ('bp_true', 'float32'), ('bq_true', 'float32'), ('br_true', 'float32'), ('phi_ahrs', 'float32'), ('theta_ahrs', 'float32'), ('psi_ahrs', 'float32'), - ('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'), - ('bp_ahrs', 'float32'), ('bq_ahrs', 'float32'), ('br_ahrs', 'float32') - ] - pos_data_type = [ ('x0_true', 'float32'), ('y0_true', 'float32'), ('z0_true', 'float32'), - ('x1_true', 'float32'), ('y1_true', 'float32'), ('z1_true', 'float32'), - ('x2_true', 'float32'), ('y2_true', 'float32'), ('z2_true', 'float32'), - ('x3_true', 'float32'), ('y3_true', 'float32'), ('z3_true', 'float32'), - ] + ('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'), + ('bp_ahrs', 'float32'), ('bq_ahrs', 'float32'), ('br_ahrs', 'float32')] + mydescr = numpy.dtype(ahrs_data_type) data = [[] for dummy in xrange(len(mydescr))] -# import code; code.interact(local=locals()) + # import code; code.interact(local=locals()) for line in outputlines: if line.startswith("#"): - print " "+line, + print " " + line, else: - fields = line.strip().split(' '); -# print fields + fields = line.strip().split(' ') + # print fields for i, number in enumerate(fields): data[i].append(number) @@ -83,77 +80,74 @@ def run_simulation(ahrs_type, build_opt, traj_nb): return numpy.rec.array(data, dtype=mydescr) - def plot_simulation_results(plot_true_state, lsty, type, sim_res): print "Plotting Results" - # f, (ax1, ax2, ax3) = plt.subplots(3, sharex=True, sharey=True) + # f, (ax1, ax2, ax3) = plt.subplots(3, sharex=True, sharey=True) - subplot(3,3,1) + subplot(3, 3, 1) plt.plot(sim_res.time, sim_res.phi_ahrs, lsty, label=type) ylabel('degres') title('phi') legend() - subplot(3,3,2) + subplot(3, 3, 2) plot(sim_res.time, sim_res.theta_ahrs, lsty) title('theta') - subplot(3,3,3) + subplot(3, 3, 3) plot(sim_res.time, sim_res.psi_ahrs, lsty) title('psi') - subplot(3,3,4) + subplot(3, 3, 4) plt.plot(sim_res.time, sim_res.p_ahrs, lsty) ylabel('degres/s') title('p') - subplot(3,3,5) + subplot(3, 3, 5) plt.plot(sim_res.time, sim_res.q_ahrs, lsty) title('q') - subplot(3,3,6) + subplot(3, 3, 6) plt.plot(sim_res.time, sim_res.r_ahrs, lsty) title('r') - subplot(3,3,7) + subplot(3, 3, 7) plt.plot(sim_res.time, sim_res.bp_ahrs, lsty) ylabel('degres/s') xlabel('time in s') title('bp') - subplot(3,3,8) + subplot(3, 3, 8) plt.plot(sim_res.time, sim_res.bq_ahrs, lsty) xlabel('time in s') title('bq') - subplot(3,3,9) + subplot(3, 3, 9) plt.plot(sim_res.time, sim_res.br_ahrs, lsty) xlabel('time in s') title('br') - if plot_true_state: - subplot(3,3,1) + subplot(3, 3, 1) plt.plot(sim_res.time, sim_res.phi_true, 'r--') - subplot(3,3,2) + subplot(3, 3, 2) plot(sim_res.time, sim_res.theta_true, 'r--') - subplot(3,3,3) + subplot(3, 3, 3) plot(sim_res.time, sim_res.psi_true, 'r--') - subplot(3,3,4) + subplot(3, 3, 4) plot(sim_res.time, sim_res.p_true, 'r--') - subplot(3,3,5) + subplot(3, 3, 5) plot(sim_res.time, sim_res.q_true, 'r--') - subplot(3,3,6) + subplot(3, 3, 6) plot(sim_res.time, sim_res.r_true, 'r--') - subplot(3,3,7) + subplot(3, 3, 7) plot(sim_res.time, sim_res.bp_true, 'r--') - subplot(3,3,8) + subplot(3, 3, 8) plot(sim_res.time, sim_res.bq_true, 'r--') - subplot(3,3,9) + subplot(3, 3, 9) plot(sim_res.time, sim_res.br_true, 'r--') - def show_plot(): - plt.show(); + plt.show() diff --git a/sw/airborne/test/ahrs/compare_ahrs.py b/sw/airborne/test/ahrs/compare_ahrs.py index 69fafbac49..524af47d55 100755 --- a/sw/airborne/test/ahrs/compare_ahrs.py +++ b/sw/airborne/test/ahrs/compare_ahrs.py @@ -24,51 +24,51 @@ import ahrs_utils def main(): -# traj_nb = 0; # static -# traj_nb = 1; # sine orientation -# traj_nb = 2; # sine X quad -# traj_nb = 3; # step_phi -# traj_nb = 4; # step_phi_2nd_order -# traj_nb = 5; # step_bias -# traj_nb = 6; # coordinated circle -# traj_nb = 7; # stop stop X quad - traj_nb = 8; # bungee takeoff +# traj_nb = 0 # static +# traj_nb = 1 # sine orientation +# traj_nb = 2 # sine X quad +# traj_nb = 3 # step_phi +# traj_nb = 4 # step_phi_2nd_order +# traj_nb = 5 # step_bias +# traj_nb = 6 # coordinated circle +# traj_nb = 7 # stop stop X quad + traj_nb = 8 # bungee takeoff - build_opt1 = []; -# build_opt1 = ["Q="]; - build_opt1 += ["FREQUENCY=120"]; -# build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"]; - build_opt1 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"]; - build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]; -# build_opt1 += ["DISABLE_MAG_UPDATE=1"]; -# build_opt1 += ["USE_GPS_HEADING=1"]; -# build_opt1 += ["MAG_UPDATE_YAW_ONLY=1"]; + build_opt1 = [] +# build_opt1 = ["Q="] + build_opt1 += ["FREQUENCY=120"] +# build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"] + build_opt1 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"] + build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"] +# build_opt1 += ["DISABLE_MAG_UPDATE=1"] +# build_opt1 += ["USE_GPS_HEADING=1"] +# build_opt1 += ["MAG_UPDATE_YAW_ONLY=1"] -# ahrs_type1 = "FCQ"; -# ahrs_type1 = "FCR2"; -# ahrs_type1 = "FLQ"; - ahrs_type1 = "ICQ"; -# ahrs_type1 = "FCR"; +# ahrs_type1 = "FCQ" +# ahrs_type1 = "FCR2" +# ahrs_type1 = "FLQ" + ahrs_type1 = "ICQ" +# ahrs_type1 = "FCR" sim_res1 = ahrs_utils.run_simulation(ahrs_type1, build_opt1, traj_nb) # import code; code.interact(local=locals()) - build_opt2 = []; - build_opt2 += ["FREQUENCY=120"]; - build_opt2 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"]; - build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]; - build_opt2 += ["USE_AHRS_GPS_ACCELERATIONS=1"]; -# build_opt2 += ["DISABLE_MAG_UPDATE=1"]; -# build_opt2 += ["USE_GPS_HEADING=1"]; -# build_opt2 += ["MAG_UPDATE_YAW_ONLY=0"]; -# build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"]; -# build_opt2 = build_opt1; + build_opt2 = [] + build_opt2 += ["FREQUENCY=120"] + build_opt2 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"] + build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"] + build_opt2 += ["USE_AHRS_GPS_ACCELERATIONS=1"] +# build_opt2 += ["DISABLE_MAG_UPDATE=1"] +# build_opt2 += ["USE_GPS_HEADING=1"] +# build_opt2 += ["MAG_UPDATE_YAW_ONLY=0"] +# build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"] +# build_opt2 = build_opt1 -# ahrs_type2 = "FLQ"; -# ahrs_type2 = "FCR2"; -# ahrs_type2 = "ICQ"; - ahrs_type2 = "FCR"; -# ahrs_type2 = ahrs_type1; +# ahrs_type2 = "FLQ" +# ahrs_type2 = "FCR2" +# ahrs_type2 = "ICQ" + ahrs_type2 = "FCR" +# ahrs_type2 = ahrs_type1 sim_res2 = ahrs_utils.run_simulation(ahrs_type2, build_opt2, traj_nb) diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth.c b/sw/airborne/test/ahrs/run_ahrs_on_synth.c index a0d45107a7..57f7deef13 100644 --- a/sw/airborne/test/ahrs/run_ahrs_on_synth.c +++ b/sw/airborne/test/ahrs/run_ahrs_on_synth.c @@ -37,10 +37,6 @@ static void report(void) { int output_sensors = FALSE; int output_pos = FALSE; -#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ - EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler); - RATES_FLOAT_OF_BFP(ahrs_float.imu_rate, ahrs.imu_rate); -#endif printf("%f ", aos.time); printf("%f %f %f ", DegOfRad(aos.ltp_to_imu_euler.phi), @@ -55,23 +51,37 @@ static void report(void) { DegOfRad(aos.gyro_bias.q), DegOfRad(aos.gyro_bias.r)); - - printf("%f %f %f ", DegOfRad(ahrs_float.ltp_to_imu_euler.phi), - DegOfRad(ahrs_float.ltp_to_imu_euler.theta), - DegOfRad(ahrs_float.ltp_to_imu_euler.psi)); - - printf("%f %f %f ", DegOfRad(ahrs_float.imu_rate.p), - DegOfRad(ahrs_float.imu_rate.q), - DegOfRad(ahrs_float.imu_rate.r)); - #if AHRS_TYPE == AHRS_TYPE_ICQ + struct Int32Eulers ltp_to_imu_euler_i; + INT32_EULERS_OF_QUAT(ltp_to_imu_euler_i, ahrs_impl.ltp_to_imu_quat); + struct FloatEulers ltp_to_imu_euler_f; + EULERS_FLOAT_OF_BFP(ltp_to_imu_euler_f, ltp_to_imu_euler_i); + printf("%f %f %f ", DegOfRad(ltp_to_imu_euler_f.phi), + DegOfRad(ltp_to_imu_euler_f.theta), + DegOfRad(ltp_to_imu_euler_f.psi)); + + struct FloatRates imu_rate_f; + RATES_FLOAT_OF_BFP(imu_rate_f, ahrs_impl.imu_rate); + printf("%f %f %f ", DegOfRad(imu_rate_f.p), + DegOfRad(imu_rate_f.q), + DegOfRad(imu_rate_f.r)); + struct FloatRates bias; RATES_FLOAT_OF_BFP(bias, ahrs_impl.gyro_bias); printf("%f %f %f ", DegOfRad(bias.p), DegOfRad(bias.q), DegOfRad(bias.r)); -#endif -#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR + +#elif AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR + + printf("%f %f %f ", DegOfRad(ahrs_impl.ltp_to_imu_euler.phi), + DegOfRad(ahrs_impl.ltp_to_imu_euler.theta), + DegOfRad(ahrs_impl.ltp_to_imu_euler.psi)); + + printf("%f %f %f ", DegOfRad(ahrs_impl.imu_rate.p), + DegOfRad(ahrs_impl.imu_rate.q), + DegOfRad(ahrs_impl.imu_rate.r)); + printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p), DegOfRad(ahrs_impl.gyro_bias.q), DegOfRad(ahrs_impl.gyro_bias.r));