Merge branch 'v0.24-release' into simple-offboard-update

clover/CMakeLists.txt

@@ -80,11 +80,10 @@ catkin_python_setup()
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  State.msg
+)
 
 ## Generate services in the 'srv' folder
 add_service_files(
@@ -92,6 +91,9 @@ add_service_files(
   GetTelemetry.srv
   Navigate.srv
   NavigateGlobal.srv
+  SetAltitude.srv
+  SetYaw.srv
+  SetYawRate.srv
   SetPosition.srv
   SetVelocity.srv
   SetAttitude.srv
@@ -306,4 +308,5 @@ endif()
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest(test/basic.test)
+  add_rostest(test/offboard.test)
 endif()

clover/examples/navigate_wait.py

@@ -16,11 +16,8 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
 set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
 land = rospy.ServiceProxy('land', Trigger)
 
-def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
-        frame_id='body', tolerance=0.2, auto_arm=False):
-
-    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
-        frame_id=frame_id, auto_arm=auto_arm)
+def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
+    res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
 
     if not res.success:
         return res

clover/msg/State.msg

@@ -0,0 +1,38 @@
+uint8 MODE_NONE = 0
+uint8 MODE_NAVIGATE = 1
+uint8 MODE_NAVIGATE_GLOBAL = 2
+uint8 MODE_POSITION = 3
+uint8 MODE_VELOCITY = 4
+uint8 MODE_ATTITUDE = 5
+uint8 MODE_RATES = 6
+
+uint8 YAW_MODE_YAW = 0
+uint8 YAW_MODE_YAW_RATE = 1
+uint8 YAW_MODE_YAW_TOWARDS = 2
+
+# type of offboard control
+uint8 mode
+uint8 yaw_mode
+
+# targets
+float32 x
+float32 y
+float32 z
+float32 speed
+float32 lat
+float32 lon
+float32 vx
+float32 vy
+float32 vz
+float32 roll
+float32 pitch
+float32 yaw
+float32 roll_rate
+float32 pitch_rate
+float32 yaw_rate
+float32 thrust
+
+# frames of reference
+string xy_frame_id
+string z_frame_id
+string yaw_frame_id

clover/src/autotest/autotest_aruco.py

@@ -35,11 +35,8 @@ def print_current_map_position():
     dist = rospy.wait_for_message('rangefinder/range', Range).range
     print('Map position:\tx={:.1f}\ty={:.1f}\tz={:.1f}\tyaw={:.1f}\tdist={:.2f}'.format(telem.x, telem.y, telem.z, telem.yaw, dist))
 
-def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
-        frame_id='body', tolerance=0.2, auto_arm=False):
-
-    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
-        frame_id=frame_id, auto_arm=auto_arm)
+def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
+    res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
 
     if not res.success:
         return res

clover/src/autotest/autotest_flight.py

@@ -2,7 +2,7 @@
 
 import rospy
 import math
-from math import nan
+from math import nan, inf
 import signal
 import sys
 from clover import srv
@@ -15,6 +15,8 @@ rospy.init_node('autotest_flight', disable_signals=True) # disable signals to al
 get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
 navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
 navigate_global = handle_response(rospy.ServiceProxy('navigate_global', srv.NavigateGlobal))
+set_yaw = handle_response(rospy.ServiceProxy('set_yaw', srv.SetYaw))
+set_yaw_rate = handle_response(rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate))
 set_position = handle_response(rospy.ServiceProxy('set_position', srv.SetPosition))
 set_velocity = handle_response(rospy.ServiceProxy('set_velocity', srv.SetVelocity))
 set_attitude = handle_response(rospy.ServiceProxy('set_attitude', srv.SetAttitude))
@@ -28,11 +30,8 @@ def interrupt(sig, frame):
 
 signal.signal(signal.SIGINT, interrupt)
 
-def navigate_wait(x=0, y=0, z=0, yaw=nan, yaw_rate=0, speed=0.5, \
-        frame_id='body', tolerance=0.2, auto_arm=False):
-
-    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
-        frame_id=frame_id, auto_arm=auto_arm)
+def navigate_wait(x=0, y=0, z=0, yaw=nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
+    res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
 
     if not res.success:
         return res
@@ -69,17 +68,17 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
 rospy.sleep(2)
 set_position(frame_id='body')
 
-input('Rotate right 90° [enter] ')
-navigate(yaw=-math.pi / 2, frame_id='navigate_target')
+input('Rotate right 90° using set_yaw [enter] ')
+set_yaw(yaw=-math.pi / 2, frame_id='navigate_target')
 rospy.sleep(3)
 
 input('Use set_attitude to fly backwards [enter]')
-set_attitude(pitch=-0.3, roll=0, yaw=0, thrust=0.5, frame_id='body')
+set_attitude(roll=0, pitch=-0.3, yaw=0, thrust=0.5, frame_id='body')
 rospy.sleep(0.3)
 set_position(frame_id='body')
 
 input('Use set_attitude to fly right [enter]')
-set_attitude(pitch=0, roll=0.3, yaw=0, thrust=0.5, frame_id='body')
+set_attitude(roll=0.3, pitch=0, yaw=0, thrust=0.5, frame_id='body')
 rospy.sleep(0.5)
 set_position(frame_id='body')
 
@@ -88,13 +87,13 @@ set_rates(roll_rate=1.2, thrust=0.5)
 rospy.sleep(0.4)
 set_position(frame_id='body')
 
-input('Rotate 360° to the right using yaw_rate [enter]')
-set_position(x=nan, y=nan, z=nan, frame_id='body', yaw=nan, yaw_rate=-1)
+input('Rotate 360° to the right using set_yaw_rate [enter]')
+set_yaw_rate(yaw_rate=-1)
 rospy.sleep(2 * math.pi)
 set_position(frame_id='body')
 
-input('Return to start point [enter]')
-navigate_wait(x=start.x, y=start.y, z=start.z, yaw=start.yaw, speed=1, frame_id='map')
+input('Return to start point heading forward [enter]')
+navigate_wait(x=start.x, y=start.y, z=start.z, yaw=inf, speed=1, frame_id='map')
 
 input('Land [enter]')
 land()

clover/src/simple_offboard.cpp

@@ -23,6 +23,7 @@
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <std_srvs/Trigger.h>
+#include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <geometry_msgs/Vector3Stamped.h>
@@ -37,14 +38,19 @@
 #include <mavros_msgs/State.h>
 #include <mavros_msgs/StatusText.h>
 #include <mavros_msgs/ManualControl.h>
+#include <mavros_msgs/Altitude.h>
 
 #include <clover/GetTelemetry.h>
 #include <clover/Navigate.h>
 #include <clover/NavigateGlobal.h>
+#include <clover/SetAltitude.h>
+#include <clover/SetYaw.h>
+#include <clover/SetYawRate.h>
 #include <clover/SetPosition.h>
 #include <clover/SetVelocity.h>
 #include <clover/SetAttitude.h>
 #include <clover/SetRates.h>
+#include <clover/State.h>
 
 using std::string;
 using std::isnan;
@@ -54,6 +60,7 @@ using namespace clover;
 using mavros_msgs::PositionTarget;
 using mavros_msgs::AttitudeTarget;
 using mavros_msgs::Thrust;
+using mavros_msgs::Altitude;
 
 // tf2
 tf2_ros::Buffer tf_buffer;
@@ -81,33 +88,40 @@ bool land_only_in_offboard, nav_from_sp, check_kill_switch;
 std::map<string, string> reference_frames;
 
 // Publishers
-ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub;
+ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub, state_pub;
 
 // Service clients
 ros::ServiceClient arming, set_mode;
 
 // Containers
 ros::Timer setpoint_timer;
-tf::Quaternion tq;
 PoseStamped position_msg;
 PositionTarget position_raw_msg;
-AttitudeTarget att_raw_msg;
-Thrust thrust_msg;
-TwistStamped rates_msg;
+//TwistStamped rates_msg;
 TransformStamped target, setpoint;
 geometry_msgs::TransformStamped body;
+geometry_msgs::TransformStamped terrain;
 
 // State
 PoseStamped nav_start;
-PoseStamped setpoint_position, setpoint_position_transformed;
-Vector3Stamped setpoint_velocity, setpoint_velocity_transformed;
-QuaternionStamped setpoint_attitude, setpoint_attitude_transformed;
-float setpoint_yaw_rate;
+PointStamped setpoint_position;
+PointStamped setpoint_altitude;
+Vector3Stamped setpoint_velocity;
+float setpoint_yaw, setpoint_roll, setpoint_pitch;
+Vector3 setpoint_rates;
+string yaw_frame_id;
+float setpoint_thrust;
 float nav_speed;
+float setpoint_lat = NAN, setpoint_lon = NAN;
 bool busy = false;
 bool wait_armed = false;
 bool nav_from_sp_flag = false;
 
+// Last published
+PoseStamped setpoint_pose_local;
+Vector3Stamped setpoint_velocity_local;
+float yaw_local;
+
 enum setpoint_type_t {
 	NONE,
 	NAVIGATE,
@@ -115,7 +129,10 @@ enum setpoint_type_t {
 	POSITION,
 	VELOCITY,
 	ATTITUDE,
-	RATES
+	RATES,
+	_ALTITUDE,
+	_YAW,
+	_YAW_RATE,
 };
 
 enum setpoint_type_t setpoint_type = NONE;
@@ -170,7 +187,7 @@ void handleLocalPosition(const PoseStamped& pose)
 {
 	local_position = pose;
 	publishBodyFrame();
-	// TODO: terrain?, home?
+	// TODO: home?
 }
 
 // wait for transform without interrupting publishing setpoints
@@ -188,6 +205,20 @@ inline bool waitTransform(const string& target, const string& source,
 	return false;
 }
 
+void handleAltitude(const Altitude& alt)
+{
+	// publish terrain frame
+	if (!std::isfinite(alt.bottom_clearance)) return;
+	// terrain.header.stamp = alt.header.stamp;
+
+	if (!waitTransform(local_frame, body.child_frame_id, alt.header.stamp, ros::Duration(0.1))) return;
+
+	auto t = tf_buffer.lookupTransform(local_frame, body.child_frame_id, alt.header.stamp);
+	t.child_frame_id = terrain.child_frame_id;
+	t.transform.translation.z -= alt.bottom_clearance;
+	static_transform_broadcaster->sendTransform(t);
+}
+
 #define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout))
 
 bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
@@ -207,11 +238,11 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
 	res.vx = NAN;
 	res.vy = NAN;
 	res.vz = NAN;
-	res.pitch = NAN;
 	res.roll = NAN;
+	res.pitch = NAN;
 	res.yaw = NAN;
-	res.pitch_rate = NAN;
 	res.roll_rate = NAN;
+	res.pitch_rate = NAN;
 	res.yaw_rate = NAN;
 	res.voltage = NAN;
 	res.cell_voltage = NAN;
@@ -341,20 +372,20 @@ inline float getDistance(const Point& from, const Point& to)
 	return hypot(from.x - to.x, from.y - to.y, from.z - to.z);
 }
 
-void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint)
+void getNavigateSetpoint(const ros::Time& stamp, const float speed, Point& nav_setpoint)
 {
 	if (wait_armed) {
 		// don't start navigating if we're waiting arming
 		nav_start.header.stamp = stamp;
 	}
 
-	float distance = getDistance(nav_start.pose.position, setpoint_position_transformed.pose.position);
+	float distance = getDistance(nav_start.pose.position, setpoint_pose_local.pose.position);
 	float time = distance / speed;
 	float passed = std::min((stamp - nav_start.header.stamp).toSec() / time, 1.0);
 
-	nav_setpoint.x = nav_start.pose.position.x + (setpoint_position_transformed.pose.position.x - nav_start.pose.position.x) * passed;
-	nav_setpoint.y = nav_start.pose.position.y + (setpoint_position_transformed.pose.position.y - nav_start.pose.position.y) * passed;
-	nav_setpoint.z = nav_start.pose.position.z + (setpoint_position_transformed.pose.position.z - nav_start.pose.position.z) * passed;
+	nav_setpoint.x = nav_start.pose.position.x + (setpoint_pose_local.pose.position.x - nav_start.pose.position.x) * passed;
+	nav_setpoint.y = nav_start.pose.position.y + (setpoint_pose_local.pose.position.y - nav_start.pose.position.y) * passed;
+	nav_setpoint.z = nav_start.pose.position.z + (setpoint_pose_local.pose.position.z - nav_start.pose.position.z) * passed;
 }
 
 PoseStamped globalToLocal(double lat, double lon)
