PX4中的mavlink

简介

px4与地面站的通信协议是mavlink,对于其消息格式的介绍看这里和这里。
需要注意几点:

  • 不光是px4与qgroundcontrol通信通过mavlink,有一些sensor也支持mavlink。
  • mavlink在github中是一个单独的仓库,通过git submodule的方式集成到了px4中。
  • mavlink协议本身并不复杂,用户也不同关心底层的实现(发送、接收、校验、打包、解包)等,只需要提供消息格式(在mavlink中选择现成的or自定义)、通信周期就可以了。

我主要关心几个问题:

  1. mavlink是如何编译的?
  2. mavlink有哪些参数?
  3. mavlink是如何启动的?
  4. px4中都有哪些现有的mavlink消息?px4中都有哪些现有的mavlink消息?
  5. 如何定义新的mavlink消息?
  6. mavlink是怎么配置硬件设备的?
  7. mavlink是怎么把uorb的消息打包的?
  8. mavlink是怎么定时发送消息的?
  9. mavlink是怎么接收消息的?
  10. mavlink是怎么解析接收消息的?

mavlink是如何编译的?

在linux下,看posix_sitl_default.cmake中的modules/mavlink

mavlink有哪些参数?

在ROMFS/px4fmu_common/init.d/rcS中:

set MAVLINK_F default
set MAVLINK_COMPANION_DEVICE /dev/ttyS2
set MAV_TYPE none

mavlink是如何启动的?

  • 在nuttx下
    见ROMFS/px4fmu_common/init.d/rcS
# Start mavlink streams.
#
sh /etc/init.d/rc.mavlink 

...
#
# Boot is complete, inform MAVLink app(s) that the system is now fully up and running.
#
mavlink boot_complete

rc.mavlink中的内容:

if ! ver hwcmp AEROFC_V1
then
	# Start MAVLink
	mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
fi
...
if [ "x${MAVLINK_F}" == xdefault ]
then
	# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
	set MAVLINK_F "-r 1200 -f"
	
	# Use ttyS1 for MAVLink on FMUv4 in addition to ttyS0 (debug)
	if ver hwcmp PX4FMU_V4
	then
		set MAVLINK_F "-r 1200 -d /dev/ttyS1"
		# Start MAVLink on Wifi (ESP8266 port)
		mavlink start -r 20000 -b 921600 -d /dev/ttyS0
	fi

	# Use ttyS3 (TELEM4) for MAVLink on FMUv5 in addition to ttyS1 (TELEM1) and ttyS2 (TELEM2)
	if ver hwcmp PX4FMU_V5
	then
		mavlink start -r 2000 -b 57600 -d /dev/ttyS3
	fi
fi

其中,ver是px4的指令,介绍见这里。Source: systemcmds/ver,
Tool to print various version information。
mavlink start后面的-r表示rate(通信周期),-b表示波特率,-d表示端口。

  • 在linux下
    见init.d-posix/rcS
# 启动mavlink,和GCS连接
# GCS link
# -r表示rate,-s表示stream_name,-u表示network_port
mavlink start -x -u $udp_gcs_port_local -r 4000000
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local
mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local
mavlink stream -r 50 -s ATTITUDE -u $udp_gcs_port_local
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_gcs_port_local
mavlink stream -r 50 -s ATTITUDE_TARGET -u $udp_gcs_port_local
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u $udp_gcs_port_local
mavlink stream -r 20 -s RC_CHANNELS -u $udp_gcs_port_local
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local

# API/Offboard link
mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote
...
mavlink boot_complete

上面用到了两个脚本:

  • mavlink start:启动mavlink,执行mavlink::start。
  • mavlink stream:添加mavlink command,执行mavlink::stream_command。POSITION_TARGET_LOCAL_NED等是common.xml中定义的消息。

px4中都有哪些现有的mavlink消息?

