Fix natnet2ivy rotation order

[tblaha]

@tmldeponti recently found that the pitch and roll components of the natnet2ivy.py are sometimes wrong. Further investigation showed that this happens due to a wrong rotation order that shows up when not using options --x_side right and --ac_nose far, and when the craft is yawed away from pointing to the "north". Also, the yaw angle component seemed unaffected, which is why this bug hasn't shown up before (optitrack pitch/roll components are not typically used by estimators AFAIK?).

The diff looks huge, but actually the only functional change is post-multiplying q_nose_correction instead of pre-multiplying here:

paparazzi/sw/ground_segment/python/natnet3.x/natnet2ivy.py

Lines 310 to 312 in 41998a1

	quat = Quat(real=quat[3], imaginary=[quat[0], quat[1], quat[2]])
	quat = q_nose_correction * (q_total * quat * q_total.inverse)
	quat = quat.elements[[1, 2, 3, 0]].tolist()

To really make sure this time that it works for all options, I generated test cases, and encapsulated some more script code in functions so they can be tested if --test is given as argument (in which case all other arguments are ignored and only the tests are performed)

Todo:

• test flight with --x_angle set to some numerical value

[fvantienen]

What about

paparazzi/sw/airborne/modules/ins/ins_ext_pose.c

Line 181 in fe35af7

orient.qi = quat_i;

? This seems incorrect I think?
In my opinion the message going to the drone should always be in 1 orientation such that this can be added to the messages.xml file.

Maybe we should add some description of the conversions also in the modules.

[tblaha]

Thanks! Why is this different from ins_ekf2.cpp? And what is the purpose of gps_datalink.c if the INS also read that message directly?

paparazzi/sw/airborne/modules/ins/ins_ekf2.cpp

Lines 711 to 714 in fe35af7

	sample.quat(0) = DL_EXTERNAL_POSE_body_qi(buf);
	sample.quat(1) = DL_EXTERNAL_POSE_body_qy(buf);
	sample.quat(2) = DL_EXTERNAL_POSE_body_qx(buf);
	sample.quat(3) = -DL_EXTERNAL_POSE_body_qz(buf);

The way I see it, natnet2ivy.py has always been sending attitude in ENU, meaning the rotation of a Right-Forward-Up local frame with respect to the East-North-Up frame. This can be converted to Forward-Right-Down-wrt-North-East-Down by simple exchanging x and y, and negating z. (the same way as you would convert a position).
This choice is not configurable in natnet2ivy.py, but what is configurable is how that ENU frame is oriented with respect to the observer. But position and attitude should always be consistent in that frame.

[tblaha]

Ah, I didnt see the theta=-theta line a few lines down:

paparazzi/sw/airborne/modules/ins/ins_ext_pose.c

Lines 181 to 187 in fe35af7

	orient.qi = quat_i;
	orient.qx = quat_y; //north
	orient.qy = -quat_x; //east
	orient.qz = -quat_z; //down
	float_eulers_of_quat(&orient_eulers, &orient);
	orient_eulers.theta = -orient_eulers.theta;

Then it all makes sense, I think and is the same ins_ekf2.cpp and also the same as @tmldeponti 's change in #3233

[gautierhattenberger]

Thanks for pointing this rotation error. After a recent update of Natnet, we have also a new version of NatNetClient.py, so we need to merge them correctly to check if this is still working for us. I'll let you know.

[tblaha]

Hey Gautier! Perfect, I'm available for testing new stuff as well. On a related note, we have been developing a C++ Natnet client with to goal of keeping the CLI as similar as possible between different target robot/drone operating systems in operation at TU Delft. It's also not completely finished, and I'm not quite sure if it's possible/desirable to include it with paparazzi as well, but I'd like to hear your thoughts.

https://github.com/tudelft/UnifiedOptitrackClients