SlugsMAV.cc 5.73 KB
Newer Older
lm's avatar
lm committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23
/*=====================================================================

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

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

24 25
#include "SlugsMAV.h"

26 27
#include <QDebug>

28 29 30 31 32 33
SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) :
        UAS(mavlink, id)//,
        // Place other initializers here
{
}

34 35 36 37 38 39 40 41
/**
 * 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
 */
42 43
void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
44 45 46
    // Let UAS handle the default message set
    UAS::receiveMessage(link, message);

unknown's avatar
unknown committed
47
    // Handle your special messages mavlink_message_t* msg = &message;
48 49 50 51
    switch (message.msgid)
    {
    case MAVLINK_MSG_ID_HEARTBEAT:
        {
52
            qDebug() << "SLUGS RECEIVED HEARTBEAT";
unknown's avatar
unknown committed
53 54
            break;
        }
55 56 57

#ifdef MAVLINK_ENABLED_SLUGS_MESSAGES

unknown's avatar
unknown committed
58 59 60 61 62 63
    case MAVLINK_MSG_ID_CPU_LOAD:
        {
            mavlink_cpu_load_t cpu_load;
            quint64 time = getUnixTime(0);
            mavlink_msg_cpu_load_decode(&message,&cpu_load);

64 65 66
                emit valueChanged(uasId, tr("SensorDSC Load"), cpu_load.sensLoad, time);
                emit valueChanged(uasId, tr("ControlDSC Load"), cpu_load.ctrlLoad, time);
                emit valueChanged(uasId, tr("Battery Volt"), cpu_load.batVolt, time);
unknown's avatar
unknown committed
67 68 69 70 71 72 73 74

            break;
        }
    case MAVLINK_MSG_ID_AIR_DATA:
        {
            mavlink_air_data_t air_data;
            quint64 time = getUnixTime(0);
            mavlink_msg_air_data_decode(&message,&air_data);
75 76 77
                emit valueChanged(uasId, tr("Dynamic Pressure"), air_data.dynamicPressure,time);
                emit valueChanged(uasId, tr("Static Pressure"),air_data.staticPressure, time);
                emit valueChanged(uasId, tr("Temp"),air_data.temperature,time);
unknown's avatar
unknown committed
78 79 80 81 82 83 84 85

            break;
        }
    case MAVLINK_MSG_ID_SENSOR_BIAS:
        {
            mavlink_sensor_bias_t sensor_bias;
            quint64 time = getUnixTime(0);
            mavlink_msg_sensor_bias_decode(&message,&sensor_bias);
86 87 88 89 90 91
                emit valueChanged(uasId,tr("Ax Bias"),sensor_bias.axBias, time);
                emit valueChanged(uasId,tr("Ay Bias"),sensor_bias.ayBias,time);
                emit valueChanged(uasId,tr("Az Bias"),sensor_bias.azBias,time);
                emit valueChanged(uasId,tr("Gx Bias"),sensor_bias.gxBias,time);
                emit valueChanged(uasId,tr("Gy Bias"),sensor_bias.gyBias,time);
                emit valueChanged(uasId,tr("Gz Bias"),sensor_bias.gzBias,time);
unknown's avatar
unknown committed
92 93 94 95 96 97 98 99

            break;
        }
    case MAVLINK_MSG_ID_DIAGNOSTIC:
        {
            mavlink_diagnostic_t diagnostic;
            quint64 time = getUnixTime(0);
            mavlink_msg_diagnostic_decode(&message,&diagnostic);
100 101 102 103 104 105
                emit valueChanged(uasId,tr("Diag F1"),diagnostic.diagFl1,time);
                emit valueChanged(uasId,tr("Diag F2"),diagnostic.diagFl2,time);
                emit valueChanged(uasId,tr("Diag F3"),diagnostic.diagFl3,time);
                emit valueChanged(uasId,tr("Diag S1"),diagnostic.diagSh1,time);
                emit valueChanged(uasId,tr("Diag S2"),diagnostic.diagSh2,time);
                emit valueChanged(uasId,tr("Diag S3"),diagnostic.diagSh3,time);
unknown's avatar
unknown committed
106 107 108 109 110 111 112 113

            break;
        }
    case MAVLINK_MSG_ID_PILOT_CONSOLE:
        {
            mavlink_pilot_console_t pilot;
            quint64 time = getUnixTime(0);
            mavlink_msg_pilot_console_decode(&message,&pilot);
114 115 116 117 118
                emit valueChanged(uasId,"dt",pilot.dt,time);
                emit valueChanged(uasId,"dla",pilot.dla,time);
                emit valueChanged(uasId,"dra",pilot.dra,time);
                emit valueChanged(uasId,"dr",pilot.dr,time);
                emit valueChanged(uasId,"de",pilot.de,time);
unknown's avatar
unknown committed
119 120 121 122 123 124 125 126

            break;
        }
    case MAVLINK_MSG_ID_PWM_COMMANDS:
        {
            mavlink_pwm_commands_t pwm;
            quint64 time = getUnixTime(0);
            mavlink_msg_pwm_commands_decode(&message,&pwm);
127 128 129 130 131 132 133 134 135 136
                emit valueChanged(uasId,"dt_c",pwm.dt_c,time);
                emit valueChanged(uasId,"dla_c",pwm.dla_c,time);
                emit valueChanged(uasId,"dra_c",pwm.dra_c,time);
                emit valueChanged(uasId,"dr_c",pwm.dr_c,time);
                emit valueChanged(uasId,"dle_c",pwm.dle_c,time);
                emit valueChanged(uasId,"dre_c",pwm.dre_c,time);
                emit valueChanged(uasId,"dlf_c",pwm.dlf_c,time);
                emit valueChanged(uasId,"drf_c",pwm.drf_c,time);
                emit valueChanged(uasId,"da1_c",pwm.aux1,time);
                emit valueChanged(uasId,"da2_c",pwm.aux2,time);
unknown's avatar
unknown committed
137 138


139 140
            break;
        }
141 142
#endif

143
    default:
144
        qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid;
145 146 147
        break;
    }
}