在px4的工程中,在Firmware/mavlink/include/mavlink/v2.0/message_definitions目录下,有如下的xml消息:
├── ardupilotmega.xml
├── ASLUAV.xml
├── autoquad.xml
├── common.xml
├── icarous.xml
├── matrixpilot.xml
├── minimal.xml
├── paparazzi.xml
├── python_array_test.xml
├── slugs.xml
├── standard.xml
├── test.xml
├── ualberta.xml
└── uAvionix.xml
且在目录的上一级,每个xml都有对应的文件夹,包括各自的.h文件(都是MAVLink comm protocol generated from xml)。
关于上述xml的解释,见https://mavlink.gitbooks.io/mavlink-devguide/en/messages/

  • 默认的消息
    在这些消息中,只有heartbeat是必须的(qgc需要据此判断连接是否正常),其他都是可选的。
    mavlink_main.cpp中_mode默认是MAVLINK_MODE_NORMAL。在mavlin_main.cpp中,有
	/* add default streams depending on mode */
	if (_mode != MAVLINK_MODE_IRIDIUM) {    //这里的Iridium应该是铱星通信系统

		/* HEARTBEAT is constant rate stream, rate never adjusted */
		configure_stream("HEARTBEAT", 1.0f);

		/* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */
		configure_stream("STATUSTEXT", 20.0f);

		/* COMMAND_LONG stream: use unlimited rate to send all commands */
		configure_stream("COMMAND_LONG");

	}
  • 默认的其他消息:
    在configure_streams_to_default中有:
	switch (_mode) {
	case MAVLINK_MODE_NORMAL:
		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
		configure_stream_local("ALTITUDE", 1.0f);
		configure_stream_local("ATTITUDE", 20.0f);
		configure_stream_local("ATTITUDE_TARGET", 2.0f);
		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
		configure_stream_local("COLLISION", unlimited_rate);
		configure_stream_local("DEBUG", 1.0f);
		configure_stream_local("DEBUG_VECT", 1.0f);
		configure_stream_local("DISTANCE_SENSOR", 0.5f);
		configure_stream_local("ESTIMATOR_STATUS", 0.5f);
		configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
		configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
		configure_stream_local("GPS_RAW_INT", 1.0f);
		configure_stream_local("GPS2_RAW", 1.0f);
		configure_stream_local("HIGHRES_IMU", 1.5f);
		configure_stream_local("HOME_POSITION", 0.5f);
		configure_stream_local("LOCAL_POSITION_NED", 1.0f);
		configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
		configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f);
		configure_stream_local("OPTICAL_FLOW_RAD", 1.0f);
		configure_stream_local("PING", 0.1f);
		configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f);
		configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.5f);
		configure_stream_local("RC_CHANNELS", 5.0f);
		configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
		configure_stream_local("SYS_STATUS", 1.0f);
		configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
		configure_stream_local("VFR_HUD", 4.0f);
		configure_stream_local("VISION_POSITION_ESTIMATE", 1.0f);
		configure_stream_local("WIND_COV", 1.0f);
		break;
		//...
		default:
		ret = -1;
		break;
	}

如何定义新的mavlink消息?

  1. 首先要安装mavlink generator
  2. 定义一个数据结构,比如说msg/ca_trajectory.msg
  3. 根据上面的数据结构写对应的xml
  4. 用mavlink generator生成mavlink消息(一个包含.h的文件夹)

后两个过程在这里。

mavlink是怎么配置硬件设备的?

见mavlink在rcS中的启动脚本(上面论述过)。

mavlink是怎么把uorb的消息打包的?

  1. 最底层。添加mavlink的头文件和uorb消息到mavlink_messages.cpp,比如自己添加的一个ca_trajectory消息:
#include 
#include 

或者一个mavlink中的common消息:

mavlink_msg_attitude_target.h  //被common.h所include,common.h被mavlink.h所include

这个.h文件就是打包过的数据结构

MAVPACKED(
typedef struct __mavlink_attitude_target_t {
 uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/
 float q[4]; /*<  Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/
 float body_roll_rate; /*< [rad/s] Body roll rate*/
 float body_pitch_rate; /*< [rad/s] Body pitch rate*/
 float body_yaw_rate; /*< [rad/s] Body yaw rate*/
 float thrust; /*<  Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)*/
 uint8_t type_mask; /*<  Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude*/
}) mavlink_attitude_target_t;

而且还包含了打包函数(mavlink_msg_attitude_target_pack)、encode函数(调用pack函数)、发送函数(mavlink_msg_attitude_target_send_struct)、decode函数(mavlink_msg_attitude_target_decode,调用unpack函数)、upack函数等。
2. 在mavlink_messages.cpp中创建一个新的类(可以看到其他消息也是这样做的),继承自MavlinkStream。这个类包括send函数

bool send(const hrt_abstime t)
	{
		vehicle_attitude_setpoint_s att_sp;

		if (_att_sp_sub->update(&_att_sp_time, &att_sp)) {

			vehicle_rates_setpoint_s att_rates_sp = {};
			_att_rates_sp_sub->update(&att_rates_sp);

			mavlink_attitude_target_t msg = {};

			msg.time_boot_ms = att_sp.timestamp / 1000;

			if (att_sp.q_d_valid) {
				memcpy(&msg.q[0], &att_sp.q_d[0], sizeof(msg.q));

			} else {
				matrix::Quatf q = matrix::Eulerf(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body);
				memcpy(&msg.q[0], q.data(), sizeof(msg.q));
			}

			msg.body_roll_rate = att_rates_sp.roll;
			msg.body_pitch_rate = att_rates_sp.pitch;
			msg.body_yaw_rate = att_rates_sp.yaw;

			msg.thrust = att_sp.thrust;

			mavlink_msg_attitude_target_send_struct(_mavlink->get_channel(), &msg);

			return true;
		}

		return false;
	}

可以看出,send函数等待uorb更新,并把uorb数据转为mavlink数据,并通过mavlink_msg_attitude_target_send_struct发送出去。
3. 附加流类streams_list的到mavlink_messages.cpp底部,这是因为streams_list包括了所有预先定义的mavlink消息,在执行create_mavlink_stream时,需要到streams_list中检查是否有定义。

static const StreamListItem streams_list[] = {
	StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static),
	StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static),
	StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static, &MavlinkStreamCommandLong::get_id_static),
	StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static, &MavlinkStreamSysStatus::get_id_static),
	StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static, &MavlinkStreamHighresIMU::get_id_static),
	StreamListItem(&MavlinkStreamScaledIMU::new_instance, &MavlinkStreamScaledIMU::get_name_static, &MavlinkStreamScaledIMU::get_id_static),
	StreamListItem(&MavlinkStreamScaledIMU2::new_instance, &MavlinkStreamScaledIMU2::get_name_static, &MavlinkStreamScaledIMU2::get_id_static),
	StreamListItem(&MavlinkStreamScaledIMU3::new_instance, &MavlinkStreamScaledIMU3::get_name_static, &MavlinkStreamScaledIMU3::get_id_static),
	StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static, &MavlinkStreamAttitude::get_id_static),
	StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static, &MavlinkStreamAttitudeQuaternion::get_id_static),
	StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static, &MavlinkStreamVFRHUD::get_id_static),
	StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static, &MavlinkStreamGPSRawInt::get_id_static),
	StreamListItem(&MavlinkStreamGPS2Raw::new_instance, &MavlinkStreamGPS2Raw::get_name_static, &MavlinkStreamGPS2Raw::get_id_static),
	StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static, &MavlinkStreamSystemTime::get_id_static),
	StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static, &MavlinkStreamTimesync::get_id_static),
	StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static, &MavlinkStreamGlobalPositionInt::get_id_static),
	StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static, &MavlinkStreamLocalPositionNED::get_id_static),
	StreamListItem(&MavlinkStreamVisionPositionEstimate::new_instance, &MavlinkStreamVisionPositionEstimate::get_name_static, &MavlinkStreamVisionPositionEstimate::get_id_static),
	StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static, &MavlinkStreamLocalPositionNEDCOV::get_id_static),
	StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static, &MavlinkStreamEstimatorStatus::get_id_static),
	StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static, &MavlinkStreamAttPosMocap::get_id_static),
	StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static, &MavlinkStreamHomePosition::get_id_static),
	StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static, &MavlinkStreamServoOutputRaw<0>::get_id_static),
	StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static, &MavlinkStreamServoOutputRaw<1>::get_id_static),
	StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static, &MavlinkStreamServoOutputRaw<2>::get_id_static),
	StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static, &MavlinkStreamServoOutputRaw<3>::get_id_static),
	StreamListItem(&MavlinkStreamHILActuatorControls::new_instance, &MavlinkStreamHILActuatorControls::get_name_static, &MavlinkStreamHILActuatorControls::get_id_static),
	StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static, &MavlinkStreamPositionTargetGlobalInt::get_id_static),
	StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static, &MavlinkStreamLocalPositionSetpoint::get_id_static),
	StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static, &MavlinkStreamAttitudeTarget::get_id_static),
	StreamListItem(&MavlinkStreamRCChannels::new_instance, &MavlinkStreamRCChannels::get_name_static, &MavlinkStreamRCChannels::get_id_static),
	StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static, &MavlinkStreamManualControl::get_id_static),
	StreamListItem(&MavlinkStreamTrajectoryRepresentationWaypoints::new_instance, &MavlinkStreamTrajectoryRepresentationWaypoints::get_name_static, &MavlinkStreamTrajectoryRepresentationWaypoints::get_id_static),
	StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static, &MavlinkStreamOpticalFlowRad::get_id_static),
	StreamListItem(&MavlinkStreamActuatorControlTarget<0>::new_instance, &MavlinkStreamActuatorControlTarget<0>::get_name_static, &MavlinkStreamActuatorControlTarget<0>::get_id_static),
	StreamListItem(&MavlinkStreamActuatorControlTarget<1>::new_instance, &MavlinkStreamActuatorControlTarget<1>::get_name_static, &MavlinkStreamActuatorControlTarget<1>::get_id_static),
	StreamListItem(&MavlinkStreamActuatorControlTarget<2>::new_instance, &MavlinkStreamActuatorControlTarget<2>::get_name_static, &MavlinkStreamActuatorControlTarget<2>::get_id_static),
	StreamListItem(&MavlinkStreamActuatorControlTarget<3>::new_instance, &MavlinkStreamActuatorControlTarget<3>::get_name_static, &MavlinkStreamActuatorControlTarget<3>::get_id_static),
	StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static, &MavlinkStreamNamedValueFloat::get_id_static),
	StreamListItem(&MavlinkStreamDebug::new_instance, &MavlinkStreamDebug::get_name_static, &MavlinkStreamDebug::get_id_static),
	StreamListItem(&MavlinkStreamDebugVect::new_instance, &MavlinkStreamDebugVect::get_name_static, &MavlinkStreamDebugVect::get_id_static),
	StreamListItem(&MavlinkStreamNavControllerOutput::new_instance, &MavlinkStreamNavControllerOutput::get_name_static, &MavlinkStreamNavControllerOutput::get_id_static),
	StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static, &MavlinkStreamCameraCapture::get_id_static),
	StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static, &MavlinkStreamCameraTrigger::get_id_static),
	StreamListItem(&MavlinkStreamCameraImageCaptured::new_instance, &MavlinkStreamCameraImageCaptured::get_name_static, &MavlinkStreamCameraImageCaptured::get_id_static),
	StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static, &MavlinkStreamDistanceSensor::get_id_static),
	StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static, &MavlinkStreamExtendedSysState::get_id_static),
	StreamListItem(&MavlinkStreamAltitude::new_instance, &MavlinkStreamAltitude::get_name_static, &MavlinkStreamAltitude::get_id_static),
	StreamListItem(&MavlinkStreamADSBVehicle::new_instance, &MavlinkStreamADSBVehicle::get_name_static, &MavlinkStreamADSBVehicle::get_id_static),
	StreamListItem(&MavlinkStreamCollision::new_instance, &MavlinkStreamCollision::get_name_static, &MavlinkStreamCollision::get_id_static),
	StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static),
	StreamListItem(&MavlinkStreamMountOrientation::new_instance, &MavlinkStreamMountOrientation::get_name_static, &MavlinkStreamMountOrientation::get_id_static),
	StreamListItem(&MavlinkStreamHighLatency2::new_instance, &MavlinkStreamHighLatency2::get_name_static, &MavlinkStreamHighLatency2::get_id_static),
	StreamListItem(&MavlinkStreamGroundTruth::new_instance, &MavlinkStreamGroundTruth::get_name_static, &MavlinkStreamGroundTruth::get_id_static),
	StreamListItem(&MavlinkStreamPing::new_instance, &MavlinkStreamPing::get_name_static, &MavlinkStreamPing::get_id_static)
};

