Skip to content

Q
qgroundcontrol
Commit e7979fd9 authored by Don Gagne's avatar Don Gagne
Browse files
Options

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
parent 01c4ef3c
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 567 additions and 712 deletions
src/AutoPilotPlugins/APM/APMSensorsComponentController.cc
View file @ e7979fd9
......@@ -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);
......
src/AutoPilotPlugins/Common/ESP8266ComponentController.cc
View file @ e7979fd9
......@@ -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,
......
src/FactSystem/ParameterManager.cc
View file @ e7979fd9
......@@ -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);
}
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
View file @ e7979fd9
......@@ -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, &paramValue);
mavlink_msg_param_value_encode_chan(message->sysid,
message->compid,
vehicle->priorityLink()->mavlinkChannel(),
message,
&paramValue);
}
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, &paramSet);
mavlink_msg_param_set_encode_chan(message->sysid, message->compid, outgoingLink->mavlinkChannel(), message, &paramSet);
}
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
......
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @ e7979fd9
......@@ -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);
......
src/FirmwarePlugin/APM/APMGeoFenceManager.cc
View file @ e7979fd9
......@@ -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
......
src/FirmwarePlugin/APM/APMRallyPointManager.cc
View file @ e7979fd9
......@@ -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
......
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
View file @ e7979fd9
......@@ -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
......
src/FirmwarePlugin/FirmwarePlugin.cc
View file @ e7979fd9
......@@ -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
}
......
src/FirmwarePlugin/FirmwarePlugin.h
View file @ e7979fd9
......@@ -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.
......
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
View file @ e7979fd9
......@@ -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)
......
src/FollowMe/FollowMe.cc
View file @ e7979fd9
......@@ -140,11 +140,12 @@ void FollowMe::_sendGCSMotionReport(void)
Vehicle* vehicle = qobject_cast<Vehicle*>(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);
}
}
}
......
src/GPS/RTCM/RTCMMavlink.cc
View file @ e7979fd9
......@@ -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<Vehicle*>(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);
}
}
src/MissionManager/MissionManager.cc
View file @ e7979fd9
......@@ -90,9 +90,13 @@ void MissionManager::writeMissionItems(const QList<MissionItem*>& 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);
......
src/QmlControls/QGroundControlQmlGlobal.cc
View file @ e7979fd9
......@@ -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);
......
src/QmlControls/QGroundControlQmlGlobal.h
View file @ e7979fd9
......@@ -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);
......
src/Vehicle/MultiVehicleManager.cc
View file @ e7979fd9
......@@ -307,15 +307,16 @@ void MultiVehicleManager::_sendGCSHeartbeat(void)
Vehicle* vehicle = qobject_cast<Vehicle*>(_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);
}
}
......
src/Vehicle/Vehicle.cc
View file @ e7979fd9
......@@ -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)
......
src/Vehicle/Vehicle.h
View file @ e7979fd9
......@@ -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);
......
src/ViewWidgets/LogDownloadController.cc
View file @ e7979fd9
......@@ -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();
}
......
src/comm/LinkInterface.h
View file @ e7979fd9
......@@ -7,15 +7,6 @@
*
****************************************************************************/
/**
* @file
* @brief Brief Description
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#ifndef _LINKINTERFACE_H_
#define _LINKINTERFACE_H_
......@@ -25,6 +16,7 @@
#include <QMutexLocker>
#include <QMetaType>
#include <QSharedPointer>
#include <QDebug>
#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.
......
src/comm/LinkManager.cc
View file @ e7979fd9
......@@ -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;
......
src/comm/LogReplayLink.cc
View file @ e7979fd9
......@@ -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.
......
src/comm/MAVLinkProtocol.cc
View file @ e7979fd9
......@@ -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<LinkInterface*>(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<LinkInterface*>(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;
......
src/comm/MAVLinkProtocol.h
View file @ e7979fd9
......@@ -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);
......
src/comm/MockLink.cc
View file @ e7979fd9
......@@ -346,7 +346,7 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
for (qint64 i=0; i<cBytes; i++)
{
if (!mavlink_parse_char(getMavlinkChannel(), bytes[i], &msg, &comm)) {
if (!mavlink_parse_char(mavlinkChannel(), bytes[i], &msg, &comm)) {
continue;
}
......
src/uas/FileManager.cc
View file @ e7979fd9
......@@ -733,13 +733,14 @@ void FileManager::_sendRequest(Request* request)
}
Q_ASSERT(_vehicle);
mavlink_msg_file_transfer_protocol_pack(_systemIdQGC, // QGC System ID
0, // QGC Component ID
&message, // Mavlink Message to pack into
0, // Target network
_systemIdServer, // Target system
0, // Target component
(uint8_t*)request); // Payload
mavlink_msg_file_transfer_protocol_pack_chan(_systemIdQGC, // QGC System ID
0, // QGC Component ID
_dedicatedLink->mavlinkChannel(),
&message, // Mavlink Message to pack into
0, // Target network
_systemIdServer, // Target system
0, // Target component
(uint8_t*)request); // Payload
_vehicle->sendMessageOnLink(_dedicatedLink, message);
}
src/uas/UAS.cc
View file @ e7979fd9
This diff is collapsed.
src/ui/QGCMAVLinkInspector.cc
View file @ e7979fd9
......@@ -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<void (QComboBox::*)(int)>(&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();
......
src/ui/QGCMAVLinkInspector.h
View file @ e7979fd9
......@@ -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<int, int> systems; ///< Already observed systems
QMap<int, int> components; ///< Already observed components
QMap<int, float> onboardMessageInterval; ///< Stores the onboard selected data rate
QMap<int, QTreeWidgetItem*> 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);
......
src/ui/QGCMAVLinkInspector.ui
View file @ e7979fd9
......@@ -13,7 +13,7 @@
<property name="windowTitle">
<string>MAVLink Inspector</string>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="2,8,2,8,3">
<layout class="QGridLayout" name="gridLayout" columnstretch="2,0,0,0,0">
<property name="leftMargin">
<number>6</number>
</property>
......@@ -26,13 +26,25 @@
<property name="bottomMargin">
<number>6</number>
</property>
<item row="0" column="4">
<widget class="QPushButton" name="clearButton">
<item row="0" column="0">
<widget class="QLabel" name="label">
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>Clear</string>
<string>System</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QComboBox" name="systemComboBox"/>
</item>
<item row="0" column="3">
<widget class="QComboBox" name="componentComboBox"/>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_2">
<property name="text">
......@@ -40,10 +52,10 @@
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label">
<item row="0" column="4">
<widget class="QPushButton" name="clearButton">
<property name="text">
<string>System</string>
<string>Clear</string>
</property>
</widget>
</item>
......@@ -56,21 +68,6 @@
</column>
</widget>
</item>
<item row="3" column="0" colspan="5">
<widget class="QTreeWidget" name="rateTreeWidget">
<column>
<property name="text">
<string notr="true">1</string>
</property>
</column>
</widget>
</item>
<item row="0" column="1">
<widget class="QComboBox" name="systemComboBox"/>
</item>
<item row="0" column="3">
<widget class="QComboBox" name="componentComboBox"/>
</item>
</layout>
</widget>
<resources/>
......
src/ui/preferences/MavlinkSettings.qml
View file @ e7979fd9
......@@ -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")
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment