XSens backwards option...

paparazzi · Jun 20, 2011 · 351cfcc · 351cfcc
1 parent 8e327bf
commit 351cfcc
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
@@ -286,8 +286,13 @@ void handle_ins_msg( void) {
 
 
   // Send to Estimator (Control)
+#ifdef XSENS_BACKWARDS
+  EstimatorSetAtt((-ins_phi+ins_roll_neutral), (ins_psi + RadOfDeg(180)), (-ins_theta+ins_pitch_neutral));
+  EstimatorSetRate(-ins_p,-ins_q);
+#else
   EstimatorSetAtt(ins_phi+ins_roll_neutral, ins_psi, ins_theta+ins_pitch_neutral);
   EstimatorSetRate(ins_p,ins_q);
+#endif
 
   // Position
   float gps_east = gps.utm_pos.east / 100.;
@@ -311,7 +316,6 @@ void handle_ins_msg( void) {
   float fcourse = atan2f((float)ins_vy, (float)ins_vx);
   EstimatorSetSpeedPol(fspeed, fcourse, fclimb);
 
-
   // Now also finish filling the gps struct for telemetry purposes
   gps.gspeed = fspeed * 100.;
   gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100);