Commit e7979fd9 authored by Don Gagne's avatar Don Gagne

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
......@@ -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);
......
......@@ -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,
......
......@@ -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);
}
......@@ -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
......
......@@ -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);
......
......@@ -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
......
......@@ -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
......
......@@ -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
......
......@@ -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
}
......
......@@ -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.
......
......@@ -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)
......
......@@ -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);
}
}
}
......
......@@ -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);
}
}
......@@ -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);
......
......@@ -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);
......
......@@ -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);
......
......@@ -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);
}
}
......
......@@ -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();