@@ -385,44 +416,101 @@ PoseStamped globalToLocal(double lat, double lon)
 	return pose;
 }
 
+// publish navigate_target frame
+void publishTarget(ros::Time stamp, bool _static = false)
+{
+	bool single_frame = (setpoint_position.header.frame_id == setpoint_altitude.header.frame_id);
+
+	// handle yaw for target frame
+	if (setpoint_yaw_type == YAW || setpoint_yaw_type == YAW_RATE) { // use last set yaw for yaw_rate
+		if (setpoint_altitude.header.frame_id == yaw_frame_id) {
+			target.transform.rotation = tf::createQuaternionMsgFromYaw(setpoint_yaw);
+		} else {
+			single_frame = false;
+			target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local);
+		}
+	} else if (setpoint_yaw_type == TOWARDS) {
+		single_frame = false;
+		target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local);
+	}
+
+	if (_static && single_frame) {
+		// publish at user's command, if all frames are the same
+		target.header.frame_id = setpoint_position.header.frame_id;
+		target.header.stamp = stamp;
+		target.transform.translation.x = setpoint_position.point.x;
+		target.transform.translation.y = setpoint_position.point.y;
+		target.transform.translation.z = setpoint_position.point.z;
+
+	} else if (!_static) {
+		// publish at each iteration, if frames are different
+		target.header = setpoint_pose_local.header;
+		target.transform.translation.x = setpoint_pose_local.pose.position.x;
+		target.transform.translation.y = setpoint_pose_local.pose.position.y;
+		target.transform.translation.z = setpoint_pose_local.pose.position.z;
+	}
+
+	static_transform_broadcaster->sendTransform(target);
+}
+
 void publish(const ros::Time stamp)
 {
 	if (setpoint_type == NONE) return;
 
 	position_raw_msg.header.stamp = stamp;
-	thrust_msg.header.stamp = stamp;
-	rates_msg.header.stamp = stamp;
 
-	try {
-		// transform position and/or yaw
-		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION || setpoint_type == VELOCITY || setpoint_type == ATTITUDE) {
-			setpoint_position.header.stamp = stamp;
-			tf_buffer.transform(setpoint_position, setpoint_position_transformed, local_frame, ros::Duration(0.05));
+	// transform position
+	if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
+		setpoint_position.header.stamp = stamp;
+		setpoint_altitude.header.stamp = stamp;
+		// transform xy
+		try {
+			auto xy = tf_buffer.transform(setpoint_position, local_frame, ros::Duration(0.05));
+			setpoint_pose_local.header = xy.header;
+			setpoint_pose_local.pose.position.x = xy.point.x;
+			setpoint_pose_local.pose.position.y = xy.point.y;
+		} catch (tf2::TransformException& ex) {
+			// can't transform xy, use last known
+			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
 		}
-
-		// transform velocity
-		if (setpoint_type == VELOCITY) {
-			setpoint_velocity.header.stamp = stamp;
-			tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05));
+		// transform altitude
+		try {
+			setpoint_pose_local.pose.position.z = tf_buffer.transform(setpoint_altitude, local_frame, ros::Duration(0.05)).point.z;
+		} catch (tf2::TransformException& ex) {
+			// can't transform altitude, use last known
+			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
 		}
-
-	} catch (const tf2::TransformException& e) {
-		ROS_WARN_THROTTLE(10, "can't transform");
 	}
 
+	// transform yaw
+	if (setpoint_yaw_type == YAW) {
+		try {
+			QuaternionStamped q;
+			q.header.stamp = stamp;
+			q.header.frame_id = yaw_frame_id;
+			q.quaternion = tf::createQuaternionMsgFromYaw(setpoint_yaw);
+			yaw_local = tf2::getYaw(tf_buffer.transform(q, local_frame, ros::Duration(0.05)).quaternion);
+		} catch (tf2::TransformException& ex) {
+			// can't transform yaw, use last known
+			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
+		}
+	}
+
+	// compute navigate setpoint
 	if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
-		position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw
 		getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position);
 
 		if (setpoint_yaw_type == TOWARDS) {
-			double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
-			                           position_msg.pose.position.x - nav_start.pose.position.x);
-			position_msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_towards);
+			yaw_local = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
+			                  position_msg.pose.position.x - nav_start.pose.position.x);
 		}
+
+		position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local);
 	}
 
 	if (setpoint_type == POSITION) {
-		position_msg = setpoint_position_transformed;
+		position_msg = setpoint_pose_local;
+		position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local);
 	}
 
 	if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
