PxQuadMAV.cc 7.56 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
#include "PxQuadMAV.h"
lm's avatar
lm committed
25
#include "GAudioOutput.h"
26 27 28 29 30

PxQuadMAV::PxQuadMAV(MAVLinkProtocol* mavlink, int id) :
        UAS(mavlink, id)
{
}
31 32 33 34 35 36 37 38 39 40 41 42 43

/**
 * 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
 */
void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
    // Let UAS handle the default message set
    UAS::receiveMessage(link, message);
44
    mavlink_message_t* msg = &message;
45

lm's avatar
lm committed
46
    //qDebug() << "PX RECEIVED" << msg->sysid << msg->compid << msg->msgid;
pixhawk's avatar
pixhawk committed
47

lm's avatar
lm committed
48 49
// Only compile this portion if matching MAVLink packets have been compiled
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
pixhawk's avatar
pixhawk committed
50

lm's avatar
lm committed
51
    if (message.sysid == uasId)
52
    {
lm's avatar
lm committed
53 54 55 56
        QString uasState;
        QString stateDescription;
        QString patternPath;
        switch (message.msgid)
57
        {
lm's avatar
lm committed
58 59 60 61 62 63 64 65 66 67 68
        case MAVLINK_MSG_ID_RAW_AUX:
            {
                mavlink_raw_aux_t raw;
                mavlink_msg_raw_aux_decode(&message, &raw);
                quint64 time = getUnixTime(0);
                emit valueChanged(uasId, "Pressure", raw.baro, time);
                emit valueChanged(uasId, "Temperature", raw.temp, time);
            }
            break;
        case MAVLINK_MSG_ID_PATTERN_DETECTED:
            {
pixhawk's avatar
pixhawk committed
69 70
                mavlink_pattern_detected_t detected;
                mavlink_msg_pattern_detected_decode(&message, &detected);
lm's avatar
lm committed
71 72 73 74
                QByteArray b;
                b.resize(256);
                mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data());
                b.append('\0');
75 76 77 78 79
                QString name = QString(b);
                if (detected.type == 0)
                    emit patternDetected(uasId, name, detected.confidence, detected.detected);
                else if (detected.type == 1)
                    emit letterDetected(uasId, name, detected.confidence, detected.detected);
lm's avatar
lm committed
80 81 82 83 84 85 86 87 88 89 90
            }
            break;
    case MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT:
            {
                mavlink_watchdog_heartbeat_t payload;
                mavlink_msg_watchdog_heartbeat_decode(msg, &payload);

                emit watchdogReceived(this->uasId, payload.watchdog_id, payload.process_count);
            }
            break;

91
    case MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO:
lm's avatar
lm committed
92 93 94 95 96 97 98 99
            {
                mavlink_watchdog_process_info_t payload;
                mavlink_msg_watchdog_process_info_decode(msg, &payload);

                emit processReceived(this->uasId, payload.watchdog_id, payload.process_id, QString((const char*)payload.name), QString((const char*)payload.arguments), payload.timeout);
            }
            break;

100
    case MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS:
lm's avatar
lm committed
101 102 103 104 105 106
            {
                mavlink_watchdog_process_status_t payload;
                mavlink_msg_watchdog_process_status_decode(msg, &payload);
                emit processChanged(this->uasId, payload.watchdog_id, payload.process_id, payload.state, (payload.muted == 1) ? true : false, payload.crashes, payload.pid);
            }
            break;
107
    case MAVLINK_MSG_ID_DEBUG_VECT:
lm's avatar
lm committed
108 109 110 111
            {
                mavlink_debug_vect_t vect;
                mavlink_msg_debug_vect_decode(msg, &vect);
                QString str((const char*)vect.name);
lm's avatar
lm committed
112 113 114 115
                quint64 time = getUnixTime(vect.usec);
                emit valueChanged(uasId, str+".x", vect.x, time);
                emit valueChanged(uasId, str+".y", vect.y, time);
                emit valueChanged(uasId, str+".z", vect.z, time);
lm's avatar
lm committed
116 117
            }
            break;
lm's avatar
lm committed
118 119
    case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
            {
lm's avatar
lm committed
120 121 122
                mavlink_vision_position_estimate_t pos;
                mavlink_msg_vision_position_estimate_decode(&message, &pos);
                quint64 time = getUnixTime(pos.usec);
lm's avatar
lm committed
123
                //emit valueChanged(uasId, "vis. time", pos.usec, time);
lm's avatar
lm committed
124 125 126 127 128 129
                emit valueChanged(uasId, "vis. roll", pos.roll, time);
                emit valueChanged(uasId, "vis. pitch", pos.pitch, time);
                emit valueChanged(uasId, "vis. yaw", pos.yaw, time);
                emit valueChanged(uasId, "vis. x", pos.x, time);
                emit valueChanged(uasId, "vis. y", pos.y, time);
                emit valueChanged(uasId, "vis. z", pos.z, time);
lm's avatar
lm committed
130
            }
lm's avatar
lm committed
131
            break;
lm's avatar
lm committed
132
    case MAVLINK_MSG_ID_AUX_STATUS:
lm's avatar
lm committed
133 134 135 136 137 138 139 140 141
            {
                mavlink_aux_status_t status;
                mavlink_msg_aux_status_decode(&message, &status);
                emit loadChanged(this, status.load/10.0f);
                emit errCountChanged(uasId, "IMU", "I2C0", status.i2c0_err_count);
                emit errCountChanged(uasId, "IMU", "I2C1", status.i2c1_err_count);
                emit errCountChanged(uasId, "IMU", "SPI0", status.spi0_err_count);
                emit errCountChanged(uasId, "IMU", "SPI1", status.spi1_err_count);
                emit errCountChanged(uasId, "IMU", "UART", status.uart_total_err_count);
lm's avatar
lm committed
142
                emit UAS::valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow());
lm's avatar
lm committed
143 144
            }
            break;
pixhawk's avatar
pixhawk committed
145 146 147 148
        case MAVLINK_MSG_ID_CONTROL_STATUS:
            {
                mavlink_control_status_t status;
                mavlink_msg_control_status_decode(&message, &status);
149 150 151 152 153 154 155 156 157 158
                // Emit control status vector
                emit attitudeControlEnabled(static_cast<bool>(status.control_att));
                emit positionXYControlEnabled(static_cast<bool>(status.control_pos_xy));
                emit positionZControlEnabled(static_cast<bool>(status.control_pos_z));
                emit positionYawControlEnabled(static_cast<bool>(status.control_pos_yaw));

                // Emit localization status vector
                emit localizationChanged(this, status.position_fix);
                emit visionLocalizationChanged(this, status.vision_fix);
                emit gpsLocalizationChanged(this, status.gps_fix);
pixhawk's avatar
pixhawk committed
159 160
            }
            break;
161
    default:
lm's avatar
lm committed
162 163 164
            // Do nothing
            break;
        }
165
    }
lm's avatar
lm committed
166 167

#endif
168
}
169 170 171

void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command)
{
lm's avatar
lm committed
172
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
173 174 175 176 177 178 179
    mavlink_watchdog_command_t payload;
    payload.target_system_id = uasId;
    payload.watchdog_id = watchdogId;
    payload.process_id = processId;
    payload.command_id = (uint8_t)command;

    mavlink_message_t msg;
lm's avatar
lm committed
180
    mavlink_msg_watchdog_command_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &payload);
181
    sendMessage(msg);
lm's avatar
lm committed
182
#endif
183
}