From 0529dafb9fe0fcedbb8b887b3d31627ef4f122b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jul 2013 16:51:48 +0200 Subject: [PATCH] Robustified current home position setup --- src/uas/UAS.cc | 12 +++++++++--- src/uas/UAS.h | 1 + src/uas/UASManager.cc | 28 +++++++++++++++++++--------- src/uas/UASManager.h | 5 ++++- src/ui/QGCToolBar.cc | 5 ++++- 5 files changed, 37 insertions(+), 14 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 93d587891..3076f3292 100644 --- a/src/uas/UAS.cc +++ b/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_ptrgetSystemId(), 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); diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 7bc144950..95570049a 100644 --- a/src/uas/UAS.h +++ b/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; diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index 0312dc968..a854b310d 100644 --- a/src/uas/UASManager.cc +++ b/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 diff --git a/src/uas/UASManager.h b/src/uas/UASManager.h index 86a13d6de..1736b64f2 100644 --- a/src/uas/UASManager.h +++ b/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); diff --git a/src/ui/QGCToolBar.cc b/src/ui/QGCToolBar.cc index 859c9b136..294fb60f9 100644 --- a/src/ui/QGCToolBar.cc +++ b/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; -- 2.22.0