@@ -439,14 +527,14 @@ void publish(const ros::Time stamp)
 			                             PositionTarget::IGNORE_AFY +
 			                             PositionTarget::IGNORE_AFZ +
 			                             PositionTarget::IGNORE_YAW;
-			position_raw_msg.yaw_rate = setpoint_yaw_rate;
+			position_raw_msg.yaw_rate = setpoint_rates.z;
 			position_raw_msg.position = position_msg.pose.position;
 			position_raw_pub.publish(position_raw_msg);
 		}
 
 		// publish setpoint frame
 		if (!setpoint.child_frame_id.empty()) {
-			if (setpoint.header.stamp == position_msg.header.stamp) {
+			if (setpoint.header.stamp >= position_msg.header.stamp) {
 				return; // avoid TF_REPEATED_DATA warnings
 			}
 
@@ -458,9 +546,22 @@ void publish(const ros::Time stamp)
 			setpoint.header.stamp = position_msg.header.stamp;
 			transform_broadcaster->sendTransform(setpoint);
 		}
+
+		// publish dynamic target frame
+		publishTarget(stamp);
 	}
 
 	if (setpoint_type == VELOCITY) {
+		// transform velocity to local frame
+		setpoint_velocity.header.stamp = stamp;
+		try {
+			setpoint_velocity_local = tf_buffer.transform(setpoint_velocity, local_frame, ros::Duration(0.05));
+		} catch (tf2::TransformException& ex) {
+			// can't transform velocity, use last known
+			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
+		}
+
+		// publish velocity
 		position_raw_msg.type_mask = PositionTarget::IGNORE_PX +
 		                             PositionTarget::IGNORE_PY +
 		                             PositionTarget::IGNORE_PZ +
@@ -468,14 +569,22 @@ void publish(const ros::Time stamp)
 		                             PositionTarget::IGNORE_AFY +
 		                             PositionTarget::IGNORE_AFZ;
 		position_raw_msg.type_mask += setpoint_yaw_type == YAW ? PositionTarget::IGNORE_YAW_RATE : PositionTarget::IGNORE_YAW;
-		position_raw_msg.velocity = setpoint_velocity_transformed.vector;
-		position_raw_msg.yaw = tf2::getYaw(setpoint_position_transformed.pose.orientation);
-		position_raw_msg.yaw_rate = setpoint_yaw_rate;
+		position_raw_msg.velocity = setpoint_velocity_local.vector;
+		position_raw_msg.yaw = yaw_local;
+		position_raw_msg.yaw_rate = setpoint_rates.z;
 		position_raw_pub.publish(position_raw_msg);
 	}
 
 	if (setpoint_type == ATTITUDE) {
-		attitude_pub.publish(setpoint_position_transformed);
+		PoseStamped msg;
+		msg.header.stamp = stamp;
+		msg.header.frame_id = local_frame;
+		msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(setpoint_roll, setpoint_pitch, yaw_local);
+		attitude_pub.publish(msg);
+
+		Thrust thrust_msg;
+		thrust_msg.header.stamp = stamp;
+		thrust_msg.thrust = setpoint_thrust;
 		thrust_pub.publish(thrust_msg);
 	}
 
@@ -484,11 +593,12 @@ void publish(const ros::Time stamp)
 		// thrust_pub.publish(thrust_msg);
 		// mavros rates topics waits for rates in local frame
 		// use rates in body frame for simplicity
+		AttitudeTarget att_raw_msg;
 		att_raw_msg.header.stamp = stamp;
 		att_raw_msg.header.frame_id = fcu_frame;
 		att_raw_msg.type_mask = AttitudeTarget::IGNORE_ATTITUDE;
-		att_raw_msg.body_rate = rates_msg.twist.angular;
-		att_raw_msg.thrust = thrust_msg.thrust;
+		att_raw_msg.body_rate = setpoint_rates;
+		att_raw_msg.thrust = setpoint_thrust;
 		attitude_raw_pub.publish(att_raw_msg);
 	}
 }
@@ -528,10 +638,59 @@ inline void checkState()
 		throw std::runtime_error("No connection to FCU, https://clover.coex.tech/connection");
 }
 
+void publishState()
+{
+	clover::State msg;
+	msg.mode = setpoint_type;
+	msg.yaw_mode = setpoint_yaw_type;
+
+	if (setpoint_position.header.frame_id.empty()) {
+		msg.x = NAN;
+		msg.y = NAN;
+		msg.z = NAN;
+	} else {
+		msg.x = setpoint_position.point.x;
+		msg.y = setpoint_position.point.y;
+		msg.z = setpoint_altitude.point.z;
+	}
+
+	msg.speed = nav_speed;
+	msg.lat = setpoint_lat;
+	msg.lon = setpoint_lon;
+	msg.vx = setpoint_velocity.vector.x;
+	msg.vy = setpoint_velocity.vector.y;
+	msg.vz = setpoint_velocity.vector.z;
+	msg.roll = setpoint_roll;
+	msg.pitch = setpoint_pitch;
+	msg.yaw = !yaw_frame_id.empty() ? setpoint_yaw : NAN;
+
+	msg.roll_rate = setpoint_rates.x;
+	msg.pitch_rate = setpoint_rates.y;
+	msg.yaw_rate = setpoint_rates.z;
+	msg.thrust = setpoint_thrust;
+
+	if (setpoint_type == VELOCITY) {
+		msg.xy_frame_id = setpoint_velocity.header.frame_id;
+		msg.z_frame_id = setpoint_velocity.header.frame_id;
+	} else {
+		msg.xy_frame_id = setpoint_position.header.frame_id;
+		msg.z_frame_id = setpoint_altitude.header.frame_id;
+	}
+	msg.yaw_frame_id = yaw_frame_id;
+
+	state_pub.publish(msg);
+}
+
+inline float safe(float value) {
+	return std::isfinite(value) ? value : 0;
+}
+
 #define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }
 
+#define ENSURE_NON_INF(var) { if (std::isinf(var)) throw std::runtime_error(#var " argument cannot be Inf"); }
+
 bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
