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 */

sw/simulator/nps_ivy.c

@@ -1,38 +1,45 @@
 #include "nps_ivy.h"
+#include "nps_autopilot.h"
+#include "pprz_algebra_double.h"
 
 #include <Ivy/ivy.h>
 #include <Ivy/ivyglibloop.h>
 
-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]));
+}

sw/simulator/nps_ivy.h

@@ -1,10 +1,7 @@
 #ifndef NPS_IVY
 #define NPS_IVY
 
-#include <Ivy/ivy.h>
-#include <Ivy/ivyglibloop.h>
-
-extern void nps_ivy_init(void);
+extern void nps_ivy_transport_init(void);
 extern void nps_ivy_display(void); 
 
 #endif /* NPS_IVY */

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();
 }