Ardupilot 增加一個mavlink訊息（翻譯官網）
阿新 • • 發佈：2018-11-11
目錄
摘要
本節主要記錄自己翻譯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();
}