-           float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line
+           float roll, float pitch, float yaw, float roll_rate, float pitch_rate, float yaw_rate, // editorconfig-checker-disable-line
            float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line
            uint8_t& success, string& message) // editorconfig-checker-disable-line
 {
@@ -558,69 +717,40 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		auto search = reference_frames.find(frame_id);
 		const string& reference_frame = search == reference_frames.end() ? frame_id : search->second;
 
-		// Serve "partial" commands
+		ENSURE_NON_INF(x);
+		ENSURE_NON_INF(y);
+		ENSURE_NON_INF(z);
+		ENSURE_NON_INF(speed); // TODO: allow inf
+		ENSURE_NON_INF(vx);
+		ENSURE_NON_INF(vy);
+		ENSURE_NON_INF(vz);
+		ENSURE_NON_INF(roll);
+		ENSURE_NON_INF(pitch);
+		ENSURE_NON_INF(roll_rate);
+		ENSURE_NON_INF(pitch_rate);
+		ENSURE_NON_INF(yaw_rate);
+		ENSURE_NON_INF(thrust);
 
-		if (!auto_arm && std::isfinite(yaw) &&
-		    isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
-		    isnan(pitch) && isnan(roll) && isnan(thrust) &&
-		    isnan(lat) && isnan(lon)) {
-			// change only the yaw
-			if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
-				if (!waitTransform(setpoint_position.header.frame_id, frame_id, stamp, transform_timeout))
-					throw std::runtime_error("Can't transform from " + frame_id + " to " + setpoint_position.header.frame_id);
-
-				message = "Changing yaw only";
-
-				QuaternionStamped q;
-				q.header.frame_id = frame_id;
-				q.header.stamp = stamp;
-				q.quaternion = tf::createQuaternionMsgFromYaw(yaw); // TODO: pitch=0, roll=0 is not totally correct
-				setpoint_position.pose.orientation = tf_buffer.transform(q, setpoint_position.header.frame_id).quaternion;
-				setpoint_yaw_type = YAW;
-				goto publish_setpoint;
-			} else {
-				throw std::runtime_error("Setting yaw is possible only when position or velocity setpoints active");
-			}
-		}
-
-		if (!auto_arm && std::isfinite(yaw_rate) &&
-		    isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
-		    isnan(pitch) && isnan(roll) && isnan(yaw) && isnan(thrust) &&
-		    isnan(lat) && isnan(lon)) {
-			// change only the yaw rate
-			if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
-				message = "Changing yaw rate only";
-
-				setpoint_yaw_type = YAW_RATE;
-				setpoint_yaw_rate = yaw_rate;
-				goto publish_setpoint;
-			} else {
-				throw std::runtime_error("Setting yaw rate is possible only when position or velocity setpoints active");
-			}
-		}
-
-		// Serve normal commands
-
-		if (sp_type == NAVIGATE || sp_type == POSITION) {
-			ENSURE_FINITE(x);
-			ENSURE_FINITE(y);
-			ENSURE_FINITE(z);
-		} else if (sp_type == NAVIGATE_GLOBAL) {
+		if (sp_type == NAVIGATE_GLOBAL) {
 			ENSURE_FINITE(lat);
 			ENSURE_FINITE(lon);
-			ENSURE_FINITE(z);
-		} else if (sp_type == VELOCITY) {
-			ENSURE_FINITE(vx);
-			ENSURE_FINITE(vy);
-			ENSURE_FINITE(vz);
-		} else if (sp_type == ATTITUDE) {
-			ENSURE_FINITE(pitch);
-			ENSURE_FINITE(roll);
-			ENSURE_FINITE(thrust);
-		} else if (sp_type == RATES) {
-			ENSURE_FINITE(pitch_rate);
-			ENSURE_FINITE(roll_rate);
-			ENSURE_FINITE(thrust);
+		}
+
+		if (isfinite(x) != isfinite(y)) {
+			throw std::runtime_error("x and y can be set only together");
+		}
+
+		if (isfinite(yaw_rate)) {
+			if (sp_type > RATES && setpoint_type == ATTITUDE) {
+				throw std::runtime_error("Yaw rate cannot be set in attitude mode.");
+			}
+		}
+
+		// set_altitude
+		if (sp_type == _ALTITUDE) {
+			if (setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) {
+				throw std::runtime_error("Altitude cannot be set in velocity, attitude or rates mode.");
+			}
 		}
 
 		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
@@ -634,20 +764,13 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 				speed = default_speed;
 		}
 
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
-			if (yaw_rate != 0 && !std::isnan(yaw))
-				throw std::runtime_error("Yaw value should be NaN for setting yaw rate");
-
-			if (std::isnan(yaw_rate) && std::isnan(yaw))
-				throw std::runtime_error("Both yaw and yaw_rate cannot be NaN");
-		}
-
 		if (sp_type == NAVIGATE_GLOBAL) {
 			if (TIMEOUT(global_position, global_position_timeout))
 				throw std::runtime_error("No global position");
 		}
 
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) {
+		// if any value need to be transformed to reference frame
+		if (isfinite(x) || isfinite(y) || isfinite(z) || isfinite(vx) || isfinite(vy) || isfinite(vz) || isfinite(yaw)) {
 			// make sure transform from frame_id to reference frame available
 			if (!waitTransform(reference_frame, frame_id, stamp, transform_timeout))
 				throw std::runtime_error("Can't transform from " + frame_id + " to " + reference_frame);
@@ -664,15 +787,26 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			auto xy_in_req_frame = tf_buffer.transform(pose_local, frame_id);
 			x = xy_in_req_frame.pose.position.x;
 			y = xy_in_req_frame.pose.position.y;
+			setpoint_lat = lat;
+			setpoint_lon = lon;
 		}
 
 		// Everything fine - switch setpoint type
-		setpoint_type = sp_type;
+		if (sp_type <= RATES) {
+			setpoint_type = sp_type;
+		}
 
-		if (sp_type != NAVIGATE && sp_type != NAVIGATE_GLOBAL) {
+		if (setpoint_type != NAVIGATE && setpoint_type != NAVIGATE_GLOBAL) {
 			nav_from_sp_flag = false;
 		}
 
+		if (auto_arm || setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) {
+			// invalidate position setpoint
+			setpoint_position.header.frame_id = "";
+			setpoint_altitude.header.frame_id = "";
+			yaw_frame_id = "";
+		}
+
 		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
 			// starting point
 			if (nav_from_sp && nav_from_sp_flag) {
@@ -681,89 +815,139 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			} else {
 				nav_start = local_position;
 			}
-			nav_speed = speed;
+
+			if (!isnan(speed)) {
+				nav_speed = speed;
+			}
+
 			nav_from_sp_flag = true;
 		}
 
-		// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
-		// 	if (std::isnan(yaw) && yaw_rate == 0) {
-		// 		// keep yaw unchanged
-		//		// TODO: this is incorrect, because we need yaw in desired frame
-		// 		yaw = tf2::getYaw(local_position.pose.orientation);
-		// 	}
-		// }
+		// handle position
+		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
 
-		if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
-			// destination point and/or attitude
-			PoseStamped ps;
-			ps.header.frame_id = frame_id;
-			ps.header.stamp = stamp;
-			ps.pose.position.x = x;
-			ps.pose.position.y = y;
-			ps.pose.position.z = z;
-			ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid
+			PointStamped desired;
+			desired.header.frame_id = frame_id;
+			desired.header.stamp = stamp;
+			desired.point.x = safe(x);
+			desired.point.y = safe(y);
+			desired.point.z = safe(z);
 
-			if (sp_type == ATTITUDE) {
-				ps.pose.position.x = 0;
-				ps.pose.position.y = 0;
-				ps.pose.position.z = 0;
-				ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
-			} else if (std::isnan(yaw)) {
-				setpoint_yaw_type = YAW_RATE;
-				setpoint_yaw_rate = yaw_rate;
-			} else if (std::isinf(yaw) && yaw > 0) {
-				// yaw towards
-				setpoint_yaw_type = TOWARDS;
-				yaw = 0;
-				setpoint_yaw_rate = 0;
-			} else {
-				setpoint_yaw_type = YAW;
-				setpoint_yaw_rate = 0;
-				ps.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
+			// transform to reference frame
+			desired = tf_buffer.transform(desired, reference_frame);
+
+			// set horizontal position
+			if (isfinite(x) && isfinite(y)) {
+				setpoint_position = desired;
+			} else if (setpoint_position.header.frame_id.empty()) {
+				 // TODO: use transform for current stamp
+				setpoint_position.header = local_position.header;
+				setpoint_position.point = local_position.pose.position;
 			}
 
-			tf_buffer.transform(ps, setpoint_position, reference_frame);
+			// set altitude
+			if (isfinite(z)) {
+				setpoint_altitude = desired;
+			} else if (setpoint_altitude.header.frame_id.empty()) {
+				setpoint_altitude.header = local_position.header;
+				setpoint_altitude.point = local_position.pose.position;
+			}
 		}
 
+		// handle velocity
 		if (sp_type == VELOCITY) {
-			Vector3Stamped vel;
-			vel.header.frame_id = frame_id;
-			vel.header.stamp = stamp;
-			vel.vector.x = vx;
-			vel.vector.y = vy;
-			vel.vector.z = vz;
-			tf_buffer.transform(vel, setpoint_velocity, reference_frame);
+			// TODO: allow setting different modes by altitude and xy
+			Vector3Stamped desired;
+			desired.header.frame_id = frame_id;
+			desired.header.stamp = stamp;
+			desired.vector.x = safe(vx);
+			desired.vector.y = safe(vy);
+			desired.vector.z = safe(vz);
+
+			// transform to reference frame
+			desired = tf_buffer.transform(desired, reference_frame);
+			setpoint_velocity.header = desired.header;
+
+			// set horizontal velocity
+			if (isfinite(vx) && isfinite(vy)) {
+				setpoint_velocity.vector.x = desired.vector.x;
+				setpoint_velocity.vector.y = desired.vector.y;
+			}
+
+			// set vertical velocity
+			if (isfinite(vz)) {
+				setpoint_velocity.vector.z = desired.vector.z;
+			}
 		}
 
-		if (sp_type == ATTITUDE || sp_type == RATES) {
-			thrust_msg.thrust = thrust;
+		// handle yaw
+		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE || sp_type == _YAW) {
+			if (isfinite(yaw)) {
+				setpoint_yaw_type = YAW;
+				QuaternionStamped desired;
+				desired.header.frame_id = frame_id;
+				desired.header.stamp = stamp;
+				desired.quaternion = tf::createQuaternionMsgFromYaw(yaw);
+
+				// transform to reference frame
+				desired = tf_buffer.transform(desired, reference_frame);
+				setpoint_yaw = tf2::getYaw(desired.quaternion);
+				yaw_frame_id = reference_frame;
+
+			} else if (isinf(yaw) && yaw > 0) {
+				// yaw towards
+				setpoint_yaw_type = TOWARDS;
+
+			} else if (yaw_frame_id.empty() || sp_type == _YAW) {
+				// yaw is nan and not set previously OR set_yaw(yaw=nan) was called
+				setpoint_yaw_type = YAW;
+				setpoint_yaw = tf2::getYaw(local_position.pose.orientation); // set yaw to current yaw
+				yaw_frame_id = local_position.header.frame_id;
+			}
 		}
 
-		if (sp_type == RATES) {
-			rates_msg.twist.angular.x = roll_rate;
-			rates_msg.twist.angular.y = pitch_rate;
-			rates_msg.twist.angular.z = yaw_rate;
+		// handle roll
+		if (isfinite(roll)) {
+			setpoint_roll = roll;
+		}
+
+		// handle pitch
+		if (isfinite(pitch)) {
+			setpoint_pitch = pitch;
+		}
+
+		// handle yaw rate
+		if (isfinite(yaw_rate)) {
+			setpoint_yaw_type = YAW_RATE;
+			setpoint_rates.z = yaw_rate;
+		}
+
+		// handle pitch rate
+		if (isfinite(roll_rate)) {
+			setpoint_rates.x = roll_rate;
+		}
+
+		// handle roll rate
+		if (isfinite(pitch_rate)) {
+			setpoint_rates.y = pitch_rate;
+		}
+
+		// handle thrust
+		if (isfinite(thrust)) {
+			setpoint_thrust = thrust;
 		}
 
 		wait_armed = auto_arm;
 
-publish_setpoint:
 		publish(stamp); // calculate initial transformed messages first
 		setpoint_timer.start();
 
-		// publish target frame
-		if (!target.child_frame_id.empty()) {
-			if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-				target.header.frame_id = setpoint_position.header.frame_id;
-				target.header.stamp = stamp;
-				target.transform.translation.x = setpoint_position.pose.position.x;
-				target.transform.translation.y = setpoint_position.pose.position.y;
-				target.transform.translation.z = setpoint_position.pose.position.z;
-				target.transform.rotation = setpoint_position.pose.orientation;
-				static_transform_broadcaster->sendTransform(target);
-			}
+		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
+			publishTarget(stamp, true);
 		}
 
+		publishState();
+
 		if (auto_arm) {
 			offboardAndArm();
 			wait_armed = false;
@@ -788,27 +972,39 @@ publish_setpoint:
 }
 
 bool navigate(Navigate::Request& req, Navigate::Response& res) {
-	return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) {
-	return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
+}
+
+bool setAltitude(SetAltitude::Request& req, SetAltitude::Response& res) {
+	return serve(_ALTITUDE, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message);
+}
+
+bool setYaw(SetYaw::Request& req, SetYaw::Response& res) {
+	return serve(_YAW, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message);
+}
+
+bool setYawRate(SetYawRate::Request& req, SetYawRate::Response& res) {
+	return serve(_YAW_RATE, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, "", false, res.success, res.message);
 }
 
 bool setPosition(SetPosition::Request& req, SetPosition::Response& res) {
-	return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) {
-	return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) {
-	return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch, req.roll, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.roll, req.pitch, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setRates(SetRates::Request& req, SetRates::Response& res) {
-	return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch_rate, req.roll_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
+	return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.roll_rate, req.pitch_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
 }
 
 bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
@@ -840,9 +1036,7 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 		auto start = ros::Time::now();
 		while (ros::ok()) {
 			if (state.mode == "AUTO.LAND") {
-				res.success = true;
-				busy = false;
-				return true;
+				break;
 			}
 			if (ros::Time::now() - start > land_timeout)
 				throw std::runtime_error("Land request timed out");
@@ -851,6 +1045,18 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 			r.sleep();
 		}
 
+		// stop setpoints and invalidate position setpoint
+		setpoint_timer.stop();
+		setpoint_type = NONE;
+		setpoint_position.header.frame_id = "";
+		setpoint_altitude.header.frame_id = "";
+		yaw_frame_id = "";
+		publishState();
+
+		res.success = true;
+		busy = false;
+		return true;
+
 	} catch (const std::exception& e) {
 		res.message = e.what();
 		ROS_INFO("%s", e.what());
@@ -863,6 +1069,11 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 bool release(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 {
 	setpoint_timer.stop();
+	setpoint_type = NONE;
+	setpoint_position.header.frame_id = "";
+	setpoint_altitude.header.frame_id = "";
+	yaw_frame_id = "";
+	publishState();
 	res.success = true;
 	return true;
 }
@@ -888,6 +1099,7 @@ int main(int argc, char **argv)
 	nh_priv.param("check_kill_switch", check_kill_switch, true);
 	nh_priv.param("default_speed", default_speed, 0.5f);
 	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
+	nh_priv.param<string>("terrain_frame", terrain.child_frame_id, "terrain");
 	nh_priv.getParam("reference_frames", reference_frames);
 
 	// Default reference frames
@@ -923,6 +1135,12 @@ int main(int argc, char **argv)
 	auto manual_control_sub = nh.subscribe(mavros + "/manual_control/control", 1, &handleMessage<mavros_msgs::ManualControl, manual_control>);
 	auto local_position_sub = nh.subscribe(mavros + "/local_position/pose", 1, &handleLocalPosition);
 
+	ros::Subscriber altitude_sub;
+	if (!body.child_frame_id.empty() && !terrain.child_frame_id.empty()) {
+		terrain.header.frame_id = local_frame;
+		altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude);
+	}
+
 	// Setpoint publishers
 	position_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_position/local", 1);
 	position_raw_pub = nh.advertise<PositionTarget>(mavros + "/setpoint_raw/local", 1);
@@ -931,10 +1149,16 @@ int main(int argc, char **argv)
 	rates_pub = nh.advertise<TwistStamped>(mavros + "/setpoint_attitude/cmd_vel", 1);
 	thrust_pub = nh.advertise<Thrust>(mavros + "/setpoint_attitude/thrust", 1);
 
+	// State publisher
+	state_pub = nh_priv.advertise<clover::State>("state", 1, true);
+
 	 // Service servers
 	auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);
 	auto na_serv = nh.advertiseService("navigate", &navigate);
 	auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal);
