From d32650bf7dac93328ca1972dea3e0471204d8462 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 12 Feb 2014 22:46:20 +0100
Subject: [PATCH] Add new waypoints based on the UAV airframe type

---
 src/qgcunittest/MockUAS.h |  3 +++
 src/uas/UAS.cc            | 26 ++++++++++++++++++++++++++
 src/uas/UAS.h             |  3 +++
 src/uas/UASInterface.h    |  9 ++++++++-
 src/ui/WaypointList.cc    | 10 ++++++++++
 5 files changed, 50 insertions(+), 1 deletion(-)

diff --git a/src/qgcunittest/MockUAS.h b/src/qgcunittest/MockUAS.h
index 53a52056a..c1212268b 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 9e9ee8241..62fbbadfa 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -2178,6 +2178,32 @@ void UAS::readParametersFromStorage()
     sendMessage(msg);
 }
 
+bool UAS::isRotaryWing()
+{
+    switch (airframe) {
+        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 (airframe) {
+        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 9db1399a7..c24b3cb14 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 5a863a982..6395674ea 100644
--- a/src/uas/UASInterface.h
+++ b/src/uas/UASInterface.h
@@ -267,6 +267,9 @@ public:
      */
     virtual QList<QAction*> 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/ui/WaypointList.cc b/src/ui/WaypointList.cc
index 2a207ca00..2b2976b6a 100644
--- a/src/ui/WaypointList.cc
+++ b/src/ui/WaypointList.cc
@@ -269,6 +269,16 @@ void WaypointList::addEditable(bool onCurrentPosition)
         // Create waypoint with last frame
         Waypoint *last = waypoints.last();
         wp = WPM->createWaypoint();
+        if (uas)
+        {
+            if (uas->isRotaryWing()) {
+                wp->setAcceptanceRadius(UASInterface::WAYPOINT_RADIUS_DEFAULT_FIXED_WING);
+            }
+            else if (uas->isFixedWing())
+            {
+                wp->setAcceptanceRadius(UASInterface::WAYPOINT_RADIUS_DEFAULT_ROTARY_WING);
+            }
+        }
 //        wp->blockSignals(true);
         MAV_FRAME frame = (MAV_FRAME)last->getFrame();
         wp->setFrame(frame);
-- 
2.22.0