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

Merge branch 'master' into telemetry

Browse Source 
to resolve
Conflicts:
	sw/airborne/firmwares/fixedwing/ap_downlink.h
This commit is contained in:
Felix Ruess
2013-10-28 16:07:02 +01:00
parent 0bcdaeaaa6 13c9f3b7ca
commit 4e86e7c89b
12 changed files with 137 additions and 150 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/boards/umarim/baro_board.c
+1 -1
View File
@@ -33,7 +33,7 @@
#include "led.h"
// ADC for absolute pressure
#define BARO_ABS_ADS
#ifndef BARO_ABS_ADS
#define BARO_ABS_ADS ads1114_1
#endif
sw/airborne/firmwares/fixedwing/autopilot.h
-1
View File
@@ -32,7 +32,6 @@
#include <inttypes.h>
#include "std.h"
#include "paparazzi.h"
#include "mcu_periph/sys_time.h"
#include "generated/airframe.h"
sw/airborne/modules/gps/gps_ubx_ucenter.c
+9 -8
View File
@@ -27,7 +27,6 @@
*
*/
#include "gps_ubx_ucenter.h"
//////////////////////////////////////////////////////////////////////////////////////
@@ -295,6 +294,9 @@ static inline void gps_ubx_ucenter_config_port(void)
#if GPS_PORT_ID == GPS_PORT_UART1 || GPS_PORT_ID == GPS_PORT_UART2
UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, 38400, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
#endif
#if GPS_PORT_ID == GPS_PORT_USB
UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x0, 0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0);
#endif
}
#define GPS_SBAS_ENABLED 0x01
@@ -319,6 +321,9 @@ static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t
#if GPS_PORT_ID == GPS_PORT_UART2
UbxSend_CFG_MSG(class, id, 0, 0, rate, 0);
#endif
#if GPS_PORT_ID == GPS_PORT_USB
UbxSend_CFG_MSG(class, id, 0, 0, 0, rate);
#endif
#if GPS_PORT_ID == GPS_PORT_DDC
UbxSend_CFG_MSG(class, id, rate, 0, 0, 0);
#endif
@@ -385,10 +390,10 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr)
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 4);
break;
case 11:
#if defined FIRMWARE && FIRMWARE == ROTORCRAFT
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 1);
#else
#if GPS_UBX_UCENTER_SLOW_NAV_SOL
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 8);
#else
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 1);
#endif
break;
case 12:
@@ -416,7 +421,3 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr)
}
return TRUE; // Continue, except for the last case
}
sw/airborne/modules/sensors/baro_mpl3115.c
+1 -1
View File
@@ -20,7 +20,7 @@
*/
/**
* @file modules/sensors/baro_mpl3155.c
* @file modules/sensors/baro_mpl3115.c
*
* Module for the baro MPL3115A2 from Freescale (i2c)
*
sw/airborne/modules/sensors/baro_mpl3115.h
+1 -1
View File
@@ -20,7 +20,7 @@
*/
/**
* @file modules/sensors/baro_mpl3155.h
* @file modules/sensors/baro_mpl3115.h
*
* Module for the baro MPL3115A2 from Freescale (i2c)
*
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
+38 -44
View File
@@ -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()
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));