从 MAVROS 到 PX4 飞控的数据流向

2019-02-21 23:54:09 2059

分类专栏：

本文链接：

版权

上一篇分析了 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::BODY_NED || static_cast
    
     (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 消息处理函数是

voidMavlinkReceiver::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 坐标系的方式来进行赋值，并不是按照设定的坐标类型来赋值。

你可能感兴趣的文章