diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt
index 2b9df55b..83a12abd 100644
--- a/src/msgs/CMakeLists.txt
+++ b/src/msgs/CMakeLists.txt
@@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Currentdraw.msg"
   "msg/JointReceiver.msg"
   "msg/SystemInfo.msg"
+  "msg/Heading.msg"
   "srv/SetController.srv"
   "action/NavigateToGPS.action"
   DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces action_msgs
diff --git a/src/msgs/msg/Heading.msg b/src/msgs/msg/Heading.msg
new file mode 100644
index 00000000..7f2007c9
--- /dev/null
+++ b/src/msgs/msg/Heading.msg
@@ -0,0 +1,8 @@
+# heading: ROS convention (0=East, CCW, radians). Converted from compass bearing,
+#          but does not account for magnetic declination.
+# compass_bearing: raw magnetometer reading (0=magnetic North, CW, degrees).
+
+std_msgs/Header header
+float64 heading
+float64 heading_acc
+float64 compass_bearing
diff --git a/src/subsystems/navigation/athena_gps/CMakeLists.txt b/src/subsystems/navigation/athena_gps/CMakeLists.txt
index 33850750..ea72dfae 100644
--- a/src/subsystems/navigation/athena_gps/CMakeLists.txt
+++ b/src/subsystems/navigation/athena_gps/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(msgs REQUIRED)
 find_package(MAVSDK REQUIRED)
 add_executable(athena_gps src/PixhawkNode.cpp)
 
