diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 484404ae8e370b5ba355c082c56e160b55a41c8b..27056fbf060447bc711fe80aa0aaabecc910942c 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -358,19 +358,19 @@ void MAVLinkSimulationLink::mainloop() if (rate10hzCounter == 1000 / rate / 9) { rate10hzCounter = 1; - float lastX = x; - float lastY = y; - float lastZ = z; - float hackDt = 0.1f; // 100 ms + double lastX = x; + double lastY = y; + double lastZ = z; + double hackDt = 0.1f; // 100 ms // Move X Position x = 12.0*sin(((double)circleCounter)/200.0); y = 5.0*cos(((double)circleCounter)/200.0); z = 1.8 + 1.2*sin(((double)circleCounter)/200.0); - float xSpeed = (x - lastX)/hackDt; - float ySpeed = (y - lastY)/hackDt; - float zSpeed = (z - lastZ)/hackDt; + double xSpeed = (x - lastX)/hackDt; + double ySpeed = (y - lastY)/hackDt; + double zSpeed = (z - lastZ)/hackDt; @@ -414,12 +414,12 @@ void MAVLinkSimulationLink::mainloop() memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; -// // GLOBAL POSITION VEHICLE 2 -// mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, xSpeed, ySpeed, zSpeed); -// bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); -// //add data into datastream -// memcpy(stream+streampointer,buffer, bufferlength); -// streampointer += bufferlength; + // GLOBAL POSITION VEHICLE 2 + mavlink_msg_global_position_int_pack(systemId+1, componentId+1, &ret, 0, (473780.28137103+(x+0.00001))*1E3, (85489.9892510421+((y/2)+0.00001))*1E3, (z+550.0)*1000.0, (z+550.0)*1000.0-1, xSpeed, ySpeed, zSpeed, yaw); + bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); + //add data into datastream + memcpy(stream+streampointer,buffer, bufferlength); + streampointer += bufferlength; // // ATTITUDE VEHICLE 2 // mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0); @@ -427,7 +427,7 @@ void MAVLinkSimulationLink::mainloop() // // GLOBAL POSITION VEHICLE 3 -// mavlink_msg_global_position_int_pack(60, componentId, &ret, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0); +// mavlink_msg_global_position_int_pack(60, componentId, &ret, 0, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0); // bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); // //add data into datastream // memcpy(stream+streampointer,buffer, bufferlength); @@ -565,6 +565,15 @@ void MAVLinkSimulationLink::mainloop() memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; + // Pack message and get size of encoded byte string + messageSize = mavlink_msg_heartbeat_pack(systemId+1, componentId+1, &msg, mavType, MAV_AUTOPILOT_GENERIC, system.base_mode, system.custom_mode, system.system_status); + // Allocate buffer with packet data + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + //qDebug() << "CRC:" << msg.ck_a << msg.ck_b; + //add data into datastream + memcpy(stream+streampointer,buffer, bufferlength); + streampointer += bufferlength; + // Send controller states diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 6bd4065300a0bab00bc120f084514c0d88904b65..89c403e0b58656bd4f5567124e26185f0b143f0b 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -82,6 +82,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), isGlobalPositionKnown(false), systemIsArmed(false) { + for (unsigned int i = 0; i<255;++i) + { + componentID[i] = -1; + componentMulti[i] = false; + } + color = UASInterface::getNextColor(); setBatterySpecs(QString("9V,9.5V,12.6V")); connect(statusTimeout, SIGNAL(timeout()), this, SLOT(updateState())); @@ -210,6 +216,27 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) QString uasState; QString stateDescription; + bool multiComponentSourceDetected = false; + bool wrongComponent = false; + + // Store component ID + if (componentID[message.msgid] == -1) + { + componentID[message.msgid] = message.compid; + } + else + { + // Got this message already + if (componentID[message.msgid] != message.compid) + { + componentMulti[message.msgid] = true; + wrongComponent = true; + } + } + + if (componentMulti[message.msgid] == true) multiComponentSourceDetected = true; + + switch (message.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: @@ -336,6 +363,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) break; case MAVLINK_MSG_ID_SYS_STATUS: { + if (multiComponentSourceDetected && message.compid != MAV_COMP_ID_IMU_2) + { + break; + } mavlink_sys_status_t state; mavlink_msg_sys_status_decode(&message, &state); @@ -371,6 +402,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) break; case MAVLINK_MSG_ID_ATTITUDE: { + if (wrongComponent) break; + mavlink_attitude_t attitude; mavlink_msg_attitude_decode(&message, &attitude); quint64 time = getUnixReferenceTime(attitude.time_boot_ms); @@ -499,7 +532,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (!isnan(alt) && !isinf(alt)) { alt = 0; - emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE"); + //emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE"); } // FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time); // Smaller than threshold and not NaN diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 0493dc4bd669d3cf1c9307c3c704644ec610f7cb..63e5c5a924876792fa0a1d0b99f741c44b8e8de2 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -544,6 +544,8 @@ protected: quint64 getUnixTimeFromMs(quint64 time); /** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */ quint64 getUnixReferenceTime(quint64 time); + int componentID[256]; + bool componentMulti[256]; protected slots: /** @brief Write settings to disk */ diff --git a/src/ui/MAVLinkDecoder.cc b/src/ui/MAVLinkDecoder.cc index 703e09f6382c1210e208bb29a78b65262e02d11c..082b71f93d5d9cd50545437a6dd54181453fd68e 100644 --- a/src/ui/MAVLinkDecoder.cc +++ b/src/ui/MAVLinkDecoder.cc @@ -7,6 +7,11 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) : mavlink_message_info_t msg[256] = MAVLINK_MESSAGE_INFO; memcpy(messageInfo, msg, sizeof(mavlink_message_info_t)*256); memset(receivedMessages, 0, sizeof(mavlink_message_t)*256); + for (unsigned int i = 0; i<255;++i) + { + componentID[i] = -1; + componentMulti[i] = false; + } // Fill filter messageFilter.insert(MAVLINK_MSG_ID_HEARTBEAT, false); @@ -69,6 +74,26 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 time) { + bool multiComponentSourceDetected = false; + bool wrongComponent = false; + + // Store component ID + if (componentID[msg->msgid] == -1) + { + componentID[msg->msgid] = msg->compid; + } + else + { + // Got this message already + if (componentID[msg->msgid] != msg->compid) + { + componentMulti[msg->msgid] = true; + wrongComponent = true; + } + } + + if (componentMulti[msg->msgid] == true) multiComponentSourceDetected = true; + // Add field tree widget item uint8_t msgid = msg->msgid; if (messageFilter.contains(msgid)) return; @@ -78,6 +103,8 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 QString name("%1.%2"); QString unit(""); name = name.arg(messageInfo[msgid].name, fieldName); + if (multiComponentSourceDetected) name.prepend(QString("C%1:").arg(msg->compid)); + name.prepend(QString("M%1:").arg(msg->sysid)); switch (messageInfo[msgid].fields[fieldid].type) { case MAVLINK_TYPE_CHAR: diff --git a/src/ui/MAVLinkDecoder.h b/src/ui/MAVLinkDecoder.h index 54f989430c937cb6a4f1d4c60bb7b2b2b48ee3f6..904b7d55424fff676af6efbaac415dbaa465b2eb 100644 --- a/src/ui/MAVLinkDecoder.h +++ b/src/ui/MAVLinkDecoder.h @@ -30,6 +30,8 @@ protected: mavlink_message_info_t messageInfo[256]; ///< Message information QMap messageFilter; ///< Message/field names not to emit QMap textMessageFilter; ///< Message/field names not to emit in text mode + int componentID[256]; ///< Multi component detection + bool componentMulti[256]; ///< Multi components detected }; diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc index a65ebb4d01c84efa6a92e53670b2ddb83243ac48..809274f7ad43e5a5637015624e43b9fc94913772 100644 --- a/src/ui/QGCMAVLinkInspector.cc +++ b/src/ui/QGCMAVLinkInspector.cc @@ -82,7 +82,7 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m { msgHz = 1; } - float newHz = 0.001f*msgHz+0.999f*messagesHz.value(message.msgid, 1); + float newHz = 0.05f*msgHz+0.95f*messagesHz.value(message.msgid, 1); messagesHz.insert(message.msgid, newHz); }