Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 04:45:37 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

XSens backwards option...

Browse Source
This commit is contained in:
Christophe De Wagter
2011-06-20 21:22:05 +02:00
parent 8e327bfe96
commit 351cfcc7f2
1 changed files with 5 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/ins/ins_xsens.c
+5 -1
View File
@@ -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);