ArduPilotMegaMAV.cc 4.2 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11
/*=====================================================================
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.
12

13 14 15 16
    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.
17

18 19
    You should have received a copy of the GNU General Public License
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
20

21 22 23 24 25 26 27 28
======================================================================*/

/**
 * @file
 *   @brief Implementation of class MainWindow
 *   @author Your Name here
 */

29
#include "ArduPilotMegaMAV.h"
30

31
#ifdef QGC_USE_ARDUPILOTMEGA_MESSAGES
32
#ifndef MAVLINK_MSG_ID_MOUNT_CONFIGURE
33 34 35
#include "ardupilotmega/mavlink_msg_mount_configure.h"
#endif

36 37
#ifndef MAVLINK_MSG_ID_MOUNT_CONTROL
#include "ardupilotmega/mavlink_msg_mount_control.h"
38
#endif
39
#endif
40

41
ArduPilotMegaMAV::ArduPilotMegaMAV(MAVLinkProtocol* mavlink, int id) :
42 43
    UAS(mavlink, id)//,
    // place other initializers here
44
{
45
    //This does not seem to work. Manually request each stream type at a specified rate.
46
    // Ask for all streams at 4 Hz
47
    //enableAllDataTransmission(4);
48 49 50 51 52 53 54 55 56
    txReqTimer = new QTimer(this);
    connect(txReqTimer,SIGNAL(timeout()),this,SLOT(sendTxRequests()));

    QTimer::singleShot(5000,this,SLOT(sendTxRequests())); //Send an initial TX request in 5 seconds.

    txReqTimer->start(300000); //Resend the TX requests every 5 minutes.
}
void ArduPilotMegaMAV::sendTxRequests()
{
57
    enableExtendedSystemStatusTransmission(2);
58
    QGC::SLEEP::msleep(250);
59
    enablePositionTransmission(3);
60
    QGC::SLEEP::msleep(250);
61
    enableExtra1Transmission(10);
62
    QGC::SLEEP::msleep(250);
63
    enableExtra2Transmission(10);
64
    QGC::SLEEP::msleep(250);
65
    enableExtra3Transmission(2);
66
    QGC::SLEEP::msleep(250);
67
    enableRawSensorDataTransmission(2);
68
    QGC::SLEEP::msleep(250);
69
    enableRCChannelDataTransmission(2);
70
}
71 72 73 74 75 76 77 78 79

/**
 * This function is called by MAVLink once a complete, uncorrupted (CRC check valid)
 * mavlink packet is received.
 *
 * @param link Hardware link the message came from (e.g. /dev/ttyUSB0 or UDP port).
 *             messages can be sent back to the system via this link
 * @param message MAVLink message, as received from the MAVLink protocol stack
 */
80
void ArduPilotMegaMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
81 82 83
{
    // Let UAS handle the default message set
    UAS::receiveMessage(link, message);
84 85

    if (message.sysid == uasId) {
86
        // Handle your special messages
87
        switch (message.msgid) {
88 89
        case MAVLINK_MSG_ID_HEARTBEAT:
        {
90 91 92
            //qDebug() << "ARDUPILOT RECEIVED HEARTBEAT";
            break;
        }
93
        default:
94
            //qDebug() << "\nARDUPILOT RECEIVED MESSAGE WITH ID" << message.msgid;
95 96 97 98
            break;
        }
    }
}
99 100
void ArduPilotMegaMAV::setMountConfigure(unsigned char mode, bool stabilize_roll,bool stabilize_pitch,bool stabilize_yaw)
{
101
#ifdef QGC_USE_ARDUPILOTMEGA_MESSAGES
102 103 104 105
    //Only supported by APM
    mavlink_message_t msg;
    mavlink_msg_mount_configure_pack(255,1,&msg,this->uasId,1,mode,stabilize_roll,stabilize_pitch,stabilize_yaw);
    sendMessage(msg);
106 107 108 109 110
#else
    Q_UNUSED(mode);
    Q_UNUSED(stabilize_roll);
    Q_UNUSED(stabilize_pitch);
    Q_UNUSED(stabilize_yaw);
111
#endif
112 113 114
}
void ArduPilotMegaMAV::setMountControl(double pa,double pb,double pc,bool islatlong)
{
115
#ifdef QGC_USE_ARDUPILOTMEGA_MESSAGES
116 117 118 119 120 121 122 123 124 125
    mavlink_message_t msg;
    if (islatlong)
    {
        mavlink_msg_mount_control_pack(255,1,&msg,this->uasId,1,pa*10000000.0,pb*10000000.0,pc*10000000.0,0);
    }
    else
    {
        mavlink_msg_mount_control_pack(255,1,&msg,this->uasId,1,pa,pb,pc,0);
    }
    sendMessage(msg);
126 127 128 129 130
#else
    Q_UNUSED(pa);
    Q_UNUSED(pb);
    Q_UNUSED(pc);
    Q_UNUSED(islatlong);
131
#endif
132
}