diff --git a/src/qgcunittest/MockUAS.h b/src/qgcunittest/MockUAS.h index 418df5d5b7581ed802f6695f8d0b6b110f68bbff..8f6ad10b430f5d2d7c0b8651906d919e7eeddafd 100644 --- a/src/qgcunittest/MockUAS.h +++ b/src/qgcunittest/MockUAS.h @@ -168,6 +168,9 @@ public slots: virtual void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites) { Q_UNUSED(time_us); Q_UNUSED(lat); Q_UNUSED(lon); Q_UNUSED(alt); Q_UNUSED(fix_type); Q_UNUSED(eph); Q_UNUSED(epv); Q_UNUSED(vel); Q_UNUSED(vn); Q_UNUSED(ve); Q_UNUSED(vd); Q_UNUSED(cog); Q_UNUSED(satellites); Q_ASSERT(false); }; + virtual bool isRotaryWing() { Q_ASSERT(false); return false; } + virtual bool isFixedWing() { Q_ASSERT(false); return false; } + private: int _systemType; int _systemId; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 83dbe0c0be100680d6a0c355d8251db05dd5a83d..93165d2373af86cad84b21636db9f3731f3f17c1 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2170,6 +2170,32 @@ void UAS::readParametersFromStorage() sendMessage(msg); } +bool UAS::isRotaryWing() +{ + switch (type) { + case MAV_TYPE_QUADROTOR: + /* fallthrough */ + case MAV_TYPE_COAXIAL: + case MAV_TYPE_HELICOPTER: + case MAV_TYPE_HEXAROTOR: + case MAV_TYPE_OCTOROTOR: + case MAV_TYPE_TRICOPTER: + return true; + default: + return false; + } +} + +bool UAS::isFixedWing() +{ + switch (type) { + case MAV_TYPE_FIXED_WING: + return true; + default: + return false; + } +} + /** * @param rate The update rate in Hz the message should be sent */ diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 9db1399a78ccd600a3cf356945aa47b2c4821b10..c24b3cb14847cfa0ffdb3d33a919702730497d8b 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -306,6 +306,9 @@ public: return nedAttGlobalOffset; } + bool isRotaryWing(); + bool isFixedWing(); + #if defined(QGC_PROTOBUF_ENABLED) && defined(QGC_USE_PIXHAWK_MESSAGES) px::GLOverlay getOverlay() { diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 5a863a982ca888caebcd93f810d517a971606304..6395674eaa683d2e7ea4af968ecb2ed929645693 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -267,6 +267,9 @@ public: */ virtual QList getActions() const = 0; + static const unsigned int WAYPOINT_RADIUS_DEFAULT_FIXED_WING = 25; + static const unsigned int WAYPOINT_RADIUS_DEFAULT_ROTARY_WING = 5; + public slots: /** @brief Set a new name for the system */ @@ -376,6 +379,11 @@ public slots: virtual void startGyroscopeCalibration() = 0; virtual void startPressureCalibration() = 0; + /** @brief Return if this a rotary wing */ + virtual bool isRotaryWing() = 0; + /** @brief Return if this is a fixed wing */ + virtual bool isFixedWing() = 0; + /** @brief Set the current battery type and voltages */ virtual void setBatterySpecs(const QString& specs) = 0; /** @brief Get the current battery type and specs */ @@ -393,7 +401,6 @@ public slots: /** @brief Send raw GPS for sensor HIL */ virtual void sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites) = 0; - protected: QColor color; diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 888a548e707cf80f7f696e12be379991bccefcb9..fc48a3bbdee1a522673a6686b49080c9de05edc8 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -1063,11 +1063,23 @@ int UASWaypointManager::getFrameRecommendation() float UASWaypointManager::getAcceptanceRadiusRecommendation() { - if (waypointsEditable.count() > 0) { + if (waypointsEditable.count() > 0) + { return waypointsEditable.last()->getAcceptanceRadius(); - } else { - return 10.0f; } + else + { + if (uas->isRotaryWing()) + { + return UASInterface::WAYPOINT_RADIUS_DEFAULT_ROTARY_WING; + } + else if (uas->isFixedWing()) + { + return UASInterface::WAYPOINT_RADIUS_DEFAULT_FIXED_WING; + } + } + + return 10.0f; } float UASWaypointManager::getHomeAltitudeOffsetDefault()