Robustified current home position setup

src/uas/UAS.cc

@@ -139,7 +139,8 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     hilEnabled(false),
     sensorHil(false),
     lastSendTimeGPS(0),
-    lastSendTimeSensors(0)
+    lastSendTimeSensors(0),
+    blockHomePositionChanges(false)
 {
     for (unsigned int i = 0; i<255;++i)
     {
@@ -1646,9 +1647,12 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
 */
 void UAS::setHomePosition(double lat, double lon, double alt)
 {
+    if (blockHomePositionChanges)
+        return;
+
     QMessageBox msgBox;
     msgBox.setIcon(QMessageBox::Warning);
-    msgBox.setText("Setting new World Coordinate Frame Origin");
+    msgBox.setText(tr("Set a new home position for vehicle %s").arg(getUASName()));
     msgBox.setInformativeText("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location");
     msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
     msgBox.setDefaultButton(QMessageBox::Cancel);
@@ -1674,6 +1678,8 @@ void UAS::setHomePosition(double lat, double lon, double alt)
         qDebug() << "lat:" << home.latitude << " lon:" << home.longitude;
         mavlink_msg_set_gps_global_origin_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &home);
         sendMessage(msg);
+    } else {
+        blockHomePositionChanges = true;
     }
 }
 
@@ -1684,7 +1690,7 @@ void UAS::setLocalOriginAtCurrentGPSPosition()
 {
     QMessageBox msgBox;
     msgBox.setIcon(QMessageBox::Warning);
-    msgBox.setText("Setting new World Coordinate Frame Origin");
+    msgBox.setText("Set the home position at the current GPS position?");
     msgBox.setInformativeText("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location");
     msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
     msgBox.setDefaultButton(QMessageBox::Cancel);

src/uas/UAS.h

@@ -464,6 +464,7 @@ protected: //COMMENTS FOR TEST UNIT
     QByteArray imageRecBuffer;  ///< Buffer for the incoming bytestream
     QImage image;               ///< Image data of last completely transmitted image
     quint64 imageStart;
+    bool blockHomePositionChanges;   ///< Block changes to the home position
 
 #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES)
     px::GLOverlay overlay;

src/uas/UASManager.cc

@@ -75,9 +75,9 @@ bool UASManager::setHomePosition(double lat, double lon, double alt)
         && lat <= 90.0 && lat >= -90.0 && lon <= 180.0 && lon >= -180.0)
         {
 
-        if (homeLat != lat) changed = true;
-        if (homeLon != lon) changed = true;
-        if (homeAlt != alt) changed = true;
+        if (fabs(homeLat - lat) > 1e-7) changed = true;
+        if (fabs(homeLon - lon) > 1e-7) changed = true;
+        if (fabs(homeAlt - alt) > 0.5f) changed = true;
 
         // Initialize conversion reference in any case
         initReference(lat, lon, alt);
@@ -89,17 +89,27 @@ bool UASManager::setHomePosition(double lat, double lon, double alt)
             homeAlt = alt;
 
             emit homePositionChanged(homeLat, homeLon, homeAlt);
-
-            // Update all UAVs
-            foreach (UASInterface* mav, systems)
-            {
-                mav->setHomePosition(homeLat, homeLon, homeAlt);
-            }
         }
     }
     return changed;
 }
 
+bool UASManager::setHomePositionAndNotify(double lat, double lon, double alt)
+{
+    // Checking for NaN and infitiny
+    // and checking for borders
+    bool changed = setHomePosition(lat, lon, alt);
+
+    if (changed)
+    {
+        // Update all UAVs
+        foreach (UASInterface* mav, systems)
+        {
+            mav->setHomePosition(homeLat, homeLon, homeAlt);
+        }
+    }
+}
+
 /**
  * @param x1 Point 1 coordinate in x dimension
  * @param y1 Point 1 coordinate in y dimension

src/uas/UASManager.h

@@ -231,9 +231,12 @@ public slots:
     /** @brief Shut down the onboard operating system down */
     bool shutdownActiveUAS();
 
-    /** @brief Set the current home position on all UAVs*/
+    /** @brief Set the current home position, but do not change it on the UAVs */
     bool setHomePosition(double lat, double lon, double alt);
 
+    /** @brief Set the current home position on all UAVs*/
+    bool setHomePositionAndNotify(double lat, double lon, double alt);
+
     /** @brief Set the safety limits in local position frame */
     void setLocalNEDSafetyBorders(double x1, double y1, double z1, double x2, double y2, double z2);
 

src/ui/QGCToolBar.cc

@@ -36,6 +36,8 @@ QGCToolBar::QGCToolBar(QWidget *parent) :
     batteryVoltage(0),
     wpId(0),
     wpDistance(0),
+    altitudeMSL(0),
+    altitudeRel(0),
     systemArmed(false),
     currentLink(NULL),
     firstAction(NULL)
@@ -334,7 +336,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active)
         connect(mav, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString)));
         connect(mav, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
         connect(mav, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(receiveTextMessage(int,int,int,QString)));
-        connect(mav, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBatteryRemaining(UASInterface*,double,double,int)));
+        connect(mav, SIGNAL(batteryChanged(UASInterface*,double,double,double,int)), this, SLOT(updateBatteryRemaining(UASInterface*,double,double,double,int)));
         connect(mav, SIGNAL(armingChanged(bool)), this, SLOT(updateArmingState(bool)));
         connect(mav, SIGNAL(heartbeatTimeout(bool, unsigned int)), this, SLOT(heartbeatTimeout(bool,unsigned int)));
         connect(mav, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(globalPositionChanged(UASInterface*,double,double,double,quint64)));
@@ -451,6 +453,7 @@ void QGCToolBar::updateBatteryRemaining(UASInterface* uas, double voltage, doubl
 {
     Q_UNUSED(uas);
     Q_UNUSED(seconds);
+    Q_UNUSED(current);
     if (batteryPercent != percent || batteryVoltage != voltage) changed = true;
     batteryPercent = percent;
     batteryVoltage = voltage;