[paparazzi-commits] [5093] XSens in Fixed Wing Aircraft

[Christophe De Wagter]

Revision: 5093
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5093
Author:   dewagter
Date:     2010-07-21 11:23:37 +0000 (Wed, 21 Jul 2010)
Log Message:
-----------
XSens in Fixed Wing Aircraft

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/modules/ins/ins.h
    paparazzi3/trunk/sw/airborne/modules/ins/ins_xsens.c

Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins.h      2010-07-21 10:23:29 UTC 
(rev 5092)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins.h      2010-07-21 11:23:37 UTC 
(rev 5093)
@@ -65,6 +65,7 @@
 
 extern void ins_init( void );
 extern void ins_periodic_task( void );
+void handle_ins_msg( void);
 void parse_ins_msg( void );
 void parse_ins_buffer( uint8_t );
 

Modified: paparazzi3/trunk/sw/airborne/modules/ins/ins_xsens.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/ins/ins_xsens.c        2010-07-21 
10:23:29 UTC (rev 5092)
+++ paparazzi3/trunk/sw/airborne/modules/ins/ins_xsens.c        2010-07-21 
11:23:37 UTC (rev 5093)
@@ -133,13 +133,23 @@
   XSENS_GoToConfig();
   XSENS_SetOutputMode(xsens_output_mode);
   XSENS_SetOutputSettings(xsens_output_settings);
-  //XSENS_GoToMeasurment();
+  XSENS_GoToMeasurment();
 }
 
 void ins_periodic_task( void ) {
   RunOnceEvery(100,XSENS_ReqGPSStatus());
 }
 
+#include "estimator.h"
+
+void handle_ins_msg( void) {
+  if (xsens_id == XSENS_MTData_ID) {
+    EstimatorSetAtt(ins_phi,-ins_psi, -ins_theta);
+    EstimatorSetRate(ins_p,ins_q);
+    EstimatorSetSpeedPol(gps_gspeed, -ins_psi, ins_vz);
+  }
+}
+
 void parse_ins_msg( void ) {
   uint8_t offset = 0;
   if (xsens_id == XSENS_ReqOutputModeAck_ID) {