From fd2fb38e5af42b7cef4efa6d476156506745d6b2 Mon Sep 17 00:00:00 2001
From: albertoesmp <albertoesmp@gmail.com>
Date: Sat, 1 Jun 2024 00:12:34 +0200
Subject: [PATCH] Update for ARINC 705 format for interpolated platforms.

---
 src/maths/Directions.cpp                    |  6 ------
 src/platform/InterpolatedMovingPlatform.cpp | 10 +++++-----
 2 files changed, 5 insertions(+), 11 deletions(-)

diff --git a/src/maths/Directions.cpp b/src/maths/Directions.cpp
index 672518636..57982dbae 100644
--- 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
index e9c0c523d..96e28551f 100644
--- 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: