Update for ARINC 705 format for interpolated platforms.

3dgeo-heidelberg · May 31, 2024 · fd2fb38 · fd2fb38
1 parent 937ac2f
commit fd2fb38
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 11 deletions.
diff --git a/src/maths/Directions.cpp b/src/maths/Directions.cpp
@@ -3,12 +3,6 @@
 
 // ***  CONSTANTS  *** //
 // ******************* //
-// Canonical basis
 const glm::dvec3 Directions::right = glm::dvec3(1, 0, 0);
 const glm::dvec3 Directions::forward = glm::dvec3(0, 1, 0);
 const glm::dvec3 Directions::up = glm::dvec3(0, 0, 1);
-
-// ARINC 705 norm
-const glm::dvec3 Directions::yaw = glm::dvec3(0, 0, -1);
-const glm::dvec3 Directions::roll = glm::dvec3(0, 1, 0);
-const glm::dvec3 Directions::pitch = glm::dvec3(1, 0, 0);
diff --git a/src/platform/InterpolatedMovingPlatform.cpp b/src/platform/InterpolatedMovingPlatform.cpp
@@ -50,17 +50,17 @@ InterpolatedMovingPlatform::InterpolatedMovingPlatform(
             break;
         case RotationSpec::ARINC_705:
             calcAttitude = [] (arma::Col<double> const x) -> Rotation {
-                return Rotation(Directions::yaw, x[2]).applyTo(
-                    Rotation(Directions::roll, x[0])
+                return Rotation(Directions::right, x[1]).applyTo(
+                    Rotation(Directions::forward, x[0])
                 ).applyTo(
-                    Rotation(Directions::pitch, x[1])
+                    Rotation(Directions::up, -x[2])
                 );
             };
             _getRollPitchYaw = [] (
                 double &roll, double &pitch, double &yaw, Rotation &attitude
             ) -> void {
-                attitude.getAngles(&RotationOrder::ZYX, yaw, pitch, roll);
-                yaw = (yaw < M_PI) ? -yaw : PI_2 - yaw;
+                attitude.getAngles(&RotationOrder::XYZ, pitch, roll, yaw);
+                yaw = -yaw;
             };
             break;
         default: