diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 984456a9a359db90e33ce1ebfd892b49c2bd2c70..f654a3802929531439a043d67865eb2396b5e577 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -76,7 +76,9 @@ airframe(0), attitudeKnown(false), paramManager(NULL), attitudeStamped(false), -lastAttitude(0) +lastAttitude(0), +isLocalPositionKnown(false), +isGlobalPositionKnown(false) { color = UASInterface::getNextColor(); setBattery(LIPOLY, 3); @@ -545,6 +547,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) GAudioOutput::instance()->notifyPositive(); } positionLock = true; + isLocalPositionKnown = true; } break; case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: @@ -573,6 +576,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) GAudioOutput::instance()->notifyPositive(); } positionLock = true; + isGlobalPositionKnown = true; //TODO fix this hack for forwarding of global position for patch antenna tracking forwardMessage(message); } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index fb83f832c7bfff8ea4e36b25719f10bf0cc8b71f..862fff005594886b8eb10dc4cbe8b40b0e99a4f0 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -105,6 +105,14 @@ public: double getAltitude() const { return altitude; } + virtual bool localPositionKnown() const + { + return isLocalPositionKnown; + } + virtual bool globalPositionKnown() const + { + return isGlobalPositionKnown; + } double getRoll() const { return roll; @@ -198,14 +206,16 @@ protected: //COMMENTS FOR TEST UNIT quint64 imageStart; QMap* > parameters; ///< All parameters - bool paramsOnceRequested; ///< If the parameter list has been read at least once - int airframe; ///< The airframe type - bool attitudeKnown; ///< True if attitude was received, false else + bool paramsOnceRequested; ///< If the parameter list has been read at least once + int airframe; ///< The airframe type + bool attitudeKnown; ///< True if attitude was received, false else QGCUASParamManager* paramManager; ///< Parameter manager class - QString shortStateText; ///< Short textual state description - QString shortModeText; ///< Short textual mode description - bool attitudeStamped; ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV - quint64 lastAttitude; ///< Timestamp of last attitude measurement + QString shortStateText; ///< Short textual state description + QString shortModeText; ///< Short textual mode description + bool attitudeStamped; ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV + quint64 lastAttitude; ///< Timestamp of last attitude measurement + bool isLocalPositionKnown; ///< If the local position has been received for this MAV + bool isGlobalPositionKnown; ///< If the global position has been received for this MAV public: /** @brief Set the current battery type */ diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 18a2d0291574b2018f5690a88dcc380db200b9db..b1af1b2d56ccb7d1ad42e3b151c7f0d25267db6d 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -74,10 +74,12 @@ public: virtual double getLocalX() const = 0; virtual double getLocalY() const = 0; virtual double getLocalZ() const = 0; + virtual bool localPositionKnown() const = 0; virtual double getLatitude() const = 0; virtual double getLongitude() const = 0; virtual double getAltitude() const = 0; + virtual bool globalPositionKnown() const = 0; virtual double getRoll() const = 0; virtual double getPitch() const = 0; diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index e2013f5c21909b10a134b771750259cf27e03a65..5e20cd95ec0990000ad5613d669a1e767f39b925 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -166,54 +166,72 @@ void WaypointList::read() void WaypointList::add() { - if (uas) { + if (uas) + { const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); Waypoint *wp; - - - if (waypoints.size() > 0) { + if (waypoints.size() > 0) + { // Create waypoint with last frame Waypoint *last = waypoints.at(waypoints.size()-1); wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getParam1(), last->getParam2(), last->getParam3(), last->getParam4(), last->getAutoContinue(), false, last->getFrame(), last->getAction()); uas->getWaypointManager()->addWaypoint(wp); - } else { - // Create global frame waypoint per default - wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0.0, 0.0, 0.0, 0.0, true, true, MAV_FRAME_GLOBAL, MAV_CMD_NAV_WAYPOINT); - uas->getWaypointManager()->addWaypoint(wp); + } + else + { + // Create first waypoint at current MAV position + addCurrentPositonWaypoint(); } } } void WaypointList::addCurrentPositonWaypoint() { - /* TODO: implement with new waypoint structure if (uas) { - // For Global Waypoints - //if(isGlobalWP) - //{ - //isLocalWP = false; - //} - //else + const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + Waypoint *wp; + Waypoint *last = 0; + if (waypoints.size() > 0) { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - if (waypoints.size() > 0) + last = waypoints.at(waypoints.size()-1); + } + + if (uas->globalPositionKnown()) + { + float acceptanceRadiusGlobal = 10.0f; + float holdTime = 0.0f; + float yawGlobal = 0.0f; + if (last) { - Waypoint *last = waypoints.at(waypoints.size()-1); - Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); - uas->getWaypointManager()->addWaypoint(wp); + acceptanceRadiusGlobal = last->getAcceptanceRadius(); + holdTime = last->getHoldTime(); + yawGlobal = last->getYaw(); } - else + // Create global frame waypoint per default + wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), yawGlobal, acceptanceRadiusGlobal, holdTime, 0.0, true, true, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT); + uas->getWaypointManager()->addWaypoint(wp); + } + else if (uas->localPositionKnown()) + { + float acceptanceRadiusLocal = 0.2f; + float holdTime = 0.5f; + if (last) { - Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000); - uas->getWaypointManager()->addWaypoint(wp); + acceptanceRadiusLocal = last->getAcceptanceRadius(); + holdTime = last->getHoldTime(); } - - //isLocalWP = true; + // Create local frame waypoint as second option + wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, true, MAV_FRAME_LOCAL, MAV_CMD_NAV_WAYPOINT); + uas->getWaypointManager()->addWaypoint(wp); + } + else + { + // Do nothing + updateStatusLabel(tr("Not adding waypoint, no position known of MAV known yet.")); } } - */ } void WaypointList::updateStatusLabel(const QString &string) @@ -223,23 +241,30 @@ void WaypointList::updateStatusLabel(const QString &string) void WaypointList::changeCurrentWaypoint(quint16 seq) { - if (uas) { + if (uas) + { uas->getWaypointManager()->setCurrentWaypoint(seq); } } void WaypointList::currentWaypointChanged(quint16 seq) { - if (uas) { + if (uas) + { const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - if (seq < waypoints.size()) { - for(int i = 0; i < waypoints.size(); i++) { + if (seq < waypoints.size()) + { + for(int i = 0; i < waypoints.size(); i++) + { WaypointView* widget = wpViews.find(waypoints[i]).value(); - if (waypoints[i]->getId() == seq) { + if (waypoints[i]->getId() == seq) + { widget->setCurrent(true); - } else { + } + else + { widget->setCurrent(false); } }