/*===================================================================== QGroundControl Open Source Ground Control Station (c) 2009, 2010 QGROUNDCONTROL PROJECT 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 . ======================================================================*/ #include "PxQuadMAV.h" #include "GAudioOutput.h" PxQuadMAV::PxQuadMAV(MAVLinkProtocol* mavlink, int id) : UAS(mavlink, id) { } /** * 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) { // Only compile this portion if matching MAVLink packets have been compiled #ifdef MAVLINK_ENABLED_PIXHAWK mavlink_message_t* msg = &message; if (message.sysid == uasId) { switch (message.msgid) { 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", raw.baro, time); emit valueChanged(uasId, "Temperature", "raw", raw.temp, time); } break; 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; case MAVLINK_MSG_ID_PATTERN_DETECTED: { mavlink_pattern_detected_t detected; mavlink_msg_pattern_detected_decode(&message, &detected); QByteArray b; b.resize(256); mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data()); b.append('\0'); 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); } 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; case MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO: { 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; case MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS: { 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; case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: { mavlink_vision_position_estimate_t pos; mavlink_msg_vision_position_estimate_decode(&message, &pos); quint64 time = getUnixTime(pos.usec); //emit valueChanged(uasId, "vis. time", pos.usec, time); 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); 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); } break; case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: { 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: { 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(uasId, "Load", "%", ((float)status.load)/10.0f, getUnixTime()); } break; default: // Let UAS handle the default message set UAS::receiveMessage(link, message); break; } } #else // Let UAS handle the default message set UAS::receiveMessage(link, message); Q_UNUSED(link); Q_UNUSED(message); #endif } void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command) { #ifdef MAVLINK_ENABLED_PIXHAWK 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; mavlink_msg_watchdog_command_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &payload); sendMessage(msg); #else Q_UNUSED(watchdogId); Q_UNUSED(processId); Q_UNUSED(command); #endif }