From 6a1f18d6567d275128da44ea41035f0bce20a77d Mon Sep 17 00:00:00 2001
From: isvogor <isvogor@gmail.com>
Date: Wed, 12 Apr 2017 15:29:27 -0400
Subject: [PATCH] test: suspecting offset is ignored

---
 src/roscontroller.cpp | 34 +++++++++++++++++++++-------------
 src/testflockfev.bzz  |  2 +-
 2 files changed, 22 insertions(+), 14 deletions(-)

diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index d029c5e..a4f3269 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -448,9 +448,9 @@ namespace rosbzz_node{
 			}
 		} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
 			/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
-			// roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
+			roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
 
-			roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0);
+			//roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0);
 		}
 	}
 	/*----------------------------------------------
@@ -777,21 +777,29 @@ namespace rosbzz_node{
 		// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
 
 		mavros_msgs::PositionTarget moveMsg;
+		const uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);
+
 		moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_OFFSET_NED;
-		moveMsg.type_mask = mavros_msgs::PositionTarget::IGNORE_AFX |
-		                                     mavros_msgs::PositionTarget::IGNORE_AFY |
-		                                     mavros_msgs::PositionTarget::IGNORE_AFZ;
-		// alt: 0b0000111111111000
+		moveMsg.type_mask = ignore_all_except_xyz_y;
+
 		moveMsg.header.frame_id = 1;
 		moveMsg.header.seq = setpoint_counter++;
 		moveMsg.header.stamp = ros::Time::now();
-		moveMsg.position.x = 0.5;
-		moveMsg.position.y = 0.0;
-		moveMsg.position.z = 2;
-		moveMsg.velocity.x = 0.5;
-		moveMsg.velocity.y = 0.0;
-		moveMsg.velocity.z = 0.5;
-		moveMsg.yaw = 0;
+
+		my_x += x;
+		my_y += y;
+
+		moveMsg.position.x = x;
+		moveMsg.position.y = y;
+		moveMsg.position.z = 3;
+		moveMsg.yaw_rate = 0.0;
+		moveMsg.yaw = 0.0;
+
+
+		geometry_msgs::Vector3 zeroVec;
+		zeroVec.x = 0.0; zeroVec.y = 0.0; zeroVec.z = 0.0;
+		moveMsg.acceleration_or_force = zeroVec;
+		moveMsg.velocity = zeroVec;
 
 		localsetpoint_pub.publish(moveMsg);
 
diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz
index 3bda4c7..7b1df3f 100644
--- a/src/testflockfev.bzz
+++ b/src/testflockfev.bzz
@@ -50,7 +50,7 @@ function hexagon() {
   	statef=land
   	} else {
   	timeW = timeW+1
-  	uav_moveto(0.0,0.02)
+  	uav_moveto(0.0,0.35)
   	}
 }