MAVLinkLightProtocol.cc 4.96 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31
/*=====================================================================

QGroundControl Open Source Ground Control Station

(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL 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.

    QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Example implementation of a different protocol than the default MAVLink.
 *   @author Lorenz Meier <mail@qgroundcontrol.org>
 */

#include "MAVLinkLightProtocol.h"
#include "UASManager.h"
32
#include "ArduPilotMegaMAV.h"
33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88
#include "LinkManager.h"

MAVLinkLightProtocol::MAVLinkLightProtocol() :
    MAVLinkProtocol()
{
}


/**
 * @param message message to send
 */
void MAVLinkLightProtocol::sendMessage(mavlink_light_message_t message)
{
    // Get all links connected to this unit
    QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);

    // Emit message on all links that are currently connected
    QList<LinkInterface*>::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 MAVLinkLightProtocol::sendMessage(LinkInterface* link, mavlink_light_message_t message)
{
    // Create buffer
    uint8_t buffer[100]; // MAXIMUM PACKET LENGTH, INCLUDING STX BYTES
    // Write message into buffer, prepending start sign
    //int len = mavlink_msg_to_send_buffer(buffer, &message);


    // FIXME TO SEND BUFFER FUNCTION MISSING
    Q_UNUSED(message);
    int len = 0;

    // 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 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
 **/
89
void MAVLinkLightProtocol::receiveBytes(LinkInterface* link, QByteArray b)
90
{
91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115
    Q_UNUSED(link);
    Q_UNUSED(b);
//    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);
//
//    // Run through all bytes
//    for (int position = 0; position < bytesToRead; position++)
//    {
//        mavlink_light_message_t msg;
//        // FIXME PARSE
//        if (1 == 0/* parsing returned a message */)
//        {
//
//            int sysid = 0; // system id from message, or always null if only one MAV is supported
//            UASInterface* uas = UASManager::instance()->getUASForId(sysid);
//
//            // Check and (if necessary) create UAS object
//            if (uas == NULL)
//            {
116
//                ArduPilotMegaMAV* mav = new ArduPilotMAV(this, sysid); // FIXME change to msg.sysid if this field exists
117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135
//                // Connect this robot to the UAS object
//                // it is IMPORTANT here to use the right object type,
//                // else the slot of the parent object is called (and thus the special
//                // packets never reach their goal)
//                connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
//                uas = mav;
//                // Make UAS aware that this link can be used to communicate with the actual robot
//                uas->addLink(link);
//                // 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, msg);
//        }
//    }
//    receiveMutex.unlock();
136 137 138 139 140 141 142 143 144
}

/**
 * @return The name of this protocol
 **/
QString MAVLinkLightProtocol::getName()
{
    return QString(tr("MAVLinkLight protocol"));
}