fix inconsistency in direction of yaw when using set_position in BODY…

… frames and fix problems with yaw in setponit_raw

mavros/include/mavros/frame_tf.h

@@ -62,6 +62,8 @@ enum class StaticTF {
 	ENU_TO_NED,		//!< change from expressed WRT ENU frame to WRT NED frame
 	AIRCRAFT_TO_BASELINK,	//!< change from expressed WRT aircraft frame to WRT to baselink frame
 	BASELINK_TO_AIRCRAFT,	//!< change from expressed WRT baselnk to WRT aircraft
+	ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK,//!< change orientation from being expressed in aircraft frame to baselink frame in an absolute frame of reference.
+	ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT,//!< change orientation from being expressed in baselink frame to aircraft frame in an absolute frame of reference
 };
 
 /**
@@ -186,6 +188,26 @@ inline T transform_orientation_baselink_aircraft(const T &in) {
 	return detail::transform_orientation(in, StaticTF::BASELINK_TO_AIRCRAFT);
 }
 
+/**
+ * @brief Transform from attitude represented WRT aircraft frame to
+ *		  attitude represented WRT base_link frame, treating aircraft frame 
+ *		  as in an absolute frame of reference (local NED).
+ */
+template<class T>
+inline T transform_orientation_absolute_frame_aircraft_baselink(const T &in) {
+	return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK);
+}
+
+/**
+ * @brief Transform from attitude represented WRT baselink frame to
+ *		  attitude represented WRT body frame, treating baselink frame 
+ *		  as in an absolute frame of reference (local NED).
+ */
+template<class T>
+inline T transform_orientation_absolute_frame_baselink_aircraft(const T &in) {
+	return detail::transform_orientation(in, StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT);
+}
+
 /**
  * @brief Transform data expressed in NED to ENU frame.
  */

mavros/src/lib/ftf_frame_conversions.cpp

@@ -85,6 +85,10 @@ Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const Stat
 	case StaticTF::AIRCRAFT_TO_BASELINK:
 	case StaticTF::BASELINK_TO_AIRCRAFT:
 		return q * AIRCRAFT_BASELINK_Q;
+
+	case StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK:
+	case StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT:
+		return AIRCRAFT_BASELINK_Q * q;	
 	}
 }
 

mavros/src/plugins/setpoint_position.cpp

@@ -146,7 +146,7 @@ class SetpointPositionPlugin : public plugin::PluginBase,
 
 		auto q = [&] () {
 				if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) {
-					return ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()));
+					return ftf::transform_orientation_absolute_frame_aircraft_baselink(Eigen::Quaterniond(tr.rotation()));
 				} else {
 					return ftf::transform_orientation_enu_ned(
 						ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())));

mavros/src/plugins/setpoint_raw.cpp

@@ -166,16 +166,19 @@ class SetpointRawPlugin : public plugin::PluginBase,
 			position = ftf::transform_frame_baselink_aircraft(position);
 			velocity = ftf::transform_frame_baselink_aircraft(velocity);
 			af = ftf::transform_frame_baselink_aircraft(af);
+			yaw = ftf::quaternion_get_yaw(
+					ftf::transform_orientation_absolute_frame_aircraft_baselink(
+							ftf::quaternion_from_rpy(0.0, 0.0, req->yaw)));
 		} else {
 			position = ftf::transform_frame_enu_ned(position);
 			velocity = ftf::transform_frame_enu_ned(velocity);
 			af = ftf::transform_frame_enu_ned(af);
-		}
-
-		yaw = ftf::quaternion_get_yaw(
+			yaw = ftf::quaternion_get_yaw(
 					ftf::transform_orientation_aircraft_baselink(
 						ftf::transform_orientation_ned_enu(
 							ftf::quaternion_from_rpy(0.0, 0.0, req->yaw))));
+		}
+
 		Eigen::Vector3d ang_vel_enu(0.0, 0.0, req->yaw_rate);
 		auto ang_vel_ned = ftf::transform_frame_ned_enu(ang_vel_enu);
 		yaw_rate = ang_vel_ned.z();