+	auto sl_serv = nh.advertiseService("set_altitude", &setAltitude);
+	auto ya_serv = nh.advertiseService("set_yaw", &setYaw);
+	auto yr_serv = nh.advertiseService("set_yaw_rate", &setYawRate);
 	auto sp_serv = nh.advertiseService("set_position", &setPosition);
 	auto sv_serv = nh.advertiseService("set_velocity", &setVelocity);
 	auto sa_serv = nh.advertiseService("set_attitude", &setAttitude);
@@ -948,7 +1172,7 @@ int main(int argc, char **argv)
 	position_msg.header.frame_id = local_frame;
 	position_raw_msg.header.frame_id = local_frame;
 	position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED;
-	rates_msg.header.frame_id = fcu_frame;
+	//rates_msg.header.frame_id = fcu_frame;
 
 	ROS_INFO("ready");
 	ros::spin();

clover/srv/GetTelemetry.srv

@@ -13,11 +13,11 @@ float32 alt
 float32 vx
 float32 vy
 float32 vz
-float32 pitch
 float32 roll
+float32 pitch
 float32 yaw
-float32 pitch_rate
 float32 roll_rate
+float32 pitch_rate
 float32 yaw_rate
 float32 voltage
 float32 cell_voltage

clover/srv/Navigate.srv

@@ -2,7 +2,6 @@ float32 x
 float32 y
 float32 z
 float32 yaw
-float32 yaw_rate
 float32 speed
 string frame_id
 bool auto_arm

clover/srv/NavigateGlobal.srv

@@ -2,7 +2,6 @@ float64 lat
 float64 lon
 float32 z
 float32 yaw
-float32 yaw_rate
 float32 speed
 string frame_id
 bool auto_arm

clover/srv/SetAltitude.srv

@@ -0,0 +1,5 @@
+float32 z
+string frame_id
+---
+bool success
+string message

clover/srv/SetAttitude.srv

@@ -1,5 +1,5 @@
-float32 pitch
 float32 roll
+float32 pitch
 float32 yaw
 float32 thrust
 string frame_id

clover/srv/SetPosition.srv

@@ -2,7 +2,6 @@ float32 x
 float32 y
 float32 z
 float32 yaw
-float32 yaw_rate
 string frame_id
 bool auto_arm
 ---

clover/srv/SetRates.srv

@@ -1,5 +1,5 @@
-float32 pitch_rate
 float32 roll_rate
