px4与地面站的通信协议是mavlink,对于其消息格式的介绍看这里和这里。
需要注意几点:
我主要关心几个问题:
在linux下,看posix_sitl_default.cmake中的modules/mavlink
在ROMFS/px4fmu_common/init.d/rcS中:
set MAVLINK_F default
set MAVLINK_COMPANION_DEVICE /dev/ttyS2
set MAV_TYPE none
# 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表示端口。
# 启动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
上面用到了两个脚本:
在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/
/* 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");
}
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在rcS中的启动脚本(上面论述过)。
#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)
};
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_main.cpp的task_main中调用:
MavlinkReceiver::receive_start(&_receive_thread, this);
recieve_start创建了一个thread,在thread中,用MavlinkReceiver::start_helper创建MavlinkReceiver对象,并执行MavlinkReceiver::receive_thread(void *arg),接收数据。
在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调用的。