Commit 01f26954 authored by Don Gagne's avatar Don Gagne

Add priorityLink and sendMessageOnLink

parent d6b4f5c3
......@@ -116,6 +116,7 @@ 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);
_uas = new UAS(_mavlink, this, _firmwarePluginManager);
......@@ -435,26 +436,61 @@ 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()) {
return false;
}
emit _sendMessageOnLinkOnThread(link, message);
return true;
}
void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
{
// Give the plugin a chance to adjust
_firmwarePlugin->adjustMavlinkMessage(this, &message);
static const uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
mavlink_finalize_message_chan(&message, _mavlink->getSystemId(), _mavlink->getComponentId(), link->getMavlinkChannel(), message.len, messageKeys[message.msgid]);
// Write message into buffer, prepending start sign
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
int len = mavlink_msg_to_send_buffer(buffer, &message);
link->writeBytes((const char*)buffer, len);
}
void Vehicle::_sendMessage(mavlink_message_t message)
{
// Emit message on all links that are currently connected
foreach (LinkInterface* link, _links) {
if (link->isConnected()) {
MAVLinkProtocol* mavlink = _mavlink;
// Give the plugin a chance to adjust
_firmwarePlugin->adjustMavlinkMessage(this, &message);
static const uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getMavlinkChannel(), message.len, messageKeys[message.msgid]);
// Write message into buffer, prepending start sign
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
int len = mavlink_msg_to_send_buffer(buffer, &message);
_sendMessageOnLink(link, message);
}
}
}
link->writeBytes((const char*)buffer, len);
/// @return Direct usb connection link to board if one, NULL if none
LinkInterface* Vehicle::priorityLink(void)
{
foreach (LinkInterface* link, _links) {
if (link->isConnected()) {
SerialLink* pSerialLink = qobject_cast<SerialLink*>(link);
if (pSerialLink) {
LinkConfiguration* pLinkConfig = pSerialLink->getLinkConfiguration();
if (pLinkConfig) {
SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(pLinkConfig);
if (pSerialConfig && pSerialConfig->usbDirect()) {
return link;
}
}
}
}
}
return _links.count() ? _links[0] : NULL;
}
void Vehicle::setLatitude(double latitude)
......
......@@ -166,9 +166,16 @@ public:
MAV_AUTOPILOT firmwareType(void) { return _firmwareType; }
MAV_TYPE vehicleType(void) { return _vehicleType; }
/// Sends this specified message to all links accociated with this vehicle
/// 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);
/// 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);
......@@ -292,6 +299,7 @@ signals:
/// 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 ();
void newMessageCountChanged ();
......@@ -340,6 +348,7 @@ 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);
void _parametersReady(bool parametersReady);
......
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