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

[test] some python cleanup

Browse Source
This commit is contained in:
Felix Ruess
2013-10-29 11:40:07 +01:00
parent 931cefe43e
commit 81631c2f8e
2 changed files with 71 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/test/ahrs/ahrs_utils.py
+65 -72
View File
@@ -1,6 +1,5 @@
#! /usr/bin/env python
# $Id$
# Copyright (C) 2011 Antoine Drouin
#
# This file is part of Paparazzi.
@@ -21,37 +20,32 @@
# Boston, MA 02111-1307, USA.
#
#import os
#from optparse import OptionParser
#import scipy
#from scipy import optimize
import shlex, subprocess
from pylab import *
from array import array
import numpy
from __future__ import print_function
import subprocess
import numpy as np
import matplotlib.pyplot as plt
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
# print args
#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
print "Running simulation"
print " using traj " + str(traj_nb)
print(" # " + i, end=' ')
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)
outputlines = p.stdout.readlines()
p.wait()
# for i in outputlines:
# print " "+i,
# print "\n"
# print(" "+i, end=' ')
# print("\n")
ahrs_data_type = [('time', 'float32'),
('phi_true', 'float32'), ('theta_true', 'float32'), ('psi_true', 'float32'),
@@ -61,92 +55,91 @@ def run_simulation(ahrs_type, build_opt, traj_nb):
('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'),
('bp_ahrs', 'float32'), ('bq_ahrs', 'float32'), ('br_ahrs', 'float32')]
mydescr = numpy.dtype(ahrs_data_type)
mydescr = np.dtype(ahrs_data_type)
data = [[] for dummy in xrange(len(mydescr))]
# import code; code.interact(local=locals())
for line in outputlines:
if line.startswith("#"):
print " " + line,
print(" " + line, end=' ')
else:
fields = line.strip().split(' ')
# print fields
#print(fields)
for i, number in enumerate(fields):
data[i].append(number)
print
print()
for i in xrange(len(mydescr)):
data[i] = cast[mydescr[i]](data[i])
data[i] = np.cast[mydescr[i]](data[i])
return numpy.rec.array(data, dtype=mydescr)
return np.rec.array(data, dtype=mydescr)
def plot_simulation_results(plot_true_state, lsty, type, sim_res):
print "Plotting Results"
def plot_simulation_results(plot_true_state, lsty, label, sim_res):
print("Plotting Results")
# f, (ax1, ax2, ax3) = plt.subplots(3, sharex=True, sharey=True)
subplot(3, 3, 1)
plt.plot(sim_res.time, sim_res.phi_ahrs, lsty, label=type)
ylabel('degres')
title('phi')
legend()
plt.subplot(3, 3, 1)
plt.plot(sim_res.time, sim_res.phi_ahrs, lsty, label=label)
plt.ylabel('degres')
plt.title('phi')
plt.legend()
subplot(3, 3, 2)
plot(sim_res.time, sim_res.theta_ahrs, lsty)
title('theta')
plt.subplot(3, 3, 2)
plt.plot(sim_res.time, sim_res.theta_ahrs, lsty)
plt.title('theta')
subplot(3, 3, 3)
plot(sim_res.time, sim_res.psi_ahrs, lsty)
title('psi')
plt.subplot(3, 3, 3)
plt.plot(sim_res.time, sim_res.psi_ahrs, lsty)
plt.title('psi')
subplot(3, 3, 4)
plt.subplot(3, 3, 4)
plt.plot(sim_res.time, sim_res.p_ahrs, lsty)
ylabel('degres/s')
title('p')
plt.ylabel('degres/s')
plt.title('p')
subplot(3, 3, 5)
plt.subplot(3, 3, 5)
plt.plot(sim_res.time, sim_res.q_ahrs, lsty)
title('q')
plt.title('q')
subplot(3, 3, 6)
plt.subplot(3, 3, 6)
plt.plot(sim_res.time, sim_res.r_ahrs, lsty)
title('r')
plt.title('r')
subplot(3, 3, 7)
plt.subplot(3, 3, 7)
plt.plot(sim_res.time, sim_res.bp_ahrs, lsty)
ylabel('degres/s')
xlabel('time in s')
title('bp')
plt.ylabel('degres/s')
plt.xlabel('time in s')
plt.title('bp')
subplot(3, 3, 8)
plt.subplot(3, 3, 8)
plt.plot(sim_res.time, sim_res.bq_ahrs, lsty)
xlabel('time in s')
title('bq')
plt.xlabel('time in s')
plt.title('bq')
subplot(3, 3, 9)
plt.subplot(3, 3, 9)
plt.plot(sim_res.time, sim_res.br_ahrs, lsty)
xlabel('time in s')
title('br')
plt.xlabel('time in s')
plt.title('br')
if plot_true_state:
subplot(3, 3, 1)
plt.subplot(3, 3, 1)
plt.plot(sim_res.time, sim_res.phi_true, 'r--')
subplot(3, 3, 2)
plot(sim_res.time, sim_res.theta_true, 'r--')
subplot(3, 3, 3)
plot(sim_res.time, sim_res.psi_true, 'r--')
subplot(3, 3, 4)
plot(sim_res.time, sim_res.p_true, 'r--')
subplot(3, 3, 5)
plot(sim_res.time, sim_res.q_true, 'r--')
subplot(3, 3, 6)
plot(sim_res.time, sim_res.r_true, 'r--')
subplot(3, 3, 7)
plot(sim_res.time, sim_res.bp_true, 'r--')
subplot(3, 3, 8)
plot(sim_res.time, sim_res.bq_true, 'r--')
subplot(3, 3, 9)
plot(sim_res.time, sim_res.br_true, 'r--')
plt.subplot(3, 3, 2)
plt.plot(sim_res.time, sim_res.theta_true, 'r--')
plt.subplot(3, 3, 3)
plt.plot(sim_res.time, sim_res.psi_true, 'r--')
plt.subplot(3, 3, 4)
plt.plot(sim_res.time, sim_res.p_true, 'r--')
plt.subplot(3, 3, 5)
plt.plot(sim_res.time, sim_res.q_true, 'r--')
plt.subplot(3, 3, 6)
plt.plot(sim_res.time, sim_res.r_true, 'r--')
plt.subplot(3, 3, 7)
plt.plot(sim_res.time, sim_res.bp_true, 'r--')
plt.subplot(3, 3, 8)
plt.plot(sim_res.time, sim_res.bq_true, 'r--')
plt.subplot(3, 3, 9)
plt.plot(sim_res.time, sim_res.br_true, 'r--')
def show_plot():
sw/airborne/test/ahrs/compare_ahrs.py
+6
View File
@@ -20,8 +20,11 @@
# Boston, MA 02111-1307, USA.
#
import os
import sys
import ahrs_utils
def main():
# traj_nb = 0 # static
@@ -77,4 +80,7 @@ def main():
ahrs_utils.show_plot()
if __name__ == "__main__":
script_path = os.path.dirname(os.path.realpath(sys.argv[0]))
if script_path != os.path.abspath(os.getcwd()):
sys.exit("Please run this script from " + script_path)
main()