版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/lixiaoweimashixiao/article/details/82592865
目录
摘要
本节主要记录自己翻译ardupilot增加一条新的mavlink消息的过程，如果有错误的欢迎批评指正。微信：lxw15982962929或者手机：18129927205，扣扣：995439743
第一： 翻译官网资料
增加一条新的mavlink消息（Adding a new MAVLink Message）
飞控和地面站（地面站有mission planner, Droid Planner等等）之间的数据和命令的传输是采用串口连接并通过Mavlink协议议进行传递。本页提供了一些高级的建议，用于添加一条新的MavLink消息。
这些指令仅仅在Linux上测试过（确切地说，是在Windows电脑系统上运行Ubuntu的vmware workstation）。设置VM的指令在SITL页面上。如果你能运行SITL，你应该能按照这里的建议。这些指令不能在Windows或MAC上运行。
Step #1
首先确保已经安装了最新的Ardupilot固件代码。然后检查 mavproxy是否安装（MAVProxy是一个纯Python语言写的地面站程序）。 mavproxy可以通过在终端窗口中运行下面的命令命令来更新：
sudo pip install --upgrade mavproxy在ubuntu中演示上面的命令：
Step #2:
首先决定要添加什么类型的消息，以及它将如何与现有的MavLink消息页相匹配，不至于与现有的Mavlink消息相冲突。
例如，您可能希望向无人机发送新的导航命令，以便它能够在任务中（即，在自动模式下）执行一个功能（比如翻转）。在这种情况下，您将需要一个新的MAV_CMD_NAV_TRICK，类似于MAV_CMD_NAV_WAYPOINT的定义在 MAVLink消息页中搜索“MAV_CMD_NAV_WAYPOINT”）。
或者，你可能想从无人机上发送一种新的传感器数据到地面站。也许类似于SCALED_PRESSURE这个消息。
Step #3:
将新的消息定义添加到MavLink子模块中的common.xml或ardupilotmega.xml文件中。
如果希望将此命令添加到MAVLink协议，则应该将其添加到ardupilot/modules/mavlink/message_definitions/v1.0/..xml`文件。如果它只供您个人使用，或者只供Copter、Plane、Rover应用层使用，那么应该将其添加到ardupilotmega.xml文件中。
Step #4:
从2016年1月开始，在编译项目时会自动生成源代码，但在此日期之前，您需要切换到ardupilot目录（cd ardupilot），然后运行下面命令手动生成源代码。
./libraries/GCS_MAVLink/generate.shStep #5:
添加功能函数到主无人机代码来处理发送或接收命令到/来自地面站。所以在编辑xml文件之后一定要这样做，若要生成mavlink分组代码需要采用命令（make px4-v2）进行编译。mavlink的生成是首先进行的，所以项目编译是否成功并不重要，因为其他源代码发生了更改。
顶层应用层的代码很可能在无人机代码的GCS_MAVLink.cpp文件中或在./libraries/GCS_MAVLink/GCS类中。
在第一个示例中，我们要添加对新导航命令的支持（.e. a trick），需要以下内容：
- 扩展库 AP_Mission库中的
mission_cmd_to_mavlink()和mavlink_to_mission_cmd()函数，将以MAVProxy命令转换为AP_Mission::Mission_Command结构体。 - 向无人机增加一个新的
commands_logic.cpp‘s的start_command() and verify_command()函数用来检测MAV_CMD_NAV_TRICK这些应该调用两个新的函数，这些函数创建do_trick()和ValIFyIOFILE（）（见下文）。 - 创建这两个新函数，
do_trick()和verify_trick()函数，它们以某种方式命令无人机执行该功能（或者通过调用control_auto.cpp中的另一个函数来设置auto_mode变量和一个新的auto_trick_start()函数）。当第一次调用命令时，将调用do_trick()函数。verify_trick()将重复调用10Hz（或更高）直到该功能完成。当功能已经完成时，verify_trick()函数应该返回true。
Step #6:
决定如何处理GCS中的信息。最简单的方法之一就是使用Mavproxy。Mavproxy使用pymavlink 定义MAVLink消息，因此需要重建pymavlink 以包含自定义消息。
- 移除当前已经初始化的pymavlink版本，采用命令
pip uninstall pymavlink - 初始化更新版本，在命令窗口中切换到
ardupilot/modules/mavlink/pymavlink目录，运行python setup.py install --user - Mavproxy 现在能够发送或接收新消息。要让它打印出来或发送消息，你需要实现一个模块。模块是Python插件，允许您向Mavproxy添加功能。默认情况下，位于Ubuntu的
/usr/local/lib/python2.7/dist-packages/MAVProxy/modules/.目录下。下面是一个打印**MY_CUSTOM_PACKET**包消息内容的模块的示例。看看其他模块，关于如何使用命令行接口触发消息的发送。
#!/usr/bin/env python
'''Custom'''
import time, os
from MAVProxy.modules.lib import mp_module
from pymavlink import mavutil
import sys, traceback
class CustomModule(mp_module.MPModule):
def __init__(self, mpstate):
super(CustomModule, self).__init__(mpstate, "Custom", "Custom module")
'''initialisation code'''
def mavlink_packet(self, m):
'handle a mavlink packet'''
if m.get_type() == 'MY_CUSTOM_PACKET':
print "My Int: %(x).2f" % \
{"x" : m.intField}
def init(mpstate):
'''initialise module'''
return CustomModule(mpstate)警告：
如果添加的消息ID大于255，则需要启用MavLink 2的使能。这可以通过将相关的Serialnl协议参数设置为2，并用Mavproxy来实现参数–mav20启动。
Step #7:
考虑将代码贡献回主代码库。与其他开发人员讨论 Gitter 和/或raise a pull request。如果你提出一个下载请求，最好把这个变化分成至少两个单独的提交。一个提交到.xml文件的更改（Step #3），另一个提交给无人机代码的更改。
第二： Mavlink代码信息
void GCS::send_message(enum ap_message id)
{
for (uint8_t i=0; i<num_gcs(); i++)
{
if (chan(i).initialised) //初始化
{
chan(i).send_message(id);//飞控向地面站发送数据
}
}
}
void GCS_MAVLINK::send_message(enum ap_message id)
{
uint8_t i, nextid;
if (id == MSG_HEARTBEAT) //判断是否是心跳包
{
save_signing_timestamp(false);
}
//如果可能的话：看看我们是否可以发送延迟消息----see if we can send the deferred messages, if any:
push_deferred_messages();
//如果没有延迟消息，尝试立即发送：----if there are no deferred messages, attempt to send straight away:
if (num_deferred_messages == 0)
{
if (try_send_message(id)) //发送消息
{
// yay, we sent it!
return;
}
}
//这次我们没有发送信息，所以试着推迟：---- we failed to send the message this time around, so try to defer:
if (num_deferred_messages == ARRAY_SIZE(deferred_messages))
{
// the defer buffer is full, discard this attempt to send.
// Note that the message *may* already be in the defer buffer
//延迟缓冲区已满，放弃此尝试发送,注意，消息*可能已经在延迟缓冲区中。
return;
}
//检查此消息是否被延迟：----- check if this message is deferred:
for (i=0, nextid = next_deferred_message; i < num_deferred_messages; i++)
{
if (deferred_messages[nextid] == id)
{
//已经推迟了---- it's already deferred
return;
}
nextid++;
if (nextid == ARRAY_SIZE(deferred_messages))
{
nextid = 0;
}
}
// 尚未推迟，推迟---not already deferred, defer it
deferred_messages[nextid] = id;
num_deferred_messages++;
}bool GCS_MAVLINK::try_send_message(const enum ap_message id)
{
if (telemetry_delayed()) //数传是否延迟
{
return false;
}
bool ret = true;
switch(id)
{
case MSG_ATTITUDE: //发送姿态信息----1
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude();
break;
case MSG_NEXT_PARAM: //发送挂载信息----2
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
queued_param_send();
break;
case MSG_HEARTBEAT: //发送心态包信息----3
CHECK_PAYLOAD_SIZE(HEARTBEAT);
last_heartbeat_time = AP_HAL::millis();
send_heartbeat();
break;
case MSG_HWSTATUS: //发送飞控板电压信息----4
CHECK_PAYLOAD_SIZE(HWSTATUS);
send_hwstatus();
break;
case MSG_LOCATION: //发送位置初始化信息-----5
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_global_position_int();
break;
case MSG_CURRENT_WAYPOINT: //发送航点信息------------6
case MSG_MISSION_ITEM_REACHED:
case MSG_NEXT_WAYPOINT:
ret = try_send_mission_message(id);
break;
case MSG_MAG_CAL_PROGRESS: //发送地磁信息------------7
case MSG_MAG_CAL_REPORT:
ret = try_send_compass_message(id);
break;
case MSG_BATTERY_STATUS: //发送电池信息------------8
send_battery_status();
break;
case MSG_BATTERY2: //发送电池2信息------------9
CHECK_PAYLOAD_SIZE(BATTERY2);
send_battery2();
break;
case MSG_EKF_STATUS_REPORT: //发送EKF信息------------10
#if AP_AHRS_NAVEKF_AVAILABLE
CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT);
AP::ahrs_navekf().send_ekf_status_report(chan);
#endif
break;
case MSG_EXTENDED_STATUS2: //发送外部状态信息------------11
CHECK_PAYLOAD_SIZE(MEMINFO);
send_meminfo();
break;
case MSG_RANGEFINDER: //发送测距仪信息------------12
CHECK_PAYLOAD_SIZE(RANGEFINDER);
send_rangefinder_downward();
ret = send_distance_sensor();
ret = ret && send_proximity();
break;
case MSG_CAMERA_FEEDBACK: //发送相机信息------------13
{
AP_Camera *camera = AP::camera();
if (camera == nullptr) {
break;
}
CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK);
camera->send_feedback(chan);
}
break;
case MSG_SYSTEM_TIME: //发送系统时间信息------------14
CHECK_PAYLOAD_SIZE(SYSTEM_TIME);
send_system_time();
break;
case MSG_GPS_RAW: //发送GPS原始信息------------15
CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
AP::gps().send_mavlink_gps_raw(chan);
break;
case MSG_GPS_RTK: //发送RTK信息------------16
CHECK_PAYLOAD_SIZE(GPS_RTK);
AP::gps().send_mavlink_gps_rtk(chan, 0);
break;
case MSG_GPS2_RAW: //发送GPS2信息------------17
CHECK_PAYLOAD_SIZE(GPS2_RAW);
AP::gps().send_mavlink_gps2_raw(chan);
break;
case MSG_GPS2_RTK: //发送gps-rtk2信息------------18
CHECK_PAYLOAD_SIZE(GPS2_RTK);
AP::gps().send_mavlink_gps_rtk(chan, 1);
break;
case MSG_LOCAL_POSITION: //发送当地位置信息------------19
CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED);
send_local_position();
break;
case MSG_POSITION_TARGET_GLOBAL_INT: //发送位置目标信息------------20
CHECK_PAYLOAD_SIZE(POSITION_TARGET_GLOBAL_INT);
send_position_target_global_int();
break;
case MSG_RADIO_IN: //遥控器的数据处理，采用mavlink------------21
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
send_radio_in(); //发送遥控器数据
break;
case MSG_RAW_IMU1: //发送IMU1信息------------22
CHECK_PAYLOAD_SIZE(RAW_IMU);
send_raw_imu();
break;
case MSG_RAW_IMU2: //发送IMU2信息------------23
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_scaled_pressure();
break;
case MSG_RAW_IMU3: //发送IMU3信息------------24
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_sensor_offsets();
break;
case MSG_SERVO_OUTPUT_RAW: //发送电机输出信息------------25 CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
send_servo_output_raw();
break;
case MSG_SIMSTATE: //发送AHR2姿态信息------------26
CHECK_PAYLOAD_SIZE(SIMSTATE);
send_simstate();
CHECK_PAYLOAD_SIZE(AHRS2);
send_ahrs2();
break;
case MSG_AHRS: //发送AHRS姿态信息------------27
CHECK_PAYLOAD_SIZE(AHRS);
send_ahrs();
break;
case MSG_VFR_HUD: //发送油门通道------------28
CHECK_PAYLOAD_SIZE(VFR_HUD);
send_vfr_hud();
break;
case MSG_VIBRATION: //振动------------29
CHECK_PAYLOAD_SIZE(VIBRATION);
send_vibration();
break;
case MSG_ESC_TELEMETRY: // 电调------------30
{
#ifdef HAVE_AP_BLHELI_SUPPORT
CHECK_PAYLOAD_SIZE(ESC_TELEMETRY_1_TO_4);
AP_BLHeli *blheli = AP_BLHeli::get_singleton();
if (blheli)
{
blheli->send_esc_telemetry_mavlink(uint8_t(chan));
}
#endif
break;
}
default:
// try_send_message must always at some stage return true for
// a message, or we will attempt to infinitely retry the
// message as part of send_message.
// This message will be sent out at the same rate as the
// unknown message, so should be safe.
gcs().send_text(MAV_SEVERITY_DEBUG, "Sending unknown message (%u)", id);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("Sending unknown ap_message %u", id);
#endif
break;
}
return ret;
}（1）分析姿态包数据
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE); //ATTITUDE这个在哪里，在mavlink_msg_attitude.h文件夹
send_attitude(); //发送姿态信息
break;void GCS_MAVLINK::send_attitude() const
{
const AP_AHRS &ahrs = AP::ahrs();
const Vector3f omega = ahrs.get_gyro();
mavlink_msg_attitude_send(
chan, //通道
AP_HAL::millis(), //记录时间
ahrs.roll, //横滚角，单位是弧度，设置30会显示1718.8;也就是30*180/3.14
ahrs.pitch, //俯仰角度
ahrs.yaw, //偏航角度
omega.x, //横滚角速度
omega.y, //俯仰角速度
omega.z); //偏航角速度
}
static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_ATTITUDE_LEN]; //MAVLINK_MSG_ID_ATTITUDE_LEN=28
_mav_put_uint32_t(buf, 0, time_boot_ms);
_mav_put_float(buf, 4, roll); //横滚角
_mav_put_float(buf, 8, pitch); //俯仰角度
_mav_put_float(buf, 12, yaw); //偏航角度
_mav_put_float(buf, 16, rollspeed); //横滚角速度
_mav_put_float(buf, 20, pitchspeed); //俯仰角速度
_mav_put_float(buf, 24, yawspeed); //偏航角速度
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); //发送数据
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN);
#endif
#else
mavlink_attitude_t packet;
packet.time_boot_ms = time_boot_ms;
packet.roll = roll;
packet.pitch = pitch;
packet.yaw = yaw;
packet.rollspeed = rollspeed;
packet.pitchspeed = pitchspeed;
packet.yawspeed = yawspeed;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_LEN); //发送包
#endif
#endif
}（2）分析挂载参数包数据
case MSG_NEXT_PARAM: //20
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
queued_param_send(); //发送参数队列
break;void GCS_MAVLINK::queued_param_send()
{
if (!initialised)
{
return;
}
// send one parameter async reply if pending
send_parameter_reply();
if (_queued_parameter == nullptr)
{
return;
}
uint16_t bytes_allowed;
uint8_t count;
uint32_t tnow = AP_HAL::millis();
uint32_t tstart = AP_HAL::micros();
// use at most 30% of bandwidth on parameters. The constant 26 is
// 1/(1000 * 1/8 * 0.001 * 0.3)
bytes_allowed = 57 * (tnow - _queued_parameter_send_time_ms) * 26;
if (bytes_allowed > comm_get_txspace(chan)) {
bytes_allowed = comm_get_txspace(chan);
}
count = bytes_allowed / (MAVLINK_MSG_ID_PARAM_VALUE_LEN + packet_overhead());
// when we don't have flow control we really need to keep the
// param download very slow, or it tends to stall
if (!have_flow_control() && count > 5) {
count = 5;
}
while (_queued_parameter != nullptr && count--)
{
AP_Param *vp;
float value;
// copy the current parameter and prepare to move to the next
vp = _queued_parameter;
// if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float(_queued_parameter_type);
char param_name[AP_MAX_NAME_SIZE];
vp->copy_name_token(_queued_parameter_token, param_name, sizeof(param_name), true);
mavlink_msg_param_value_send( //只关心这里
chan,
param_name,
value,
mav_var_type(_queued_parameter_type),
_queued_parameter_count,
_queued_parameter_index);
_queued_parameter = AP_Param::next_scalar(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index++;
if (AP_HAL::micros() - tstart > 1000) {
// don't use more than 1ms sending blocks of parameters
break;
}
}
_queued_parameter_send_time_ms = tnow;
}static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_PARAM_VALUE_LEN];
_mav_put_float(buf, 0, param_value);
_mav_put_uint16_t(buf, 4, param_count);
_mav_put_uint16_t(buf, 6, param_index);
_mav_put_uint8_t(buf, 24, param_type);
_mav_put_char_array(buf, 8, param_id, 16);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
#endif
#else
mavlink_param_value_t packet;
packet.param_value = param_value;
packet.param_count = param_count;
packet.param_index = param_index;
packet.param_type = param_type;
mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN);
#endif
#endif
}（3）分析心跳包数据
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
last_heartbeat_time = AP_HAL::millis();
send_heartbeat(); //发送心跳包数据
break;void GCS_MAVLINK::send_heartbeat() const
{
mavlink_msg_heartbeat_send(
chan,
frame_type(), //飞行器类型，四旋翼，还是六轴灯 MAV_TYPE ENUM
MAV_AUTOPILOT_ARDUPILOTMEGA,//飞控型号MAV_AUTOPILOT ENUM
base_mode(), //系统当前模式MAV_MODE_FLAG，各种使能定义
custom_mode(), //用户自定义模式
system_status()); //系统状态MAV_STATE ，系统正在初始化，正在启动，未初始化等等
}心跳包一般用来表明发出该消息的设备是否活跃（一般以1Hz发送），消息接收端会根据是否及时收到了心跳包来判断是否和消息发送端失去了联系。
心跳包由5个数据成员组成，占用8个字节。
1、 type：飞行器类型，表示了当前发消息的是什么飞行器，如四旋翼，直升机等。type的取值对应枚举类型MAV_TYPE（如四旋翼，对应数值2）。
2、autopilot：飞控类型，如apm，Pixhawk等，发送心跳包的飞行器的飞控类型。autopilot的取枚举类型MAV_AUTOPILOT。
3、base mode（基本模式）：飞控现在所处的飞行模式，这个参数要看各个飞控自己的定义方式，会有不同的组合、计算方式。
4、custom mode（用户模式）：飞控现在所处的飞行模式，这个参数要看各个飞控自己的定义方式，会有不同的组合、计算方式。
5、system status：系统状态，见MAV_STATE枚举变量。
static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN];
_mav_put_uint32_t(buf, 0, custom_mode);
_mav_put_uint8_t(buf, 4, type);
_mav_put_uint8_t(buf, 5, autopilot);
_mav_put_uint8_t(buf, 6, base_mode);
_mav_put_uint8_t(buf, 7, system_status);
_mav_put_uint8_t(buf, 8, 3);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
#else
mavlink_heartbeat_t packet;
packet.custom_mode = custom_mode;
packet.type = type;
packet.autopilot = autopilot;
packet.base_mode = base_mode;
packet.system_status = system_status;
packet.mavlink_version = 3;
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC);
#endif
}（4）分析主板电压数据
case MSG_HWSTATUS:
CHECK_PAYLOAD_SIZE(HWSTATUS);
send_hwstatus(); //硬件状态
break;void GCS_MAVLINK::send_hwstatus()
{
mavlink_msg_hwstatus_send(
chan,
hal.analogin->board_voltage()*1000,
0);
}static inline void mavlink_msg_hwstatus_send(mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_HWSTATUS_LEN];
_mav_put_uint16_t(buf, 0, Vcc);
_mav_put_uint8_t(buf, 2, I2Cerr);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN);
#endif
#else
mavlink_hwstatus_t packet;
packet.Vcc = Vcc;
packet.I2Cerr = I2Cerr;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)&packet, MAVLINK_MSG_ID_HWSTATUS_LEN);
#endif
#endif
}
（5）位置初始化数据
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_global_position_int(); //位置信息
break;void GCS_MAVLINK::send_global_position_int()
{
AP_AHRS &ahrs = AP::ahrs();
ahrs.get_position(global_position_current_loc); // return value ignored; we send stale data
Vector3f vel;
ahrs.get_velocity_NED(vel);
mavlink_msg_global_position_int_send(
chan,
AP_HAL::millis(),
global_position_current_loc.lat, // in 1E7 degrees 维度信息
global_position_current_loc.lng, // in 1E7 degrees 经度信息
global_position_int_alt(), // millimeters above ground/sea level 高度信息
global_position_int_relative_alt(), // millimeters above home 相对高度
vel.x * 100, // X speed cm/s (+ve North) x轴速度
vel.y * 100, // Y speed cm/s (+ve East) y轴速度
vel.z * 100, // Z speed cm/s (+ve Down) z轴速度
ahrs.yaw_sensor); // compass heading in 1/100 degree 罗盘机头
}
static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN];
_mav_put_uint32_t(buf, 0, time_boot_ms);
_mav_put_int32_t(buf, 4, lat);
_mav_put_int32_t(buf, 8, lon);
_mav_put_int32_t(buf, 12, alt);
_mav_put_int32_t(buf, 16, relative_alt);
_mav_put_int16_t(buf, 20, vx);
_mav_put_int16_t(buf, 22, vy);
_mav_put_int16_t(buf, 24, vz);
_mav_put_uint16_t(buf, 26, hdg);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
#endif
#else
mavlink_global_position_int_t packet;
packet.time_boot_ms = time_boot_ms;
packet.lat = lat;
packet.lon = lon;
packet.alt = alt;
packet.relative_alt = relative_alt;
packet.vx = vx;
packet.vy = vy;
packet.vz = vz;
packet.hdg = hdg;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN);
#endif
#endif
}（6）任务航点信息
case MSG_CURRENT_WAYPOINT: //航点信息
case MSG_MISSION_ITEM_REACHED: //任务项
case MSG_NEXT_WAYPOINT:
ret = try_send_mission_message(id);
break;bool GCS_MAVLINK::try_send_mission_message(const enum ap_message id)
{
AP_Mission *mission = get_mission();
if (mission == nullptr) {
return true;
}
bool ret = true;
switch (id) {
case MSG_CURRENT_WAYPOINT:
CHECK_PAYLOAD_SIZE(MISSION_CURRENT);
mavlink_msg_mission_current_send(chan, mission->get_current_nav_index());
ret = true;
break;
case MSG_MISSION_ITEM_REACHED:
CHECK_PAYLOAD_SIZE(MISSION_ITEM_REACHED);
mavlink_msg_mission_item_reached_send(chan, mission_item_reached_index);
ret = true;
break;
case MSG_NEXT_WAYPOINT:
CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
queued_waypoint_send();
ret = true;
break;
default:
ret = true;
break;
}
return ret;
}（7）姿态2信息
case MSG_SIMSTATE:
CHECK_PAYLOAD_SIZE(SIMSTATE);
send_simstate(); //sitl仿真数据
CHECK_PAYLOAD_SIZE(AHRS2);
send_ahrs2(); //姿态2信息void GCS_MAVLINK::send_ahrs2()
{
#if AP_AHRS_NAVEKF_AVAILABLE
const AP_AHRS &ahrs = AP::ahrs();
Vector3f euler;
struct Location loc {};
if (ahrs.get_secondary_attitude(euler))
{
mavlink_msg_ahrs2_send(chan,
euler.x,
euler.y,
euler.z,
loc.alt*1.0e-2f,
loc.lat,
loc.lng);
}
const AP_AHRS_NavEKF &_ahrs = reinterpret_cast<const AP_AHRS_NavEKF&>(ahrs);
const NavEKF2 &ekf2 = _ahrs.get_NavEKF2_const();
if (ekf2.activeCores() > 0 &&
HAVE_PAYLOAD_SPACE(chan, AHRS3))
{
ekf2.getLLH(loc);
ekf2.getEulerAngles(-1,euler);
mavlink_msg_ahrs3_send(chan,
euler.x,
euler.y,
euler.z,
loc.alt*1.0e-2f,
loc.lat,
loc.lng,
0, 0, 0, 0);
}
#endif
}
// return secondary attitude solution if available, as eulers in radians
bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
// EKF is secondary
EKF2.getEulerAngles(-1, eulers);
return _ekf2_started;
case EKF_TYPE2:
case EKF_TYPE3:
default:
// DCM is secondary
eulers = _dcm_attitude;
return true;
}
}可以看出AHRS2主要是从EKF获取的姿态角度和经纬度
（8）气压计信息
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_scaled_pressure();
break;void GCS_MAVLINK::send_scaled_pressure()
{
uint32_t now = AP_HAL::millis();
const AP_Baro &barometer = AP::baro();
float pressure = barometer.get_pressure(0);
float diff_pressure = 0; // pascal
AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
if (airspeed != nullptr) {
diff_pressure = airspeed->get_differential_pressure();
}
mavlink_msg_scaled_pressure_send(
chan,
now,
pressure*0.01f, // hectopascal
diff_pressure*0.01f, // hectopascal
barometer.get_temperature(0)*100); // 0.01 degrees C
if (barometer.num_instances() > 1 &&
HAVE_PAYLOAD_SPACE(chan, SCALED_PRESSURE2)) {
pressure = barometer.get_pressure(1);
mavlink_msg_scaled_pressure2_send(
chan,
now,
pressure*0.01f, // hectopascal
(pressure - barometer.get_ground_pressure(1))*0.01f, // hectopascal
barometer.get_temperature(1)*100); // 0.01 degrees C
}
send_scaled_pressure3();
}