Newer
Older
/*=====================================================================
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/>.
======================================================================*/
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
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);
pixhawk
committed
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;
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);
{
mavlink_watchdog_heartbeat_t payload;
mavlink_msg_watchdog_heartbeat_decode(msg, &payload);
emit watchdogReceived(this->uasId, payload.watchdog_id, payload.process_count);
}
break;
{
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. 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);
{
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());
default:
// Let UAS handle the default message set
UAS::receiveMessage(link, message);
#else
// Let UAS handle the default message set
UAS::receiveMessage(link, message);
Q_UNUSED(link);
Q_UNUSED(message);
void PxQuadMAV::sendProcessCommand(int watchdogId, int processId, unsigned int command)
{
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);
#else
Q_UNUSED(watchdogId);
Q_UNUSED(processId);
Q_UNUSED(command);