Skip to content

Commit

Permalink
[tests] improve ahrs test scripts
Browse files Browse the repository at this point in the history 
* added bungee_takeoff trajectory
* some fixes to test float_dcm
  • Loading branch information
@flixr
flixr committed Oct 5, 2012
1 parent aabb8d6 commit 4135fd7
Show file tree
Hide file tree
Showing 5 changed files with 119 additions and 55 deletions.
15 changes: 9 additions & 6 deletions sw/airborne/test/ahrs/Makefile
View file
Expand Up @@ -70,10 +70,14 @@ ifndef USE_GPS_HEADING
USE_GPS_HEADING = 0
endif

ifndef FREQUENCY
FREQENCY = 512
endif


AHRS_CFLAGS += -DAHRS_TYPE=$(AHRS_TYPE)
AHRS_CFLAGS += -DPERIODIC_FREQUENCY=512
AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512
AHRS_CFLAGS += -DPERIODIC_FREQUENCY=$(FREQUENCY)
AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(FREQUENCY)

ifeq ($(PROPAGATE_LOW_PASS_RATES), 1)
AHRS_CFLAGS += -DAHRS_PROPAGATE_LOW_PASS_RATES
Expand All @@ -93,6 +97,9 @@ endif
ifeq ($(DISABLE_MAG_UPDATE), 1)
AHRS_CFLAGS += -DDISABLE_MAG_UPDATE
endif
ifeq ($(USE_AHRS_GPS_ACCELERATIONS), 1)
AHRS_CFLAGS += -DUSE_AHRS_GPS_ACCELERATIONS
endif


AHRS_SRCS += ../../subsystems/ahrs.c \
Expand All @@ -113,10 +120,6 @@ 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_CFLAGS += -DINS_ROLL_NEUTRAL_DEFAULT=0
AHRS_CFLAGS += -DINS_PITCH_NEUTRAL_DEFAULT=0
AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512
AHRS_CFLAGS += -DDCM_UPDATE_AFTER_PROPAGATE
AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_dcm.c
endif
ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR2)
Expand Down
61 changes: 56 additions & 5 deletions sw/airborne/test/ahrs/ahrs_on_synth.c
View file
Expand Up @@ -42,6 +42,9 @@ static void traj_coordinated_circle_update(void);
static void traj_stop_stop_x_init(void);
static void traj_stop_stop_x_update(void);

static void traj_bungee_takeoff_init(void);
static void traj_bungee_takeoff_update(void);

struct traj traj[] = {
{.name="static", .desc="blaa",
.init_fun=traj_static_static_init, .update_fun=traj_static_static_update},
Expand All @@ -58,7 +61,9 @@ struct traj traj[] = {
{.name="coordinated circle", .desc="blaa2",
.init_fun=traj_coordinated_circle_init, .update_fun=traj_coordinated_circle_update},
{.name="stop stop x", .desc="blaa2",
.init_fun=traj_stop_stop_x_init, .update_fun=traj_stop_stop_x_update}
.init_fun=traj_stop_stop_x_init, .update_fun=traj_stop_stop_x_update},
{.name="bungee", .desc="blaa2",
.init_fun=traj_bungee_takeoff_init, .update_fun=traj_bungee_takeoff_update}
};


