Skip to content

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

Fix priority link and default component id usage

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