diff --git a/src/subsystems/navigation/athena_gps/CMakeLists.txt b/src/subsystems/navigation/athena_gps/CMakeLists.txt
index 33850750..155907c4 100644
--- a/src/subsystems/navigation/athena_gps/CMakeLists.txt
+++ b/src/subsystems/navigation/athena_gps/CMakeLists.txt
@@ -14,6 +14,8 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(msgs REQUIRED)
 find_package(MAVSDK REQUIRED)
 add_executable(athena_gps src/PixhawkNode.cpp)
 
@@ -26,6 +28,8 @@ ament_target_dependencies(athena_gps
         rclcpp
         std_msgs
         sensor_msgs
+        geometry_msgs
+        msgs
 )
 target_link_libraries(athena_gps
         MAVSDK::mavsdk
diff --git a/src/subsystems/navigation/athena_gps/package.xml b/src/subsystems/navigation/athena_gps/package.xml
index 2cd9b998..f65a4edc 100644
--- a/src/subsystems/navigation/athena_gps/package.xml
+++ b/src/subsystems/navigation/athena_gps/package.xml
@@ -12,6 +12,7 @@
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
   <depend>msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
diff --git a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp
index c7d3d1b7..1b168609 100644
--- a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp
+++ b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp
@@ -7,6 +7,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/imu.hpp"
 #include "sensor_msgs/msg/nav_sat_fix.hpp"
+#include "geometry_msgs/msg/quaternion_stamped.hpp"
+#include "msgs/msg/heading.hpp"
 #include <mavsdk/mavsdk.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
 
@@ -31,6 +33,8 @@ class PixhawkNode : public rclcpp::Node
         this->declare_parameter("gps_frame_id", "gps_link");
         this->declare_parameter("imu_topic", "imu/data");
         this->declare_parameter("gps_topic", "gps/fix");
+        this->declare_parameter("quaternion_topic", "imu/quaternion");
+        this->declare_parameter("heading_topic", "imu/heading");
         this->declare_parameter("mavsdk_system_id", 10);
         this->declare_parameter("mavsdk_component_id", 1);
         this->declare_parameter("mavsdk_always_send_heartbeats", false);
@@ -43,6 +47,8 @@ class PixhawkNode : public rclcpp::Node
         gps_frame_id_ = this->get_parameter("gps_frame_id").as_string();
         std::string imu_topic = this->get_parameter("imu_topic").as_string();
         std::string gps_topic = this->get_parameter("gps_topic").as_string();
+        std::string quaternion_topic = this->get_parameter("quaternion_topic").as_string();
+        std::string heading_topic = this->get_parameter("heading_topic").as_string();
         int mavsdk_system_id = this->get_parameter("mavsdk_system_id").as_int();
         int mavsdk_component_id = this->get_parameter("mavsdk_component_id").as_int();
         bool mavsdk_always_send_heartbeats = this->get_parameter("mavsdk_always_send_heartbeats").as_bool();
@@ -50,6 +56,8 @@ class PixhawkNode : public rclcpp::Node
         // Create publishers unconditionally so they're always valid
         imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>(imu_topic, 10);
         gps_publisher_ = this->create_publisher<sensor_msgs::msg::NavSatFix>(gps_topic, 10);
+        quaternion_publisher_ = this->create_publisher<geometry_msgs::msg::QuaternionStamped>(quaternion_topic, 10);
+        heading_publisher_ = this->create_publisher<msgs::msg::Heading>(heading_topic, 10);
 
         // Create MAVSDK instance
         mavsdk_ = std::make_unique<Mavsdk>(Mavsdk::Configuration{
@@ -73,6 +81,8 @@ class PixhawkNode : public rclcpp::Node
 
     rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
     rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_publisher_;
+    rclcpp::Publisher<geometry_msgs::msg::QuaternionStamped>::SharedPtr quaternion_publisher_;
+    rclcpp::Publisher<msgs::msg::Heading>::SharedPtr heading_publisher_;
     rclcpp::TimerBase::SharedPtr init_timer_;
 
     Telemetry::GpsInfo last_gps_info_;
@@ -144,6 +154,7 @@ class PixhawkNode : public rclcpp::Node
 
         telemetry_->subscribe_attitude_quaternion([this](const Telemetry::Quaternion &quaternion) {
             last_attitude_quaternion_ = quaternion;
+            attitude_quaternion_callback(quaternion);
         });
 
         telemetry_->subscribe_raw_gps([this](const Telemetry::RawGps &raw_gps) {
@@ -208,6 +219,45 @@ class PixhawkNode : public rclcpp::Node
         imu_publisher_->publish(msg);
     }
 
+    void attitude_quaternion_callback(const Telemetry::Quaternion quaternion) {
+        auto msg = geometry_msgs::msg::QuaternionStamped();
+        msg.header.stamp = this->now();
+        msg.header.frame_id = imu_frame_id_;
+
+        quaternion_frd_to_flu(quaternion,
+                             msg.quaternion.w,
+                             msg.quaternion.x,
+                             msg.quaternion.y,
+                             msg.quaternion.z);
+
+        quaternion_publisher_->publish(msg);
+
+        // Derive heading from the FLU (ENU-referenced) orientation. The yaw
+        // about the up axis is already in ROS convention: 0=East, CCW, radians.
+        const double w = msg.quaternion.w;
+        const double x = msg.quaternion.x;
+        const double y = msg.quaternion.y;
+        const double z = msg.quaternion.z;
+        double yaw = std::atan2(2.0 * (w * z + x * y),
+                                1.0 - 2.0 * (y * y + z * z));
+
+        auto heading_msg = msgs::msg::Heading();
+        heading_msg.header.stamp = msg.header.stamp;
+        heading_msg.header.frame_id = imu_frame_id_;
+        heading_msg.heading = yaw;
+        heading_msg.heading_acc = 0.0;  // not provided by the fused attitude
+
+        // compass_bearing: 0=North, CW, degrees. Convert from ENU heading and
+        // wrap into [0, 360). Does not account for magnetic declination.
+        double bearing_deg = std::fmod(90.0 - yaw * 180.0 / M_PI, 360.0);
+        if (bearing_deg < 0.0) {
+            bearing_deg += 360.0;
+        }
+        heading_msg.compass_bearing = bearing_deg;
+
+        heading_publisher_->publish(heading_msg);
+    }
+
     void raw_gps_callback(const Telemetry::RawGps raw_gps) {
         auto gps_msg = sensor_msgs::msg::NavSatFix();