PxQuadMAV.cc 7.45 KB
Newer Older
lm's avatar
lm committed
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
    
lm's avatar
lm committed
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
    
lm's avatar
lm committed
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
    
lm's avatar
lm committed
21 22
======================================================================*/

23
#include "PxQuadMAV.h"
lm's avatar
lm committed
24
#include "GAudioOutput.h"
25 26 27 28 29

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

/**
 * 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)
{
41
    // Only compile this portion if matching MAVLink packets have been compiled
lm's avatar
lm committed
42
#ifdef MAVLINK_ENABLED_PIXHAWK
pixhawk's avatar
pixhawk committed
43
    mavlink_message_t* msg = &message;
pixhawk's avatar
pixhawk committed
44

lm's avatar
lm committed
45
    if (message.sysid == uasId)
46
    {
lm's avatar
lm committed
47
        switch (message.msgid)
48
        {
lm's avatar
lm committed
49 50 51 52 53
        case MAVLINK_MSG_ID_RAW_AUX:
            {
                mavlink_raw_aux_t raw;
                mavlink_msg_raw_aux_decode(&message, &raw);
                quint64 time = getUnixTime(0);
54 55
                emit valueChanged(uasId, "Pressure", "raw", raw.baro, time);
                emit valueChanged(uasId, "Temperature", "raw", raw.temp, time);
lm's avatar
lm committed
56 57
            }
            break;
58 59 60 61 62 63 64 65 66
        case MAVLINK_MSG_ID_IMAGE_TRIGGERED:
            {
                // FIXME Kind of a hack to load data from disk
                mavlink_image_triggered_t img;
                mavlink_msg_image_triggered_decode(&message, &img);
                qDebug() << "IMAGE AVAILABLE:" << img.timestamp;
                emit imageStarted(img.timestamp);
            }
            break;
lm's avatar
lm committed
67 68
        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
            }
            break;
82
        case MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT:
lm's avatar
lm committed
83 84 85 86 87 88 89 90
            {
                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_VISION_POSITION_ESTIMATE:
lm's avatar
lm committed
108
            {
lm's avatar
lm committed
109 110 111
                mavlink_vision_position_estimate_t pos;
                mavlink_msg_vision_position_estimate_decode(&message, &pos);
                quint64 time = getUnixTime(pos.usec);
lm's avatar
lm committed
112
                //emit valueChanged(uasId, "vis. time", pos.usec, time);
113 114 115
                emit valueChanged(uasId, "vis. roll", "rad", pos.roll, time);
                emit valueChanged(uasId, "vis. pitch", "rad", pos.pitch, time);
                emit valueChanged(uasId, "vis. yaw", "rad", pos.yaw, time);
lm's avatar
lm committed
116 117 118
                emit valueChanged(uasId, "vis. x", "m", pos.x, time);
                emit valueChanged(uasId, "vis. y", "m", pos.y, time);
                emit valueChanged(uasId, "vis. z", "m", pos.z, time);
lm's avatar
lm committed
119
            }
lm's avatar
lm committed
120
            break;
lm's avatar
lm committed
121
        case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
122 123 124 125 126 127 128 129 130 131 132 133 134 135 136
            {
                mavlink_vicon_position_estimate_t pos;
                mavlink_msg_vicon_position_estimate_decode(&message, &pos);
                quint64 time = getUnixTime(pos.usec);
                //emit valueChanged(uasId, "vis. time", pos.usec, time);
                emit valueChanged(uasId, "vicon roll", "rad", pos.roll, time);
                emit valueChanged(uasId, "vicon pitch", "rad", pos.pitch, time);
                emit valueChanged(uasId, "vicon yaw", "rad", pos.yaw, time);
                emit valueChanged(uasId, "vicon x", "m", pos.x, time);
                emit valueChanged(uasId, "vicon y", "m", pos.y, time);
                emit valueChanged(uasId, "vicon z", "m", pos.z, time);
                emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
            }
            break;
        case MAVLINK_MSG_ID_AUX_STATUS:
lm's avatar
lm committed
137 138 139 140 141 142 143 144 145
            {
                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);
146
                emit valueChanged(uasId, "Load", "%", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow());
lm's avatar
lm committed
147 148
            }
            break;
149 150 151
        default:
            // Let UAS handle the default message set
            UAS::receiveMessage(link, message);
lm's avatar
lm committed
152 153
            break;
        }
154
    }
lm's avatar
lm committed
155 156

#endif
157
}
158 159 160

void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command)
{
lm's avatar
lm committed
161
#ifdef MAVLINK_ENABLED_PIXHAWK
162 163 164 165 166 167 168
    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
169
    mavlink_msg_watchdog_command_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &payload);
170
    sendMessage(msg);
lm's avatar
lm committed
171 172 173 174
#else
    Q_UNUSED(watchdogId);
    Q_UNUSED(processId);
    Q_UNUSED(command);
lm's avatar
lm committed
175
#endif
176
}