diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp
index a9be8eb267b161d32b137ad04a956f8f1964e349..63ab340e2afa5234d680c98fb823e6c091d345b1 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 93f1c5e5a7628675379b810f74357ff0f84da0da..0b1569529fc9b496db22fde35ca2174f82f661fd 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 8d3a7e19607d0b631fd8864c7e03942d833c88c4..4ff4eea6a6ef021e36ed3e54203b88e9b3baf9db 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();