Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

[test] update compare_ahrs

Browse Source
This commit is contained in:
Felix Ruess
2013-10-24 00:07:22 +02:00
parent f6bb5f1fad
commit 3fe72e28e3
4 changed files with 115 additions and 124 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/test/ahrs/Makefile
+14 -27
View File
@@ -3,14 +3,13 @@ Q=@
CC = gcc 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 LDFLAGS = -lm -lgsl -lgslcblas
CFLAGS += CFLAGS +=
# imu wants airframe to fetch its neutrals # imu wants airframe to fetch its neutrals
# ahrs wants airframe to fetch IMU_BODY_TO_IMU_ANGLES # ahrs wants airframe to fetch IMU_BODY_TO_IMU_ANGLES
#CFLAGS += -I../../../../var/BOOZ2_A7
CFLAGS += -I../../../../var/ahrs_test CFLAGS += -I../../../../var/ahrs_test
@@ -51,28 +50,15 @@ AHRS_TYPE = AHRS_TYPE_ICQ
#AHRS_TYPE = AHRS_TYPE_FCR2 #AHRS_TYPE = AHRS_TYPE_FCR2
#AHRS_TYPE = AHRS_TYPE_FCQ #AHRS_TYPE = AHRS_TYPE_FCQ
endif 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 PROPAGATE_LOW_PASS_RATES ?= 0
FREQENCY = 512 GRAVITY_UPDATE_NORM_HEURISTIC ?= 0
endif 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) 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_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_quat.h\"
AHRS_SRCS += ../../subsystems/ahrs/ahrs_int_cmpl_quat.c AHRS_SRCS += ../../subsystems/ahrs/ahrs_int_cmpl_quat.c
endif 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) ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR)
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\" AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_dcm.c AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_dcm.c
@@ -135,9 +117,14 @@ endif
RAOS_SRCS = ./ahrs_on_synth.c \ RAOS_SRCS = ./ahrs_on_synth.c \
../../state.c \
../../subsystems/imu.c \ ../../subsystems/imu.c \
../../subsystems/imu/imu_dummy.c \ ../../subsystems/imu/imu_dummy.c \
../../math/pprz_trig_int.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 ../../../simulator/nps/nps_random.c
sw/airborne/test/ahrs/ahrs_utils.py
+38 -44
View File
@@ -31,48 +31,45 @@ from array import array
import numpy import numpy
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
def run_simulation(ahrs_type, build_opt, traj_nb): def run_simulation(ahrs_type, build_opt, traj_nb):
print "\nBuilding ahrs" print "\nBuilding ahrs"
args = ["make", "clean", "run_ahrs_on_synth", "AHRS_TYPE=AHRS_TYPE_"+ahrs_type] + build_opt args = ["make", "clean", "run_ahrs_on_synth", "AHRS_TYPE=AHRS_TYPE_" + ahrs_type] + build_opt
# print args # print args
p = subprocess.Popen(args=args, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False) p = subprocess.Popen(args=args, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False)
outputlines = p.stdout.readlines() outputlines = p.stdout.readlines()
p.wait() p.wait()
for i in outputlines: for i in outputlines:
print " # "+i, print " # " + i,
print print
print "Running simulation" print "Running simulation"
print " using traj " + str(traj_nb) 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() outputlines = p.stdout.readlines()
p.wait() p.wait()
# for i in outputlines: # for i in outputlines:
# print " "+i, # print " "+i,
# print "\n" # print "\n"
ahrs_data_type = [('time', 'float32'), ahrs_data_type = [('time', 'float32'),
('phi_true', 'float32'), ('theta_true', 'float32'), ('psi_true', 'float32'), ('phi_true', 'float32'), ('theta_true', 'float32'), ('psi_true', 'float32'),
('p_true', 'float32'), ('q_true', 'float32'), ('r_true', 'float32'), ('p_true', 'float32'), ('q_true', 'float32'), ('r_true', 'float32'),
('bp_true', 'float32'), ('bq_true', 'float32'), ('br_true', 'float32'), ('bp_true', 'float32'), ('bq_true', 'float32'), ('br_true', 'float32'),
('phi_ahrs', 'float32'), ('theta_ahrs', 'float32'), ('psi_ahrs', 'float32'), ('phi_ahrs', 'float32'), ('theta_ahrs', 'float32'), ('psi_ahrs', 'float32'),
('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'), ('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'),
('bp_ahrs', 'float32'), ('bq_ahrs', 'float32'), ('br_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'),
]
mydescr = numpy.dtype(ahrs_data_type) mydescr = numpy.dtype(ahrs_data_type)
data = [[] for dummy in xrange(len(mydescr))] data = [[] for dummy in xrange(len(mydescr))]
# import code; code.interact(local=locals()) # import code; code.interact(local=locals())
for line in outputlines: for line in outputlines:
if line.startswith("#"): if line.startswith("#"):
print " "+line, print " " + line,
else: else:
fields = line.strip().split(' '); fields = line.strip().split(' ')
# print fields # print fields
for i, number in enumerate(fields): for i, number in enumerate(fields):
data[i].append(number) data[i].append(number)
@@ -83,77 +80,74 @@ def run_simulation(ahrs_type, build_opt, traj_nb):
return numpy.rec.array(data, dtype=mydescr) return numpy.rec.array(data, dtype=mydescr)
def plot_simulation_results(plot_true_state, lsty, type, sim_res): def plot_simulation_results(plot_true_state, lsty, type, sim_res):
print "Plotting Results" 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) plt.plot(sim_res.time, sim_res.phi_ahrs, lsty, label=type)
ylabel('degres') ylabel('degres')
title('phi') title('phi')
legend() legend()
subplot(3,3,2) subplot(3, 3, 2)
plot(sim_res.time, sim_res.theta_ahrs, lsty) plot(sim_res.time, sim_res.theta_ahrs, lsty)
title('theta') title('theta')
subplot(3,3,3) subplot(3, 3, 3)
plot(sim_res.time, sim_res.psi_ahrs, lsty) plot(sim_res.time, sim_res.psi_ahrs, lsty)
title('psi') title('psi')
subplot(3,3,4) subplot(3, 3, 4)
plt.plot(sim_res.time, sim_res.p_ahrs, lsty) plt.plot(sim_res.time, sim_res.p_ahrs, lsty)
ylabel('degres/s') ylabel('degres/s')
title('p') title('p')
subplot(3,3,5) subplot(3, 3, 5)
plt.plot(sim_res.time, sim_res.q_ahrs, lsty) plt.plot(sim_res.time, sim_res.q_ahrs, lsty)
title('q') title('q')
subplot(3,3,6) subplot(3, 3, 6)
plt.plot(sim_res.time, sim_res.r_ahrs, lsty) plt.plot(sim_res.time, sim_res.r_ahrs, lsty)
title('r') title('r')
subplot(3,3,7) subplot(3, 3, 7)
plt.plot(sim_res.time, sim_res.bp_ahrs, lsty) plt.plot(sim_res.time, sim_res.bp_ahrs, lsty)
ylabel('degres/s') ylabel('degres/s')
xlabel('time in s') xlabel('time in s')
title('bp') title('bp')
subplot(3,3,8) subplot(3, 3, 8)
plt.plot(sim_res.time, sim_res.bq_ahrs, lsty) plt.plot(sim_res.time, sim_res.bq_ahrs, lsty)
xlabel('time in s') xlabel('time in s')
title('bq') title('bq')
subplot(3,3,9) subplot(3, 3, 9)
plt.plot(sim_res.time, sim_res.br_ahrs, lsty) plt.plot(sim_res.time, sim_res.br_ahrs, lsty)
xlabel('time in s') xlabel('time in s')
title('br') title('br')
if plot_true_state: if plot_true_state:
subplot(3,3,1) subplot(3, 3, 1)
plt.plot(sim_res.time, sim_res.phi_true, 'r--') 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--') 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--') 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--') 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--') 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--') 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--') 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--') 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--') plot(sim_res.time, sim_res.br_true, 'r--')
def show_plot(): def show_plot():
plt.show(); plt.show()
sw/airborne/test/ahrs/compare_ahrs.py
+38 -38
View File
@@ -24,51 +24,51 @@ import ahrs_utils
def main(): def main():
# traj_nb = 0; # static # traj_nb = 0 # static
# traj_nb = 1; # sine orientation # traj_nb = 1 # sine orientation
# traj_nb = 2; # sine X quad # traj_nb = 2 # sine X quad
# traj_nb = 3; # step_phi # traj_nb = 3 # step_phi
# traj_nb = 4; # step_phi_2nd_order # traj_nb = 4 # step_phi_2nd_order
# traj_nb = 5; # step_bias # traj_nb = 5 # step_bias
# traj_nb = 6; # coordinated circle # traj_nb = 6 # coordinated circle
# traj_nb = 7; # stop stop X quad # traj_nb = 7 # stop stop X quad
traj_nb = 8; # bungee takeoff traj_nb = 8 # bungee takeoff
build_opt1 = []; build_opt1 = []
# build_opt1 = ["Q="]; # build_opt1 = ["Q="]
build_opt1 += ["FREQUENCY=120"]; build_opt1 += ["FREQUENCY=120"]
# build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"]; # build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"]
build_opt1 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"]; build_opt1 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"]
build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]; build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]
# build_opt1 += ["DISABLE_MAG_UPDATE=1"]; # build_opt1 += ["DISABLE_MAG_UPDATE=1"]
# build_opt1 += ["USE_GPS_HEADING=1"]; # build_opt1 += ["USE_GPS_HEADING=1"]
# build_opt1 += ["MAG_UPDATE_YAW_ONLY=1"]; # build_opt1 += ["MAG_UPDATE_YAW_ONLY=1"]
# ahrs_type1 = "FCQ"; # ahrs_type1 = "FCQ"
# ahrs_type1 = "FCR2"; # ahrs_type1 = "FCR2"
# ahrs_type1 = "FLQ"; # ahrs_type1 = "FLQ"
ahrs_type1 = "ICQ"; ahrs_type1 = "ICQ"
# ahrs_type1 = "FCR"; # ahrs_type1 = "FCR"
sim_res1 = ahrs_utils.run_simulation(ahrs_type1, build_opt1, traj_nb) sim_res1 = ahrs_utils.run_simulation(ahrs_type1, build_opt1, traj_nb)
# import code; code.interact(local=locals()) # import code; code.interact(local=locals())
build_opt2 = []; build_opt2 = []
build_opt2 += ["FREQUENCY=120"]; build_opt2 += ["FREQUENCY=120"]
build_opt2 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"]; build_opt2 += ["GRAVITY_UPDATE_COORDINATED_TURN=1"]
build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]; build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"]
build_opt2 += ["USE_AHRS_GPS_ACCELERATIONS=1"]; build_opt2 += ["USE_AHRS_GPS_ACCELERATIONS=1"]
# build_opt2 += ["DISABLE_MAG_UPDATE=1"]; # build_opt2 += ["DISABLE_MAG_UPDATE=1"]
# build_opt2 += ["USE_GPS_HEADING=1"]; # build_opt2 += ["USE_GPS_HEADING=1"]
# build_opt2 += ["MAG_UPDATE_YAW_ONLY=0"]; # build_opt2 += ["MAG_UPDATE_YAW_ONLY=0"]
# build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"]; # build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"]
# build_opt2 = build_opt1; # build_opt2 = build_opt1
# ahrs_type2 = "FLQ"; # ahrs_type2 = "FLQ"
# ahrs_type2 = "FCR2"; # ahrs_type2 = "FCR2"
# ahrs_type2 = "ICQ"; # ahrs_type2 = "ICQ"
ahrs_type2 = "FCR"; ahrs_type2 = "FCR"
# ahrs_type2 = ahrs_type1; # ahrs_type2 = ahrs_type1
sim_res2 = ahrs_utils.run_simulation(ahrs_type2, build_opt2, traj_nb) sim_res2 = ahrs_utils.run_simulation(ahrs_type2, build_opt2, traj_nb)
sw/airborne/test/ahrs/run_ahrs_on_synth.c
+25 -15
View File
@@ -37,10 +37,6 @@ static void report(void) {
int output_sensors = FALSE; int output_sensors = FALSE;
int output_pos = 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 ", aos.time);
printf("%f %f %f ", DegOfRad(aos.ltp_to_imu_euler.phi), 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.q),
DegOfRad(aos.gyro_bias.r)); 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 #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; struct FloatRates bias;
RATES_FLOAT_OF_BFP(bias, ahrs_impl.gyro_bias); RATES_FLOAT_OF_BFP(bias, ahrs_impl.gyro_bias);
printf("%f %f %f ", DegOfRad(bias.p), printf("%f %f %f ", DegOfRad(bias.p),
DegOfRad(bias.q), DegOfRad(bias.q),
DegOfRad(bias.r)); 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), printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p),
DegOfRad(ahrs_impl.gyro_bias.q), DegOfRad(ahrs_impl.gyro_bias.q),
DegOfRad(ahrs_impl.gyro_bias.r)); DegOfRad(ahrs_impl.gyro_bias.r));