Expand Down Expand Up @@ -133,21 +138,24 @@ void aos_init(int traj_nb) {
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n");
#endif
#ifdef AHRS_MAG_UPDATE_YAW_ONLY
#if AHRS_MAG_UPDATE_YAW_ONLY
printf("# AHRS_MAG_UPDATE_YAW_ONLY\n");
#endif
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n");
#endif
#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n");
#endif
#ifdef PERFECT_SENSORS
printf("# PERFECT_SENSORS\n");
#endif
#ifdef AHRS_USE_GPS_HEADING
#if AHRS_USE_GPS_HEADING
printf("# AHRS_USE_GPS_HEADING\n");
#endif
#if USE_AHRS_GPS_ACCELERATIONS
printf("# USE_AHRS_GPS_ACCELERATIONS\n");
#endif

printf("# tajectory : %s\n", aos.traj->name);

Expand Down Expand Up @@ -192,6 +200,12 @@ void aos_compute_sensors(void) {
ahrs_impl.ltp_vel_norm = FLOAT_VECT3_NORM(aos.ltp_vel);
ahrs_impl.ltp_vel_norm_valid = TRUE;
#endif
#if AHRS_TYPE == AHRS_TYPE_FCR
ahrs_impl.gps_speed = FLOAT_VECT3_NORM(aos.ltp_vel);
ahrs_impl.gps_age = 0;
ahrs_update_gps();
//RunOnceEvery(100,printf("# gps accel: %f\n", ahrs_impl.gps_acceleration));
#endif
#if AHRS_TYPE == AHRS_TYPE_ICQ
ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(FLOAT_VECT3_NORM(aos.ltp_vel));
ahrs_impl.ltp_vel_norm_valid = TRUE;
Expand Down Expand Up @@ -237,13 +251,19 @@ void aos_run(void) {
float heading = aos.heading_meas;
#endif

#if AHRS_TYPE == AHRS_TYPE_FCR
ahrs_impl.gps_course = aos.heading_meas;
ahrs_impl.gps_course_valid = TRUE;
#else
if (aos.time > 10) {
if (!ahrs_impl.heading_aligned) {
ahrs_realign_heading(heading);
} else {
RunOnceEvery(100,ahrs_update_heading(heading));
}
}
#endif

#endif // AHRS_USE_GPS_HEADING

#ifndef DISABLE_ALIGNEMENT
Expand Down Expand Up @@ -445,3 +465,34 @@ static void traj_stop_stop_x_update(void){
FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);

}

static void traj_bungee_takeoff_init(void) {

aos.traj->te = 40.;
EULERS_ASSIGN(aos.ltp_to_imu_euler, 0, RadOfDeg(10), 0);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);

}

static void traj_bungee_takeoff_update(void) {
const float initial_bungee_accel = 20.0; // in m/s^2
const float start = 5;
const float duration = 2;

struct FloatVect3 accel = {0, 0, 0}; //acceleration in imu x-direction in m/s^2

if (aos.time > start && aos.time < start+duration) {
accel.x = initial_bungee_accel * (1 - (aos.time - start) / duration);
}
else {
accel.x = 0;
}

struct FloatQuat imu2ltp;
QUAT_INVERT(imu2ltp, aos.ltp_to_imu_quat);
FLOAT_QUAT_VMULT(aos.ltp_accel, imu2ltp, accel);

FLOAT_VECT3_INTEGRATE_FI(aos.ltp_vel, aos.ltp_accel, aos.dt);
FLOAT_VECT3_INTEGRATE_FI(aos.ltp_pos, aos.ltp_vel, aos.dt);

}
7 changes: 4 additions & 3 deletions sw/airborne/test/ahrs/ahrs_utils.py
View file
Expand Up @@ -84,15 +84,16 @@ def run_simulation(ahrs_type, build_opt, traj_nb):



def plot_simulation_results(plot_true_state, lsty, sim_res):
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)
plt.plot(sim_res.time, sim_res.phi_ahrs, lsty)
plt.plot(sim_res.time, sim_res.phi_ahrs, lsty, label=type)
ylabel('degres')
title('phi')
legend()

subplot(3,3,2)
plot(sim_res.time, sim_res.theta_ahrs, lsty)
Expand Down Expand Up @@ -155,4 +156,4 @@ def plot_simulation_results(plot_true_state, lsty, sim_res):


def show_plot():
plt.show();
plt.show();
89 changes: 49 additions & 40 deletions sw/airborne/test/ahrs/compare_ahrs.py
View file
@@ -1,6 +1,5 @@
#! /usr/bin/env python

# $Id$
# Copyright (C) 2011 Antoine Drouin
#
# This file is part of Paparazzi.
Expand All @@ -25,47 +24,57 @@

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 = 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 += ["PROPAGATE_LOW_PASS_RATES=1"];
build_opt1 = ["GRAVITY_UPDATE_COORDINATED_TURN=1"];
# build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=0"];
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";
sim_res1 = ahrs_utils.run_simulation(ahrs_type1, build_opt1, traj_nb)
# import code; code.interact(local=locals())
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_opt2 = [];
build_opt2 = ["GRAVITY_UPDATE_COORDINATED_TURN=1"];
# build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=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 = "FCQ";
# ahrs_type2 = "ICQ";
# ahrs_type2 = ahrs_type1;
sim_res2 = ahrs_utils.run_simulation(ahrs_type2, build_opt2, traj_nb)
# ahrs_type1 = "FCQ";
# ahrs_type1 = "FCR2";
# ahrs_type1 = "FLQ";
ahrs_type1 = "ICQ";
# ahrs_type1 = "FCR";

ahrs_utils.plot_simulation_results(False, 'b', sim_res1)
ahrs_utils.plot_simulation_results(True, 'g', sim_res2)
ahrs_utils.show_plot()
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;

# 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)

ahrs_utils.plot_simulation_results(False, 'b', ahrs_type1, sim_res1)
ahrs_utils.plot_simulation_results(True, 'g', ahrs_type2, sim_res2)
ahrs_utils.show_plot()

if __name__ == "__main__":
main()
main()
2 changes: 1 addition & 1 deletion sw/airborne/test/ahrs/run_ahrs_on_synth.c
View file
Expand Up @@ -71,7 +71,7 @@ static void report(void) {
DegOfRad(bias.q),
DegOfRad(bias.r));
#endif
#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ
#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR
printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p),
DegOfRad(ahrs_impl.gyro_bias.q),
DegOfRad(ahrs_impl.gyro_bias.r));
Expand Down

0 comments on commit 4135fd7

Please sign in to comment.