new controller tweaks

Onboard-SDK-ROS/include/dji_osdk_ros/bus_driver.h

@@ -111,7 +111,7 @@ ServiceAck obtainCtrlAuthority();
 
 bool takeoff();
 
-bool relativePosition(float x, float y, float z, float yaw);
+std::vector<float> relativePosition(float x, float y, float z, float yaw);
 
 float applyRotationAngle(float targetYaw);
 
@@ -127,10 +127,12 @@ void calibratePositionReference(std::vector<float> requestedPosition, std::vecto
 
 void updateMagnitudeScaler(float newValue);
 
-std::vector<float> applyRotationMatrixScaling(std::vector<float> inputVec);
+float flatAngleBetween(std::vector<float> a, std::vector<float> b);
+
+void updateAngleScaler(float newValue);
 
 bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Overwatch::Response &res);
-std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec);
+// std::vector<float> DoCalibrationMove(std::vector<float> calibrationVec);
 
 void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg);
 void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg);

Onboard-SDK-ROS/scripts/qualisysFeedback.py

@@ -25,7 +25,7 @@ def main():
     name = rospy.get_param("~trackName")
     namespace = rospy.get_param("~qualisysName")
     rospy.loginfo("looking for track of name " + str(name) + " ...")
-    pub = rospy.Publisher(namespace + "/pose_feedback", Transform, queue_size = 1)
+    pub = rospy.Publisher(namespace, Transform, queue_size = 1)
     rospy.Subscriber("/" + namespace + "/qualisys/" + name + "/pose", PoseStamped, callback, queue_size = 1)
     rospy.spin()