QGC 连接功能 底层执行逻辑
从点击这个 【Connect】按钮开始。
1,main.cc 函数 调用了 自定义的 QGCApplication类->这个类的构造函数中 定义了 QGCToolBox类
_toolbox = new QGCToolbox(this);
QGCToolBox类的 构造函数中 定义了 所需要的所有模块类
LinkManager* linkManager(void) { return _linkManager; }// 管理连接,包括串口,tcp,UDP,蓝牙,模拟等
MAVLinkProtocol* mavlinkProtocol(void) { return _mavlinkProtocol; }//对 收到的数据 进行解析
MultiVehicleManager* multiVehicleManager(void) { return _multiVehicleManager; }//对 多架飞机进行管理
QGCMapEngineManager* mapEngineManager(void) { return _mapEngineManager; }
QGCImageProvider* imageProvider() { return _imageProvider; }
UASMessageHandler* uasMessageHandler(void) { return _uasMessageHandler; }
FollowMe* followMe(void) { return _followMe; }
QGCPositionManager* qgcPositionManager(void) { return _qgcPositionManager; }
VideoManager* videoManager(void) { return _videoManager; }
MAVLinkLogManager* mavlinkLogManager(void) { return _mavlinkLogManager; }
2,Connect 按钮点击时-》LinkSettings.qml 文件总 调用 LinkManager总 createConnectedLink函数
QGCButton {
text: qsTr("Connect")
enabled: _currentSelection && !_currentSelection.link
onClicked: {
QGroundControl.linkManager.createConnectedLink(_currentSelection)
}
}
3,LinkManager.cc
// This should only be used by Qml code
void LinkManager::createConnectedLink(LinkConfiguration* config)
{
for(int i = 0; i < _sharedConfigurations.count(); i++) {
SharedLinkConfigurationPointer& sharedConf = _sharedConfigurations[i];
if (sharedConf->name() == config->name())
createConnectedLink(sharedConf);
}
}
4,LinkManager 根据不同的配置文件(LinkConfiguation) 创建不同连接。用到了 工厂模式
LinkInterface* LinkManager::createConnectedLink(SharedLinkConfigurationPointer& config)
{
if (!config) {
qWarning() << "LinkManager::createConnectedLink called with NULL config";
return NULL;
}
LinkInterface* pLink = NULL;
switch(config->type()) {
#ifndef NO_SERIAL_LINK
case LinkConfiguration::TypeSerial:
{
SerialConfiguration* serialConfig = dynamic_cast<SerialConfiguration*>(config.data());
if (serialConfig) {
pLink = new SerialLink(config);
if (serialConfig->usbDirect()) {
_activeLinkCheckList.append((SerialLink*)pLink);
if (!_activeLinkCheckTimer.isActive()) {
_activeLinkCheckTimer.start();
}
}
}
}
break;
#endif
case LinkConfiguration::TypeUdp:
pLink = new UDPLink(config);
break;
case LinkConfiguration::TypeTcp:
pLink = new TCPLink(config);
break;
#ifdef QGC_ENABLE_BLUETOOTH
case LinkConfiguration::TypeBluetooth:
pLink = new BluetoothLink(config);
break;
#endif
#ifndef __mobile__
case LinkConfiguration::TypeLogReplay:
pLink = new LogReplayLink(config);
break;
#endif
#ifdef QT_DEBUG
case LinkConfiguration::TypeMock:
pLink = new MockLink(config);
break;
#endif
case LinkConfiguration::TypeLast:
default:
break;
}
if (pLink) {
_addLink(pLink);
connectLink(pLink);
}
return pLink;
}
bool LinkManager::connectLink(LinkInterface* link)
{
if (link) {
if (_connectionsSuspendedMsg()) {
return false;
}
return link->_connect();
} else {
qWarning() << "Internal error";
return false;
}
}
5,SerialLink.cc 以串口为例,建立连接后 开始接受原始数据。
void SerialLink::_readBytes(void)
{
qint64 byteCount = _port->bytesAvailable();
if (byteCount) {
QByteArray buffer;
buffer.resize(byteCount);
_port->read(buffer.data(), buffer.size());
emit bytesReceived(this, buffer);
}
}
接收到的数据 发送给 MAVLinkProtocol 处理。所有的连接 都继承自 LinkInferface。
connect(link, &LinkInterface::bytesReceived, _mavlinkProtocol, &MAVLinkProtocol::receiveBytes);
6,MAVLinkProtocol 收到数据后 开始按照 mavlink协议 进行解包
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{
// Since receiveBytes signals cross threads we can end up with signals in the queue
// that come through after the link is disconnected. For these we just drop the data
// since the link is closed.
if (!_linkMgr->containsLink(link)) {
return;
}
// receiveMutex.lock();
mavlink_message_t message;
mavlink_status_t status;
int mavlinkChannel = link->mavlinkChannel();
static int nonmavlinkCount = 0;
static bool checkedUserNonMavlink = false;
static bool warnedUserNonMavlink = false;
for (int position = 0; position < b.size(); position++) {
unsigned int decodeState = mavlink_parse_char(mavlinkChannel, (uint8_t)(b[position]), &message, &status);
if (decodeState == 0 && !link->decodedFirstMavlinkPacket())
{
nonmavlinkCount++;
if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)
{
//2000 bytes with no mavlink message. Are we connected to a mavlink capable device?
if (!checkedUserNonMavlink)
{
link->requestReset();
checkedUserNonMavlink = true;
}
else
{
warnedUserNonMavlink = true;
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. "
"Please check if the baud rates of %1 and your autopilot are the same.").arg(qgcApp()->applicationName()));
}
}
}
if (decodeState == 1)
{
if (!link->decodedFirstMavlinkPacket()) {
mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
if (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
qDebug() << "Switching outbound to mavlink 2.0 due to incoming mavlink 2.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
}
link->setDecodedFirstMavlinkPacket(true);
}
// Log data
if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
// Write the uint64 time in microseconds in big endian format before the message.
// This timestamp is saved in UTC time. We are only saving in ms precision because
// getting more than this isn't possible with Qt without a ton of extra code.
quint64 time = (quint64)QDateTime::currentMSecsSinceEpoch() * 1000;
qToBigEndian(time, buf);
// Then write the message to the buffer
int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &message);
// Determine how many bytes were written by adding the timestamp size to the message size
len += sizeof(quint64);
// Now write this timestamp/message pair to the log.
QByteArray b((const char*)buf, len);
if(_tempLogFile.write(b) != len)
{
// If there's an error logging data, raise an alert and stop logging.
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("MAVLink Logging failed. Could not write to file %1, logging disabled.").arg(_tempLogFile.fileName()));
_stopLogging();
_logSuspendError = true;
}
// Check for the vehicle arming going by. This is used to trigger log save.
if (!_vehicleWasArmed && message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
mavlink_heartbeat_t state;
mavlink_msg_heartbeat_decode(&message, &state);
if (state.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
_vehicleWasArmed = true;
}
}
}
if (message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
// Start loggin on first heartbeat
_startLogging();
mavlink_heartbeat_t heartbeat;
mavlink_msg_heartbeat_decode(&message, &heartbeat);
emit vehicleHeartbeatInfo(link, message.sysid, message.compid, heartbeat.mavlink_version, heartbeat.autopilot, heartbeat.type);
}
7,收到 一帧 心跳包 后,MultiVehicleManager 会 实例出 一架飞机
connect(_mavlinkProtocol, &MAVLinkProtocol::vehicleHeartbeatInfo, this, &MultiVehicleManager::_vehicleHeartbeatInfo);
void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType)
{
Vehicle* vehicle = new Vehicle(link, vehicleId, componentId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, _firmwarePluginManager, _joystickManager);
}
8,同时 vehicle 会绑定 第6步 收到的mavlink协议。
connect(_mavlink, &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived);
9,这样 飞机 这个 类(Vehicle) 就收到了来自飞控的所有mavlink 数据。之后 qml界面 可对这些数据进行处理。
void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
if (message.sysid != _id && message.sysid != 0) {
// We allow RADIO_STATUS messages which come from a link the vehicle is using to pass through and be handled
if (!(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS && _containsLink(link))) {
return;
}
}
if (!_containsLink(link)) {
_addLink(link);
}
//-- Check link status
_messagesReceived++;
emit messagesReceivedChanged();
if(!_heardFrom) {
if(message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
_heardFrom = true;
_compID = message.compid;
_messageSeq = message.seq + 1;
}
} else {
if(_compID == message.compid) {
uint16_t seq_received = (uint16_t)message.seq;
uint16_t packet_lost_count = 0;
//-- Account for overflow during packet loss
if(seq_received < _messageSeq) {
packet_lost_count = (seq_received + 255) - _messageSeq;
} else {
packet_lost_count = seq_received - _messageSeq;
}
_messageSeq = message.seq + 1;
_messagesLost += packet_lost_count;
if(packet_lost_count)
emit messagesLostChanged();
}
}
// Mark this vehicle as active
_connectionActive();
// Give the plugin a change to adjust the message contents
if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) {
return;
}
switch (message.msgid) {
case MAVLINK_MSG_ID_HOME_POSITION:
_handleHomePosition(message);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
_handleHeartbeat(message);
break;
case MAVLINK_MSG_ID_RADIO_STATUS:
_handleRadioStatus(message);
break;
case MAVLINK_MSG_ID_RC_CHANNELS:
_handleRCChannels(message);
break;
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
_handleRCChannelsRaw(message);
break;
case MAVLINK_MSG_ID_BATTERY_STATUS:
_handleBatteryStatus(message);
break;
case MAVLINK_MSG_ID_SYS_STATUS:
_handleSysStatus(message);
break;
case MAVLINK_MSG_ID_RAW_IMU:
emit mavlinkRawImu(message);
break;
case MAVLINK_MSG_ID_SCALED_IMU:
emit mavlinkScaledImu1(message);
break;
case MAVLINK_MSG_ID_SCALED_IMU2:
emit mavlinkScaledImu2(message);
break;
case MAVLINK_MSG_ID_SCALED_IMU3:
emit mavlinkScaledImu3(message);
break;
case MAVLINK_MSG_ID_VIBRATION:
_handleVibration(message);
break;
case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
_handleExtendedSysState(message);
break;
case MAVLINK_MSG_ID_COMMAND_ACK:
_handleCommandAck(message);
break;
case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
_handleAutopilotVersion(link, message);
break;
case MAVLINK_MSG_ID_WIND_COV:
_handleWindCov(message);
break;
case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
_handleHilActuatorControls(message);
break;
case MAVLINK_MSG_ID_LOGGING_DATA:
_handleMavlinkLoggingData(message);
break;
case MAVLINK_MSG_ID_LOGGING_DATA_ACKED:
_handleMavlinkLoggingDataAcked(message);
break;
case MAVLINK_MSG_ID_GPS_RAW_INT:
_handleGpsRawInt(message);
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
_handleGlobalPositionInt(message);
break;
case MAVLINK_MSG_ID_ALTITUDE:
_handleAltitude(message);
break;
case MAVLINK_MSG_ID_VFR_HUD:
_handleVfrHud(message);
break;
case MAVLINK_MSG_ID_SCALED_PRESSURE:
_handleScaledPressure(message);
break;
case MAVLINK_MSG_ID_SCALED_PRESSURE2:
_handleScaledPressure2(message);
break;
case MAVLINK_MSG_ID_SCALED_PRESSURE3:
_handleScaledPressure3(message);
break;
case MAVLINK_MSG_ID_CAMERA_FEEDBACK:
_handleCameraFeedback(message);
break;
case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
_handleCameraImageCaptured(message);
break;
case MAVLINK_MSG_ID_SERIAL_CONTROL:
{
mavlink_serial_control_t ser;
mavlink_msg_serial_control_decode(&message, &ser);
emit mavlinkSerialControl(ser.device, ser.flags, ser.timeout, ser.baudrate, QByteArray(reinterpret_cast<const char*>(ser.data), ser.count));
}
break;
// Following are ArduPilot dialect messages
case MAVLINK_MSG_ID_WIND:
_handleWind(message);
break;
}
// This must be emitted after the vehicle processes the message. This way the vehicle state is up to date when anyone else
// does processing.
emit mavlinkMessageReceived(message);
_uas->receiveMessage(message);
}