From 0bf76fb68ac4d152e5dc0361c441504e2af2be86 Mon Sep 17 00:00:00 2001 From: hengli Date: Thu, 16 Feb 2012 13:05:40 +0100 Subject: [PATCH 01/10] Minor GUI flaws fixed. Visualize links between poses from different component sources for each MAV. --- src/ui/map3D/Pixhawk3DWidget.cc | 77 +++++++++++++++++++++++++++++++++++++++-- src/ui/map3D/Pixhawk3DWidget.h | 1 + 2 files changed, 75 insertions(+), 3 deletions(-) diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index ca2c3c3..4ca9ef1 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -164,7 +164,9 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component, (float)qrand() / RAND_MAX, (float)qrand() / RAND_MAX, 0.5); + systemData.trailNode()->addDrawable(createTrail(color)); + systemData.trailNode()->addDrawable(createLink(uas->getColor())); } QVector& trail = systemData.trailMap()[component]; @@ -222,7 +224,11 @@ Pixhawk3DWidget::attitudeChanged(UASInterface* uas, int component, void Pixhawk3DWidget::showViewParamWindow(void) { - if (!mViewParamWidget->isVisible()) + if (mViewParamWidget->isVisible()) + { + mViewParamWidget->hide(); + } + else { mViewParamWidget->show(); } @@ -817,6 +823,7 @@ void Pixhawk3DWidget::buildLayout(void) { QPushButton* viewParamWindowButton = new QPushButton(this); + viewParamWindowButton->setCheckable(true); viewParamWindowButton->setText("View Parameters"); QHBoxLayout* layoutTop = new QHBoxLayout; @@ -1332,6 +1339,35 @@ Pixhawk3DWidget::createTrail(const osg::Vec4& color) return geometry; } +osg::ref_ptr +Pixhawk3DWidget::createLink(const QColor& color) +{ + osg::ref_ptr geometry(new osg::Geometry()); + geometry->setUseDisplayList(false); + + osg::ref_ptr vertices(new osg::Vec3dArray()); + geometry->setVertexArray(vertices); + + osg::ref_ptr drawArrays(new osg::DrawArrays(osg::PrimitiveSet::LINES)); + geometry->addPrimitiveSet(drawArrays); + + osg::ref_ptr colorArray(new osg::Vec4Array); + colorArray->push_back(osg::Vec4(color.redF(), color.greenF(), color.blueF(), 1.0f)); + geometry->setColorArray(colorArray); + geometry->setColorBinding(osg::Geometry::BIND_OVERALL); + + osg::ref_ptr stateset(new osg::StateSet); + osg::ref_ptr linewidth(new osg::LineWidth()); + linewidth->setWidth(3.0f); + stateset->setAttributeAndModes(linewidth, osg::StateAttribute::ON); + stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF); + stateset->setMode(GL_LINE_SMOOTH, osg::StateAttribute::ON); + stateset->setMode(GL_BLEND, osg::StateAttribute::ON); + geometry->setStateSet(stateset); + + return geometry; +} + osg::ref_ptr Pixhawk3DWidget::createImagery(void) { @@ -1522,7 +1558,8 @@ Pixhawk3DWidget::updateHUD(UASInterface* uas, MAV_FRAME frame) mStatusText->setText(oss.str()); bool darkBackground = true; - if (mImageryNode->getImageryType() == Imagery::GOOGLE_MAP) + if (frame == MAV_FRAME_GLOBAL && + mImageryNode->getImageryType() == Imagery::GOOGLE_MAP) { darkBackground = false; } @@ -1544,13 +1581,15 @@ Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ, { it.next(); - osg::Geometry* geometry = trailNode->getDrawable(it.value())->asGeometry(); + // update trail + osg::Geometry* geometry = trailNode->getDrawable(it.value() * 2)->asGeometry(); osg::DrawArrays* drawArrays = reinterpret_cast(geometry->getPrimitiveSet(0)); osg::ref_ptr vertices(new osg::Vec3Array); const QVector& trail = trailMap.value(it.key()); + vertices->reserve(trail.size()); for (int i = 0; i < trail.size(); ++i) { vertices->push_back(osg::Vec3d(trail[i].y() - robotY, @@ -1562,6 +1601,38 @@ Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ, drawArrays->setFirst(0); drawArrays->setCount(vertices->size()); geometry->dirtyBound(); + + // update link + geometry = trailNode->getDrawable(it.value() * 2 + 1)->asGeometry(); + drawArrays = reinterpret_cast(geometry->getPrimitiveSet(0)); + + vertices = new osg::Vec3Array; + + if (!trail.empty()) + { + QVector3D p(trail.back().x() - robotX, + trail.back().y() - robotY, + trail.back().z() - robotZ); + + double length = p.length(); + p.normalize(); + + for (double i = 0.1; i < length - 0.1; i += 0.3) + { + QVector3D v = p * i; + + vertices->push_back(osg::Vec3d(v.y(), v.x(), -v.z())); + } + } + if (vertices->size() % 2 == 1) + { + vertices->pop_back(); + } + + geometry->setVertexArray(vertices); + drawArrays->setFirst(0); + drawArrays->setCount(vertices->size()); + geometry->dirtyBound(); } } diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index bc25bec..22e75e6 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -123,6 +123,7 @@ private: osg::ref_ptr createLocalGrid(void); osg::ref_ptr createWorldGrid(void); osg::ref_ptr createTrail(const osg::Vec4& color); + osg::ref_ptr createLink(const QColor& color); osg::ref_ptr createImagery(void); osg::ref_ptr createPointCloud(void); osg::ref_ptr createTarget(const QColor& color); From 1529ae86ccf91de1e445562c981f78983f8e73a3 Mon Sep 17 00:00:00 2001 From: hengli Date: Thu, 16 Feb 2012 13:44:01 +0100 Subject: [PATCH 02/10] Disabled mouse-based throwing movement of camera in 3D view. --- src/ui/map3D/GCManipulator.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ui/map3D/GCManipulator.cc b/src/ui/map3D/GCManipulator.cc index b5248c4..a224bfe 100644 --- a/src/ui/map3D/GCManipulator.cc +++ b/src/ui/map3D/GCManipulator.cc @@ -74,8 +74,8 @@ GCManipulator::handle(const osgGA::GUIEventAdapter& ea, if (isMouseMoving()) { if (calcMovement()) { us.requestRedraw(); - us.requestContinuousUpdate(true); - _thrown = true; + us.requestContinuousUpdate(false); + _thrown = false; } } else { flushMouseEventStack(); From ae060b62b899be98b007ce8c01765dc98fa4a628 Mon Sep 17 00:00:00 2001 From: Tobias Simon Date: Thu, 16 Feb 2012 14:26:32 +0100 Subject: [PATCH 03/10] increased GPS sat timeout from 1 second to 10 seconds sending GPS sat information via a low-bandwidth link like the UM96 @ 9600 baud should also work at low frequencies like 0.2 Hz --- src/ui/HSIDisplay.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 9edb219..3fc426a 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -1106,8 +1106,8 @@ void HSIDisplay::drawGPS(QPainter &painter) i.next(); GPSSatellite* sat = i.value(); - // Check if update is not older than 5 seconds, else delete satellite - if (sat->lastUpdate + 1000000 < currTime) { + // Check if update is not older than 10 seconds, else delete satellite + if (sat->lastUpdate + 10000000 < currTime) { // Delete and go to next satellite gpsSatellites.remove(i.key()); if (i.hasNext()) { From 8cf835c3905c5dc3a4309f8f11fb9e0e1beaa84d Mon Sep 17 00:00:00 2001 From: Bryant Mairs Date: Thu, 16 Feb 2012 09:07:31 -0800 Subject: [PATCH 04/10] Exposed HEARTBEAT.system_status and HEARTBEAT.base_mode to the realtime plotter widget via the valueChanged signal. --- src/uas/UAS.cc | 45 +++++++++++++++++++++------------------------ src/uas/UASInterface.h | 10 ++++------ 2 files changed, 25 insertions(+), 30 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index efd8b44..dd1c27f 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -282,6 +282,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit heartbeat(this); mavlink_heartbeat_t state; mavlink_msg_heartbeat_decode(&message, &state); + + // Send the base_mode and system_status values to the plotter. This uses the ground time + // so the Ground Time checkbox must be ticked for these values to display + quint64 time = getUnixTime(); + QString name = QString("M%1:HEARTBEAT.%2").arg(message.sysid); + emit valueChanged(uasId, name.arg("base_mode"), "none", state.base_mode, time); + emit valueChanged(uasId, name.arg("system_status"), "none", state.system_status, time); + // Set new type if it has changed if (this->type != state.type) { @@ -384,20 +392,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) GAudioOutput::instance()->stopEmergency(); GAudioOutput::instance()->say(audiostring.toLower()); } - - // if (state.system_status == MAV_STATE_POWEROFF) - // { - // emit systemRemoved(this); - // emit systemRemoved(); - // } } break; - // case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: - // case MAVLINK_MSG_ID_NAMED_VALUE_INT: - // // Receive named value message - // receiveMessageNamedValue(message); - // break; case MAVLINK_MSG_ID_SYS_STATUS: { if (multiComponentSourceDetected && wrongComponent) @@ -432,19 +429,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) stopLowBattAlarm(); } - // Trigger drop rate updates as needed. Here we convert the incoming - // drop_rate_comm value from 1/100 of a percent in a uint16 to a true - // percentage as a float. We also cap the incoming value at 100% as defined - // by the MAVLink specifications. - if (state.drop_rate_comm > 10000) - { - emit dropRateChanged(this->getUASID(), 100.0f); - } - else - { - emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f); - } - } + // Trigger drop rate updates as needed. Here we convert the incoming + // drop_rate_comm value from 1/100 of a percent in a uint16 to a true + // percentage as a float. We also cap the incoming value at 100% as defined + // by the MAVLink specifications. + if (state.drop_rate_comm > 10000) + { + emit dropRateChanged(this->getUASID(), 100.0f); + } + else + { + emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f); + } + } break; case MAVLINK_MSG_ID_ATTITUDE: { diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 4162ba1..7197f28 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -408,24 +408,22 @@ signals: void deactivated(); /** @brief The robot is manually controlled **/ void manualControl(); + /** @brief A value of the robot has changed. * * Typically this is used to send lowlevel information like the battery voltage to the plotting facilities of - * the groundstation + * the groundstation. The data here should be converted to human-readable values before being passed, so ideally + * SI units. * * @param uasId ID of this system * @param name name of the value, e.g. "battery voltage" + * @param unit The units this variable is in as an abbreviation. For system-dependent (such as raw ADC values) use "raw", for unitless values use "none". * @param value the value that changed * @param msec the timestamp of the message, in milliseconds */ - - //void valueChanged(const int uasId, const QString& name, const double value, const quint64 msec); void valueChanged(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec); void valueChanged(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec); -// void valueChanged(const int uasId, const QString& name, const double value, const quint64 msec); -// //void valueChanged(UASInterface* uas, QString name, double value, quint64 msec); - void voltageChanged(int uasId, double voltage); void waypointUpdated(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool active); void waypointSelected(int uasId, int id); From 956cf4cd35b0e38a89b623fb541d123b0a9d6d59 Mon Sep 17 00:00:00 2001 From: TobiasSimon Date: Thu, 16 Feb 2012 19:22:44 +0100 Subject: [PATCH 05/10] fixes segfault when starting QGC without having a UAS selected --- src/ui/map3D/Pixhawk3DWidget.cc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index 4ca9ef1..de8ef89 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -778,7 +778,10 @@ Pixhawk3DWidget::update(void) // updateImagery(robotX, robotY, robotZ, utmZone); } - updateHUD(mActiveUAS, frame); + if (mActiveUAS) + { + updateHUD(mActiveUAS, frame); + } layout()->update(); } From 5114ff8ea1b3cd77098a98400a87b232a9e12028 Mon Sep 17 00:00:00 2001 From: Bryant Mairs Date: Thu, 16 Feb 2012 11:20:57 -0800 Subject: [PATCH 06/10] Added logging of useful values from the HEARTBEAT and SYS_STATUS messages to the realtime plotter. Removed some dead code. Added more valueChanged() signals to account for every data type. This moves the conversion over into the valueChanged receivers, which while it will require more slot-functions, makes it easier to add more code as any necessary conversion is done internally to the slot and so the signaler doesn't need to know the details. I also added some more details on the types of units expected by valueChanged(). --- src/uas/UAS.cc | 33 +++++++++++---- src/uas/UASInterface.h | 11 ++++- src/ui/HDDisplay.cc | 66 +++++++++++++++++++++++++----- src/ui/HDDisplay.h | 29 +++++++++---- src/ui/MAVLinkDecoder.h | 12 ++++-- src/ui/MainWindow.cc | 1 - src/ui/linechart/LinechartWidget.cc | 81 ++++++++++++++++--------------------- src/ui/linechart/LinechartWidget.h | 27 ++++++++----- src/ui/linechart/Linecharts.cc | 41 ++++++++++++------- 9 files changed, 197 insertions(+), 104 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index dd1c27f..e9bd5e8 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -404,8 +404,22 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) mavlink_sys_status_t state; mavlink_msg_sys_status_decode(&message, &state); + // Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present. + quint64 time = getUnixTime(); + QString name = QString("M%1:SYS_STATUS.%2").arg(message.sysid); + emit valueChanged(uasId, name.arg("sensors_enabled"), "bits", state.onboard_control_sensors_enabled, time); + emit valueChanged(uasId, name.arg("sensors_health"), "bits", state.onboard_control_sensors_health, time); + emit valueChanged(uasId, name.arg("errors_comm"), "-", state.errors_comm, time); + emit valueChanged(uasId, name.arg("errors_count1"), "-", state.errors_count1, time); + emit valueChanged(uasId, name.arg("errors_count2"), "-", state.errors_count2, time); + emit valueChanged(uasId, name.arg("errors_count3"), "-", state.errors_count3, time); + emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time); + + // Process CPU load. emit loadChanged(this,state.load/10.0f); + emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time); + // Battery charge/time remaining/voltage calculations currentVoltage = state.voltage_battery/1000.0f; lpVoltage = filterVoltage(currentVoltage); @@ -415,9 +429,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { chargeLevel = state.battery_remaining; } - //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining; emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining); - emit voltageChanged(message.sysid, state.voltage_battery/1000); + emit valueChanged(uasId, name.arg("battery_remaining"), "%", getChargeLevel(), time); + emit voltageChanged(message.sysid, currentVoltage); + emit valueChanged(uasId, name.arg("battery_voltage"), "V", currentVoltage, time); + + // And if the battery current draw is measured, log that also. + if (state.current_battery != -1) + { + emit valueChanged(uasId, name.arg("battery_current"), "A", ((double)state.current_battery) / 100.0f, time); + } // LOW BATTERY ALARM if (lpVoltage < warnVoltage) @@ -435,12 +456,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // by the MAVLink specifications. if (state.drop_rate_comm > 10000) { - emit dropRateChanged(this->getUASID(), 100.0f); - } - else - { - emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f); + state.drop_rate_comm = 10000; } + emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f); + emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time); } break; case MAVLINK_MSG_ID_ATTITUDE: diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 7197f28..4aab4d3 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -417,12 +417,19 @@ signals: * * @param uasId ID of this system * @param name name of the value, e.g. "battery voltage" - * @param unit The units this variable is in as an abbreviation. For system-dependent (such as raw ADC values) use "raw", for unitless values use "none". + * @param unit The units this variable is in as an abbreviation. For system-dependent (such as raw ADC values) use "raw", for bitfields use "bits", for true/false or on/off use "bool", for unitless values use "-". * @param value the value that changed * @param msec the timestamp of the message, in milliseconds */ + void valueChanged(const int uasId, const QString& name, const QString& unit, const quint8 value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const qint8 value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const quint16 value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const qint16 value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const quint32 value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const qint32 value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const quint64 value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const qint64 value, const quint64 msec); void valueChanged(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec); - void valueChanged(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec); void voltageChanged(int uasId, double voltage); void waypointUpdated(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool active); diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index 53a5d63..fd9e6d2 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -478,16 +478,13 @@ void HDDisplay::renderOverlay() */ void HDDisplay::setActiveUAS(UASInterface* uas) { + // Disconnect any previously connected active UAS if (this->uas != NULL) { - // Disconnect any previously connected active MAV - disconnect(this->uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64))); - disconnect(this->uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), this, SLOT(updateValue(int,QString,QString,int,quint64))); + removeSource(this->uas); } // Now connect the new UAS - // Setup communication - connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64))); - connect(uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), this, SLOT(updateValue(int,QString,QString,int,quint64))); + addSource(uas); this->uas = uas; } @@ -842,28 +839,75 @@ float HDDisplay::refLineWidthToPen(float line) return line * 2.50f; } +// Connect a generic source void HDDisplay::addSource(QObject* obj) { //genericSources.append(obj); // FIXME XXX HACK // if (plots.size() > 0) // { - // Connect generic source - connect(obj, SIGNAL(valueChanged(int,QString,QString,int,quint64)), this, SLOT(updateValue(int,QString,QString,int,quint64))); - connect(obj, SIGNAL(valueChanged(int,QString,QString,unsigned int,quint64)), this, SLOT(updateValue(int,QString,QString,unsigned int,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,qint8,quint64)), this, SLOT(updateValue(int,QString,QString,qint8,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), this, SLOT(updateValue(int,QString,QString,quint8,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,qint16,quint64)), this, SLOT(updateValue(int,QString,QString,qint16,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,quint16,quint64)), this, SLOT(updateValue(int,QString,QString,quint16,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,qint32,quint64)), this, SLOT(updateValue(int,QString,QString,qint32,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,quint32,quint64)), this, SLOT(updateValue(int,QString,QString,quint32,quint64))); connect(obj, SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), this, SLOT(updateValue(int,QString,QString,quint64,quint64))); connect(obj, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), this, SLOT(updateValue(int,QString,QString,qint64,quint64))); connect(obj, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64))); // } } -void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec) +// Disconnect a generic source +void HDDisplay::removeSource(QObject* obj) +{ + //genericSources.append(obj); + // FIXME XXX HACK +// if (plots.size() > 0) +// { + disconnect(obj, SIGNAL(valueChanged(int,QString,QString,qint8,quint64)), this, SLOT(updateValue(int,QString,QString,qint8,quint64))); + disconnect(obj, SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), this, SLOT(updateValue(int,QString,QString,quint8,quint64))); + disconnect(obj, SIGNAL(valueChanged(int,QString,QString,qint16,quint64)), this, SLOT(updateValue(int,QString,QString,qint16,quint64))); + disconnect(obj, SIGNAL(valueChanged(int,QString,QString,quint16,quint64)), this, SLOT(updateValue(int,QString,QString,quint16,quint64))); + disconnect(obj, SIGNAL(valueChanged(int,QString,QString,qint32,quint64)), this, SLOT(updateValue(int,QString,QString,qint32,quint64))); + disconnect(obj, SIGNAL(valueChanged(int,QString,QString,quint32,quint64)), this, SLOT(updateValue(int,QString,QString,quint32,quint64))); + disconnect(obj, SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), this, SLOT(updateValue(int,QString,QString,quint64,quint64))); + disconnect(obj, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), this, SLOT(updateValue(int,QString,QString,qint64,quint64))); + disconnect(obj, SIGNAL(valueChanged(int,QString,QString,double,quint64)), this, SLOT(updateValue(int,QString,QString,double,quint64))); +// } +} + +void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const qint8 value, const quint64 msec) +{ + if (!intValues.contains(name)) intValues.insert(name, true); + updateValue(uasId, name, unit, (double)value, msec); +} + +void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const quint8 value, const quint64 msec) +{ + if (!intValues.contains(name)) intValues.insert(name, true); + updateValue(uasId, name, unit, (double)value, msec); +} + +void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const qint16 value, const quint64 msec) +{ + if (!intValues.contains(name)) intValues.insert(name, true); + updateValue(uasId, name, unit, (double)value, msec); +} + +void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const quint16 value, const quint64 msec) +{ + if (!intValues.contains(name)) intValues.insert(name, true); + updateValue(uasId, name, unit, (double)value, msec); +} + +void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const qint32 value, const quint64 msec) { if (!intValues.contains(name)) intValues.insert(name, true); updateValue(uasId, name, unit, (double)value, msec); } -void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const unsigned int value, const quint64 msec) +void HDDisplay::updateValue(const int uasId, const QString& name, const QString& unit, const quint32 value, const quint64 msec) { if (!intValues.contains(name)) intValues.insert(name, true); updateValue(uasId, name, unit, (double)value, msec); diff --git a/src/ui/HDDisplay.h b/src/ui/HDDisplay.h index cdec1cc..3993495 100644 --- a/src/ui/HDDisplay.h +++ b/src/ui/HDDisplay.h @@ -64,18 +64,31 @@ public: ~HDDisplay(); public slots: - /** @brief Update a HDD double value */ - void updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec); - /** @brief Update a HDD integer value */ - void updateValue(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec); - /** @brief Update a HDD integer value */ - void updateValue(const int uasId, const QString& name, const QString& unit, const unsigned int value, const quint64 msec); - /** @brief Update a HDD integer value */ + /** @brief Update the HDD with new int8 data */ + void updateValue(const int uasId, const QString& name, const QString& unit, const qint8 value, const quint64 msec); + /** @brief Update the HDD with new uint8 data */ + void updateValue(const int uasId, const QString& name, const QString& unit, const quint8 value, const quint64 msec); + /** @brief Update the HDD with new int16 data */ + void updateValue(const int uasId, const QString& name, const QString& unit, const qint16 value, const quint64 msec); + /** @brief Update the HDD with new uint16 data */ + void updateValue(const int uasId, const QString& name, const QString& unit, const quint16 value, const quint64 msec); + /** @brief Update the HDD with new int32 data */ + void updateValue(const int uasId, const QString& name, const QString& unit, const qint32 value, const quint64 msec); + /** @brief Update the HDD with new uint32 data */ + void updateValue(const int uasId, const QString& name, const QString& unit, const quint32 value, const quint64 msec); + /** @brief Update the HDD with new int64 data */ void updateValue(const int uasId, const QString& name, const QString& unit, const qint64 value, const quint64 msec); - /** @brief Update a HDD integer value */ + /** @brief Update the HDD with new uint64 data */ void updateValue(const int uasId, const QString& name, const QString& unit, const quint64 value, const quint64 msec); + /** @brief Update the HDD with new double data */ + void updateValue(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec); + virtual void setActiveUAS(UASInterface* uas); + + /** @brief Connects a source to the updateValue() signals */ void addSource(QObject* obj); + /** @brief Disconnects a source to the updateValue() signals */ + void removeSource(QObject* obj); /** @brief Removes a plot item by the action data */ void removeItemByAction(); diff --git a/src/ui/MAVLinkDecoder.h b/src/ui/MAVLinkDecoder.h index c0c570b..27969f5 100644 --- a/src/ui/MAVLinkDecoder.h +++ b/src/ui/MAVLinkDecoder.h @@ -12,12 +12,16 @@ public: signals: void textMessageReceived(int uasid, int componentid, int severity, const QString& text); - void valueChanged(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec); - void valueChanged(const int uasId, const QString& name, const QString& unit, const int value, const quint64 msec); - void valueChanged(const int uasId, const QString& name, const QString& unit, const unsigned int value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const quint8 value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const qint8 value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const quint16 value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const qint16 value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const quint32 value, const quint64 msec); + void valueChanged(const int uasId, const QString& name, const QString& unit, const qint32 value, const quint64 msec); void valueChanged(const int uasId, const QString& name, const QString& unit, const quint64 value, const quint64 msec); void valueChanged(const int uasId, const QString& name, const QString& unit, const qint64 value, const quint64 msec); - + void valueChanged(const int uasId, const QString& name, const QString& unit, const double value, const quint64 msec); + public slots: /** @brief Receive one message from the protocol and decode it */ diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 84e23d9..f1f53e4 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -1167,7 +1167,6 @@ void MainWindow::addLink(LinkInterface *link) MAVLinkSimulationLink* sim = dynamic_cast(link); if (sim) { - //connect(sim, SIGNAL(valueChanged(int,QString,double,quint64)), linechart, SLOT(appendData(int,QString,double,quint64))); connect(ui.actionSimulate, SIGNAL(triggered(bool)), sim, SLOT(connectLink(bool))); } } diff --git a/src/ui/linechart/LinechartWidget.cc b/src/ui/linechart/LinechartWidget.cc index d580cb3..da28570 100644 --- a/src/ui/linechart/LinechartWidget.cc +++ b/src/ui/linechart/LinechartWidget.cc @@ -294,40 +294,37 @@ void LinechartWidget::createLayout() connect(scalingLogButton, SIGNAL(clicked()), activePlot, SLOT(setLogarithmicScaling())); } -void LinechartWidget::appendData(int uasId, QString curve, double value, quint64 usec) +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, qint8 value, quint64 usec) { - static const QString unit("-"); - if ((selectedMAV == -1 && isVisible()) || (selectedMAV == uasId && isVisible())) - { - // Order matters here, first append to plot, then update curve list - activePlot->appendData(curve+unit, usec, value); - // Store data - QLabel* label = curveLabels->value(curve+unit, NULL); - // Make sure the curve will be created if it does not yet exist - if(!label) - { - addCurve(curve, unit); - } - } + appendData(uasId, curve, unit, static_cast(value), usec); +} - // Log data - if (logging) - { - if (activePlot->isVisible(curve+unit)) - { - if (usec == 0) usec = QGC::groundTimeMilliseconds(); - if (logStartTime == 0) logStartTime = usec; - qint64 time = usec - logStartTime; - if (time < 0) time = 0; +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, quint8 value, quint64 usec) +{ + appendData(uasId, curve, unit, static_cast(value), usec); +} - logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value,'g',18) + "\n").toLatin1()); - logFile->flush(); - } - } +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, qint16 value, quint64 usec) +{ + appendData(uasId, curve, unit, static_cast(value), usec); } +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, quint16 value, quint64 usec) +{ + appendData(uasId, curve, unit, static_cast(value), usec); +} -void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec) +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, qint32 value, quint64 usec) +{ + appendData(uasId, curve, unit, static_cast(value), usec); +} + +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, quint32 value, quint64 usec) +{ + appendData(uasId, curve, unit, static_cast(value), usec); +} + +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, qint64 value, quint64 usec) { if ((selectedMAV == -1 && isVisible()) || (selectedMAV == uasId && isVisible())) { @@ -338,9 +335,12 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString& // Make sure the curve will be created if it does not yet exist if(!label) { - //qDebug() << "ADDING CURVE IN APPENDDATE DOUBLE"; + intData.insert(curve+unit, 0); addCurve(curve, unit); } + + // Add int data + intData.insert(curve+unit, value); } // Log data @@ -353,23 +353,13 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString& qint64 time = usec - logStartTime; if (time < 0) time = 0; - logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value,'g',18) + "\n").toLatin1()); + logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1()); logFile->flush(); } } } -void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, int value, quint64 usec) -{ - appendData(uasId, curve, unit, static_cast(value), usec); -} - -void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, unsigned int value, quint64 usec) -{ - appendData(uasId, curve, unit, static_cast(value), usec); -} - -void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, qint64 value, quint64 usec) +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, quint64 value, quint64 usec) { if ((selectedMAV == -1 && isVisible()) || (selectedMAV == uasId && isVisible())) { @@ -404,7 +394,7 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString& } } -void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, quint64 value, quint64 usec) +void LinechartWidget::appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec) { if ((selectedMAV == -1 && isVisible()) || (selectedMAV == uasId && isVisible())) { @@ -415,12 +405,9 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString& // Make sure the curve will be created if it does not yet exist if(!label) { - intData.insert(curve+unit, 0); + //qDebug() << "ADDING CURVE IN APPENDDATE DOUBLE"; addCurve(curve, unit); } - - // Add int data - intData.insert(curve+unit, value); } // Log data @@ -433,7 +420,7 @@ void LinechartWidget::appendData(int uasId, const QString& curve, const QString& qint64 time = usec - logStartTime; if (time < 0) time = 0; - logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value) + "\n").toLatin1()); + logFile->write(QString(QString::number(time) + "\t" + QString::number(uasId) + "\t" + curve + "\t" + QString::number(value,'g',18) + "\n").toLatin1()); logFile->flush(); } } diff --git a/src/ui/linechart/LinechartWidget.h b/src/ui/linechart/LinechartWidget.h index ff429bc..d2a5826 100644 --- a/src/ui/linechart/LinechartWidget.h +++ b/src/ui/linechart/LinechartWidget.h @@ -77,18 +77,25 @@ public slots: void recolor(); /** @brief Set short names for curves */ void setShortNames(bool enable); - /** @brief Append data without unit */ - void appendData(int uasId, QString curve, double data, quint64 usec); - /** @brief Append data with unit */ - void appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec); - /** @brief Append data as int with unit */ - void appendData(int uasId, const QString& curve, const QString& unit, int value, quint64 usec); - /** @brief Append data as unsigned int with unit */ - void appendData(int uasId, const QString& curve, const QString& unit, unsigned int value, quint64 usec); - /** @brief Append data as int64 with unit */ + /** @brief Append int8 data to the given curve. */ + void appendData(int uasId, const QString& curve, const QString& unit, qint8 value, quint64 usec); + /** @brief Append uint8 data to the given curve. */ + void appendData(int uasId, const QString& curve, const QString& unit, quint8 value, quint64 usec); + /** @brief Append int16 data to the given curve. */ + void appendData(int uasId, const QString& curve, const QString& unit, qint16 value, quint64 usec); + /** @brief Append uint16 data to the given curve. */ + void appendData(int uasId, const QString& curve, const QString& unit, quint16 value, quint64 usec); + /** @brief Append int32 data to the given curve. */ + void appendData(int uasId, const QString& curve, const QString& unit, qint32 value, quint64 usec); + /** @brief Append uint32 data to the given curve. */ + void appendData(int uasId, const QString& curve, const QString& unit, quint32 value, quint64 usec); + /** @brief Append int64 data to the given curve. */ void appendData(int uasId, const QString& curve, const QString& unit, qint64 value, quint64 usec); - /** @brief Append data as uint64 with unit */ + /** @brief Append uint64 data to the given curve. */ void appendData(int uasId, const QString& curve, const QString& unit, quint64 value, quint64 usec); + /** @brief Append double data to the given curve. */ + void appendData(int uasId, const QString& curve, const QString& unit, double value, quint64 usec); + void takeButtonClick(bool checked); void setPlotWindowPosition(int scrollBarValue); void setPlotWindowPosition(quint64 position); diff --git a/src/ui/linechart/Linecharts.cc b/src/ui/linechart/Linecharts.cc index b7f954d..5e7b841 100644 --- a/src/ui/linechart/Linecharts.cc +++ b/src/ui/linechart/Linecharts.cc @@ -93,12 +93,17 @@ void Linecharts::addSystem(UASInterface* uas) LinechartWidget* widget = new LinechartWidget(uas->getUASID(), this); addWidget(widget); plots.insert(uas->getUASID(), widget); - // Values without unit - //connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), widget, SLOT(appendData(int,QString,double,quint64))); - // Values with unit as double - connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64))); - // Values with unit as integer - connect(uas, SIGNAL(valueChanged(int,QString,QString,int,quint64)), widget, SLOT(appendData(int,QString,QString,int,quint64))); + + // Connect valueChanged signals + connect(uas, SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), widget, SLOT(appendData(int,QString,QString,quint8,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,qint8,quint64)), widget, SLOT(appendData(int,QString,QString,qint8,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,quint16,quint64)), widget, SLOT(appendData(int,QString,QString,quint16,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,qint16,quint64)), widget, SLOT(appendData(int,QString,QString,qint16,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,quint32,quint64)), widget, SLOT(appendData(int,QString,QString,quint32,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,qint32,quint64)), widget, SLOT(appendData(int,QString,QString,qint32,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), widget, SLOT(appendData(int,QString,QString,quint64,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), widget, SLOT(appendData(int,QString,QString,qint64,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,QString,double,quint64)), widget, SLOT(appendData(int,QString,QString,double,quint64))); connect(widget, SIGNAL(logfileWritten(QString)), this, SIGNAL(logfileWritten(QString))); // Set system active if this is the only system @@ -110,11 +115,15 @@ void Linecharts::addSystem(UASInterface* uas) // Connect generic sources for (int i = 0; i < genericSources.count(); ++i) { - connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,int,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,int,quint64))); - connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,unsigned int,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,unsigned int,quint64))); - connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint64,quint64))); - connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint64,quint64))); - connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,double,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,double,quint64))); + connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint8,quint64))); + connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,qint8,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint8,quint64))); + connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,quint16,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint16,quint64))); + connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,qint16,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint16,quint64))); + connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,quint32,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint32,quint64))); + connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,qint32,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint32,quint64))); + connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint64,quint64))); + connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint64,quint64))); + connect(genericSources[i], SIGNAL(valueChanged(int,QString,QString,double,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,double,quint64))); } // Select system selectSystem(uas->getUASID()); @@ -130,10 +139,14 @@ void Linecharts::addSource(QObject* obj) if (plots.size() > 0) { // Connect generic source - connect(obj, SIGNAL(valueChanged(int,QString,QString,int,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,int,quint64))); - connect(obj, SIGNAL(valueChanged(int,QString,QString,unsigned int,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,unsigned int,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,quint8,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint8,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,qint8,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint8,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,quint16,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint16,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,qint16,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint16,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,quint32,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint32,quint64))); + connect(obj, SIGNAL(valueChanged(int,QString,QString,qint32,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint32,quint64))); connect(obj, SIGNAL(valueChanged(int,QString,QString,quint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,quint64,quint64))); connect(obj, SIGNAL(valueChanged(int,QString,QString,qint64,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,qint64,quint64))); connect(obj, SIGNAL(valueChanged(int,QString,QString,double,quint64)), plots.values().first(), SLOT(appendData(int,QString,QString,double,quint64))); } -} +} \ No newline at end of file From 9405b6f1fe2c736acac296600bfd89d73973a245 Mon Sep 17 00:00:00 2001 From: TobiasSimon Date: Sat, 18 Feb 2012 13:21:01 +0100 Subject: [PATCH 07/10] fixed out-of-order bug in packet loss calculation --- src/comm/MAVLinkProtocol.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 77f309f..d3924cf 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -370,7 +370,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) int16_t lostMessages = message.seq - expectedIndex; if (lostMessages < 0) { - lostMessages += 256; + // Usually, this happens in the case of an out-of order packet + lostMessages = 0; } qDebug() << QString("Lost %1 messages: expected sequence ID %2 but received %3.").arg(lostMessages).arg(expectedIndex).arg(message.seq); From 42e3368f8c9c46c1e0ec502568c1f542ed3231ea Mon Sep 17 00:00:00 2001 From: TobiasSimon Date: Sat, 18 Feb 2012 13:32:27 +0100 Subject: [PATCH 08/10] fixed debug output and removed tab indents --- src/comm/MAVLinkProtocol.cc | 42 ++++++++++++++++++++++-------------------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index d3924cf..aaf9378 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -351,33 +351,35 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // Update last message sequence ID uint8_t expectedIndex; - if (lastIndex[message.sysid][message.compid] == -1) + if (lastIndex[message.sysid][message.compid] == -1) { lastIndex[message.sysid][message.compid] = message.seq; expectedIndex = message.seq; - } + } else { - // NOTE: Using uint8_t here auto-wraps the number around to 0. + // NOTE: Using uint8_t here auto-wraps the number around to 0. expectedIndex = lastIndex[message.sysid][message.compid] + 1; - } - - // Make some noise if a message was skipped + } + + // Make some noise if a message was skipped //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "EXPECTED INDEX:" << expectedIndex << "SEQ" << message.seq; - if (message.seq != expectedIndex) - { - // Determine how many messages were skipped accounting for 0-wraparound - int16_t lostMessages = message.seq - expectedIndex; - if (lostMessages < 0) - { - // Usually, this happens in the case of an out-of order packet - lostMessages = 0; - } - qDebug() << QString("Lost %1 messages: expected sequence ID %2 but received %3.").arg(lostMessages).arg(expectedIndex).arg(message.seq); - - totalLossCounter += lostMessages; - currLossCounter += lostMessages; - } + if (message.seq != expectedIndex) + { + // Determine how many messages were skipped accounting for 0-wraparound + int16_t lostMessages = message.seq - expectedIndex; + if (lostMessages < 0) + { + // Usually, this happens in the case of an out-of order packet + lostMessages = 0; + } + else + { + qDebug() << QString("Lost %1 messages: expected sequence ID %2 but received %3.").arg(lostMessages).arg(expectedIndex).arg(message.seq); + } + totalLossCounter += lostMessages; + currLossCounter += lostMessages; + } // Update the last sequence ID lastIndex[message.sysid][message.compid] = message.seq; From 2a42532fc9a55ce540f9ddc1f80715e4f0df267f Mon Sep 17 00:00:00 2001 From: TobiasSimon Date: Sat, 18 Feb 2012 15:58:00 +0100 Subject: [PATCH 09/10] symptom: heartbeat Hz value was always 0 cause: check for null pointer had no effect (static memory) solution: fill mavlink messages (and thus) message type with 0xff. this patch assumes that message ID 0xff is never used positiv side effect: qgroundcontrol uses 2% less cpu on my machine (eeepc) --- src/ui/QGCMAVLinkInspector.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ui/QGCMAVLinkInspector.cc b/src/ui/QGCMAVLinkInspector.cc index 48cc8f0..a3892d8 100644 --- a/src/ui/QGCMAVLinkInspector.cc +++ b/src/ui/QGCMAVLinkInspector.cc @@ -24,7 +24,7 @@ QGCMAVLinkInspector::QGCMAVLinkInspector(MAVLinkProtocol* protocol, QWidget *par 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); + memset(receivedMessages, 0xFF, sizeof(mavlink_message_t)*256); connect(protocol, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), this, SLOT(receiveMessage(LinkInterface*,mavlink_message_t))); QStringList header; @@ -95,7 +95,7 @@ void QGCMAVLinkInspector::refreshView() { mavlink_message_t* msg = receivedMessages+i; // Ignore NULL values - if (!msg) continue; + if (msg->msgid == 0xFF) continue; // Update the tree view QString messageName("%1 (%2 Hz, #%3)"); float msgHz = (1.0f-updateHzLowpass)*messagesHz.value(msg->msgid, 0) + updateHzLowpass*((float)messageCount.value(msg->msgid, 0))/((float)updateInterval/1000.0f); From 6411609deac3faabb0e5422b8008e8d85bc9ebc5 Mon Sep 17 00:00:00 2001 From: TobiasSimon Date: Sun, 19 Feb 2012 21:21:44 +0100 Subject: [PATCH 10/10] Added emits for gps and controller flags in HSI window --- src/uas/UAS.cc | 18 ++++++++++++++++++ src/ui/HSIDisplay.cc | 20 ++++++++++++++++++++ 2 files changed, 38 insertions(+) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index e9bd5e8..e63e542 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -450,6 +450,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) stopLowBattAlarm(); } + + // control_sensors_enabled: + // relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control + emit attitudeControlEnabled(state.onboard_control_sensors_enabled & (1 << 11)); + emit positionYawControlEnabled(state.onboard_control_sensors_enabled & (1 << 12)); + emit positionZControlEnabled(state.onboard_control_sensors_enabled & (1 << 13)); + emit positionXYControlEnabled(state.onboard_control_sensors_enabled & (1 << 14)); + // Trigger drop rate updates as needed. Here we convert the incoming // drop_rate_comm value from 1/100 of a percent in a uint16 to a true // percentage as a float. We also cap the incoming value at 100% as defined @@ -579,6 +587,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) speedZ = pos.vz/100.0; emit globalPositionChanged(this, latitude, longitude, altitude, time); emit speedChanged(this, speedX, speedY, speedZ, time); + // Set internal state if (!positionLock) { @@ -600,6 +609,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // only accept values in a realistic range // quint64 time = getUnixTime(pos.time_usec); quint64 time = getUnixTime(pos.time_usec); + + emit gpsLocalizationChanged(this, pos.fix_type); + // TODO: track localization state not only for gps but also for other loc. sources + int loc_type = pos.fix_type; + if (loc_type == 1) + { + loc_type = 0; + } + emit localizationChanged(this, loc_type); if (pos.fix_type > 2) { diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index acaf4f0..295480a 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -472,25 +472,45 @@ void HSIDisplay::drawPositionLock(float x, float y, QString label, int status, b void HSIDisplay::updatePositionLock(UASInterface* uas, bool lock) { Q_UNUSED(uas); + bool changed = positionLock != lock; positionLock = lock; + if (changed) + { + update(); + } } void HSIDisplay::updateAttitudeControllerEnabled(bool enabled) { + bool changed = attControlEnabled != enabled; attControlEnabled = enabled; attControlKnown = true; + if (changed) + { + update(); + } } void HSIDisplay::updatePositionXYControllerEnabled(bool enabled) { + bool changed = xyControlEnabled != enabled; xyControlEnabled = enabled; xyControlKnown = true; + if (changed) + { + update(); + } } void HSIDisplay::updatePositionZControllerEnabled(bool enabled) { + bool changed = zControlEnabled != enabled; zControlEnabled = enabled; zControlKnown = true; + if (changed) + { + update(); + } } void HSIDisplay::updateObjectPosition(unsigned int time, int id, int type, const QString& name, int quality, float bearing, float distance)