Skip to content
GitLab
Commit b9d48040 authored by LM's avatar LM
Browse files

Added support for multiple components emitting the same message

parent ba326ee6
Hide whitespace changes
Inline Side-by-side
src/comm/MAVLinkSimulationLink.cc
View file @ b9d48040
......@@ -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
......
src/uas/UAS.cc
View file @ b9d48040
......@@ -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
......
src/uas/UAS.h
View file @ b9d48040
......@@ -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 */
......
src/ui/MAVLinkDecoder.cc
View file @ b9d48040
......@@ -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:
......
src/ui/MAVLinkDecoder.h
View file @ b9d48040
......@@ -30,6 +30,8 @@ protected:
mavlink_message_info_t messageInfo[256]; ///< Message information
QMap<uint16_t, bool> messageFilter; ///< Message/field names not to emit
QMap<uint16_t, bool> textMessageFilter; ///< Message/field names not to emit in text mode
int componentID[256]; ///< Multi component detection
bool componentMulti[256]; ///< Multi components detected
};
......
src/ui/QGCMAVLinkInspector.cc
View file @ b9d48040
......@@ -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);
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment