diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 7411ca22..002e6ff6 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -75,12 +75,11 @@ private:
                 has_swithed = true;
             }
 
-            cur_yaw += 1 % 360;
-            msg.velocity[0] = 0;
+            msg.velocity[0] = 5;
             msg.velocity[1] = 0;
             msg.velocity[2] = D_SPEED(0);
             msg.yawspeed = 0.5;
-            msg.yaw = degrees_to_radians(cur_yaw);
+            msg.yaw = degrees_to_radians(80);
         }
 
         // if (setpoint_count < 30)
diff --git a/src/px4_connection/srv/SetAttitude.srv b/src/px4_connection/srv/SetAttitude.srv
new file mode 100644
index 00000000..46896943
--- /dev/null
+++ b/src/px4_connection/srv/SetAttitude.srv
@@ -0,0 +1,9 @@
+#all speeds are in meters/second
+float32 x_speed
+float32 y_speed
+float32 z_speed
+
+#angle is in degrees
+float32 angle
+---
+int status
diff --git a/src/px4_msgs b/src/px4_msgs
index b64ef047..ffc3a4cd 160000
--- a/src/px4_msgs
+++ b/src/px4_msgs
@@ -1 +1 @@
-Subproject commit b64ef0475c1d44605688f4770899fe453d532be4
+Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266