+float32 pitch_rate
 float32 yaw_rate
 float32 thrust
 bool auto_arm

clover/srv/SetVelocity.srv

@@ -2,7 +2,6 @@ float32 vx
 float32 vy
 float32 vz
 float32 yaw
-float32 yaw_rate
 string frame_id
 bool auto_arm
 ---

clover/srv/SetYaw.srv

@@ -0,0 +1,5 @@
+float32 yaw
+string frame_id
+---
+bool success
+string message

clover/srv/SetYawRate.srv

@@ -0,0 +1,4 @@
+float32 yaw_rate
+---
+bool success
+string message

clover/test/offboard.py

@@ -0,0 +1,402 @@
+import rospy
+import pytest
+from pytest import approx
+import threading
+import mavros_msgs.msg
+from geometry_msgs.msg import PoseStamped
+from clover import srv
+from clover.msg import State
+from math import nan, inf
+import tf2_ros
+import tf2_geometry_msgs
+
+@pytest.fixture()
+def node():
+    return rospy.init_node('offboard_test', anonymous=True)
+
+@pytest.fixture
+def tf_buffer():
+    buf = tf2_ros.Buffer()
+    tf2_ros.TransformListener(buf)
+    return buf
+
+def get_state():
+    return rospy.wait_for_message('/simple_offboard/state', State, timeout=1)
+
+def get_navigate_target(tf_buffer):
+    target = tf_buffer.lookup_transform('map', 'navigate_target', rospy.get_rostime(), rospy.Duration(1))
+    assert target.child_frame_id == 'navigate_target'
+    return target
+
+def test_offboard(node, tf_buffer):
+    navigate = rospy.ServiceProxy('navigate', srv.Navigate)
+    set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
+    set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude)
+    set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw)
+    set_yaw_rate = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate)
+    set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
+    set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
+    set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
+    get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+    res = navigate()
+    assert res.success == False
+    assert res.message.startswith('State timeout')
+
+    telem = get_telemetry()
+    assert telem.connected == False
+
+    state_pub = rospy.Publisher('/mavros/state', mavros_msgs.msg.State, latch=True, queue_size=1)
+    state_msg = mavros_msgs.msg.State(mode='OFFBOARD', armed=True)
+
+    def publish_state():
+        r = rospy.Rate(2)
+        while not rospy.is_shutdown():
+            state_msg.header.stamp = rospy.Time.now()
+            state_pub.publish(state_msg)
+            r.sleep()
+
+    # start publishing state
+    threading.Thread(target=publish_state, daemon=True).start()
+    rospy.sleep(0.5)
+
+    telem = get_telemetry()
+    assert telem.connected == False
+
+    res = navigate()
+    assert res.success == False
+    assert res.message.startswith('No connection to FCU')
+
+    state_msg.connected = True
+    rospy.sleep(1)
+
+    telem = get_telemetry()
+    assert telem.connected == True
+
+    res = navigate()
+    assert res.success == False
+    assert res.message.startswith('No local position')
+
+    local_position_pub = rospy.Publisher('/mavros/local_position/pose', PoseStamped, latch=True, queue_size=1)
+    local_position_msg = PoseStamped()
+    local_position_msg.header.frame_id = 'map'
+    local_position_msg.pose.position.x = 1
+    local_position_msg.pose.position.y = 2
+    local_position_msg.pose.position.z = 3
+    local_position_msg.pose.orientation.w = 1
+
+    def publish_local_position():
+        r = rospy.Rate(30)
+        while not rospy.is_shutdown():
+            local_position_msg.header.stamp = rospy.Time.now()
+            local_position_pub.publish(local_position_msg)
+            r.sleep()
+
+    # start publishing local position
+    threading.Thread(target=publish_local_position, daemon=True).start()
+    rospy.sleep(0.5)
+
+    # check body frame
+    body = tf_buffer.lookup_transform('map', 'body', rospy.get_rostime(), rospy.Duration(1))
+    assert body.child_frame_id == 'body'
+    assert body.transform.translation.x == approx(1)
+    assert body.transform.translation.y == approx(2)
+    assert body.transform.translation.z == approx(3)
+
+    res = navigate(x=3, y=2, z=1, frame_id='map')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 3
+    assert state.y == 2
+    assert state.z == 1
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'
+    target = get_navigate_target(tf_buffer)
+    assert target.header.frame_id == 'map'
+    assert target.transform.translation.x == approx(3)
+    assert target.transform.translation.y == approx(2)
+    assert target.transform.translation.z == approx(1)
+    assert target.transform.rotation.x == 0
+    assert target.transform.rotation.y == 0
+    assert target.transform.rotation.z == 0
+    assert target.transform.rotation.w == 1
+
+    # try to set only the y
+    res = navigate(x=nan, y=1, z=nan)
+    assert res.success == False
+    assert res.message.startswith('x and y can be set only together')
+
+    # set z in body frame
+    res = navigate(x=nan, y=nan, z=1, frame_id='body')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 3
+    assert state.y == 2
+    assert state.z == 4
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'
+
+    # set xy in test frame
+    res = navigate(x=1, y=2, z=nan, frame_id='test')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 1
+    assert state.y == 2
+    assert state.z == 4
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'test'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'test'
+
+    # auto_arm should invalidate the setpoint
+    res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 1
+    assert state.y == 2
+    assert state.z == 1
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'
+
+    # set_attitude should invalidate the setpoint
+    res = set_attitude()
+    assert res.success == True
+
+    res = navigate(x=5, y=6, z=nan, yaw=nan, frame_id='map')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 3
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'
+
+    # test set_altitude
+    res = set_altitude(z=7, frame_id='test')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 7
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+    assert state.yaw_frame_id == 'map'
+
+    # test set_yaw
+    res = set_yaw(yaw=0.5, frame_id='test2')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 7
+    assert state.yaw == 0.5
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+    assert state.yaw_frame_id == 'test2'
+
+    # test set_yaw_rate
+    res = set_yaw_rate(yaw_rate=2)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 7
+    assert state.yaw_rate == 2
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+
+    # navigate(yaw=nan) should keep yaw rate mode
+    res = navigate(x=nan, y=nan, z=nan, yaw=nan)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 7
+    assert state.yaw_rate == 2
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+
+    # set_yaw(nan) should change back to yaw mode
+    res = set_yaw(yaw=nan)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.yaw == 0
+    assert state.yaw_frame_id == 'map'
+
+    # test set_position
+    res = set_position(x=nan, y=nan, z=13, yaw=nan, frame_id='test2')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_POSITION
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 13
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test2'
+    assert state.yaw_frame_id == 'map'
+
+    # set_altitude should not change the mode
+    res = set_altitude(z=3, frame_id='test')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_POSITION
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 3
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+    assert state.yaw_frame_id == 'map'
+
+    # set_yaw should not change the main mode
+    res = set_yaw(yaw=1, frame_id='test2')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_POSITION
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 3
+    assert state.yaw == 1
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+    assert state.yaw_frame_id == 'test2'
+
+    # test set_velocity
+    res = set_velocity(vx=1, frame_id='body')
+    state = get_state()
+    assert state.mode == State.MODE_VELOCITY
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.vx == 1
+    assert state.vy == 0
+    assert state.vz == 0
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'
+
+    # set_altitude should not work in velocity mode
+    res = set_altitude(z=3, frame_id='test')
+    assert res.success == False
+    assert res.message.startswith('Altitude cannot be set in')
+
+    # test set_attitude
+    res = set_attitude(roll=0.1, pitch=0.2, yaw=0.3, thrust=0.5)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_ATTITUDE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.roll == approx(0.1)
+    assert state.pitch == approx(0.2)
+    assert state.yaw == approx(0.3)
+    assert state.thrust == approx(0.5)
+    assert state.yaw_frame_id == 'map'
+    msg = rospy.wait_for_message('/mavros/setpoint_attitude/attitude', PoseStamped, timeout=3)
+    # Tait-Bryan ZYX angle (rzyx) converted to quaternion
+    assert msg.pose.orientation.x == approx(0.0342708)
+    assert msg.pose.orientation.y == approx(0.10602051)
+    assert msg.pose.orientation.z == approx(0.14357218)
+    assert msg.pose.orientation.w == approx(0.98334744)
+    msg = rospy.wait_for_message('/mavros/setpoint_attitude/thrust', mavros_msgs.msg.Thrust, timeout=3)
+    assert msg.thrust == approx(0.5)
+
+    # set_yaw should work in attitude mode
+    res = set_yaw(yaw=0.7, frame_id='test2')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_ATTITUDE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.roll == approx(0.1)
+    assert state.pitch == approx(0.2)
+    assert state.yaw == approx(0.7)
+    assert state.thrust == approx(0.5)
+    assert state.yaw_frame_id == 'test2'
+
+    # set_yaw_rate should not work in attitude mode
+    res = set_yaw_rate(yaw_rate=0.3)
+    assert res.success == False
+    assert res.message.startswith('Yaw rate cannot be set in')
+
+    # test set_rates
+    res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=0.3, thrust=0.6)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_RATES
+    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
+    assert state.roll_rate == approx(0)
+    assert state.pitch_rate == approx(0)
+    assert state.yaw_rate == approx(0.3)
+    assert state.thrust == approx(0.6)
+    msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3)
+    assert msg.thrust == approx(0.6)
+
+    res = set_rates(roll_rate=0.3, pitch_rate=0.2, yaw_rate=0.1, thrust=0.4)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_RATES
+    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
+    assert state.roll_rate == approx(0.3)
+    assert state.pitch_rate == approx(0.2)
+    assert state.yaw_rate == approx(0.1)
+    assert state.thrust == approx(0.4)
+
+    res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=nan, thrust=0.3)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_RATES
+    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
+    assert state.roll_rate == approx(0.3)
+    assert state.pitch_rate == approx(0.2)
+    assert state.yaw_rate == approx(0.1)
+    assert state.thrust == approx(0.3)
+    msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3)
+    assert msg.type_mask == mavros_msgs.msg.AttitudeTarget.IGNORE_ATTITUDE
+    assert msg.body_rate.x == approx(0.3)
+    assert msg.body_rate.y == approx(0.2)
+    assert msg.body_rate.z == approx(0.1)
+
+    # set_yaw_rate should work in rates mode
+    res = set_yaw_rate(yaw_rate=0.4)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_RATES
+    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
+    assert state.roll_rate == approx(0.3)
+    assert state.pitch_rate == approx(0.2)
+    assert state.yaw_rate == approx(0.4)
+    assert state.thrust == approx(0.3)
+
+    res = set_rates(roll_rate=inf)
+    assert res.success == False
+    assert res.message == 'roll_rate argument cannot be Inf'

