From 09e23e85cf69f414f62014d8e9c157a979f94fe6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu>
Date: Mon, 9 Sep 2024 20:33:10 +0200
Subject: [PATCH] [Payload][Radio] Implement SET_ORIENTATION_QUAT telemetry
 message

---
 src/boards/Payload/Radio/MessageHandler.cpp   | 30 +++++++++++++++++++
 .../NASController/NASController.cpp           | 12 ++++++++
 .../NASController/NASController.h             |  2 ++
 3 files changed, 44 insertions(+)

diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp
index a9be8eb26..63ab340e2 100644
--- a/src/boards/Payload/Radio/MessageHandler.cpp
+++ b/src/boards/Payload/Radio/MessageHandler.cpp
@@ -183,6 +183,36 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg)
             return enqueueWack(msg);
         }
 
+        case MAVLINK_MSG_ID_SET_ORIENTATION_QUAT_TC:
+        {
+            if (parent.getModule<NASController>()->getState() !=
+                NASControllerState::READY)
+            {
+                return enqueueNack(msg);
+            }
+
+            // Scalar first quaternion, W is the first element
+            auto quat = Eigen::Quaternionf{
+                mavlink_msg_set_orientation_quat_tc_get_quat_w(&msg),
+                mavlink_msg_set_orientation_quat_tc_get_quat_x(&msg),
+                mavlink_msg_set_orientation_quat_tc_get_quat_y(&msg),
+                mavlink_msg_set_orientation_quat_tc_get_quat_z(&msg),
+            };
+
+            parent.getModule<NASController>()->setOrientation(
+                quat.normalized());
+
+            float qNorm = quat.norm();
+            if (std::abs(qNorm - 1) > 0.001)
+            {
+                return enqueueWack(msg);
+            }
+            else
+            {
+                return enqueueAck(msg);
+            }
+        }
+
         case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC:
         {
             float altitude =
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp
index 93f1c5e5a..0b1569529 100644
--- a/src/boards/Payload/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp
@@ -101,6 +101,18 @@ ReferenceValues NASController::getReferenceValues()
 
 NASControllerState NASController::getState() { return state; }
 
+void NASController::setOrientation(const Eigen::Quaternionf& quat)
+{
+    Lock<FastMutex> lock{nasMutex};
+
+    auto x               = nas.getX();
+    x(NAS::IDX_QUAT + 0) = quat.x();
+    x(NAS::IDX_QUAT + 1) = quat.y();
+    x(NAS::IDX_QUAT + 2) = quat.z();
+    x(NAS::IDX_QUAT + 3) = quat.w();
+    nas.setX(x);
+}
+
 void NASController::Init(const Event& event)
 {
     switch (event)
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.h b/src/boards/Payload/StateMachines/NASController/NASController.h
index 8d3a7e196..4ff4eea6a 100644
--- a/src/boards/Payload/StateMachines/NASController/NASController.h
+++ b/src/boards/Payload/StateMachines/NASController/NASController.h
@@ -60,6 +60,8 @@ public:
 
     NASControllerState getState();
 
+    void setOrientation(const Eigen::Quaternionf& orientation);
+
 private:
     void calibrate();
 
-- 
GitLab