Runtime fixes for multi-IMU systems

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;

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;

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 (!userXYSetPointSet)
+        {
+            setBodySetpointCoordinateXY(0, 0);
         }
 
-    if ((event->key() ==  Qt::Key_S))
-    {
-        if (!directSending)
+        if ((event->key() ==  Qt::Key_W))
         {
-            setBodySetpointCoordinateZ(uas->getLocalZ());
-            setBodySetpointCoordinateYaw(uas->getYaw());
-            setBodySetpointCoordinateXY(-1.0, 0);
+            setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()+0.2, +1.5), bodySP.y());
         }
-        else
+
+        if ((event->key() ==  Qt::Key_S))
         {
             setBodySetpointCoordinateXY(qBound(-1.5, bodySP.x()-0.2, +1.5), bodySP.y());
         }
-    }
 
         if ((event->key() ==  Qt::Key_A))
-    {
-        if (!directSending)
-        {
-            setBodySetpointCoordinateZ(uas->getLocalZ());
-            setBodySetpointCoordinateYaw(uas->getYaw());
-            setBodySetpointCoordinateXY(0, -1.0);
-        }
-        else
         {
             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
         {
             setBodySetpointCoordinateXY(bodySP.x(), qBound(-1.5, bodySP.y()+0.2, +1.5));
         }
-    }
 
         if ((event->key() ==  Qt::Key_Up))
         {
-        if (!directSending)
-        {
-            setBodySetpointCoordinateXY(0, 0);
-            setBodySetpointCoordinateYaw(uas->getYaw());
-        }
             setBodySetpointCoordinateZ(-0.5);
         }
 
         if ((event->key() ==  Qt::Key_Down))
         {
-        if (!directSending)
-        {
-            setBodySetpointCoordinateXY(0, 0);
-            setBodySetpointCoordinateYaw(uas->getYaw());
-        }
             setBodySetpointCoordinateZ(+0.5);
         }
 
         if ((event->key() ==  Qt::Key_Left))
         {
-        if (!directSending)
-        {
-            setBodySetpointCoordinateXY(0, 0);
-            setBodySetpointCoordinateZ(uas->getLocalZ());
-        }
             setBodySetpointCoordinateYaw(-0.2);
         }
 
         if ((event->key() ==  Qt::Key_Right))
         {
-        if (!directSending)
-        {
-            setBodySetpointCoordinateXY(0, 0);
-            setBodySetpointCoordinateZ(uas->getLocalZ());
-        }
             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 <ENTER> 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 <ENTER> TO SEND";
@@ -919,16 +893,11 @@ 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");
-        }
-    }
 }
 
 void HSIDisplay::updateAttitudeSetpoints(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 usec)
@@ -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;
 }
 

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:
 };

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<osg::Vec3d>& 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;