From b10e690067ce0ca0141cd5b1299807e631c52e70 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 12 Jun 2016 10:36:35 -0700 Subject: [PATCH] Fix priority link and default component id usage --- .../APM/APMSensorsComponentController.cc | 2 +- src/AutoPilotPlugins/AutoPilotPlugin.cc | 14 ++++++- src/FactSystem/ParameterLoader.cc | 15 ++++--- .../APM/ArduCopterFirmwarePlugin.cc | 8 ++-- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 16 ++++---- src/FollowMe/FollowMe.cc | 2 +- src/GPS/RTCM/RTCMMavlink.cc | 2 +- src/MissionManager/MissionManager.cc | 2 +- src/QGC.h | 2 - src/Vehicle/MultiVehicleManager.cc | 4 +- src/Vehicle/Vehicle.cc | 40 ++++++------------- src/Vehicle/Vehicle.h | 5 --- src/ViewWidgets/LogDownloadController.cc | 6 +-- src/comm/MAVLinkProtocol.cc | 4 +- src/uas/UAS.cc | 34 ++++++++-------- 15 files changed, 71 insertions(+), 85 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index 1dbcd7625..729275b78 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc @@ -452,7 +452,7 @@ void APMSensorsComponentController::nextClicked(void) ack.result = 1; mavlink_msg_command_ack_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &msg, &ack); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnPriorityLink(msg); } bool APMSensorsComponentController::compassSetupNeeded(void) const diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.cc b/src/AutoPilotPlugins/AutoPilotPlugin.cc index 38abd8755..8100333ea 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc @@ -88,8 +88,18 @@ void AutoPilotPlugin::resetAllParametersToDefaults(void) mavlink_message_t msg; MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->uas()->getUASID(), 0, MAV_CMD_PREFLIGHT_STORAGE, 0, 2, -1, 0, 0, 0, 0, 0); - _vehicle->sendMessage(msg); + mavlink_msg_command_long_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + _vehicle->id(), // Target systeem + _vehicle->defaultComponentId(), // Target component + MAV_CMD_PREFLIGHT_STORAGE, + 0, // Confirmation + 2, // 2 = Reset params to default + -1, // -1 = No change to mission storage + 0, // 0 = Ignore + 0, 0, 0, 0); // Unused + _vehicle->sendMessageOnPriorityLink(msg); } void AutoPilotPlugin::refreshAllParameters(unsigned char componentID) diff --git a/src/FactSystem/ParameterLoader.cc b/src/FactSystem/ParameterLoader.cc index d84a37428..d92d9a1be 100644 --- a/src/FactSystem/ParameterLoader.cc +++ b/src/FactSystem/ParameterLoader.cc @@ -44,7 +44,7 @@ ParameterLoader::ParameterLoader(Vehicle* vehicle) , _initialLoadComplete(false) , _waitingForDefaultComponent(false) , _saveRequired(false) - , _defaultComponentId(FactSystem::defaultComponentId) + , _defaultComponentId(MAV_COMP_ID_ALL) , _parameterSetMajorVersion(-1) , _parameterMetaData(NULL) , _totalParamCount(0) @@ -203,7 +203,7 @@ void ParameterLoader::_parameterUpdate(int uasId, int componentId, QString param int waitingParamCount = waitingReadParamIndexCount + waitingReadParamNameCount + waitingWriteParamNameCount; if (waitingParamCount) { qCDebug(ParameterLoaderLog) << "waitingParamCount:" << waitingParamCount; - } else if (_defaultComponentId != FactSystem::defaultComponentId) { + } else if (_defaultComponentId != MAV_COMP_ID_ALL) { // No more parameters to wait for, stop the timeout. Be careful to not stop timer if we don't have the default // component yet. _waitingParamTimeoutTimer.stop(); @@ -358,12 +358,11 @@ void ParameterLoader::refreshAllParameters(uint8_t componentID) void ParameterLoader::_determineDefaultComponentId(void) { - if (_defaultComponentId == FactSystem::defaultComponentId) { + if (_defaultComponentId == MAV_COMP_ID_ALL) { // We don't have a default component id yet. That means the plugin can't provide // the param to trigger off of. Instead we use the most prominent component id in // the set of parameters. Better than nothing! - _defaultComponentId = -1; int largestCompParamCount = 0; foreach(int componentId, _mapParameterName2Variant.keys()) { int compParamCount = _mapParameterName2Variant[componentId].count(); @@ -373,7 +372,7 @@ void ParameterLoader::_determineDefaultComponentId(void) } } - if (_defaultComponentId == -1) { + if (_defaultComponentId == MAV_COMP_ID_ALL) { qWarning() << "All parameters missing, unable to determine default componet id"; } } @@ -509,7 +508,7 @@ void ParameterLoader::_waitingParamTimeout(void) } } - if (!paramsRequested && _defaultComponentId == FactSystem::defaultComponentId && !_waitingForDefaultComponent) { + if (!paramsRequested && _defaultComponentId == MAV_COMP_ID_ALL && !_waitingForDefaultComponent) { // Initial load is complete but we still don't have default component params. Wait one more cycle to see if the // default component finally shows up. _waitingParamTimeoutTimer.start(); @@ -883,7 +882,7 @@ void ParameterLoader::_restartWaitingParamTimer(void) void ParameterLoader::_addMetaDataToDefaultComponent(void) { - if (_defaultComponentId == FactSystem::defaultComponentId) { + if (_defaultComponentId == MAV_COMP_ID_ALL) { // We don't know what the default component is so we can't support meta data return; } @@ -929,7 +928,7 @@ void ParameterLoader::_checkInitialLoadComplete(bool failIfNoDefaultComponent) } } - if (!failIfNoDefaultComponent && _defaultComponentId == FactSystem::defaultComponentId) { + if (!failIfNoDefaultComponent && _defaultComponentId == MAV_COMP_ID_ALL) { // We are still waiting for default component to show up return; } diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index c6fe7a16d..612bd8e79 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -133,12 +133,12 @@ void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitu cmd.param6 = 0.0f; cmd.param7 = vehicle->altitudeAMSL()->rawValue().toFloat() + altitudeRel; // AMSL meters cmd.target_system = vehicle->id(); - cmd.target_component = 0; + cmd.target_component = vehicle->defaultComponentId(); MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - vehicle->sendMessage(msg); + vehicle->sendMessageOnPriorityLink(msg); } void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) @@ -167,7 +167,7 @@ void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double memset(&cmd, 0, sizeof(mavlink_set_position_target_local_ned_t)); cmd.target_system = vehicle->id(); - cmd.target_component = 0; + cmd.target_component = vehicle->defaultComponentId(); cmd.coordinate_frame = MAV_FRAME_LOCAL_OFFSET_NED; cmd.type_mask = 0xFFF8; // Only x/y/z valid cmd.x = 0.0f; @@ -177,7 +177,7 @@ void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); mavlink_msg_set_position_target_local_ned_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - vehicle->sendMessage(msg); + vehicle->sendMessageOnPriorityLink(msg); } bool ArduCopterFirmwarePlugin::isPaused(const Vehicle* vehicle) const diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 7eb9dc0c2..4f5e26dc7 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -272,12 +272,12 @@ void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle) cmd.param6 = NAN; cmd.param7 = NAN; cmd.target_system = vehicle->id(); - cmd.target_component = 0; + cmd.target_component = vehicle->defaultComponentId(); MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - vehicle->sendMessage(msg); + vehicle->sendMessageOnPriorityLink(msg); } void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle) @@ -314,10 +314,10 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) cmd.param6 = NAN; cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel; cmd.target_system = vehicle->id(); - cmd.target_component = 0; + cmd.target_component = vehicle->defaultComponentId(); mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - vehicle->sendMessage(msg); + vehicle->sendMessageOnPriorityLink(msg); } void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) @@ -340,12 +340,12 @@ void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord cmd.param6 = gotoCoord.longitude() * 1e7; cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble(); cmd.target_system = vehicle->id(); - cmd.target_component = 0; + cmd.target_component = vehicle->defaultComponentId(); MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - vehicle->sendMessage(msg); + vehicle->sendMessageOnPriorityLink(msg); } void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) @@ -368,12 +368,12 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu cmd.param6 = NAN; cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel; cmd.target_system = vehicle->id(); - cmd.target_component = 0; + cmd.target_component = vehicle->defaultComponentId(); MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - vehicle->sendMessage(msg); + vehicle->sendMessageOnPriorityLink(msg); } void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode) diff --git a/src/FollowMe/FollowMe.cc b/src/FollowMe/FollowMe.cc index e03411ff0..cd69e9ef9 100644 --- a/src/FollowMe/FollowMe.cc +++ b/src/FollowMe/FollowMe.cc @@ -144,7 +144,7 @@ void FollowMe::_sendGCSMotionReport(void) mavlinkProtocol->getComponentId(), &message, &follow_target); - vehicle->sendMessage(message); + vehicle->sendMessageOnPriorityLink(message); } } } diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc index 3b570d538..587fc3fa3 100644 --- a/src/GPS/RTCM/RTCMMavlink.cc +++ b/src/GPS/RTCM/RTCMMavlink.cc @@ -63,6 +63,6 @@ void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg) mavlink_message_t message; mavlink_msg_gps_rtcm_data_encode(mavlinkProtocol->getSystemId(), mavlinkProtocol->getComponentId(), &message, &msg); - vehicle->sendMessage(message); + vehicle->sendMessageOnPriorityLink(message); } } diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 18f524f31..a8b68814e 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -107,7 +107,7 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC mavlink_mission_item_t missionItem; missionItem.target_system = _vehicle->id(); - missionItem.target_component = 0; + missionItem.target_component = _vehicle->defaultComponentId(); missionItem.seq = 0; missionItem.command = MAV_CMD_NAV_WAYPOINT; missionItem.param1 = 0; diff --git a/src/QGC.h b/src/QGC.h index a5dcbcc4e..f853e37f4 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -19,8 +19,6 @@ namespace QGC { -const static int defaultSystemId = 255; -const static int defaultComponentId = 0; /** * @brief Get the current ground time in microseconds. diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 061646346..a4cdf1433 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -306,12 +306,12 @@ void MultiVehicleManager::_sendGCSHeartbeat(void) mavlink_msg_heartbeat_pack(_mavlinkProtocol->getSystemId(), _mavlinkProtocol->getComponentId(), &message, - MAV_TYPE_GCS, // MAV_TYPE + MAV_TYPE_GCS, // MAV_TYPE MAV_AUTOPILOT_INVALID, // MAV_AUTOPILOT MAV_MODE_MANUAL_ARMED, // MAV_MODE 0, // custom mode MAV_STATE_ACTIVE); // MAV_STATE - vehicle->sendMessage(message); + vehicle->sendMessageOnPriorityLink(message); } } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 64fd84839..1bf55af20 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -133,7 +133,6 @@ Vehicle::Vehicle(LinkInterface* link, connect(_mavlink, &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived); - connect(this, &Vehicle::_sendMessageOnThread, this, &Vehicle::_sendMessage, Qt::QueuedConnection); connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection); connect(this, &Vehicle::flightModeChanged, this, &Vehicle::_handleFlightModeChanged); connect(this, &Vehicle::armedChanged, this, &Vehicle::_announceArmedChanged); @@ -766,11 +765,6 @@ void Vehicle::_linkInactiveOrDeleted(LinkInterface* link) } } -void Vehicle::sendMessage(mavlink_message_t message) -{ - emit _sendMessageOnThread(message); -} - bool Vehicle::sendMessageOnLink(LinkInterface* link, mavlink_message_t message) { if (!link || !_links.contains(link) || !link->isConnected()) { @@ -804,16 +798,6 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message) emit messagesSentChanged(); } -void Vehicle::_sendMessage(mavlink_message_t message) -{ - // Emit message on all links that are currently connected - foreach (LinkInterface* link, _links) { - if (link->isConnected()) { - _sendMessageOnLink(link, message); - } - } -} - /// @return Direct usb connection link to board if one, NULL if none LinkInterface* Vehicle::priorityLink(void) { @@ -1165,11 +1149,11 @@ void Vehicle::setArmed(bool armed) cmd.param6 = 0.0f; cmd.param7 = 0.0f; cmd.target_system = id(); - cmd.target_component = 0; + cmd.target_component = defaultComponentId(); mavlink_msg_command_long_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &cmd); - sendMessage(msg); + sendMessageOnPriorityLink(msg); } bool Vehicle::flightModeSetAvailable(void) @@ -1200,7 +1184,7 @@ void Vehicle::setFlightMode(const QString& flightMode) mavlink_message_t msg; mavlink_msg_set_mode_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, id(), newBaseMode, custom_mode); - sendMessage(msg); + sendMessageOnPriorityLink(msg); } else { qWarning() << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode; } @@ -1221,7 +1205,7 @@ void Vehicle::setHilMode(bool hilMode) } mavlink_msg_set_mode_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, id(), newBaseMode, _custom_mode); - sendMessage(msg); + sendMessageOnPriorityLink(msg); } bool Vehicle::missingParameters(void) @@ -1238,7 +1222,7 @@ void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool send dataStream.req_message_rate = rate; dataStream.start_stop = 1; // start dataStream.target_system = id(); - dataStream.target_component = 0; + dataStream.target_component = defaultComponentId(); mavlink_msg_request_data_stream_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &dataStream); @@ -1246,7 +1230,7 @@ void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool send // We use sendMessageMultiple since we really want these to make it to the vehicle sendMessageMultiple(msg); } else { - sendMessage(msg); + sendMessageOnPriorityLink(msg); } } @@ -1255,7 +1239,7 @@ void Vehicle::_sendMessageMultipleNext(void) if (_nextSendMessageMultipleIndex < _sendMessageMultipleList.count()) { qCDebug(VehicleLog) << "_sendMessageMultipleNext:" << _sendMessageMultipleList[_nextSendMessageMultipleIndex].message.msgid; - sendMessage(_sendMessageMultipleList[_nextSendMessageMultipleIndex].message); + sendMessageOnPriorityLink(_sendMessageMultipleList[_nextSendMessageMultipleIndex].message); if (--_sendMessageMultipleList[_nextSendMessageMultipleIndex].retryCount <= 0) { _sendMessageMultipleList.removeAt(_nextSendMessageMultipleIndex); @@ -1613,10 +1597,10 @@ void Vehicle::emergencyStop(void) cmd.param6 = 0.0f; cmd.param7 = 0.0f; cmd.target_system = id(); - cmd.target_component = 0; + cmd.target_component = defaultComponentId(); mavlink_msg_command_long_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &cmd); - sendMessage(msg); + sendMessageOnPriorityLink(msg); } void Vehicle::setCurrentMissionSequence(int seq) @@ -1626,7 +1610,7 @@ void Vehicle::setCurrentMissionSequence(int seq) } mavlink_message_t msg; mavlink_msg_mission_set_current_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, id(), _compID, seq); - sendMessage(msg); + sendMessageOnPriorityLink(msg); } void Vehicle::doCommandLong(int component, MAV_CMD command, float param1, float param2, float param3, float param4, float param5, float param6, float param7) @@ -1647,7 +1631,7 @@ void Vehicle::doCommandLong(int component, MAV_CMD command, float param1, float cmd.target_component = component; mavlink_msg_command_long_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &cmd); - sendMessage(msg); + sendMessageOnPriorityLink(msg); } void Vehicle::setPrearmError(const QString& prearmError) @@ -1695,7 +1679,7 @@ QString Vehicle::firmwareVersionTypeString(void) const void Vehicle::rebootVehicle() { - doCommandLong(id(), MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); + doCommandLong(defaultComponentId(), MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); } int Vehicle::defaultComponentId(void) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 087c0bbf5..3897c750a 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -400,9 +400,6 @@ public: /// Returns the highest quality link available to the Vehicle LinkInterface* priorityLink(void); - /// Sends a message to all links accociated with this vehicle - void sendMessage(mavlink_message_t message); - /// Sends a message to the specified link /// @return true: message sent, false: Link no longer connected bool sendMessageOnLink(LinkInterface* link, mavlink_message_t message); @@ -563,7 +560,6 @@ signals: void messagesLostChanged (); /// Used internally to move sendMessage call to main thread - void _sendMessageOnThread(mavlink_message_t message); void _sendMessageOnLinkOnThread(LinkInterface* link, mavlink_message_t message); void messageTypeChanged (); @@ -599,7 +595,6 @@ signals: private slots: void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); void _linkInactiveOrDeleted(LinkInterface* link); - void _sendMessage(mavlink_message_t message); void _sendMessageOnLink(LinkInterface* link, mavlink_message_t message); void _sendMessageMultipleNext(void); void _addNewMapTrajectoryPoint(void); diff --git a/src/ViewWidgets/LogDownloadController.cc b/src/ViewWidgets/LogDownloadController.cc index 2ae91ce26..f11c2c4ff 100644 --- a/src/ViewWidgets/LogDownloadController.cc +++ b/src/ViewWidgets/LogDownloadController.cc @@ -448,7 +448,7 @@ LogDownloadController::_requestLogData(uint8_t id, uint32_t offset, uint32_t cou qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &msg, - qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), MAV_COMP_ID_ALL, + qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->defaultComponentId(), id, offset, count); _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } @@ -476,7 +476,7 @@ LogDownloadController::_requestLogList(uint32_t start, uint32_t end) qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &msg, _vehicle->id(), - MAV_COMP_ID_ALL, + _vehicle->defaultComponentId(), start, end); _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); @@ -621,7 +621,7 @@ LogDownloadController::eraseAll(void) qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &msg, - qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), MAV_COMP_ID_ALL); + qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->defaultComponentId()); _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); refresh(); } diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 2d1d04ff8..9c7c60639 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -60,7 +60,7 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app) , m_actionGuardEnabled(false) , m_actionRetransmissionTimeout(100) , versionMismatchIgnore(false) - , systemId(QGC::defaultSystemId) + , systemId(255) #ifndef __mobile__ , _logSuspendError(false) , _logSuspendReplay(false) @@ -422,7 +422,7 @@ void MAVLinkProtocol::setSystemId(int id) /** @return Component id of this application */ int MAVLinkProtocol::getComponentId() { - return QGC::defaultComponentId; + return 0; } /** diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 787d4c07e..eeb5c9334 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1143,7 +1143,7 @@ void UAS::requestImage() { mavlink_message_t msg; mavlink_msg_data_transmission_handshake_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, MAVLINK_DATA_STREAM_IMG_JPEG, 0, 0, 0, 0, 0, 50); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnPriorityLink(msg); } } @@ -1242,7 +1242,7 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float cmd.target_system = uasId; cmd.target_component = component; mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnPriorityLink(msg); } /** @@ -1425,7 +1425,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons); } - _vehicle->sendMessage(message); + _vehicle->sendMessageOnPriorityLink(message); // Emit an update in control values to other UI elements, like the HSI display emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); } @@ -1452,15 +1452,15 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll quint8 mask = (1 << 0) | (1 << 1) | (1 << 2); // ignore rates mask |= (1 << 6); // ignore throttle mavlink_msg_set_attitude_target_pack(mavlink->getSystemId(), mavlink->getComponentId(), - &message, QGC::groundTimeMilliseconds(), this->uasId, 0, + &message, QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(), mask, q, 0, 0, 0, 0); - _vehicle->sendMessage(message); + _vehicle->sendMessageOnPriorityLink(message); quint16 position_mask = (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7) | (1 << 8); mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), mavlink->getComponentId(), - &message, QGC::groundTimeMilliseconds(), this->uasId, 0, + &message, QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(), MAV_FRAME_LOCAL_NED, position_mask, x, y, z, 0, 0, 0, 0, 0, 0, yaw, yawrate); - _vehicle->sendMessage(message); + _vehicle->sendMessageOnPriorityLink(message); qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGES: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw; //emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); @@ -1483,8 +1483,8 @@ void UAS::pairRX(int rxType, int rxSubType) mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_START_RX_PAIR, 0, rxType, rxSubType, 0, 0, 0, 0, 0); - _vehicle->sendMessage(msg); + mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, _vehicle->defaultComponentId(), MAV_CMD_START_RX_PAIR, 0, rxType, rxSubType, 0, 0, 0, 0, 0); + _vehicle->sendMessageOnPriorityLink(msg); } /** @@ -1707,7 +1707,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa mavlink_msg_hil_state_quaternion_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, q, rollspeed, pitchspeed, yawspeed, lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, ind_airspeed*100, true_airspeed*100, xacc*1000/9.81, yacc*1000/9.81, zacc*1000/9.81); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnPriorityLink(msg); } else { @@ -1785,7 +1785,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl time_us, xacc_corrupt, yacc_corrupt, zacc_corrupt, rollspeed_corrupt, pitchspeed_corrupt, yawspeed_corrupt, xmag_corrupt, ymag_corrupt, zmag_corrupt, abs_pressure_corrupt, diff_pressure_corrupt, pressure_alt_corrupt, temperature_corrupt, fields_changed); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnPriorityLink(msg); lastSendTimeSensors = QGC::groundTimeMilliseconds(); } else @@ -1822,7 +1822,7 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa mavlink_msg_hil_optical_flow_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, 0, 0 /* hack */, flow_x, flow_y, 0.0f /* hack */, 0.0f /* hack */, 0.0f /* hack */, 0 /* hack */, quality, ground_distance); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnPriorityLink(msg); lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds(); #endif } @@ -1860,7 +1860,7 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi mavlink_msg_hil_gps_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, vn*1e2, ve*1e2, vd*1e2, course*1e2, satellites); lastSendTimeGPS = QGC::groundTimeMilliseconds(); - _vehicle->sendMessage(msg); + _vehicle->sendMessageOnPriorityLink(msg); } else { @@ -1933,7 +1933,7 @@ void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 p mavlink->getComponentId(), &message, this->uasId, - 0, + _vehicle->defaultComponentId(), param_id_cstr, -1, param_rc_channel_index, @@ -1941,7 +1941,7 @@ void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 p scale, valueMin, valueMax); - _vehicle->sendMessage(message); + _vehicle->sendMessageOnPriorityLink(message); //qDebug() << "Mavlink message sent"; } @@ -1959,7 +1959,7 @@ void UAS::unsetRCToParameterMap() mavlink->getComponentId(), &message, this->uasId, - 0, + _vehicle->defaultComponentId(), param_id_cstr, -2, i, @@ -1967,7 +1967,7 @@ void UAS::unsetRCToParameterMap() 0.0f, 0.0f, 0.0f); - _vehicle->sendMessage(message); + _vehicle->sendMessageOnPriorityLink(message); } } -- 2.22.0