@@ -26,6 +27,7 @@ ament_target_dependencies(athena_gps
         rclcpp
         std_msgs
         sensor_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 2138e4c3..2cd9b998 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>msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp
index c6d9a8e9..545129b5 100644
--- a/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp
+++ b/src/subsystems/navigation/athena_gps/src/PixhawkNode.cpp
@@ -7,7 +7,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/imu.hpp"
 #include "sensor_msgs/msg/nav_sat_fix.hpp"
-#include "std_msgs/msg/float64.hpp"
+#include "msgs/msg/heading.hpp"
 #include <mavsdk/mavsdk.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
 
@@ -54,7 +54,7 @@ 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);
-        heading_publisher_ = this->create_publisher<std_msgs::msg::Float64>(heading_topic, 10);
+        heading_publisher_ = this->create_publisher<msgs::msg::Heading>(heading_topic, 10);
 
         // Create MAVSDK instance
         mavsdk_ = std::make_unique<Mavsdk>(Mavsdk::Configuration{
@@ -78,7 +78,7 @@ 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<std_msgs::msg::Float64>::SharedPtr heading_publisher_;
+    rclcpp::Publisher<msgs::msg::Heading>::SharedPtr heading_publisher_;
     rclcpp::TimerBase::SharedPtr init_timer_;
 
     Telemetry::GpsInfo last_gps_info_;
@@ -94,12 +94,6 @@ class PixhawkNode : public rclcpp::Node
     bool is_initialized_;
     bool connection_added_;
 
-    void publish_heading() {
-        auto msg = std_msgs::msg::Float64();
-        msg.data = last_heading_deg_;
-        heading_publisher_->publish(msg);
-    }
-
     void try_initialize() {
         if (is_initialized_) {
             return;
@@ -151,7 +145,6 @@ class PixhawkNode : public rclcpp::Node
 
         RCLCPP_INFO(this->get_logger(), "Telemetry rates configured successfully");
 
-        // Subscribe to telemetry streams
         telemetry_->subscribe_imu([this](const Telemetry::Imu &imu) {
             imu_callback(imu);
         });
@@ -160,11 +153,6 @@ class PixhawkNode : public rclcpp::Node
             last_attitude_quaternion_ = quaternion;
         });
 
-        telemetry_->subscribe_heading([this](Telemetry::Heading heading) {
-            last_heading_deg_ = heading.heading_deg;
-            publish_heading();
-        });
-
         telemetry_->subscribe_raw_gps([this](const Telemetry::RawGps &raw_gps) {
             raw_gps_callback(raw_gps);
         });
@@ -228,39 +216,39 @@ class PixhawkNode : public rclcpp::Node
     }
 
     void raw_gps_callback(const Telemetry::RawGps raw_gps) {
-        auto msg = sensor_msgs::msg::NavSatFix();
+        auto gps_msg = sensor_msgs::msg::NavSatFix();
 
-        msg.header.stamp = this->now();
-        msg.header.frame_id = gps_frame_id_;
+        gps_msg.header.stamp = this->now();
+        gps_msg.header.frame_id = gps_frame_id_;
 
-        msg.latitude = raw_gps.latitude_deg;
-        msg.longitude = raw_gps.longitude_deg;
-        msg.altitude = raw_gps.altitude_ellipsoid_m;
+        gps_msg.latitude = raw_gps.latitude_deg;
+        gps_msg.longitude = raw_gps.longitude_deg;
+        gps_msg.altitude = raw_gps.altitude_ellipsoid_m;
 
         // Map GPS fix to ROS fix 
         if (last_gps_info_.num_satellites > 0) {
             switch (last_gps_info_.fix_type) {
                 case Telemetry::FixType::NoGps:
                 case Telemetry::FixType::NoFix:
-                    msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
+                    gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
                     break;
                 case Telemetry::FixType::Fix2D:
-                    msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
+                    gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
                     break;
                 case Telemetry::FixType::Fix3D:
                 case Telemetry::FixType::FixDgps:
                 case Telemetry::FixType::RtkFloat:
                 case Telemetry::FixType::RtkFixed:
-                    msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX;
+                    gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_GBAS_FIX;
                     break;
                 default:
-                    msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
+                    gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
             }
         } else {
-            msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
+            gps_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX;
         }
 
-        msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS;
+        gps_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS;
 
         // calcilate variances
         if (!std::isnan(raw_gps.horizontal_uncertainty_m) &&
@@ -269,23 +257,30 @@ class PixhawkNode : public rclcpp::Node
             double h_variance = raw_gps.horizontal_uncertainty_m * raw_gps.horizontal_uncertainty_m;
             double v_variance = raw_gps.vertical_uncertainty_m * raw_gps.vertical_uncertainty_m;
 
-            msg.position_covariance[0] = h_variance;
-            msg.position_covariance[4] = h_variance;
-            msg.position_covariance[8] = v_variance;
+            gps_msg.position_covariance[0] = h_variance;
+            gps_msg.position_covariance[4] = h_variance;
+            gps_msg.position_covariance[8] = v_variance;
 
             // refine estimate using doppler
             if (!std::isnan(raw_gps.hdop) && !std::isnan(raw_gps.vdop)) {
-                msg.position_covariance[0] = h_variance * (raw_gps.hdop / 2.0);
-                msg.position_covariance[4] = h_variance * (raw_gps.hdop / 2.0);
-                msg.position_covariance[8] = v_variance * raw_gps.vdop;
+                gps_msg.position_covariance[0] = h_variance * (raw_gps.hdop / 2.0);
+                gps_msg.position_covariance[4] = h_variance * (raw_gps.hdop / 2.0);
+                gps_msg.position_covariance[8] = v_variance * raw_gps.vdop;
             }
 
-            msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
+            gps_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
         } else {
-            msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
+            gps_msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
         }
-
-        gps_publisher_->publish(msg);
+        auto heading_msg = msgs::msg::Heading();
+        heading_msg.header.stamp = this->now();
+        heading_msg.compass_bearing = raw_gps.yaw_deg;
+        // Convert from compass bearing (0=magnetic North, CW, degrees) to ROS convention (0=East, CCW, radians)
+        heading_msg.heading = (90.0 - raw_gps.yaw_deg) * M_PI / 180.0;
+        heading_msg.heading_acc = raw_gps.heading_uncertainty_deg * M_PI / 180.0;
+
+        heading_publisher_->publish(heading_msg);
+        gps_publisher_->publish(gps_msg);
     }
 };