diff --git a/include/iarc7_fc_comms/CommonFcComms.hpp b/include/iarc7_fc_comms/CommonFcComms.hpp
index 4c600c8..f3391db 100644
--- a/include/iarc7_fc_comms/CommonFcComms.hpp
+++ b/include/iarc7_fc_comms/CommonFcComms.hpp
@@ -203,15 +203,15 @@ orientation_timestamp_offset_()
         sequenced_updates.push_back(&CommonFcComms::updateBattery);
     }
 
-    initial_heading_as_offset_ = 
+    initial_heading_as_offset_ =
                             ros_utils::ParamUtils::getParam<bool>(
                             private_nh_, "initial_heading_as_offset");
 
-    calibrate_accelerometer_ = 
+    calibrate_accelerometer_ =
                             ros_utils::ParamUtils::getParam<bool>(
                             private_nh_, "calibrate_accelerometer");
 
-    publish_orientation_transform_ = 
+    publish_orientation_transform_ =
                             ros_utils::ParamUtils::getParam<bool>(
                             private_nh_, "publish_orientation_transform");
 
@@ -551,7 +551,7 @@ template<class T>
 void CommonFcComms<T>::activateFcSafety()
 {
     FcCommsReturns status = flightControlImpl_.safetyLand();
-    
+
     if (status == FcCommsReturns::kReturnOk) {
         ROS_WARN("iarc7_fc_comms: succesfully sent safety land request");
     } else {
@@ -710,6 +710,20 @@ void CommonFcComms<T>::sendOrientation(double (&attitude)[3])
         transformStamped.transform.rotation.w = q.w();
 
         transform_broadcaster_.sendTransform(transformStamped);
+
+        transformStamped.header.stamp = ros::Time::now() + orientation_timestamp_offset_;
+        transformStamped.header.frame_id = CommonConf::kTfParentName;
+        transformStamped.child_frame_id = "heading_quad";
+
+        //tf2::Quaternion q;
+       // This assumes the values are returned in the form roll pitch yaw in radians
+        q.setRPY(0.0, 0.0, -attitude[2]);
+        transformStamped.transform.rotation.x = q.x();
+        transformStamped.transform.rotation.y = q.y();
+        transformStamped.transform.rotation.z = q.z();
+        transformStamped.transform.rotation.w = q.w();
+
+        transform_broadcaster_.sendTransform(transformStamped);
     }
 }
 
diff --git a/include/iarc7_fc_comms/MspConf.hpp b/include/iarc7_fc_comms/MspConf.hpp
index 488bcce..94f5d8e 100644
--- a/include/iarc7_fc_comms/MspConf.hpp
+++ b/include/iarc7_fc_comms/MspConf.hpp
@@ -71,7 +71,7 @@ namespace FcComms
         static constexpr const double kMspMaxAngleRadians{55.0 * M_PI / 180.0};
         static constexpr const double kMspPitchScale{(kMspStickEndPoint - kMspStickMidPoint)/kMspMaxAngleRadians};
         static constexpr const double kMspRollScale{kMspPitchScale};
-        static constexpr const double kMspYawScale{0.0};// Should be filled in with values based on (kMspMax-kMspMidpoint) / max rate of rotation
+        static constexpr const double kMspYawScale{250.0};// Should be filled in with values based on (kMspMax-kMspMidpoint) / max rate of rotation
         static constexpr const double kMspThrottleScale{(kMspStickEndPoint-kMspStickStartPoint)};  // (Max-Min) * throttle + min where throttle is ranged from 0-1
         static constexpr const double kSafetyLandingThrottle{(1450.0 - kMspStickStartPoint)/kMspThrottleScale};