/*===================================================================== PIXHAWK Micro Air Vehicle Flying Robotics Toolkit Please see our website at (c) 2009, 2010 PIXHAWK PROJECT This file is part of the PIXHAWK project PIXHAWK is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. PIXHAWK is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with PIXHAWK. If not, see . ======================================================================*/ /** * @file * @brief Implementation of the MAVLink protocol * * @author Lorenz Meier * */ #include #include #include #include #include "MG.h" #include "MAVLinkProtocol.h" #include "UASInterface.h" #include "UASManager.h" #include "UASInterface.h" #include "UAS.h" #include "configuration.h" #include "LinkManager.h" #include /** * The default constructor will create a new MAVLink object sending heartbeats at * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links. */ MAVLinkProtocol::MAVLinkProtocol() : heartbeatTimer(new QTimer(this)), heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE), m_heartbeatsEnabled(false) { start(QThread::LowPriority); // Start heartbeat timer, emitting a heartbeat at the configured rate connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat())); heartbeatTimer->start(1000/heartbeatRate); } MAVLinkProtocol::~MAVLinkProtocol() { } void MAVLinkProtocol::run() { } /** * The bytes are copied by calling the LinkInterface::readBytes() method. * This method parses all incoming bytes and constructs a MAVLink packet. * It can handle multiple links in parallel, as each link has it's own buffer/ * parsing state machine. * @param link The interface to read from * @see LinkInterface **/ void MAVLinkProtocol::receiveBytes(LinkInterface* link) { receiveMutex.lock(); // Prepare buffer static const int maxlen = 4096 * 100; static char buffer[maxlen]; qint64 bytesToRead = link->bytesAvailable(); // Get all data at once, let link read the bytes in the buffer array link->readBytes(buffer, maxlen); mavlink_message_t message; mavlink_status_t status; for (int position = 0; position < bytesToRead; position++) { unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)*(buffer + position), &message, &status); if (decodeState == 1) { // ORDER MATTERS HERE! // If the matching UAS object does not yet exist, it has to be created // before emitting the packetReceived signal UASInterface* uas = UASManager::instance()->getUASForId(message.sysid); // Check and (if necessary) create UAS object if (uas == NULL) { // ORDER MATTERS HERE! // The UAS object has first to be created and connected, // only then the rest of the application can be made aware // of its existence, as it only then can send and receive // it's first messages. // First create new UAS object uas = new UAS(message.sysid); // Make UAS aware that this link can be used to communicate with the actual robot uas->addLink(link); // Connect this robot to the UAS object connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t))); // Now add UAS to "official" list, which makes the whole application aware of it UASManager::instance()->addUAS(uas); } // The packet is emitted as a whole, as it is only 255 - 261 bytes short // 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); } } receiveMutex.unlock(); } /** * @return The name of this protocol **/ QString MAVLinkProtocol::getName() { return QString(tr("MAVLink protocol")); } /** * @param message message to send */ void MAVLinkProtocol::sendMessage(mavlink_message_t message) { // Get all links connected to this unit QList links = LinkManager::instance()->getLinksForProtocol(this); // Emit message on all links that are currently connected QList::iterator i; for (i = links.begin(); i != links.end(); ++i) { sendMessage(*i, 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 uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; // Write message into buffer, prepending start sign int len = message_to_send_buffer(buffer, &message); // If link is connected if (link->isConnected()) { // Send the portion of the buffer now occupied by the message link->writeBytes((const char*)buffer, len); } } /** * The heartbeat is sent out of order and does not reset the * periodic heartbeat emission. It will be just sent in addition. * @return mavlink_message_t heartbeat message sent on serial link */ void MAVLinkProtocol::sendHeartbeat() { if (m_heartbeatsEnabled) { mavlink_message_t beat; message_heartbeat_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID,&beat, OCU); sendMessage(beat); } } /** @param enabled true to enable heartbeats emission at heartbeatRate, false to disable */ void MAVLinkProtocol::enableHeartbeats(bool enabled) { m_heartbeatsEnabled = enabled; emit heartbeatChanged(enabled); } bool MAVLinkProtocol::heartbeatsEnabled(void) { return m_heartbeatsEnabled; } /** * The default rate is 1 Hertz. * * @param rate heartbeat rate in hertz (times per second) */ void MAVLinkProtocol::setHeartbeatRate(int rate) { heartbeatRate = rate; heartbeatTimer->setInterval(1000/heartbeatRate); } /** @return heartbeat rate in Hertz */ int MAVLinkProtocol::getHeartbeatRate() { return heartbeatRate; }