From ce771d1ce033fd0728bbe2d7e68fd65ea210acbd Mon Sep 17 00:00:00 2001 From: Gustavo Oliveira Violato Date: Wed, 24 Jun 2009 10:36:59 +0000 Subject: [PATCH] --- sw/airborne/pprz_algebra_double.h | 23 ++++++++++++ sw/simulator/nps_ivy.c | 61 +++++++++++++++++-------------- sw/simulator/nps_ivy.h | 5 +-- sw/simulator/nps_main.c | 3 +- 4 files changed, 60 insertions(+), 32 deletions(-) diff --git a/sw/airborne/pprz_algebra_double.h b/sw/airborne/pprz_algebra_double.h index 3e0c9c77a8..6e1fc5a8d4 100644 --- a/sw/airborne/pprz_algebra_double.h +++ b/sw/airborne/pprz_algebra_double.h @@ -113,4 +113,27 @@ struct DoubleRates { \ } +#define DOUBLE_EULERS_OF_QUAT(_e, _q) { \ + \ + const double qx2 = (_q).qx*(_q).qx; \ + const double qy2 = (_q).qy*(_q).qy; \ + const double qz2 = (_q).qz*(_q).qz; \ + const double qiqx = (_q).qi*(_q).qx; \ + const double qiqy = (_q).qi*(_q).qy; \ + const double qiqz = (_q).qi*(_q).qz; \ + const double qxqy = (_q).qx*(_q).qy; \ + const double qxqz = (_q).qx*(_q).qz; \ + const double qyqz = (_q).qy*(_q).qz; \ + const double dcm00 = 1.0 - 2.*( qy2 + qz2 ); \ + const double dcm01 = 2.*( qxqy + qiqz ); \ + const double dcm02 = 2.*( qxqz - qiqy ); \ + const double dcm12 = 2.*( qyqz + qiqx ); \ + const double dcm22 = 1.0 - 2.*( qx2 + qy2 ); \ + \ + (_e).phi = atan2( dcm12, dcm22 ); \ + (_e).theta = -asin( dcm02 ); \ + (_e).psi = atan2( dcm01, dcm00 ); \ + \ + } + #endif /* PPRZ_ALGEBRA_DOUBLE_H */ diff --git a/sw/simulator/nps_ivy.c b/sw/simulator/nps_ivy.c index b47cb571e7..2d95b1678f 100644 --- a/sw/simulator/nps_ivy.c +++ b/sw/simulator/nps_ivy.c @@ -1,38 +1,45 @@ #include "nps_ivy.h" +#include "nps_autopilot.h" +#include "pprz_algebra_double.h" #include #include -static void -IvySendMsg("%d BOOZ_SIM_RPMS %f %f %f %f", - AC_ID, - RPM_OF_RAD_S(bfm.omega->ve[SERVO_FRONT]), - RPM_OF_RAD_S(bfm.omega->ve[SERVO_BACK]), - RPM_OF_RAD_S(bfm.omega->ve[SERVO_RIGHT]), - RPM_OF_RAD_S(bfm.omega->ve[SERVO_LEFT]) ); -IvySendMsg("%d BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f", - AC_ID, - DegOfRad(bfm.ang_rate_body->ve[AXIS_X]), - DegOfRad(bfm.ang_rate_body->ve[AXIS_Y]), - DegOfRad(bfm.ang_rate_body->ve[AXIS_Z]), - DegOfRad(bfm.eulers->ve[AXIS_X]), - DegOfRad(bfm.eulers->ve[AXIS_Y]), - DegOfRad(bfm.eulers->ve[AXIS_Z])); -IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f", - AC_ID, - (bfm.speed_ltp->ve[AXIS_X]), - (bfm.speed_ltp->ve[AXIS_Y]), - (bfm.speed_ltp->ve[AXIS_Z]), - (bfm.pos_ltp->ve[AXIS_X]), - (bfm.pos_ltp->ve[AXIS_Y]), - (bfm.pos_ltp->ve[AXIS_Z])); - - -static void ivy_transport_init(void) { - IvyInit ("BoozSim", "BoozSim READY", NULL, NULL, NULL, NULL); +extern void ivy_transport_init(void) { + IvyInit("BoozSim", "BoozSim READY", NULL, NULL, NULL, NULL); //IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)"); //IvyBindMsg(on_DL_BLOCK, NULL, "^(\\S*) BLOCK (\\S*) (\\S*)"); //IvyBindMsg(on_DL_MOVE_WP, NULL, "^(\\S*) MOVE_WP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)"); IvyStart("127.255.255.255"); } + +extern void nps_ivy_display(void) { + + DoubleEulers eulers; + + DOUBLE_EULERS_OF_QUAT(eulers, fdm.ltp_to_body_quat); + + IvySendMsg("%d COMMANDS %f %f %f %f", + AC_ID, + autopilot.commands[SERVO_FRONT], + autopilot.commands[SERVO_BACK], + autopilot.commands[SERVO_RIGHT], + autopilot.commands[SERVO_LEFT]); + IvySendMsg("%d BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f", + AC_ID, + DegOfRad(bfm.ang_rate_body->ve[AXIS_X]), + DegOfRad(bfm.ang_rate_body->ve[AXIS_Y]), + DegOfRad(bfm.ang_rate_body->ve[AXIS_Z]), + DegOfRad(bfm.eulers->ve[AXIS_X]), + DegOfRad(bfm.eulers->ve[AXIS_Y]), + DegOfRad(bfm.eulers->ve[AXIS_Z])); + IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f", + AC_ID, + (bfm.speed_ltp->ve[AXIS_X]), + (bfm.speed_ltp->ve[AXIS_Y]), + (bfm.speed_ltp->ve[AXIS_Z]), + (bfm.pos_ltp->ve[AXIS_X]), + (bfm.pos_ltp->ve[AXIS_Y]), + (bfm.pos_ltp->ve[AXIS_Z])); +} diff --git a/sw/simulator/nps_ivy.h b/sw/simulator/nps_ivy.h index 5b5e743910..8ca90cba1d 100644 --- a/sw/simulator/nps_ivy.h +++ b/sw/simulator/nps_ivy.h @@ -1,10 +1,7 @@ #ifndef NPS_IVY #define NPS_IVY -#include -#include - -extern void nps_ivy_init(void); +extern void nps_ivy_transport_init(void); extern void nps_ivy_display(void); #endif /* NPS_IVY */ diff --git a/sw/simulator/nps_main.c b/sw/simulator/nps_main.c index ad28c0b662..d950c34541 100644 --- a/sw/simulator/nps_main.c +++ b/sw/simulator/nps_main.c @@ -54,6 +54,7 @@ static void nps_main_init(void) { gettimeofday (&nps_main.host_time_start, NULL); nps_main.host_time_factor = 0.01; + nps_ivy_transport_init(); nps_fdm_init(SIM_DT); nps_sensors_init(nps_main.sim_time); nps_autopilot_init(); @@ -78,7 +79,7 @@ static void nps_main_run_sim_step(void) { static void nps_main_display(void) { printf("display at %f\n", nps_main.display_time); - + nps_ivy_display(); }