From f01ad4a2c0e4ff7d2ffb5fbaec8e29b19fe0cd60 Mon Sep 17 00:00:00 2001 From: LM Date: Mon, 20 Feb 2012 09:41:19 +0100 Subject: [PATCH] Runtime fixes for multi-IMU systems --- src/comm/MAVLinkProtocol.cc | 3 +- src/uas/UAS.cc | 4 +- src/ui/HSIDisplay.cc | 121 ++++++++++++-------------------- src/ui/HSIDisplay.h | 2 + src/ui/map3D/Pixhawk3DWidget.cc | 9 +-- 5 files changed, 57 insertions(+), 82 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 77f309f8e..d4f1a00a6 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -372,7 +372,8 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) { lostMessages += 256; } - qDebug() << QString("Lost %1 messages: expected sequence ID %2 but received %3.").arg(lostMessages).arg(expectedIndex).arg(message.seq); + // Console generates excessive load at high loss rates, needs better GUI visualization + //qDebug() << QString("Lost %1 messages for comp %4: expected sequence ID %2 but received %3.").arg(lostMessages).arg(expectedIndex).arg(message.seq).arg(message.compid); totalLossCounter += lostMessages; currLossCounter += lostMessages; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index efd8b4493..cd0099097 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -360,7 +360,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { emit navModeChanged(uasId, state.custom_mode, getNavModeText(state.custom_mode)); navMode = state.custom_mode; - navModeAudio = tr(" changed nav mode to ") + tr("FIXME"); + //navModeAudio = tr(" changed nav mode to ") + tr("FIXME"); } // AUDIO @@ -517,9 +517,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // Emit position always with component ID emit localPositionChanged(this, message.compid, pos.x, pos.y, pos.z, time); + if (!wrongComponent) { - localX = pos.x; localY = pos.y; localZ = pos.z; diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 54d2025d1..c30bc60ce 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -85,7 +85,7 @@ HSIDisplay::HSIDisplay(QWidget *parent) : bodyYawSet(0.0f), uiXSetCoordinate(0.0f), uiYSetCoordinate(0.0f), - uiZSetCoordinate(-0.51f), + uiZSetCoordinate(0.0f), uiYawSet(0.0f), metricWidth(4.0), positionLock(false), @@ -106,7 +106,9 @@ HSIDisplay::HSIDisplay(QWidget *parent) : actionPending(false), directSending(false), userSetPointSet(false), - userXYSetPointSet(false) + userXYSetPointSet(false), + userZSetPointSet(false), + userYawSetPointSet(false) { refreshTimer->setInterval(updateInterval); @@ -580,6 +582,8 @@ void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event) void HSIDisplay::mouseReleaseEvent(QMouseEvent * event) { + // FIXME hardcode yaw to current value + setBodySetpointCoordinateYaw(0); if (mouseHasMoved) { if (event->type() == QMouseEvent::MouseButtonRelease && event->button() == Qt::RightButton) @@ -648,106 +652,74 @@ void HSIDisplay::keyPressEvent(QKeyEvent* event) statusClearTimer.start(); sendBodySetPointCoordinates(); } - - if ((event->key() == Qt::Key_W)) + else { - if (!directSending) + // FIXME hardcode yaw to current value + setBodySetpointCoordinateYaw(0); + + + // Reset setpoints to current position / orientation + // if not initialized + if (!userYawSetPointSet) { - setBodySetpointCoordinateZ(uas->getLocalZ()); - setBodySetpointCoordinateYaw(uas->getYaw()); - setBodySetpointCoordinateXY(1.0, 0); + setBodySetpointCoordinateYaw(0); } - else + + if (!userZSetPointSet) { - setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()+0.2, +1.5), bodySP.y()); + setBodySetpointCoordinateZ(0); } - } - - if ((event->key() == Qt::Key_S)) - { - if (!directSending) + if (!userXYSetPointSet) { - setBodySetpointCoordinateZ(uas->getLocalZ()); - setBodySetpointCoordinateYaw(uas->getYaw()); - setBodySetpointCoordinateXY(-1.0, 0); + setBodySetpointCoordinateXY(0, 0); } - else + + if ((event->key() == Qt::Key_W)) { - setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()-0.2, +1.5), bodySP.y()); + setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()+0.2, +1.5), bodySP.y()); } - } - if ((event->key() == Qt::Key_A)) - { - if (!directSending) + if ((event->key() == Qt::Key_S)) { - setBodySetpointCoordinateZ(uas->getLocalZ()); - setBodySetpointCoordinateYaw(uas->getYaw()); - setBodySetpointCoordinateXY(0, -1.0); + setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()-0.2, +1.5), bodySP.y()); } - else + + if ((event->key() == Qt::Key_A)) { setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()-0.2, +1.5)); } - } - if ((event->key() == Qt::Key_D)) - { - if (!directSending) - { - setBodySetpointCoordinateZ(uas->getLocalZ()); - setBodySetpointCoordinateYaw(uas->getYaw()); - setBodySetpointCoordinateXY(0, 1.0); - } - else + if ((event->key() == Qt::Key_D)) { setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()+0.2, +1.5)); } - } - if ((event->key() == Qt::Key_Up)) - { - if (!directSending) + if ((event->key() == Qt::Key_Up)) { - setBodySetpointCoordinateXY(0, 0); - setBodySetpointCoordinateYaw(uas->getYaw()); + setBodySetpointCoordinateZ(-0.5); } - setBodySetpointCoordinateZ(-0.5); - } - if ((event->key() == Qt::Key_Down)) - { - if (!directSending) + if ((event->key() == Qt::Key_Down)) { - setBodySetpointCoordinateXY(0, 0); - setBodySetpointCoordinateYaw(uas->getYaw()); + setBodySetpointCoordinateZ(+0.5); } - setBodySetpointCoordinateZ(+0.5); - } - if ((event->key() == Qt::Key_Left)) - { - if (!directSending) + if ((event->key() == Qt::Key_Left)) { - setBodySetpointCoordinateXY(0, 0); - setBodySetpointCoordinateZ(uas->getLocalZ()); + setBodySetpointCoordinateYaw(-0.2); } - setBodySetpointCoordinateYaw(-0.2); - } - if ((event->key() == Qt::Key_Right)) - { - if (!directSending) + if ((event->key() == Qt::Key_Right)) { - setBodySetpointCoordinateXY(0, 0); - setBodySetpointCoordinateZ(uas->getLocalZ()); + setBodySetpointCoordinateYaw(0.2); } - setBodySetpointCoordinateYaw(0.2); } - if ((event->key() == Qt::Key_Escape)) + // Overwrite any existing non-zero body setpoints + // for escape + if ((event->key() == Qt::Key_Escape) || (event->key() == Qt::Key_R)) { setBodySetpointCoordinateXY(0, 0); setBodySetpointCoordinateZ(0); @@ -877,6 +849,7 @@ void HSIDisplay::setBodySetpointCoordinateZ(double z) if (uas) { userSetPointSet = true; + userZSetPointSet = true; // Set coordinates and send them out to MAV uiZSetCoordinate = z+uas->getLocalZ(); statusMessage = "Z SET, PRESS TO SEND"; @@ -902,6 +875,7 @@ void HSIDisplay::setBodySetpointCoordinateYaw(double yaw) uiYSetCoordinate = coord.y(); } userSetPointSet = true; + userYawSetPointSet = true; // Set coordinates and send them out to MAV uiYawSet = atan2(sin(yaw+uas->getYaw()), cos(yaw+uas->getYaw())); statusMessage = "YAW SET, PRESS TO SEND"; @@ -919,15 +893,10 @@ void HSIDisplay::sendBodySetPointCoordinates() double dx = uiXSetCoordinate - uas->getLocalX(); double dy = uiYSetCoordinate - uas->getLocalY(); double dz = uiZSetCoordinate - uas->getLocalZ(); - bool valid = (sqrt(dx*dx + dy*dy + dz*dz) < 2.0);//UASManager::instance()->isInLocalNEDSafetyLimits(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate); - if (valid) - { + bool valid = (sqrt(dx*dx + dy*dy + dz*dz) < 200.0);//UASManager::instance()->isInLocalNEDSafetyLimits(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate); +// if (valid) +// { uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet); - } - else - { - setStatusMessage("REJECTED NEW SETPOINT: OUT OF BOUNDS"); - } } } @@ -958,6 +927,8 @@ void HSIDisplay::updateUserPositionSetpoints(int uasid, float xDesired, float yD uiZSetCoordinate = zDesired; uiYawSet = yawDesired; userXYSetPointSet = true; + userZSetPointSet = true; + userYawSetPointSet = true; userSetPointSet = true; } diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h index 6397baa23..7a35dbc6d 100644 --- a/src/ui/HSIDisplay.h +++ b/src/ui/HSIDisplay.h @@ -294,6 +294,8 @@ protected: bool positionSetPointKnown; ///< Position setpoint known status flag bool userSetPointSet; ///< User set X, Y and Z bool userXYSetPointSet; ///< User set the X/Y position already + bool userZSetPointSet; ///< User set the Z position already + bool userYawSetPointSet; ///< User set the YAW position already private: }; diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index ca2c3c3d8..fa99c1beb 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -149,9 +149,6 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component, SystemContainer& systemData = mSystemContainerMap[systemId]; - // update system position - m3DWidget->systemGroup(systemId)->position()->setPosition(osg::Vec3d(y, x, -z)); - // update trail data if (!systemData.trailMap().contains(component)) { @@ -167,6 +164,10 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component, systemData.trailNode()->addDrawable(createTrail(color)); } + // update system position + // FIXME + if (component == 201) m3DWidget->systemGroup(systemId)->position()->setPosition(osg::Vec3d(y, x, -z)); + QVector& trail = systemData.trailMap()[component]; bool addToTrail = false; @@ -685,7 +686,6 @@ Pixhawk3DWidget::update(void) { double x = 0.0, y = 0.0, z = 0.0; getPosition(mActiveUAS, frame, x, y, z); - m3DWidget->recenterCamera(y, x, -z); mCameraPos = QVector3D(x, y, z); @@ -1465,6 +1465,7 @@ Pixhawk3DWidget::resizeHUD(int width, int height) void Pixhawk3DWidget::updateHUD(UASInterface* uas, MAV_FRAME frame) { + if (!uas) return; // display pose of current system double x = 0.0; double y = 0.0; -- 2.22.0