PxQuadMAV.cc 5.87 KB
Newer Older
1
#include "PxQuadMAV.h"
lm's avatar
lm committed
2
#include "GAudioOutput.h"
3 4 5 6 7

PxQuadMAV::PxQuadMAV(MAVLinkProtocol* mavlink, int id) :
        UAS(mavlink, id)
{
}
8 9 10 11 12 13 14 15 16 17 18 19 20

/**
 * 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);
21
    mavlink_message_t* msg = &message;
22

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

lm's avatar
lm committed
25 26
// Only compile this portion if matching MAVLink packets have been compiled
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
pixhawk's avatar
pixhawk committed
27

lm's avatar
lm committed
28
    if (message.sysid == uasId)
29
    {
lm's avatar
lm committed
30 31 32 33
        QString uasState;
        QString stateDescription;
        QString patternPath;
        switch (message.msgid)
34
        {
lm's avatar
lm committed
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
        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:
            {
                QByteArray b;
                b.resize(256);
                mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data());
                b.append('\0');
                QString path = QString(b);
                bool detected (mavlink_msg_pattern_detected_get_detected(&message) == 1 ? true : false );
                emit detectionReceived(uasId, path, 0, 0, 0, 0, 0, 0, 0, 0, mavlink_msg_pattern_detected_get_confidence(&message), detected);
            }
            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;

64
    case MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO:
lm's avatar
lm committed
65 66 67 68 69 70 71 72
            {
                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;

73
    case MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS:
lm's avatar
lm committed
74 75 76 77 78 79
            {
                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;
80
    case MAVLINK_MSG_ID_DEBUG_VECT:
lm's avatar
lm committed
81 82 83 84
            {
                mavlink_debug_vect_t vect;
                mavlink_msg_debug_vect_decode(msg, &vect);
                QString str((const char*)vect.name);
lm's avatar
lm committed
85 86 87 88
                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
89 90
            }
            break;
lm's avatar
lm committed
91 92
    case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
            {
lm's avatar
lm committed
93 94 95
                mavlink_vision_position_estimate_t pos;
                mavlink_msg_vision_position_estimate_decode(&message, &pos);
                quint64 time = getUnixTime(pos.usec);
lm's avatar
lm committed
96
                //emit valueChanged(uasId, "vis. time", pos.usec, time);
lm's avatar
lm committed
97 98 99 100 101 102 103 104 105 106
                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);
                emit valueChanged(uasId, "vis. vx", pos.vx, time);
                emit valueChanged(uasId, "vis. vy", pos.vy, time);
                emit valueChanged(uasId, "vis. vz", pos.vz, time);
                emit valueChanged(uasId, "vis. vyaw", pos.vyaw, time);
lm's avatar
lm committed
107
            }
lm's avatar
lm committed
108
            break;
lm's avatar
lm committed
109
    case MAVLINK_MSG_ID_AUX_STATUS:
lm's avatar
lm committed
110 111 112 113 114 115 116 117 118 119 120 121
            {
                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);
                emit valueChanged(this, "Load", ((float)status.load)/1000.0f, MG::TIME::getGroundTimeNow());
            }
            break;
122
    default:
lm's avatar
lm committed
123 124 125
            // Do nothing
            break;
        }
126
    }
lm's avatar
lm committed
127 128

#endif
129
}
130 131 132

void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command)
{
lm's avatar
lm committed
133
#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
134 135 136 137 138 139 140
    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
141
    mavlink_msg_watchdog_command_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &payload);
142
    sendMessage(msg);
lm's avatar
lm committed
143
#endif
144
}