Merge pull request #69 from Pitt-RAS/hack-heading-quad

Hack heading quad
Pitt-RAS · Aug 18, 2018 · ebed7ef · ebed7ef
2 parents 3f247e2 + eb5f97c
commit ebed7ef
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 5 deletions.
diff --git 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
@@ -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};