From 7e1940e8224fe9ca803ae910fefcd3cdefe9e2d7 Mon Sep 17 00:00:00 2001
From: Ewoud Smeur <ewoud_smeur@msn.com>
Date: Mon, 3 Aug 2015 18:35:36 +0200
Subject: [PATCH] fixing optitrack axes and altitude

---
 sw/ground_segment/misc/natnet2ivy.c | 24 ++++++++++++------------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/sw/ground_segment/misc/natnet2ivy.c b/sw/ground_segment/misc/natnet2ivy.c
index 173c48d6fd0..8d745de5343 100644
--- a/sw/ground_segment/misc/natnet2ivy.c
+++ b/sw/ground_segment/misc/natnet2ivy.c
@@ -205,9 +205,9 @@ void natnet_parse(unsigned char *in) {
       memcpy(&old_rigid, &rigidBodies[j], sizeof(struct RigidBody));
 
       memcpy(&rigidBodies[j].id, ptr, 4); ptr += 4;
-      memcpy(&rigidBodies[j].x, ptr, 4); ptr += 4;   //x --> X
+      memcpy(&rigidBodies[j].y, ptr, 4); ptr += 4;   //x --> Y
       memcpy(&rigidBodies[j].z, ptr, 4); ptr += 4;   //y --> Z
-      memcpy(&rigidBodies[j].y, ptr, 4); ptr += 4;   //z --> Y
+      memcpy(&rigidBodies[j].x, ptr, 4); ptr += 4;   //z --> X
       memcpy(&rigidBodies[j].qx, ptr, 4); ptr += 4;  //qx --> QX
       memcpy(&rigidBodies[j].qz, ptr, 4); ptr += 4;  //qy --> QZ
       memcpy(&rigidBodies[j].qy, ptr, 4); ptr += 4;  //qz --> QY
@@ -460,8 +460,8 @@ gboolean timeout_transmit_callback(gpointer data) {
     struct DoubleEulers orient_eulers;
 
     // Add the Optitrack angle to the x and y positions
-    pos.x = cos(tracking_offset_angle) * rigidBodies[i].x + sin(tracking_offset_angle) * rigidBodies[i].y;
-    pos.y = sin(tracking_offset_angle) * rigidBodies[i].x - cos(tracking_offset_angle) * rigidBodies[i].y;
+    pos.x = cos(tracking_offset_angle) * rigidBodies[i].x - sin(tracking_offset_angle) * rigidBodies[i].y;
+    pos.y = sin(tracking_offset_angle) * rigidBodies[i].x + cos(tracking_offset_angle) * rigidBodies[i].y;
     pos.z = rigidBodies[i].z;
 
     // Convert the position to ecef and lla based on the Optitrack LTP
@@ -479,8 +479,8 @@ gboolean timeout_transmit_callback(gpointer data) {
       rigidBodies[i].vel_z = rigidBodies[i].vel_z / sample_time;
 
       // Add the Optitrack angle to the x and y velocities
-      speed.x = cos(tracking_offset_angle) * rigidBodies[i].vel_x + sin(tracking_offset_angle) * rigidBodies[i].vel_y;
-      speed.y = sin(tracking_offset_angle) * rigidBodies[i].vel_x - cos(tracking_offset_angle) * rigidBodies[i].vel_y;
+      speed.x = cos(tracking_offset_angle) * rigidBodies[i].vel_x - sin(tracking_offset_angle) * rigidBodies[i].vel_y;
+      speed.y = sin(tracking_offset_angle) * rigidBodies[i].vel_x + cos(tracking_offset_angle) * rigidBodies[i].vel_y;
       speed.z = rigidBodies[i].vel_z;
 
       // Conver the speed to ecef based on the Optitrack LTP
@@ -495,7 +495,7 @@ gboolean timeout_transmit_callback(gpointer data) {
     double_eulers_of_quat(&orient_eulers, &orient);
 
     // Calculate the heading by adding the Natnet offset angle and normalizing it
-    double heading = -orient_eulers.psi-tracking_offset_angle;
+    double heading = -orient_eulers.psi+90.0/57.6 - tracking_offset_angle; //the optitrack axes are 90 degrees rotated wrt ENU
     NormRadAngle(heading);
 
     printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id
@@ -512,7 +512,7 @@ gboolean timeout_transmit_callback(gpointer data) {
       (int)(ecef_pos.z*100.0),                //int32 ECEF Z in CM
       (int)(DegOfRad(lla_pos.lat)*1e7),       //int32 LLA latitude in deg*1e7
       (int)(DegOfRad(lla_pos.lon)*1e7),       //int32 LLA longitude in deg*1e7
-      (int)(rigidBodies[i].z*1000.0),         //int32 LLA altitude in mm above elipsoid
+      (int)(lla_pos.alt*1000.0),              //int32 LLA altitude in mm above elipsoid
       (int)(rigidBodies[i].z*1000.0),         //int32 HMSL height above mean sea level in mm
       (int)(rigidBodies[i].ecef_vel.x*100.0), //int32 ECEF velocity X in cm/s
       (int)(rigidBodies[i].ecef_vel.y*100.0), //int32 ECEF velocity Y in cm/s
@@ -725,10 +725,10 @@ int main(int argc, char** argv)
 {
   // Set the default tracking system position and angle
   struct EcefCoor_d tracking_ecef;
-  tracking_ecef.x = 3924304;
-  tracking_ecef.y = 300360;
-  tracking_ecef.z = 5002162;
-  tracking_offset_angle = 123.0 / 57.6;
+  tracking_ecef.x = 3924332;
+  tracking_ecef.y = 300362;
+  tracking_ecef.z = 5002197;
+  tracking_offset_angle = 33.0 / 57.6;
   ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef);
 
   // Parse the options from cmdline