mavlink是怎么定时发送消息的?

  • 添加消息、设置发送周期
    在mavlink_main.cpp加入自定义的消息以及期望的更新频率,比如:
    configure_stream("CA_TRAJECTORY", 100.0f);

其实就是在_streams中查找stream_name,如果找到,则set_interval(新的interval)

  • 数据结构
    _streams是整个类维护的链表,用来记录目前活跃的stream消息。

  • 发送消息
    在mavlink_main.cpp的while循环中,通过下列代码遍历stream并发送:

/* update streams */
// 对_streams列表中的每个stream都更新.
// 这里的更新就包括了send stream message
MavlinkStream *stream;
LL_FOREACH(_streams, stream) {
	stream->update(t);   //t是时间

	if (!_first_heartbeat_sent) {
		if (_mode == MAVLINK_MODE_IRIDIUM) {
			if (stream->get_id() == MAVLINK_MSG_ID_HIGH_LATENCY2) {
				_first_heartbeat_sent = stream->first_message_sent();
			}
		} else {
			if (stream->get_id() == MAVLINK_MSG_ID_HEARTBEAT) {
				_first_heartbeat_sent = stream->first_message_sent();
			}
		}
	}
}

其中update会调用各个message对应的class(比如上面的MavlinkStreamHeartbeat)的send函数。

mavlink是怎么接收消息的?

mavlink_main.cpp的task_main中调用:

MavlinkReceiver::receive_start(&_receive_thread, this);

recieve_start创建了一个thread,在thread中,用MavlinkReceiver::start_helper创建MavlinkReceiver对象,并执行MavlinkReceiver::receive_thread(void *arg),接收数据。

mavlink是怎么解析接收消息的?

在mavlink_receiver.h的class MavlinkReceiver中增加一个用来处理接收信息的函数,类似这样的:

	template<class T>
	void handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
					 const vehicle_command_s &vehicle_command);
	void handle_message_command_ack(mavlink_message_t *msg);
	void handle_message_optical_flow_rad(mavlink_message_t *msg);
	...

这个消息收到之后要转成uorb,再在内部传递,因此还要在这个类中增加一个uORB消息发布者,类似于:

	orb_advert_t _global_pos_pub;
	orb_advert_t _local_pos_pub;
	orb_advert_t _attitude_pub;
	orb_advert_t _gps_pub;
	...

然后再写一段上面声明的handle函数,该函数包括了对接收的消息进行解码:

void
MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
{
	/* optical flow */
	mavlink_optical_flow_rad_t flow;
	mavlink_msg_optical_flow_rad_decode(msg, &flow);

	int32_t flow_rot_int;
	param_get(param_find("SENS_FLOW_ROT"), &flow_rot_int);
	const enum Rotation flow_rot = (Rotation)flow_rot_int;

	struct optical_flow_s f = {};

并确定函数在MavlinkReceiver::handle_message()中被调用

	case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
		handle_message_optical_flow_rad(msg);
		break;

这个handle_message是被MavlinkReceiver::receive_thread调用的。

你可能感兴趣的:(飞控)