diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index d78cdac7744d67f3d88f6f05ae715800f0b4557a..66f0280d8d9fe78537e1766bd2d6b4bd7298f76d 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -268,6 +268,20 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) if (decodeState == 1) { decodedFirstPacket = true; + + if(message.msgid == MAVLINK_MSG_ID_PING) + { + // process ping requests (tgt_system and tgt_comp must be zero) + mavlink_ping_t ping; + mavlink_msg_ping_decode(&message, &ping); + if(!ping.target_system && !ping.target_component) + { + mavlink_message_t msg; + mavlink_msg_ping_pack(getSystemId(), getComponentId(), &msg, ping.time_usec, ping.seq, message.sysid, message.compid); + sendMessage(msg); + } + } + #if defined(QGC_PROTOBUF_ENABLED) if (message.msgid == MAVLINK_MSG_ID_EXTENDED_MESSAGE) diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index c1286235b177dda58d4b2a9e472de16cdd7ab2ce..89cf722a54ad0cfe2574811d86d1e34c9d8d31f8 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -709,7 +709,7 @@ void QGCXPlaneLink::readBytes() fields_changed |= (1 << 11); emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed, - xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed); + xmag, ymag, zmag, abs_pressure, diff_pressure / 100.0, pressure_alt, temperature, fields_changed); // XXX make these GUI-configurable and add randomness int gps_fix_type = 3;