clover/test/offboard.test

@@ -0,0 +1,10 @@
+<launch>
+    <node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen"/>
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="test_frame" args="10 20 30 0 0 0 map test"/>
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="test2_frame" args="100 200 300 0 0 0 map test2"/>
+
+    <param name="test_module" value="$(find clover)/test/offboard.py"/>
+    <test test-name="offboard_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

clover_blocks/www/blocks.js

@@ -15,6 +15,7 @@ const COLOR_GPIO = 200;
 const DOCS_URL = 'https://clover.coex.tech/en/blocks.html';
 
 var frameIds = [["body", "BODY"], ["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["map", "MAP"]];
+var frameIdsWithTerrain = frameIds.concat([["terrain", "TERRAIN"]]);
 
 function considerFrameId(e) {
 	if (!(e instanceof Blockly.Events.Change || e instanceof Blockly.Events.Create)) return;
@@ -22,7 +23,7 @@ function considerFrameId(e) {
 	var frameId = this.getFieldValue('FRAME_ID');
 	// set appropriate coordinates labels
 	if (this.getInput('X')) { // block has x-y-z fields
-		if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK') {
+		if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK' || frameId == 'TERRAIN') {
 			this.getInput('X').fieldRow[0].setValue('forward');
 			this.getInput('Y').fieldRow[0].setValue('left');
 			this.getInput('Z').fieldRow[0].setValue('up');
@@ -59,8 +60,8 @@ function updateSetpointBlock(e) {
 	this.getInput('VY').setVisible(velocity);
 	this.getInput('VZ').setVisible(velocity);
 	this.getInput('YAW').setVisible(attitude);
-	this.getInput('PITCH').setVisible(attitude);
 	this.getInput('ROLL').setVisible(attitude);
+	this.getInput('PITCH').setVisible(attitude);
 	this.getInput('THRUST').setVisible(attitude);
 	this.getInput('RELATIVE_TO').setVisible(type != 'RATES');
 
@@ -73,7 +74,7 @@ function updateSetpointBlock(e) {
 
 Blockly.Blocks['navigate'] = {
 	init: function () {
-		let navFrameId = frameIds.slice();
+		let navFrameId = frameIdsWithTerrain.slice();
 		navFrameId.push(['global', 'GLOBAL_LOCAL'])
 		navFrameId.push(['global, WGS 84 alt.', 'GLOBAL'])
 		this.appendDummyInput()
@@ -163,14 +164,14 @@ Blockly.Blocks['setpoint'] = {
 		this.appendValueInput("VZ")
 			.setCheck("Number")
 			.appendField("vz");
-		this.appendValueInput("PITCH")
-			.setCheck("Number")
-			.appendField("pitch")
-			.setVisible(false);
 		this.appendValueInput("ROLL")
 			.setCheck("Number")
 			.appendField("roll")
 			.setVisible(false);
+		this.appendValueInput("PITCH")
+			.setCheck("Number")
+			.appendField("pitch")
+			.setVisible(false);
 		this.appendValueInput("YAW")
 			.setCheck("Number")
 			.appendField("yaw")
@@ -213,7 +214,7 @@ Blockly.Blocks['get_position'] = {
 			.appendField("current")
 			.appendField(new Blockly.FieldDropdown([["x", "X"], ["y", "Y"], ["z", "Z"], ["vx", "VX"], ["vy", "VY"], ["vz", "VZ"]]), "FIELD")
 			.appendField("relative to")
-			.appendField(new Blockly.FieldDropdown(frameIds), "FRAME_ID");
+			.appendField(new Blockly.FieldDropdown(frameIdsWithTerrain), "FRAME_ID");
 		this.appendValueInput("ID")
 			.setCheck("Number")
 			.appendField("with ID")
@@ -247,7 +248,7 @@ Blockly.Blocks['get_attitude'] = {
 	init: function () {
 		this.appendDummyInput()
 			.appendField("current")
-			.appendField(new Blockly.FieldDropdown([["pitch", "PITCH"], ["roll", "ROLL"], ["pitch rate", "PITCH_RATE"], ["roll rate", "ROLL_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD");
+			.appendField(new Blockly.FieldDropdown([["roll", "ROLL"], ["pitch", "PITCH"], ["roll rate", "ROLL_RATE"], ["pitch rate", "PITCH_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD");
 		this.setOutput(true, "Number");
 		this.setColour(COLOR_STATE);
 		this.setTooltip("Returns current orientation or angle rates in degree or degree per second (not radian).");
@@ -509,7 +510,7 @@ Blockly.Blocks['distance'] = {
 			.appendField("z");
 		this.appendDummyInput()
 			.appendField("relative to")
-			.appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"]]), "FRAME_ID");
+			.appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["terrain", "TERRAIN"]]), "FRAME_ID");
 		this.appendValueInput("ID")
 			.setCheck("Number")
 			.appendField("with ID")

clover_blocks/www/index.html

@@ -69,8 +69,8 @@
 				<value name="VX"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="VY"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="VZ"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
-				<value name="PITCH"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="ROLL"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
+				<value name="PITCH"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="YAW"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="THRUST"><shadow type="math_number"><field name="NUM">0.5</field></shadow></value>
 				<value name="ID"><shadow type="math_number"><field name="NUM">0</field></shadow></value>

clover_blocks/www/python.js

@@ -276,10 +276,11 @@ Blockly.Python.angle = function(block) {
 }
 
 Blockly.Python.set_yaw = function(block) {
+	rosDefinitions.setYaw = true;
 	simpleOffboard();
 	let yaw = Blockly.Python.valueToCode(block, 'YAW', Blockly.Python.ORDER_NONE);
 	let frameId = buildFrameId(block);
-	let code = `navigate(x=float('nan'), y=float('nan'), z=float('nan'), yaw=${yaw}, frame_id=${frameId})\n`;
+	let code = `set_yaw(yaw=${yaw}, frame_id=${frameId})\n`;
 	if (block.getFieldValue('WAIT') == 'TRUE') {
 		rosDefinitions.waitYaw = true;
 		simpleOffboard();
@@ -328,11 +329,11 @@ Blockly.Python.setpoint = function(block) {
 	} else if (type == 'ATTITUDE') {
 		rosDefinitions.setAttitude = true;
 		simpleOffboard();
-		return `set_attitude(pitch=${pitch}, roll=${roll}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`;
+		return `set_attitude(roll=${roll}, pitch=${pitch}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`;
 	} else if (type == 'RATES') {
 		rosDefinitions.setRates = true;
 		simpleOffboard();
-		return `set_rates(pitch_rate=${pitch}, roll_rate=${roll}, yaw_rate=${yaw}, thrust=${thrust})\n`;
+		return `set_rates(roll_rate=${roll}, pitch_rate=${pitch}, yaw_rate=${yaw}, thrust=${thrust})\n`;
 	}
 }
 

docs/en/parameters.md

@@ -60,8 +60,8 @@ The `SYS_MC_EST_GROUP` parameter defines the estimator subsystem to use.
 
 Estimator subsystem is a group of modules that calculates the current state of the copter using readings from the sensors. The copter state includes:
 
-* Angle rate of the copter – pitch_rate, roll_rate, yaw_rate;
-* Copter orientation (in the local coordinate system) – pitch, roll, yaw (one of presentations);
+* Angle rate of the copter – roll_rate, pitch_rate, yaw_rate;
+* Copter orientation (in the local coordinate system) – roll, pitch, yaw (one of presentations);
 * Copter position (in the local coordinate system) – x, y, z;
 * Copter speed (in the local coordinate system) – vx, vy, vz;
 * Global coordinates of the copter – latitude, longitude, altitude;

docs/en/simple_offboard.md

@@ -51,11 +51,11 @@ Response format:
 * `lat, lon` – drone latitude and longitude *(degrees)*, requires [GPS](gps.md) module;
 * `alt` – altitude in the global coordinate system (according to [WGS-84](https://ru.wikipedia.org/wiki/WGS_84) standard, not <abbr title="Above Mean Sea Level">AMSL</abbr>!), requires [GPS](gps.md) module;
 * `vx, vy, vz` – drone velocity *(m/s)*;
-* `pitch` – pitch angle *(radians)*;
 * `roll` – roll angle *(radians)*;
+* `pitch` – pitch angle *(radians)*;
 * `yaw` — yaw angle *(radians)*;
-* `pitch_rate` — angular pitch velocity *(rad/s)*;
 * `roll_rate` – angular roll velocity *(rad/s)*;
+* `pitch_rate` — angular pitch velocity *(rad/s)*;
 * `yaw_rate` – angular yaw velocity *(rad/s)*;
 * `voltage` – total battery voltage *(V)*;
 * `cell_voltage` – battery cell voltage *(V)*.
@@ -261,22 +261,22 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
 
 ### set_attitude
 
-Set pitch, roll, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available.
+Set roll, pitch, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available.
 
 Parameters:
 
-* `pitch`, `roll`, `yaw` – requested pitch, roll, and yaw angle *(radians)*;
+* `roll`, `pitch`, `yaw` – requested roll, pitch, and yaw angle *(radians)*;
 * `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle).
 * `auto_arm` – switch the drone to `OFFBOARD` mode and arm automatically (**the drone will take off**);
 * `frame_id` – [coordinate system](frames.md) for `yaw` (Default value: `map`).
 
 ### set_rates
 
-Set pitch, roll, and yaw rates and the throttle level (similar to [the `ACRO` mode](modes.md)). This is the lowest drone control level (excluding direct control of motor rotation speed). This service may be used to automatically perform aerobatic tricks (e.g., flips).
+Set roll, pitch, and yaw rates and the throttle level (similar to [the `ACRO` mode](modes.md)). This is the lowest drone control level (excluding direct control of motor rotation speed). This service may be used to automatically perform aerobatic tricks (e.g., flips).
 
 Parameters:
 
-* `pitch_rate`, `roll_rate`, `yaw_rate` – pitch, roll, and yaw rates *(rad/s)*;
+* `roll_rate`, `pitch_rate`, `yaw_rate` – pitch, roll, and yaw rates *(rad/s)*;
 * `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle).
 * `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**);
 

docs/en/snippets.md

@@ -144,7 +144,7 @@ Determine whether the copter is turned upside-down:
 PI_2 = math.pi / 2
 telem = get_telemetry()
 
-flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
+flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
 ```
 
 ### # {#angle-hor}
@@ -155,8 +155,8 @@ Calculate the copter horizontal angle:
 PI_2 = math.pi / 2
 telem = get_telemetry()
 
-flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2
-angle_to_horizon = math.atan(math.hypot(math.tan(telem.pitch), math.tan(telem.roll)))
+flipped = not -PI_2 <= telem.roll <= PI_2 or not -PI_2 <= telem.pitch <= PI_2
+angle_to_horizon = math.atan(math.hypot(math.tan(telem.roll), math.tan(telem.pitch)))
 if flipped:
     angle_to_horizon = math.pi - angle_to_horizon
 ```
@@ -324,7 +324,7 @@ def flip():
 
     while True:
         telem = get_telemetry()
-        flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
+        flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
         if flipped:
             break
 

docs/ru/parameters.md

@@ -60,8 +60,8 @@
 
 Estimator это подсистема, которая вычисляет текущее состояние (state) коптера, используя показания с датчиков. В состояние коптера входит:
 
-* угловая скорость коптера – pitch_rate, roll_rate, yaw_rate;
-* ориентация коптера (в локальной системе координат) – pitch (тангаж), roll (крен), yaw (рысканье) (одно из представлений);
+* угловая скорость коптера – roll_rate, pitch_rate, yaw_rate;
+* ориентация коптера (в локальной системе координат) – roll (крен), pitch (тангаж), yaw (рысканье) (одно из представлений);
 * позиция коптера (в локальной системе координат) – x, y, z;
 * скорость коптера (в локальной системе координат) – vx, vy, vz;
 * глобальные координаты коптера – latitude, longitude, altitude;

docs/ru/simple_offboard.md

@@ -51,11 +51,11 @@ land = rospy.ServiceProxy('land', Trigger)
 * `lat, lon` – широта, долгота *(градусы)*, необходимо наличие [GPS](gps.md);
 * `alt` – высота в глобальной системе координат (стандарт [WGS-84](https://ru.wikipedia.org/wiki/WGS_84), не <abbr title="Above Mean Sea Level, выше среднего уровня моря">AMSL</abbr>!), необходимо наличие [GPS](gps.md);
 * `vx, vy, vz` – скорость коптера *(м/с)*;
-* `pitch` – угол по тангажу *(радианы)*;
 * `roll` – угол по крену *(радианы)*;
+* `pitch` – угол по тангажу *(радианы)*;
 * `yaw` – угол по рысканью *(радианы)*;
-* `pitch_rate` – угловая скорость по тангажу *(рад/с)*;
 * `roll_rate` – угловая скорость по крену *(рад/с)*;
+* `pitch_rate` – угловая скорость по тангажу *(рад/с)*;
 * `yaw_rate` – угловая скорость по рысканью *(рад/с)*;
 * `voltage` – общее напряжение аккумулятора *(В)*;
 * `cell_voltage` – напряжение аккумулятора на ячейку *(В)*.
@@ -265,7 +265,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
 
 Параметры:
 
-* `pitch`, `roll`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*;
+* `roll`, `pitch`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*;
 * `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ);
 * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**);
 * `frame_id` – [система координат](frames.md), в которой задан `yaw` (по умолчанию: `map`).
@@ -276,7 +276,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
 
 Параметры:
 
-* `pitch_rate`, `roll_rate`, `yaw_rate` – угловая скорость по тангажу, крену и рыканью *(рад/с)*;
+* `roll_rate`, `pitch_rate`, `yaw_rate` – угловая скорость по тангажу, крену и рыканью *(рад/с)*;
 * `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ).
 * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**);
 

docs/ru/snippets.md

@@ -154,7 +154,7 @@ new_pose = tf_buffer.transform(pose, frame_id, transform_timeout)
 PI_2 = math.pi / 2
 telem = get_telemetry()
 
-flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
+flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
 ```
 
 ### # {#angle-hor}
@@ -165,7 +165,7 @@ flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
 PI_2 = math.pi / 2
 telem = get_telemetry()
 
-flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2
+flipped = not -PI_2 <= telem.roll <= PI_2 or not -PI_2 <= telem.pitch <= PI_2
 angle_to_horizon = math.atan(math.hypot(math.tan(telem.pitch), math.tan(telem.roll)))
 if flipped:
     angle_to_horizon = math.pi - angle_to_horizon
@@ -335,7 +335,7 @@ def flip():
 
     while True:
         telem = get_telemetry()
-        flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
+        flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
         if flipped:
             break