From e7979fd93f75ebe6462d8e8724a0af20373b1672 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 2 Oct 2016 18:35:11 -0700 Subject: [PATCH] Must use pack_chan and encode_chan due to mav 2.0 problem _finalize methods need information which is not available if all you have is a mavlink_message_t --- .../APM/APMSensorsComponentController.cc | 8 +- .../Common/ESP8266ComponentController.cc | 7 +- src/FactSystem/ParameterManager.cc | 67 ++- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 24 +- src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 4 +- src/FirmwarePlugin/APM/APMGeoFenceManager.cc | 36 +- .../APM/APMRallyPointManager.cc | 40 +- .../APM/ArduCopterFirmwarePlugin.cc | 16 +- src/FirmwarePlugin/FirmwarePlugin.cc | 3 +- src/FirmwarePlugin/FirmwarePlugin.h | 3 +- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 40 +- src/FollowMe/FollowMe.cc | 11 +- src/GPS/RTCM/RTCMMavlink.cc | 9 +- src/MissionManager/MissionManager.cc | 42 +- src/QmlControls/QGroundControlQmlGlobal.cc | 6 - src/QmlControls/QGroundControlQmlGlobal.h | 3 - src/Vehicle/MultiVehicleManager.cc | 19 +- src/Vehicle/Vehicle.cc | 75 +++- src/Vehicle/Vehicle.h | 4 - src/ViewWidgets/LogDownloadController.cc | 41 +- src/comm/LinkInterface.h | 18 +- src/comm/LinkManager.cc | 2 +- src/comm/LogReplayLink.cc | 2 +- src/comm/MAVLinkProtocol.cc | 155 +------ src/comm/MAVLinkProtocol.h | 70 +-- src/comm/MockLink.cc | 2 +- src/uas/FileManager.cc | 15 +- src/uas/UAS.cc | 422 ++++++++++-------- src/ui/QGCMAVLinkInspector.cc | 80 ---- src/ui/QGCMAVLinkInspector.h | 5 - src/ui/QGCMAVLinkInspector.ui | 41 +- src/ui/preferences/MavlinkSettings.qml | 9 - 32 files changed, 567 insertions(+), 712 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index 4b7510a50..c88766304 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc @@ -468,9 +468,13 @@ void APMSensorsComponentController::nextClicked(void) ack.command = 0; ack.result = 1; - mavlink_msg_command_ack_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &msg, &ack); + mavlink_msg_command_ack_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + &ack); - _vehicle->sendMessageOnPriorityLink(msg); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); if (_compassMotCalInProgress) { _stopCalibration(StopCalibrationSuccess); diff --git a/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc b/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc index 5e6ad14c5..bfe76f384 100644 --- a/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc +++ b/src/AutoPilotPlugins/Common/ESP8266ComponentController.cc @@ -318,9 +318,11 @@ void ESP8266ComponentController::_reboot() { mavlink_message_t msg; - mavlink_msg_command_long_pack( + + mavlink_msg_command_long_pack_chan( qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), &msg, _vehicle->id(), MAV_COMP_ID_UDP_BRIDGE, @@ -339,9 +341,10 @@ void ESP8266ComponentController::_restoreDefaults() { mavlink_message_t msg; - mavlink_msg_command_long_pack( + mavlink_msg_command_long_pack_chan( qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), &msg, _vehicle->id(), MAV_COMP_ID_UDP_BRIDGE, diff --git a/src/FactSystem/ParameterManager.cc b/src/FactSystem/ParameterManager.cc index a7c45a39c..f286e54eb 100644 --- a/src/FactSystem/ParameterManager.cc +++ b/src/FactSystem/ParameterManager.cc @@ -395,7 +395,12 @@ void ParameterManager::refreshAllParameters(uint8_t componentID) Q_ASSERT(mavlink); mavlink_message_t msg; - mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), componentID); + mavlink_msg_param_request_list_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + _vehicle->id(), + componentID); _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); QString what = (componentID == MAV_COMP_ID_ALL) ? "MAV_COMP_ID_ALL" : QString::number(componentID); @@ -618,13 +623,14 @@ void ParameterManager::_readParameterRaw(int componentId, const QString& paramNa char fixedParamName[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN]; strncpy(fixedParamName, paramName.toStdString().c_str(), sizeof(fixedParamName)); - mavlink_msg_param_request_read_pack(_mavlink->getSystemId(), // Our system id - _mavlink->getComponentId(), // Our component id - &msg, // Pack into this mavlink_message_t - _vehicle->id(), // Target system id - componentId, // Target component id - fixedParamName, // Named parameter being requested - paramIndex); // Parameter index being requested, -1 for named + mavlink_msg_param_request_read_pack_chan(_mavlink->getSystemId(), // QGC system id + _mavlink->getComponentId(), // QGC component id + _vehicle->priorityLink()->mavlinkChannel(), + &msg, // Pack into this mavlink_message_t + _vehicle->id(), // Target system id + componentId, // Target component id + fixedParamName, // Named parameter being requested + paramIndex); // Parameter index being requested, -1 for named _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } @@ -677,7 +683,11 @@ void ParameterManager::_writeParameterRaw(int componentId, const QString& paramN strncpy(p.param_id, paramName.toStdString().c_str(), sizeof(p.param_id)); mavlink_message_t msg; - mavlink_msg_param_set_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &p); + mavlink_msg_param_set_encode_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + &p); _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } @@ -756,7 +766,11 @@ void ParameterManager::_tryCacheHashLoad(int vehicleId, int componentId, QVarian p.target_system = (uint8_t)_vehicle->id(); p.target_component = (uint8_t)componentId; mavlink_message_t msg; - mavlink_msg_param_set_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &p); + mavlink_msg_param_set_encode_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + &p); _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); // Give the user some feedback things loaded properly @@ -787,7 +801,13 @@ void ParameterManager::_saveToEEPROM(void) _saveRequired = false; if (_vehicle->firmwarePlugin()->isCapable(_vehicle, FirmwarePlugin::MavCmdPreflightStorageCapability)) { mavlink_message_t msg; - mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _vehicle->id(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0); + mavlink_msg_command_long_pack_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + _vehicle->id(), + 0, + MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0); _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); qCDebug(ParameterManagerLog) << "_saveToEEPROM"; } else { @@ -1409,17 +1429,18 @@ void ParameterManager::resetAllParametersToDefaults(void) mavlink_message_t msg; MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - 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); + mavlink_msg_command_long_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &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->sendMessageOnLink(_vehicle->priorityLink(), msg); } diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index f5f05aa3d..753aebc40 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -251,10 +251,14 @@ void APMFirmwarePlugin::_handleParamValue(Vehicle* vehicle, mavlink_message_t* m paramValue.param_value = paramUnion.param_float; - mavlink_msg_param_value_encode(message->sysid, message->compid, message, ¶mValue); + mavlink_msg_param_value_encode_chan(message->sysid, + message->compid, + vehicle->priorityLink()->mavlinkChannel(), + message, + ¶mValue); } -void APMFirmwarePlugin::_handleParamSet(Vehicle* vehicle, mavlink_message_t* message) +void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) { Q_UNUSED(vehicle); @@ -294,7 +298,7 @@ void APMFirmwarePlugin::_handleParamSet(Vehicle* vehicle, mavlink_message_t* mes qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type; } - mavlink_msg_param_set_encode(message->sysid, message->compid, message, ¶mSet); + mavlink_msg_param_set_encode_chan(message->sysid, message->compid, outgoingLink->mavlinkChannel(), message, ¶mSet); } bool APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* message) @@ -447,7 +451,7 @@ bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_m return true; } -void APMFirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) +void APMFirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) { //-- Don't process messages to/from UDP Bridge. It doesn't suffer from these issues if (message->compid == MAV_COMP_ID_UDP_BRIDGE) { @@ -456,7 +460,7 @@ void APMFirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_m switch (message->msgid) { case MAVLINK_MSG_ID_PARAM_SET: - _handleParamSet(vehicle, message); + _handleOutgoingParamSet(vehicle, outgoingLink, message); break; } } @@ -518,7 +522,10 @@ void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const break; } - mavlink_msg_statustext_encode(message->sysid, message->compid, message, &statusText); + mavlink_msg_statustext_encode(message->sysid, + message->compid, + message, + &statusText); } void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message) const @@ -527,7 +534,10 @@ void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message) const mavlink_msg_statustext_decode(message, &statusText); statusText.severity = MAV_SEVERITY_INFO; - mavlink_msg_statustext_encode(message->sysid, message->compid, message, &statusText); + mavlink_msg_statustext_encode(message->sysid, + message->compid, + message, + &statusText); } void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* message) const diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index a15051ca6..8f79d543f 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -83,7 +83,7 @@ public: void pauseVehicle (Vehicle* vehicle); int manualControlReservedButtonCount(void); bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) final; - void adjustOutgoingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message) final; + void adjustOutgoingMavlinkMessage (Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) final; void initializeVehicle (Vehicle* vehicle) final; bool sendHomePositionToVehicle (void) final; void addMetaDataToFact (QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) final; @@ -115,7 +115,7 @@ private: void _setInfoSeverity(mavlink_message_t* message) const; QString _getMessageText(mavlink_message_t* message) const; void _handleParamValue(Vehicle* vehicle, mavlink_message_t* message); - void _handleParamSet(Vehicle* vehicle, mavlink_message_t* message); + void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); bool _handleStatusText(Vehicle* vehicle, mavlink_message_t* message); void _handleHeartbeat(Vehicle* vehicle, mavlink_message_t* message); void _soloVideoHandshake(Vehicle* vehicle); diff --git a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc index 18753af2d..7b019f73b 100644 --- a/src/FirmwarePlugin/APM/APMGeoFenceManager.cc +++ b/src/FirmwarePlugin/APM/APMGeoFenceManager.cc @@ -178,13 +178,14 @@ void APMGeoFenceManager::_requestFencePoint(uint8_t pointIndex) MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); qCDebug(GeoFenceManagerLog) << "APMGeoFenceManager::_requestFencePoint" << pointIndex; - mavlink_msg_fence_fetch_point_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - _vehicle->id(), - _vehicle->defaultComponentId(), - pointIndex); - _vehicle->sendMessageOnPriorityLink(msg); + mavlink_msg_fence_fetch_point_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + _vehicle->id(), + _vehicle->defaultComponentId(), + pointIndex); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } void APMGeoFenceManager::_sendFencePoint(uint8_t pointIndex) @@ -206,16 +207,17 @@ void APMGeoFenceManager::_sendFencePoint(uint8_t pointIndex) // Total point count, +1 polygon close in last index, +1 for breach in index 0 uint8_t totalPointCount = _polygon.count() + 1 + 1; - mavlink_msg_fence_point_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - _vehicle->id(), - _vehicle->defaultComponentId(), - pointIndex, // Index of point to set - totalPointCount, - fenceCoord.latitude(), - fenceCoord.longitude()); - _vehicle->sendMessageOnPriorityLink(msg); + mavlink_msg_fence_point_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + _vehicle->id(), + _vehicle->defaultComponentId(), + pointIndex, // Index of point to set + totalPointCount, + fenceCoord.latitude(), + fenceCoord.longitude()); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } bool APMGeoFenceManager::inProgress(void) const diff --git a/src/FirmwarePlugin/APM/APMRallyPointManager.cc b/src/FirmwarePlugin/APM/APMRallyPointManager.cc index be39947f9..8061f8e57 100644 --- a/src/FirmwarePlugin/APM/APMRallyPointManager.cc +++ b/src/FirmwarePlugin/APM/APMRallyPointManager.cc @@ -109,13 +109,14 @@ void APMRallyPointManager::_requestRallyPoint(uint8_t pointIndex) MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); qCDebug(RallyPointManagerLog) << "APMRallyPointManager::_requestRallyPoint" << pointIndex; - mavlink_msg_rally_fetch_point_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - _vehicle->id(), - _vehicle->defaultComponentId(), - pointIndex); - _vehicle->sendMessageOnPriorityLink(msg); + mavlink_msg_rally_fetch_point_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + _vehicle->id(), + _vehicle->defaultComponentId(), + pointIndex); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } void APMRallyPointManager::_sendRallyPoint(uint8_t pointIndex) @@ -124,18 +125,19 @@ void APMRallyPointManager::_sendRallyPoint(uint8_t pointIndex) MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); QGeoCoordinate point = _rgPoints[pointIndex]; - mavlink_msg_rally_point_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - _vehicle->id(), - _vehicle->defaultComponentId(), - pointIndex, - _rgPoints.count(), - point.latitude() * 1e7, - point.longitude() * 1e7, - point.altitude(), - 0, 0, 0); // break_alt, land_dir, flags - _vehicle->sendMessageOnPriorityLink(msg); + mavlink_msg_rally_point_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + _vehicle->id(), + _vehicle->defaultComponentId(), + pointIndex, + _rgPoints.count(), + point.latitude() * 1e7, + point.longitude() * 1e7, + point.altitude(), + 0, 0, 0); // break_alt, land_dir, flags + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } bool APMRallyPointManager::inProgress(void) const diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index d1420de92..d9bc3b4e8 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -141,9 +141,13 @@ void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitu cmd.target_component = vehicle->defaultComponentId(); MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + vehicle->priorityLink()->mavlinkChannel(), + &msg, + &cmd); - vehicle->sendMessageOnPriorityLink(msg); + vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); } void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) @@ -179,9 +183,13 @@ void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double cmd.z = -(altitudeRel - vehicle->altitudeRelative()->rawValue().toDouble()); MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_msg_set_position_target_local_ned_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + mavlink_msg_set_position_target_local_ned_encode_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + vehicle->priorityLink()->mavlinkChannel(), + &msg, + &cmd); - vehicle->sendMessageOnPriorityLink(msg); + vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); } bool ArduCopterFirmwarePlugin::isPaused(const Vehicle* vehicle) const diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 4e71b0200..bbc330969 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -112,9 +112,10 @@ bool FirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_mess return true; } -void FirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) +void FirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message) { Q_UNUSED(vehicle); + Q_UNUSED(outgoingLink); Q_UNUSED(message); // Generic plugin does no message adjustment } diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index bc464e635..935cf1de8 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -159,8 +159,9 @@ public: /// This is handy to adjust or differences in mavlink spec implementations such that the base code can remain /// mavlink generic. /// @param vehicle Vehicle message came from + /// @param outgoingLink Link that messae is going out on /// @param message[in,out] Mavlink message to adjust if needed. - virtual void adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message); + virtual void adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); /// Determines how to handle the first item of the mission item list. Internally to QGC the first item /// is always the home position. diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 4385d2e9f..934774eef 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -283,9 +283,13 @@ void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle) cmd.target_component = vehicle->defaultComponentId(); MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + vehicle->priorityLink()->mavlinkChannel(), + &msg, + &cmd); - vehicle->sendMessageOnPriorityLink(msg); + vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); } void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle) @@ -317,8 +321,12 @@ void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& cmd.param7 = centerCoord.isValid() ? centerCoord.altitude() : NAN; cmd.target_system = vehicle->id(); cmd.target_component = vehicle->defaultComponentId(); - mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - vehicle->sendMessageOnPriorityLink(msg); + mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + vehicle->priorityLink()->mavlinkChannel(), + &msg, + &cmd); + vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); } void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) @@ -347,8 +355,12 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) cmd.target_system = vehicle->id(); cmd.target_component = vehicle->defaultComponentId(); - mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - vehicle->sendMessageOnPriorityLink(msg); + mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + vehicle->priorityLink()->mavlinkChannel(), + &msg, + &cmd); + vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); } void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) @@ -374,9 +386,13 @@ void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord cmd.target_component = vehicle->defaultComponentId(); MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + vehicle->priorityLink()->mavlinkChannel(), + &msg, + &cmd); - vehicle->sendMessageOnPriorityLink(msg); + vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); } void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) @@ -402,9 +418,13 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu cmd.target_component = vehicle->defaultComponentId(); MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + vehicle->priorityLink()->mavlinkChannel(), + &msg, + &cmd); - vehicle->sendMessageOnPriorityLink(msg); + vehicle->sendMessageOnLink(vehicle->priorityLink(), msg); } void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode) diff --git a/src/FollowMe/FollowMe.cc b/src/FollowMe/FollowMe.cc index f5bcb4533..196e3223e 100644 --- a/src/FollowMe/FollowMe.cc +++ b/src/FollowMe/FollowMe.cc @@ -140,11 +140,12 @@ void FollowMe::_sendGCSMotionReport(void) Vehicle* vehicle = qobject_cast(vehicles[i]); if(vehicle->flightMode().compare(PX4FirmwarePlugin::followMeFlightMode, Qt::CaseInsensitive) == 0) { mavlink_message_t message; - mavlink_msg_follow_target_encode(mavlinkProtocol->getSystemId(), - mavlinkProtocol->getComponentId(), - &message, - &follow_target); - vehicle->sendMessageOnPriorityLink(message); + mavlink_msg_follow_target_encode_chan(mavlinkProtocol->getSystemId(), + mavlinkProtocol->getComponentId(), + vehicle->priorityLink()->mavlinkChannel(), + &message, + &follow_target); + vehicle->sendMessageOnLink(vehicle->priorityLink(), message); } } } diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc index 587fc3fa3..d5586fc1d 100644 --- a/src/GPS/RTCM/RTCMMavlink.cc +++ b/src/GPS/RTCM/RTCMMavlink.cc @@ -61,8 +61,11 @@ void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg) for (int i = 0; i < vehicles.count(); i++) { Vehicle* vehicle = qobject_cast(vehicles[i]); mavlink_message_t message; - mavlink_msg_gps_rtcm_data_encode(mavlinkProtocol->getSystemId(), - mavlinkProtocol->getComponentId(), &message, &msg); - vehicle->sendMessageOnPriorityLink(message); + mavlink_msg_gps_rtcm_data_encode_chan(mavlinkProtocol->getSystemId(), + mavlinkProtocol->getComponentId(), + vehicle->priorityLink()->mavlinkChannel(), + &message, + &msg); + vehicle->sendMessageOnLink(vehicle->priorityLink(), message); } } diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 3131a28a9..fa0ce387b 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -90,9 +90,13 @@ void MissionManager::writeMissionItems(const QList& missionItems) missionCount.target_component = MAV_COMP_ID_MISSIONPLANNER; missionCount.count = _missionItems.count(); - mavlink_msg_mission_count_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionCount); - _dedicatedLink = _vehicle->priorityLink(); + mavlink_msg_mission_count_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + &missionCount); + _vehicle->sendMessageOnLink(_dedicatedLink, message); _startAckTimeout(AckMissionRequest); emit inProgressChanged(true); @@ -125,9 +129,13 @@ void MissionManager::writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoC missionItem.current = altChangeOnly ? 3 : 2; missionItem.autocontinue = true; - mavlink_msg_mission_item_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &messageOut, &missionItem); - _dedicatedLink = _vehicle->priorityLink(); + mavlink_msg_mission_item_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &messageOut, + &missionItem); + _vehicle->sendMessageOnLink(_dedicatedLink, messageOut); _startAckTimeout(AckGuidedItem); emit inProgressChanged(true); @@ -157,9 +165,13 @@ void MissionManager::requestMissionItems(void) request.target_system = _vehicle->id(); request.target_component = MAV_COMP_ID_MISSIONPLANNER; - mavlink_msg_mission_request_list_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &request); - _dedicatedLink = _vehicle->priorityLink(); + mavlink_msg_mission_request_list_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + &request); + _vehicle->sendMessageOnLink(_dedicatedLink, message); _startAckTimeout(AckMissionCount); emit inProgressChanged(true); @@ -223,7 +235,11 @@ void MissionManager::_readTransactionComplete(void) missionAck.target_component = MAV_COMP_ID_MISSIONPLANNER; missionAck.type = MAV_MISSION_ACCEPTED; - mavlink_msg_mission_ack_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionAck); + mavlink_msg_mission_ack_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + &missionAck); _vehicle->sendMessageOnLink(_dedicatedLink, message); @@ -269,7 +285,11 @@ void MissionManager::_requestNextMissionItem(void) missionRequest.target_component = MAV_COMP_ID_MISSIONPLANNER; missionRequest.seq = _itemIndicesToRead[0]; - mavlink_msg_mission_request_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionRequest); + mavlink_msg_mission_request_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &message, + &missionRequest); _vehicle->sendMessageOnLink(_dedicatedLink, message); _startAckTimeout(AckMissionItem); @@ -377,7 +397,11 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message) missionItem.current = missionRequest.seq == 0; missionItem.autocontinue = item->autoContinue(); - mavlink_msg_mission_item_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &messageOut, &missionItem); + mavlink_msg_mission_item_encode_chan(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _dedicatedLink->mavlinkChannel(), + &messageOut, + &missionItem); _vehicle->sendMessageOnLink(_dedicatedLink, messageOut); _startAckTimeout(AckMissionRequest); diff --git a/src/QmlControls/QGroundControlQmlGlobal.cc b/src/QmlControls/QGroundControlQmlGlobal.cc index b48247b45..abd618bc3 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.cc +++ b/src/QmlControls/QGroundControlQmlGlobal.cc @@ -188,12 +188,6 @@ void QGroundControlQmlGlobal::setIsSaveLogPromptNotArmed(bool prompt) emit isSaveLogPromptNotArmedChanged(prompt); } -void QGroundControlQmlGlobal::setIsMultiplexingEnabled(bool enable) -{ - qgcApp()->toolbox()->mavlinkProtocol()->enableMultiplexing(enable); - emit isMultiplexingEnabledChanged(enable); -} - void QGroundControlQmlGlobal::setIsVersionCheckEnabled(bool enable) { qgcApp()->toolbox()->mavlinkProtocol()->enableVersionCheck(enable); diff --git a/src/QmlControls/QGroundControlQmlGlobal.h b/src/QmlControls/QGroundControlQmlGlobal.h index 1f10653ff..691802184 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.h +++ b/src/QmlControls/QGroundControlQmlGlobal.h @@ -87,7 +87,6 @@ public: Q_PROPERTY(qreal baseFontPointSize READ baseFontPointSize WRITE setBaseFontPointSize NOTIFY baseFontPointSizeChanged) // MavLink Protocol - Q_PROPERTY(bool isMultiplexingEnabled READ isMultiplexingEnabled WRITE setIsMultiplexingEnabled NOTIFY isMultiplexingEnabledChanged) Q_PROPERTY(bool isVersionCheckEnabled READ isVersionCheckEnabled WRITE setIsVersionCheckEnabled NOTIFY isVersionCheckEnabledChanged) Q_PROPERTY(int mavlinkSystemID READ mavlinkSystemID WRITE setMavlinkSystemID NOTIFY mavlinkSystemIDChanged) @@ -179,7 +178,6 @@ public: bool virtualTabletJoystick () { return _virtualTabletJoystick; } qreal baseFontPointSize () { return _baseFontPointSize; } - bool isMultiplexingEnabled () { return _toolbox->mavlinkProtocol()->multiplexingEnabled(); } bool isVersionCheckEnabled () { return _toolbox->mavlinkProtocol()->versionCheckEnabled(); } int mavlinkSystemID () { return _toolbox->mavlinkProtocol()->getSystemId(); } @@ -204,7 +202,6 @@ public: void setVirtualTabletJoystick (bool enabled); void setBaseFontPointSize (qreal size); - void setIsMultiplexingEnabled (bool enable); void setIsVersionCheckEnabled (bool enable); void setMavlinkSystemID (int id); diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index b87931117..139955c62 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -307,15 +307,16 @@ void MultiVehicleManager::_sendGCSHeartbeat(void) Vehicle* vehicle = qobject_cast(_vehicles[i]); mavlink_message_t message; - mavlink_msg_heartbeat_pack(_mavlinkProtocol->getSystemId(), - _mavlinkProtocol->getComponentId(), - &message, - 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->sendMessageOnPriorityLink(message); + mavlink_msg_heartbeat_pack_chan(_mavlinkProtocol->getSystemId(), + _mavlinkProtocol->getComponentId(), + vehicle->priorityLink()->mavlinkChannel(), + &message, + 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->sendMessageOnLink(vehicle->priorityLink(), message); } } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index f5e5c13bd..7afa35f0c 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -221,7 +221,11 @@ Vehicle::Vehicle(LinkInterface* link, versionCmd.param2 = versionCmd.param3 = versionCmd.param4 = versionCmd.param5 = versionCmd.param6 = versionCmd.param7 = 0; versionCmd.target_system = id(); versionCmd.target_component = MAV_COMP_ID_ALL; - mavlink_msg_command_long_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &versionMsg, &versionCmd); + mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &versionMsg, + &versionCmd); sendMessageMultiple(versionMsg); _firmwarePlugin->initializeVehicle(this); @@ -874,10 +878,7 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message) } // Give the plugin a chance to adjust - _firmwarePlugin->adjustOutgoingMavlinkMessage(this, &message); - - static const uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS; - mavlink_finalize_message_chan(&message, _mavlink->getSystemId(), _mavlink->getComponentId(), link->getMavlinkChannel(), message.len, message.len, messageKeys[message.msgid]); + _firmwarePlugin->adjustOutgoingMavlinkMessage(this, link, &message); // Write message into buffer, prepending start sign uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; @@ -1277,9 +1278,13 @@ void Vehicle::setArmed(bool armed) cmd.target_system = id(); cmd.target_component = defaultComponentId(); - mavlink_msg_command_long_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &cmd); + mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + &cmd); - sendMessageOnPriorityLink(msg); + sendMessageOnLink(priorityLink(), msg); } bool Vehicle::flightModeSetAvailable(void) @@ -1309,8 +1314,14 @@ void Vehicle::setFlightMode(const QString& flightMode) newBaseMode |= base_mode; mavlink_message_t msg; - mavlink_msg_set_mode_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, id(), newBaseMode, custom_mode); - sendMessageOnPriorityLink(msg); + mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + id(), + newBaseMode, + custom_mode); + sendMessageOnLink(priorityLink(), msg); } else { qWarning() << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode; } @@ -1330,8 +1341,14 @@ void Vehicle::setHilMode(bool hilMode) newBaseMode |= MAV_MODE_FLAG_HIL_ENABLED; } - mavlink_msg_set_mode_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, id(), newBaseMode, _custom_mode); - sendMessageOnPriorityLink(msg); + mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + id(), + newBaseMode, + _custom_mode); + sendMessageOnLink(priorityLink(), msg); } void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple) @@ -1345,13 +1362,17 @@ void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool send dataStream.target_system = id(); dataStream.target_component = defaultComponentId(); - mavlink_msg_request_data_stream_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &dataStream); + mavlink_msg_request_data_stream_encode_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + &dataStream); if (sendMultiple) { // We use sendMessageMultiple since we really want these to make it to the vehicle sendMessageMultiple(msg); } else { - sendMessageOnPriorityLink(msg); + sendMessageOnLink(priorityLink(), msg); } } @@ -1360,7 +1381,7 @@ void Vehicle::_sendMessageMultipleNext(void) if (_nextSendMessageMultipleIndex < _sendMessageMultipleList.count()) { qCDebug(VehicleLog) << "_sendMessageMultipleNext:" << _sendMessageMultipleList[_nextSendMessageMultipleIndex].message.msgid; - sendMessageOnPriorityLink(_sendMessageMultipleList[_nextSendMessageMultipleIndex].message); + sendMessageOnLink(priorityLink(), _sendMessageMultipleList[_nextSendMessageMultipleIndex].message); if (--_sendMessageMultipleList[_nextSendMessageMultipleIndex].retryCount <= 0) { _sendMessageMultipleList.removeAt(_nextSendMessageMultipleIndex); @@ -1764,9 +1785,13 @@ void Vehicle::emergencyStop(void) cmd.param7 = 0.0f; cmd.target_system = id(); cmd.target_component = defaultComponentId(); - mavlink_msg_command_long_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &cmd); + mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + &cmd); - sendMessageOnPriorityLink(msg); + sendMessageOnLink(priorityLink(), msg); } void Vehicle::setCurrentMissionSequence(int seq) @@ -1775,8 +1800,14 @@ void Vehicle::setCurrentMissionSequence(int seq) seq--; } mavlink_message_t msg; - mavlink_msg_mission_set_current_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, id(), _compID, seq); - sendMessageOnPriorityLink(msg); + mavlink_msg_mission_set_current_pack_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + id(), + _compID, + seq); + sendMessageOnLink(priorityLink(), msg); } void Vehicle::doCommandLong(int component, MAV_CMD command, float param1, float param2, float param3, float param4, float param5, float param6, float param7) @@ -1795,9 +1826,13 @@ void Vehicle::doCommandLong(int component, MAV_CMD command, float param1, float cmd.param7 = param7; cmd.target_system = id(); cmd.target_component = component; - mavlink_msg_command_long_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &cmd); + mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + &cmd); - sendMessageOnPriorityLink(msg); + sendMessageOnLink(priorityLink(), msg); } void Vehicle::setPrearmError(const QString& prearmError) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index a5f13de86..d88bd0e2e 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -430,10 +430,6 @@ public: /// @return true: message sent, false: Link no longer connected bool sendMessageOnLink(LinkInterface* link, mavlink_message_t message); - /// Sends a message to the priority link - /// @return true: message sent, false: Link no longer connected - bool sendMessageOnPriorityLink(mavlink_message_t message) { return sendMessageOnLink(priorityLink(), message); } - /// Sends the specified messages multiple times to the vehicle in order to attempt to /// guarantee that it makes it to the vehicle. void sendMessageMultiple(mavlink_message_t message); diff --git a/src/ViewWidgets/LogDownloadController.cc b/src/ViewWidgets/LogDownloadController.cc index 1a7fb9622..e00347209 100644 --- a/src/ViewWidgets/LogDownloadController.cc +++ b/src/ViewWidgets/LogDownloadController.cc @@ -458,12 +458,13 @@ LogDownloadController::_requestLogData(uint8_t id, uint32_t offset, uint32_t cou id += _apmOneBased; qCDebug(LogDownloadLog) << "Request log data (id:" << id << "offset:" << offset << "size:" << count << ")"; mavlink_message_t msg; - mavlink_msg_log_request_data_pack( - qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - &msg, - qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->defaultComponentId(), - id, offset, count); + mavlink_msg_log_request_data_pack_chan( + qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->defaultComponentId(), + id, offset, count); _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } } @@ -485,14 +486,15 @@ LogDownloadController::_requestLogList(uint32_t start, uint32_t end) qCDebug(LogDownloadLog) << "Request log entry list (" << start << "through" << end << ")"; _setListing(true); mavlink_message_t msg; - mavlink_msg_log_request_list_pack( - qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - &msg, - _vehicle->id(), - _vehicle->defaultComponentId(), - start, - end); + mavlink_msg_log_request_list_pack_chan( + qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + _vehicle->id(), + _vehicle->defaultComponentId(), + start, + end); _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); //-- Wait 5 seconds before bitching about not getting anything _timer.start(5000); @@ -650,11 +652,12 @@ LogDownloadController::eraseAll(void) { if(_vehicle && _uas) { mavlink_message_t msg; - mavlink_msg_log_erase_pack( - qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), - qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), - &msg, - qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->defaultComponentId()); + mavlink_msg_log_erase_pack_chan( + qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), + qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->defaultComponentId()); _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); refresh(); } diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index ed47e7ab6..ecb894740 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -7,15 +7,6 @@ * ****************************************************************************/ - -/** -* @file -* @brief Brief Description -* -* @author Lorenz Meier -* -*/ - #ifndef _LINKINTERFACE_H_ #define _LINKINTERFACE_H_ @@ -25,6 +16,7 @@ #include #include #include +#include #include "QGCMAVLink.h" @@ -124,7 +116,13 @@ public: /// mavlink channel to use for this link, as used by mavlink_parse_char. The mavlink channel is only /// set into the link when it is added to LinkManager - uint8_t getMavlinkChannel(void) const { Q_ASSERT(_mavlinkChannelSet); return _mavlinkChannel; } + uint8_t mavlinkChannel(void) const + { + if (!_mavlinkChannelSet) { + qWarning() << "Call to LinkInterface::mavlinkChannel with _mavlinkChannelSet == false"; + } + return _mavlinkChannel; + } // These are left unimplemented in order to cause linker errors which indicate incorrect usage of // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager. diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index d24a0ea0a..6719aca21 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -270,7 +270,7 @@ void LinkManager::_deleteLink(LinkInterface* link) } // Free up the mavlink channel associated with this link - _mavlinkChannelsUsedBitMask &= ~(1 << link->getMavlinkChannel()); + _mavlinkChannelsUsedBitMask &= ~(1 << link->mavlinkChannel()); _links.removeOne(link); delete link; diff --git a/src/comm/LogReplayLink.cc b/src/comm/LogReplayLink.cc index 273d35886..4a15dd841 100644 --- a/src/comm/LogReplayLink.cc +++ b/src/comm/LogReplayLink.cc @@ -167,7 +167,7 @@ quint64 LogReplayLink::_seekToNextMavlinkMessage(mavlink_message_t* nextMsg) char nextByte; mavlink_status_t comm; while (_logFile.getChar(&nextByte)) { // Loop over every byte - bool messageFound = mavlink_parse_char(getMavlinkChannel(), nextByte, nextMsg, &comm); + bool messageFound = mavlink_parse_char(mavlinkChannel(), nextByte, nextMsg, &comm); // If we've found a message, jump back to the start of the message, grab the timestamp, // and go back to the end of this file. diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index edce7f308..70b85e8be 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -51,13 +51,7 @@ const char* MAVLinkProtocol::_logFileExtension = "mavlink"; ///< Ext */ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app) : QGCTool(app) - , m_multiplexingEnabled(false) , m_enable_version_check(true) - , m_paramRetransmissionTimeout(350) - , m_paramRewriteTimeout(500) - , m_paramGuardEnabled(true) - , m_actionGuardEnabled(false) - , m_actionRetransmissionTimeout(100) , versionMismatchIgnore(false) , systemId(255) #ifndef __mobile__ @@ -124,7 +118,6 @@ void MAVLinkProtocol::loadSettings() QSettings settings; settings.beginGroup("QGC_MAVLINK_PROTOCOL"); enableVersionCheck(settings.value("VERSION_CHECK_ENABLED", m_enable_version_check).toBool()); - enableMultiplexing(settings.value("MULTIPLEXING_ENABLED", m_multiplexingEnabled).toBool()); // Only set system id if it was valid int temp = settings.value("GCS_SYSTEM_ID", systemId).toInt(); @@ -132,15 +125,6 @@ void MAVLinkProtocol::loadSettings() { systemId = temp; } - - // Parameter interface settings - bool ok; - temp = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", m_paramRetransmissionTimeout).toInt(&ok); - if (ok) m_paramRetransmissionTimeout = temp; - temp = settings.value("PARAMETER_REWRITE_TIMEOUT", m_paramRewriteTimeout).toInt(&ok); - if (ok) m_paramRewriteTimeout = temp; - m_paramGuardEnabled = settings.value("PARAMETER_TRANSMISSION_GUARD_ENABLED", m_paramGuardEnabled).toBool(); - settings.endGroup(); } void MAVLinkProtocol::storeSettings() @@ -149,18 +133,13 @@ void MAVLinkProtocol::storeSettings() QSettings settings; settings.beginGroup("QGC_MAVLINK_PROTOCOL"); settings.setValue("VERSION_CHECK_ENABLED", m_enable_version_check); - settings.setValue("MULTIPLEXING_ENABLED", m_multiplexingEnabled); settings.setValue("GCS_SYSTEM_ID", systemId); // Parameter interface settings - settings.setValue("PARAMETER_RETRANSMISSION_TIMEOUT", m_paramRetransmissionTimeout); - settings.setValue("PARAMETER_REWRITE_TIMEOUT", m_paramRewriteTimeout); - settings.setValue("PARAMETER_TRANSMISSION_GUARD_ENABLED", m_paramGuardEnabled); - settings.endGroup(); } void MAVLinkProtocol::resetMetadataForLink(const LinkInterface *link) { - int channel = link->getMavlinkChannel(); + int channel = link->mavlinkChannel(); totalReceiveCounter[channel] = 0; totalLossCounter[channel] = 0; totalErrorCounter[channel] = 0; @@ -188,7 +167,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) mavlink_message_t message; mavlink_status_t status; - int mavlinkChannel = link->getMavlinkChannel(); + int mavlinkChannel = link->mavlinkChannel(); static int mavlink09Count = 0; static int nonmavlinkCount = 0; @@ -235,19 +214,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) { decodedFirstPacket = true; - if(message.msgid == MAVLINK_MSG_ID_PING) - { - // process ping requests (tgt_system and tgt_comp must be zero) - mavlink_ping_t ping; - mavlink_msg_ping_decode(&message, &ping); - if(!ping.target_system && !ping.target_component) - { - mavlink_message_t msg; - mavlink_msg_ping_pack(getSystemId(), getComponentId(), &msg, ping.time_usec, ping.seq, message.sysid, message.compid); - _sendMessage(msg); - } - } - if(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS) { // process telemetry status message @@ -375,19 +341,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // kind of inefficient, but no issue for a groundstation pc. // It buys as reentrancy for the whole code over all threads emit messageReceived(link, message); - - // Multiplex message if enabled - if (m_multiplexingEnabled) - { - // Emit message on all links that are currently connected - for (int i=0; i<_linkMgr->links()->count(); i++) { - LinkInterface* currLink = _linkMgr->links()->value(i); - - // Only forward this message to the other links, - // not the link the message was received on - if (currLink && currLink != link) _sendMessage(currLink, message, message.sysid, message.compid); - } - } } } } @@ -417,110 +370,6 @@ int MAVLinkProtocol::getComponentId() return 0; } -/** - * @param message message to send - */ -void MAVLinkProtocol::_sendMessage(mavlink_message_t message) -{ - for (int i=0; i<_linkMgr->links()->count(); i++) { - LinkInterface* link = _linkMgr->links()->value(i); - _sendMessage(link, message); - } -} - -/** - * @param link the link to send the message over - * @param message message to send - */ -void MAVLinkProtocol::_sendMessage(LinkInterface* link, mavlink_message_t message) -{ - // Create buffer - static uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - // Rewriting header to ensure correct link ID is set - static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS; - mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getMavlinkChannel(), message.len, message.len, messageKeys[message.msgid]); - // Write message into buffer, prepending start sign - int len = mavlink_msg_to_send_buffer(buffer, &message); - // If link is connected - if (link->isConnected()) - { - // Send the portion of the buffer now occupied by the message - link->writeBytesSafe((const char*)buffer, len); - } -} - -/** - * @param link the link to send the message over - * @param message message to send - * @param systemid id of the system the message is originating from - * @param componentid id of the component the message is originating from - */ -void MAVLinkProtocol::_sendMessage(LinkInterface* link, mavlink_message_t message, quint8 systemid, quint8 componentid) -{ - // Create buffer - static uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - // Rewriting header to ensure correct link ID is set - static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS; - mavlink_finalize_message_chan(&message, systemid, componentid, link->getMavlinkChannel(), message.len, message.len, messageKeys[message.msgid]); - // Write message into buffer, prepending start sign - int len = mavlink_msg_to_send_buffer(buffer, &message); - // If link is connected - if (link->isConnected()) - { - // Send the portion of the buffer now occupied by the message - link->writeBytesSafe((const char*)buffer, len); - } -} - -void MAVLinkProtocol::enableMultiplexing(bool enabled) -{ - bool changed = false; - if (enabled != m_multiplexingEnabled) changed = true; - - m_multiplexingEnabled = enabled; - if (changed) emit multiplexingChanged(m_multiplexingEnabled); -} - -void MAVLinkProtocol::enableParamGuard(bool enabled) -{ - if (enabled != m_paramGuardEnabled) { - m_paramGuardEnabled = enabled; - emit paramGuardChanged(m_paramGuardEnabled); - } -} - -void MAVLinkProtocol::enableActionGuard(bool enabled) -{ - if (enabled != m_actionGuardEnabled) { - m_actionGuardEnabled = enabled; - emit actionGuardChanged(m_actionGuardEnabled); - } -} - -void MAVLinkProtocol::setParamRetransmissionTimeout(int ms) -{ - if (ms != m_paramRetransmissionTimeout) { - m_paramRetransmissionTimeout = ms; - emit paramRetransmissionTimeoutChanged(m_paramRetransmissionTimeout); - } -} - -void MAVLinkProtocol::setParamRewriteTimeout(int ms) -{ - if (ms != m_paramRewriteTimeout) { - m_paramRewriteTimeout = ms; - emit paramRewriteTimeoutChanged(m_paramRewriteTimeout); - } -} - -void MAVLinkProtocol::setActionRetransmissionTimeout(int ms) -{ - if (ms != m_actionRetransmissionTimeout) { - m_actionRetransmissionTimeout = ms; - emit actionRetransmissionTimeoutChanged(m_actionRetransmissionTimeout); - } -} - void MAVLinkProtocol::enableVersionCheck(bool enabled) { m_enable_version_check = enabled; diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index 6d181eea1..4a78d7f99 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -64,54 +64,30 @@ public: bool versionCheckEnabled() const { return m_enable_version_check; } - /** @brief Get the multiplexing state */ - bool multiplexingEnabled() const { - return m_multiplexingEnabled; - } /** @brief Get the protocol version */ int getVersion() { return MAVLINK_VERSION; } - /** @brief Get state of parameter retransmission */ - bool paramGuardEnabled() { - return m_paramGuardEnabled; - } - /** @brief Get parameter read timeout */ - int getParamRetransmissionTimeout() { - return m_paramRetransmissionTimeout; - } - /** @brief Get parameter write timeout */ - int getParamRewriteTimeout() { - return m_paramRewriteTimeout; - } - /** @brief Get state of action retransmission */ - bool actionGuardEnabled() { - return m_actionGuardEnabled; - } - /** @brief Get parameter read timeout */ - int getActionRetransmissionTimeout() { - return m_actionRetransmissionTimeout; - } /** * Retrieve a total of all successfully parsed packets for the specified link. * @returns -1 if this is not available for this protocol, # of packets otherwise. */ qint32 getReceivedPacketCount(const LinkInterface *link) const { - return totalReceiveCounter[link->getMavlinkChannel()]; + return totalReceiveCounter[link->mavlinkChannel()]; } /** * Retrieve a total of all parsing errors for the specified link. * @returns -1 if this is not available for this protocol, # of errors otherwise. */ qint32 getParsingErrorCount(const LinkInterface *link) const { - return totalErrorCounter[link->getMavlinkChannel()]; + return totalErrorCounter[link->mavlinkChannel()]; } /** * Retrieve a total of all dropped packets for the specified link. * @returns -1 if this is not available for this protocol, # of packets otherwise. */ qint32 getDroppedPacketCount(const LinkInterface *link) const { - return totalLossCounter[link->getMavlinkChannel()]; + return totalLossCounter[link->mavlinkChannel()]; } /** * Reset the counters for all metadata for this link. @@ -131,24 +107,6 @@ public slots: /** @brief Set the system id of this application */ void setSystemId(int id); - /** @brief Enabled/disable packet multiplexing */ - void enableMultiplexing(bool enabled); - - /** @brief Enable / disable parameter retransmission */ - void enableParamGuard(bool enabled); - - /** @brief Enable / disable action retransmission */ - void enableActionGuard(bool enabled); - - /** @brief Set parameter read timeout */ - void setParamRetransmissionTimeout(int ms); - - /** @brief Set parameter write timeout */ - void setParamRewriteTimeout(int ms); - - /** @brief Set parameter read timeout */ - void setActionRetransmissionTimeout(int ms); - /** @brief Enable / disable version check */ void enableVersionCheck(bool enabled); @@ -166,13 +124,7 @@ public slots: #endif protected: - bool m_multiplexingEnabled; ///< Enable/disable packet multiplexing bool m_enable_version_check; ///< Enable checking of version match of MAV and QGC - int m_paramRetransmissionTimeout; ///< Timeout for parameter retransmission - int m_paramRewriteTimeout; ///< Timeout for sending re-write request - bool m_paramGuardEnabled; ///< Parameter retransmission/rewrite enabled - bool m_actionGuardEnabled; ///< Action request retransmission enabled - int m_actionRetransmissionTimeout; ///< Timeout for parameter retransmission QMutex receiveMutex; ///< Mutex to protect receiveBytes function int lastIndex[256][256]; ///< Store the last received sequence ID for each system/componenet pair int totalReceiveCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< The total number of successfully received messages @@ -189,24 +141,12 @@ signals: /** @brief Message received and directly copied via signal */ void messageReceived(LinkInterface* link, mavlink_message_t message); - /** @brief Emitted if multiplexing is started / stopped */ - void multiplexingChanged(bool enabled); /** @brief Emitted if version check is enabled / disabled */ void versionCheckChanged(bool enabled); /** @brief Emitted if a message from the protocol should reach the user */ void protocolStatusMessage(const QString& title, const QString& message); /** @brief Emitted if a new system ID was set */ void systemIdChanged(int systemId); - /** @brief Emitted if param guard status changed */ - void paramGuardChanged(bool enabled); - /** @brief Emitted if param read timeout changed */ - void paramRetransmissionTimeoutChanged(int ms); - /** @brief Emitted if param write timeout changed */ - void paramRewriteTimeoutChanged(int ms); - /** @brief Emitted if action guard status changed */ - void actionGuardChanged(bool enabled); - /** @brief Emitted if action request timeout changed */ - void actionRetransmissionTimeoutChanged(int ms); void receiveLossPercentChanged(int uasId, float lossPercent); void receiveLossTotalChanged(int uasId, int totalLoss); @@ -232,10 +172,6 @@ private slots: void _vehicleCountChanged(int count); private: - void _sendMessage(mavlink_message_t message); - void _sendMessage(LinkInterface* link, mavlink_message_t message); - void _sendMessage(LinkInterface* link, mavlink_message_t message, quint8 systemid, quint8 componentid); - #ifndef __mobile__ bool _closeLogFile(void); void _startLogging(void); diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index ed00f659d..b06c41ec0 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -346,7 +346,7 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes) for (qint64 i=0; imavlinkChannel(), + &message, // Mavlink Message to pack into + 0, // Target network + _systemIdServer, // Target system + 0, // Target component + (uint8_t*)request); // Payload _vehicle->sendMessageOnLink(_dedicatedLink, message); } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 32b74d9d0..840bf7c3a 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -798,21 +798,22 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType) } mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - uasId, - _vehicle->defaultComponentId(), // target component - MAV_CMD_PREFLIGHT_CALIBRATION, // command id - 0, // 0=first transmission of command - gyroCal, // gyro cal - magCal, // mag cal - 0, // ground pressure - radioCal, // radio cal - accelCal, // accel cal - airspeedCal, // PX4: airspeed cal, ArduPilot: compass mot - escCal); // esc cal - _vehicle->sendMessageOnPriorityLink(msg); + mavlink_msg_command_long_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + uasId, + _vehicle->defaultComponentId(), // target component + MAV_CMD_PREFLIGHT_CALIBRATION, // command id + 0, // 0=first transmission of command + gyroCal, // gyro cal + magCal, // mag cal + 0, // ground pressure + radioCal, // radio cal + accelCal, // accel cal + airspeedCal, // PX4: airspeed cal, ArduPilot: compass mot + escCal); // esc cal + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } void UAS::stopCalibration(void) @@ -822,21 +823,22 @@ void UAS::stopCalibration(void) } mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - uasId, - _vehicle->defaultComponentId(), // target component - MAV_CMD_PREFLIGHT_CALIBRATION, // command id - 0, // 0=first transmission of command - 0, // gyro cal - 0, // mag cal - 0, // ground pressure - 0, // radio cal - 0, // accel cal - 0, // airspeed cal - 0); // unused - _vehicle->sendMessageOnPriorityLink(msg); + mavlink_msg_command_long_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + uasId, + _vehicle->defaultComponentId(), // target component + MAV_CMD_PREFLIGHT_CALIBRATION, // command id + 0, // 0=first transmission of command + 0, // gyro cal + 0, // mag cal + 0, // ground pressure + 0, // radio cal + 0, // accel cal + 0, // airspeed cal + 0); // unused + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } void UAS::startBusConfig(UASInterface::StartBusConfigType calType) @@ -857,21 +859,22 @@ void UAS::startBusConfig(UASInterface::StartBusConfigType calType) } mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - uasId, - _vehicle->defaultComponentId(), // target component - MAV_CMD_PREFLIGHT_UAVCAN, // command id - 0, // 0=first transmission of command - actuatorCal, // actuators - 0, - 0, - 0, - 0, - 0, - 0); - _vehicle->sendMessageOnPriorityLink(msg); + mavlink_msg_command_long_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + uasId, + _vehicle->defaultComponentId(), // target component + MAV_CMD_PREFLIGHT_UAVCAN, // command id + 0, // 0=first transmission of command + actuatorCal, // actuators + 0, + 0, + 0, + 0, + 0, + 0); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } void UAS::stopBusConfig(void) @@ -881,21 +884,22 @@ void UAS::stopBusConfig(void) } mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - uasId, - _vehicle->defaultComponentId(), // target component - MAV_CMD_PREFLIGHT_UAVCAN, // command id - 0, // 0=first transmission of command - 0, - 0, - 0, - 0, - 0, - 0, - 0); - _vehicle->sendMessageOnPriorityLink(msg); + mavlink_msg_command_long_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + uasId, + _vehicle->defaultComponentId(), // target component + MAV_CMD_PREFLIGHT_UAVCAN, // command id + 0, // 0=first transmission of command + 0, + 0, + 0, + 0, + 0, + 0, + 0); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } /** @@ -1144,8 +1148,13 @@ void UAS::requestImage() if (imagePacketsArrived == 0) { 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->sendMessageOnPriorityLink(msg); + mavlink_msg_data_transmission_handshake_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + MAVLINK_DATA_STREAM_IMG_JPEG, + 0, 0, 0, 0, 0, 50); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } } @@ -1243,8 +1252,12 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float cmd.param7 = param7; cmd.target_system = uasId; cmd.target_component = component; - mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - _vehicle->sendMessageOnPriorityLink(msg); + mavlink_msg_command_long_encode_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + &cmd); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } /** @@ -1299,19 +1312,19 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t float attitudeQuaternion[4]; mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion); uint8_t typeMask = 0x7; // disable rate control - mavlink_msg_set_attitude_target_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &message, - QGC::groundTimeUsecs(), - this->uasId, - 0, - typeMask, - attitudeQuaternion, - 0, - 0, - 0, - thrust - ); + mavlink_msg_set_attitude_target_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &message, + QGC::groundTimeUsecs(), + this->uasId, + 0, + typeMask, + attitudeQuaternion, + 0, + 0, + 0, + thrust); } else if (joystickMode == Vehicle::JoystickModePosition) { // Send the the local position setpoint (local pos sp external message) static float px = 0; @@ -1322,26 +1335,26 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t py += roll; pz -= 2.0f*(thrust-0.5); uint16_t typeMask = (1<<11)|(7<<6)|(7<<3); // select only POSITION control - mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &message, - QGC::groundTimeUsecs(), - this->uasId, - 0, - MAV_FRAME_LOCAL_NED, - typeMask, - px, - py, - pz, - 0, - 0, - 0, - 0, - 0, - 0, - yaw, - 0 - ); + mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &message, + QGC::groundTimeUsecs(), + this->uasId, + 0, + MAV_FRAME_LOCAL_NED, + typeMask, + px, + py, + pz, + 0, + 0, + 0, + 0, + 0, + 0, + yaw, + 0); } else if (joystickMode == Vehicle::JoystickModeForce) { // Send the the force setpoint (local pos sp external message) float dcm[3][3]; @@ -1350,26 +1363,26 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t const float fy = -dcm[1][2] * thrust; const float fz = -dcm[2][2] * thrust; uint16_t typeMask = (3<<10)|(7<<3)|(7<<0)|(1<<9); // select only FORCE control (disable everything else) - mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &message, - QGC::groundTimeUsecs(), - this->uasId, - 0, - MAV_FRAME_LOCAL_NED, - typeMask, - 0, - 0, - 0, - 0, - 0, - 0, - fx, - fy, - fz, - 0, - 0 - ); + mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &message, + QGC::groundTimeUsecs(), + this->uasId, + 0, + MAV_FRAME_LOCAL_NED, + typeMask, + 0, + 0, + 0, + 0, + 0, + 0, + fx, + fy, + fz, + 0, + 0); } else if (joystickMode == Vehicle::JoystickModeVelocity) { // Send the the local velocity setpoint (local pos sp external message) static float vx = 0; @@ -1382,26 +1395,26 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t vz -= 2.0f*(thrust-0.5); yawrate += yaw; //XXX: not sure what scale to apply here uint16_t typeMask = (1<<10)|(7<<6)|(7<<0); // select only VELOCITY control - mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &message, - QGC::groundTimeUsecs(), - this->uasId, - 0, - MAV_FRAME_LOCAL_NED, - typeMask, - 0, - 0, - 0, - vx, - vy, - vz, - 0, - 0, - 0, - 0, - yawrate - ); + mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &message, + QGC::groundTimeUsecs(), + this->uasId, + 0, + MAV_FRAME_LOCAL_NED, + typeMask, + 0, + 0, + 0, + vx, + vy, + vz, + 0, + 0, + 0, + 0, + yawrate); } else if (joystickMode == Vehicle::JoystickModeRC) { // Save the new manual control inputs @@ -1424,10 +1437,15 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t //qDebug() << newRollCommand << newPitchCommand << newYawCommand << newThrustCommand; // Send the MANUAL_COMMAND message - mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons); + mavlink_msg_manual_control_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &message, + this->uasId, + newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons); } - _vehicle->sendMessageOnPriorityLink(message); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message); // Emit an update in control values to other UI elements, like the HSI display emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); } @@ -1453,16 +1471,20 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll // Do not control rates and throttle 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, _vehicle->defaultComponentId(), - mask, q, 0, 0, 0, 0); - _vehicle->sendMessageOnPriorityLink(message); + mavlink_msg_set_attitude_target_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &message, + QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(), + mask, q, 0, 0, 0, 0); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), 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, _vehicle->defaultComponentId(), - MAV_FRAME_LOCAL_NED, position_mask, x, y, z, 0, 0, 0, 0, 0, 0, yaw, yawrate); - _vehicle->sendMessageOnPriorityLink(message); + mavlink_msg_set_position_target_local_ned_pack_chan(mavlink->getSystemId(), mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &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->sendMessageOnLink(_vehicle->priorityLink(), 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()); @@ -1485,8 +1507,14 @@ void UAS::pairRX(int rxType, int rxSubType) mavlink_message_t 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); + mavlink_msg_command_long_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + uasId, + _vehicle->defaultComponentId(), + MAV_CMD_START_RX_PAIR, 0, rxType, rxSubType, 0, 0, 0, 0, 0); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } /** @@ -1706,10 +1734,13 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa sinPhi_2 * sinTheta_2 * cosPsi_2); mavlink_message_t msg; - 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->sendMessageOnPriorityLink(msg); + mavlink_msg_hil_state_quaternion_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &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->sendMessageOnLink(_vehicle->priorityLink(), msg); } else { @@ -1783,11 +1814,14 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl float temperature_corrupt = addZeroMeanNoise(temperature,temperature_var); mavlink_message_t msg; - mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, - 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->sendMessageOnPriorityLink(msg); + mavlink_msg_hil_sensor_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &msg, + 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->sendMessageOnLink(_vehicle->priorityLink(), msg); lastSendTimeSensors = QGC::groundTimeMilliseconds(); } else @@ -1821,10 +1855,13 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa { #if 0 mavlink_message_t msg; - 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); + mavlink_msg_hil_optical_flow_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &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->sendMessageOnPriorityLink(msg); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds(); #endif } @@ -1859,10 +1896,13 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi course = (course / M_PI) * 180.0f; mavlink_message_t msg; - 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); + mavlink_msg_hil_gps_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &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->sendMessageOnPriorityLink(msg); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); } else { @@ -1931,19 +1971,20 @@ void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 p } } - mavlink_msg_param_map_rc_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &message, - this->uasId, - _vehicle->defaultComponentId(), - param_id_cstr, - -1, - param_rc_channel_index, - value0, - scale, - valueMin, - valueMax); - _vehicle->sendMessageOnPriorityLink(message); + mavlink_msg_param_map_rc_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &message, + this->uasId, + _vehicle->defaultComponentId(), + param_id_cstr, + -1, + param_rc_channel_index, + value0, + scale, + valueMin, + valueMax); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message); //qDebug() << "Mavlink message sent"; } @@ -1957,19 +1998,20 @@ void UAS::unsetRCToParameterMap() for (int i = 0; i < 3; i++) { mavlink_message_t message; - mavlink_msg_param_map_rc_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &message, - this->uasId, - _vehicle->defaultComponentId(), - param_id_cstr, - -2, - i, - 0.0f, - 0.0f, - 0.0f, - 0.0f); - _vehicle->sendMessageOnPriorityLink(message); + mavlink_msg_param_map_rc_pack_chan(mavlink->getSystemId(), + mavlink->getComponentId(), + _vehicle->priorityLink()->mavlinkChannel(), + &message, + this->uasId, + _vehicle->defaultComponentId(), + param_id_cstr, + -2, + i, + 0.0f, + 0.0f, + 0.0f, + 0.0f); + _vehicle->sendMessageOnLink(_vehicle->priorityLink(), message); } } diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc index 71d6a8317..d214ba01c 100644 --- a/src/ui/QGCMAVLinkInspector.cc +++ b/src/ui/QGCMAVLinkInspector.cc @@ -37,16 +37,6 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(const QString& title, QAction* action, header << tr("Type"); ui->treeWidget->setHeaderLabels(header); - // Set up the column headers for the rate listing - QStringList rateHeader; - rateHeader << tr("Name"); - rateHeader << tr("#ID"); - rateHeader << tr("Rate"); - ui->rateTreeWidget->setHeaderLabels(rateHeader); - connect(ui->rateTreeWidget, &QTreeWidget::itemChanged, - this, &QGCMAVLinkInspector::rateTreeItemChanged); - ui->rateTreeWidget->hide(); - // Connect the UI connect(ui->systemComboBox, static_cast(&QComboBox::currentIndexChanged), this, &QGCMAVLinkInspector::selectDropDownMenuSystem); @@ -78,23 +68,11 @@ void QGCMAVLinkInspector::selectDropDownMenuSystem(int dropdownid) { selectedSystemID = ui->systemComboBox->itemData(dropdownid).toInt(); rebuildComponentList(); - - if (selectedSystemID != 0 && selectedComponentID != 0) { - ui->rateTreeWidget->show(); - } else { - ui->rateTreeWidget->hide(); - } } void QGCMAVLinkInspector::selectDropDownMenuComponent(int dropdownid) { selectedComponentID = ui->componentComboBox->itemData(dropdownid).toInt(); - - if (selectedSystemID != 0 && selectedComponentID != 0) { - ui->rateTreeWidget->show(); - } else { - ui->rateTreeWidget->hide(); - } } void QGCMAVLinkInspector::rebuildComponentList() @@ -195,8 +173,6 @@ void QGCMAVLinkInspector::clearView() onboardMessageInterval.clear(); ui->treeWidget->clear(); - ui->rateTreeWidget->clear(); - } void QGCMAVLinkInspector::refreshView() @@ -311,25 +287,6 @@ void QGCMAVLinkInspector::refreshView() if (!strcmp(msgname, "EMPTY")) { continue; } - - // Update the tree view - QString messageName("%1"); - messageName = messageName.arg(msgname); - if (!rateTreeWidgetItems.contains(i)) - { - QStringList fields; - fields << messageName; - fields << QString("%1").arg(i); - fields << "OFF / --- Hz"; - QTreeWidgetItem* widget = new QTreeWidgetItem(fields); - widget->setFlags(widget->flags() | Qt::ItemIsEditable); - rateTreeWidgetItems.insert(i, widget); - ui->rateTreeWidget->addTopLevelItem(widget); - } - - // Set Hz - //QTreeWidgetItem* message = rateTreeWidgetItems.value(i); - //message->setData(0, Qt::DisplayRole, QVariant(messageName)); } } @@ -476,43 +433,6 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m } } -void QGCMAVLinkInspector::changeStreamInterval(int msgid, int interval) -{ - //REQUEST_DATA_STREAM - if (selectedSystemID == 0 || selectedComponentID == 0) { - return; - } - - mavlink_request_data_stream_t stream; - stream.target_system = selectedSystemID; - stream.target_component = selectedComponentID; - stream.req_stream_id = msgid; - stream.req_message_rate = interval; - stream.start_stop = (interval > 0); - - mavlink_message_t msg; - mavlink_msg_request_data_stream_encode(_protocol->getSystemId(), _protocol->getComponentId(), &msg, &stream); - -#if 0 - // FIXME: Is this really used? - _protocol->sendMessage(msg); -#endif -} - -void QGCMAVLinkInspector::rateTreeItemChanged(QTreeWidgetItem* paramItem, int column) -{ - if (paramItem && column > 0) { - - int key = paramItem->data(1, Qt::DisplayRole).toInt(); - QVariant value = paramItem->data(2, Qt::DisplayRole); - float interval = 1000 / value.toFloat(); - - qDebug() << "Stream " << key << "interval" << interval; - - changeStreamInterval(key, interval); - } -} - QGCMAVLinkInspector::~QGCMAVLinkInspector() { clearView(); diff --git a/src/ui/QGCMAVLinkInspector.h b/src/ui/QGCMAVLinkInspector.h index c1be7c4b8..9069fb54c 100644 --- a/src/ui/QGCMAVLinkInspector.h +++ b/src/ui/QGCMAVLinkInspector.h @@ -36,8 +36,6 @@ public slots: /** @Brief Select a component through the drop down menu */ void selectDropDownMenuComponent(int dropdownid); - void rateTreeItemChanged(QTreeWidgetItem* paramItem, int column); - protected: MAVLinkProtocol *_protocol; ///< MAVLink instance int selectedSystemID; ///< Currently selected system @@ -45,7 +43,6 @@ protected: QMap systems; ///< Already observed systems QMap components; ///< Already observed components QMap onboardMessageInterval; ///< Stores the onboard selected data rate - QMap rateTreeWidgetItems; ///< Available rate tree widget items QTimer updateTimer; ///< Only update at 1 Hz to not overload the GUI mavlink_message_info_t messageInfo[256]; // Store the metadata for all available MAVLink messages. @@ -63,8 +60,6 @@ protected: void updateField(int sysid, int msgid, int fieldid, QTreeWidgetItem* item); /** @brief Rebuild the list of components */ void rebuildComponentList(); - /** @brief Change the stream interval */ - void changeStreamInterval(int msgid, int interval); /* @brief Create a new tree for a new UAS */ void addUAStoTree(int sysId); diff --git a/src/ui/QGCMAVLinkInspector.ui b/src/ui/QGCMAVLinkInspector.ui index ab42ff309..5617d51ee 100644 --- a/src/ui/QGCMAVLinkInspector.ui +++ b/src/ui/QGCMAVLinkInspector.ui @@ -13,7 +13,7 @@ MAVLink Inspector - + 6 @@ -26,13 +26,25 @@ 6 - - + + + + + 100 + 16777215 + + - Clear + System + + + + + + @@ -40,10 +52,10 @@ - - + + - System + Clear @@ -56,21 +68,6 @@ - - - - - 1 - - - - - - - - - - diff --git a/src/ui/preferences/MavlinkSettings.qml b/src/ui/preferences/MavlinkSettings.qml index 349b9d385..37ac33284 100644 --- a/src/ui/preferences/MavlinkSettings.qml +++ b/src/ui/preferences/MavlinkSettings.qml @@ -69,15 +69,6 @@ Rectangle { } } //----------------------------------------------------------------- - //-- Mavlink Multiplexing - QGCCheckBox { - text: qsTr("Enable multiplexing (forward packets to all other links)") - checked: QGroundControl.isMultiplexingEnabled - onClicked: { - QGroundControl.isMultiplexingEnabled = checked - } - } - //----------------------------------------------------------------- //-- Mavlink Version Check QGCCheckBox { text: qsTr("Only accept MAVs with same protocol version") -- 2.22.0