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 29 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
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
sw/airborne/test/ahrs/ahrs_utils.py
+34 -40
View File
@@ -31,25 +31,27 @@ 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'),
@@ -57,22 +59,17 @@ def run_simulation(ahrs_type, build_opt, traj_nb):
('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'),
]
('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)
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()
sw/airborne/test/ahrs/compare_ahrs.py
+38 -38
View File
@@ -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)
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_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));