从 MAVROS 到 PX4 飞控的数据流向

上一篇分析了 MAVROS 中数据收发的实现方法。当用户发送一个 ros 话题的消息后，对应的 plugin 中的回调函数被处罚，ros 消息被包装成 mavlink 消息，从链路中发送出去。

下面以 "/mavros/setpoint_position/local" 话题为例进行分析。

一、MAVROS 部分

用户代码发送了一个 "/mavros/setpoint_position/local" 话题的消息，会触发 SetpointPositionPlugin::setpoint_cb 函数，如下所示：

	void setpoint_cb(const geometry_msgs::PoseStamped::ConstPtr &req)
	{
		Eigen::Affine3d tr;
		tf::poseMsgToEigen(req->pose, tr);

		send_position_target(req->header.stamp, tr);
	}

进而调用了 send_position_target 函数，实际上发送 GPS 位置时实际上也是将GPS位置转换为局部坐标系发送的，调用的也是这个函数。

	void send_position_target(const ros::Time &stamp, const Eigen::Affine3d &tr)
	{
		using mavlink::common::MAV_FRAME;

		/* Documentation start from bit 1 instead 0;
		 * Ignore velocity and accel vectors, yaw rate.
		 *
		 * In past versions on PX4 there been bug described in #273.
		 * If you got similar issue please try update firmware first.
		 */
		const uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);

		auto p = [&] () {
				if (static_cast<MAV_FRAME>(mav_frame) == MAV_FRAME::BODY_NED || static_cast<MAV_FRAME>(mav_frame) == MAV_FRAME::BODY_OFFSET_NED) {
					return ftf::transform_frame_baselink_aircraft(Eigen::Vector3d(tr.translation()));
				} else {
					return ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
				}
			} ();

		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()));
				} else {
					return ftf::transform_orientation_enu_ned(
						ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())));
				}
			} ();

		set_position_target_local_ned(stamp.toNSec() / 1000000,
			utils::enum_value(mav_frame),
			ignore_all_except_xyz_y,
			p,
			Eigen::Vector3d::Zero(),
			Eigen::Vector3d::Zero(),
			ftf::quaternion_get_yaw(q), 0.0);
	}

从这里可以看到坐标系的转换，一般情况下，输入值都是 ENU 坐标系的，需要转换成 NED 坐标系之后再调用 set_position_target_local_ned 函数，将 mavlink 消息发送给飞控。

而当定义了坐标系为 MAV_FRAME::BODY_NED 时没有进行坐标翻转，而是进行了另一种转换。

 set_position_target_local_ned 函数的参数列表如下所示：

	void set_position_target_local_ned(uint32_t time_boot_ms, uint8_t coordinate_frame,
			uint16_t type_mask,
			Eigen::Vector3d p,
			Eigen::Vector3d v,
			Eigen::Vector3d af,
			float yaw, float yaw_rate)

其中，coordinate_frame 就是坐标系类型，type_mask 是控制类型掩码，没有文档对这些进行说明，因此需要从 PX4 飞控源码中寻找对应的代码来分析。

二、 PX4 飞控部分

对应的 mavlink 消息处理函数是

void
MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)

代码很长就不贴了。

首先找 type_mask 的相关内容

		/* convert mavlink type (local, NED) to uORB offboard control struct */
		offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
		offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask & 0x4);
		offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
		offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
		bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
		/* yaw ignore flag mapps to ignore_attitude */
		offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
		/* yawrate ignore flag mapps to ignore_bodyrate */
		offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);


		bool is_takeoff_sp = (bool)(set_position_target_local_ned.type_mask & 0x1000);
		bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000);
		bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000);
		bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000);

这些 bool 数值都会参与到控制的决策过程。

然后找 coordinate_frame ，只有这个赋值语句

					/* set the local vel values */
					if (!offboard_control_mode.ignore_velocity) {
						pos_sp_triplet.current.velocity_valid = true;
						pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
						pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
						pos_sp_triplet.current.vz = set_position_target_local_ned.vz;

						pos_sp_triplet.current.velocity_frame =
							set_position_target_local_ned.coordinate_frame;

					} else {
						pos_sp_triplet.current.velocity_valid = false;
					}

在控制代码  void MulticopterPositionControl::control_offboard(float dt) 中坐标系类型起作用

				if (_pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
					/* set position setpoint move rate */
					_vel_sp(0) = _pos_sp_triplet.current.vx;
					_vel_sp(1) = _pos_sp_triplet.current.vy;

				} else if (_pos_sp_triplet.current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
					// Transform velocity command from body frame to NED frame
					_vel_sp(0) = cosf(_yaw) * _pos_sp_triplet.current.vx - sinf(_yaw) * _pos_sp_triplet.current.vy;
					_vel_sp(1) = sinf(_yaw) * _pos_sp_triplet.current.vx + cosf(_yaw) * _pos_sp_triplet.current.vy;

				} else {
					warn_rate_limited("Unknown velocity offboard coordinate frame");
				}

也就是说，当坐标类型为 LOCAL_NED 时，直接将值赋值给 速度期望；而当坐标类型是 BODY_NED 时，需要进行一次坐标转换；其他类型均引发错误。

需要特别注意，从代码过程可以分析得知，用户设定的坐标类型只在最终控制端发生了作用。用户进行对坐标数值进行赋值的时候，只能按照 ENU 坐标系的方式来进行赋值，并不是按照设定的坐标类型来赋值。