Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-21 03:43:26 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

fixing optitrack axes and altitude

Browse Source
This commit is contained in:
Ewoud Smeur
2015-08-03 18:35:36 +02:00
parent 1d00988b0f
commit 7e1940e822
1 changed files with 12 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/ground_segment/misc/natnet2ivy.c
+12 -12
View File
@@ -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