Addition of APM guided mode via right-click context menu in map view.

The right click menu will only show up when connected to an APM autopilot,
since this feature is APM only.

src/uas/UASWaypointManager.cc

@@ -799,6 +799,40 @@ void UASWaypointManager::readWaypoints(bool readToEdit)
 
     }
 }
+bool UASWaypointManager::guidedModeSupported()
+{
+    return (uas->getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA);
+}
+
+void UASWaypointManager::goToWaypoint(Waypoint *wp)
+{
+    //Don't try to send a guided mode message to an AP that does not support it.
+    if (uas->getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA)
+    {
+        mavlink_mission_item_t mission;
+        memset(&mission, 0, sizeof(mavlink_mission_item_t));   //initialize with zeros
+        //const Waypoint *cur_s = waypointsEditable.at(i);
+
+        mission.autocontinue = 0;
+        mission.current = 2; //2 for guided mode
+        mission.param1 = wp->getParam1();
+        mission.param2 = wp->getParam2();
+        mission.param3 = wp->getParam3();
+        mission.param4 = wp->getParam4();
+        mission.frame = wp->getFrame();
+        mission.command = wp->getAction();
+        mission.seq = 0;     // don't read out the sequence number of the waypoint class
+        mission.x = wp->getX();
+        mission.y = wp->getY();
+        mission.z = wp->getZ();
+        mavlink_message_t message;
+        mission.target_system = uasid;
+        mission.target_component = MAV_COMP_ID_MISSIONPLANNER;
+        mavlink_msg_mission_item_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &mission);
+        uas->sendMessage(message);
+        QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
+    }
+}
 
 void UASWaypointManager::writeWaypoints()
 {

src/uas/UASWaypointManager.h

@@ -66,7 +66,9 @@ private:
 public:
     UASWaypointManager(UAS* uas=NULL);   ///< Standard constructor
     ~UASWaypointManager();
+    bool guidedModeSupported();
 
+    void goToWaypoint(Waypoint *wp);
     /** @name Received message handlers */
     /*@{*/
     void handleWaypointCount(quint8 systemId, quint8 compId, quint16 count);                            ///< Handles received waypoint count messages

src/ui/map/QGCMapWidget.cc

@@ -19,7 +19,56 @@ QGCMapWidget::QGCMapWidget(QWidget *parent) :
     homeAltitude(0)
 {
     // Widget is inactive until shown
+    defaultGuidedAlt = -1;
     loadSettings(false);
+
+    this->setContextMenuPolicy(Qt::ActionsContextMenu);
+
+}
+void QGCMapWidget::guidedActionTriggered()
+{
+    if (currWPManager)
+    {
+        if (defaultGuidedAlt == -1)
+        {
+            if (!guidedAltActionTriggered())
+            {
+                return;
+            }
+        }
+        // Create new waypoint and send it to the WPManager to send out.
+        internals::PointLatLng pos = map->FromLocalToLatLng(mousePressPos.x(), mousePressPos.y());
+        qDebug() << "Guided action requested. Lat:" << pos.Lat() << "Lon:" << pos.Lng();
+        Waypoint wp;
+        wp.setLatitude(pos.Lat());
+        wp.setLongitude(pos.Lng());
+        wp.setAltitude(defaultGuidedAlt);
+        currWPManager->goToWaypoint(&wp);
+    }
+}
+bool QGCMapWidget::guidedAltActionTriggered()
+{
+    bool ok = false;
+    int tmpalt = QInputDialog::getInt(this,"Altitude","Enter default altitude (in meters) of destination point for guided mode",100,0,30000,1,&ok);
+    if (!ok)
+    {
+        //Use has chosen cancel. Do not send the waypoint
+        return false;
+    }
+    defaultGuidedAlt = tmpalt;
+    guidedActionTriggered();
+    return true;
+}
+
+void QGCMapWidget::mousePressEvent(QMouseEvent *event)
+{
+    mapcontrol::OPMapWidget::mousePressEvent(event);
+}
+
+void QGCMapWidget::mouseReleaseEvent(QMouseEvent *event)
+{
+    mousePressPos = event->pos();
+    mapcontrol::OPMapWidget::mouseReleaseEvent(event);
 }
 
 QGCMapWidget::~QGCMapWidget()
@@ -202,6 +251,17 @@ void QGCMapWidget::activeUASSet(UASInterface* uas)
     if (uas)
     {
         currWPManager = uas->getWaypointManager();
+        if (currWPManager->guidedModeSupported())
+        {
+            QAction *guidedaction = new QAction(this);
+            guidedaction->setText("Go To Here (Guided Mode)");
+            connect(guidedaction,SIGNAL(triggered()),this,SLOT(guidedActionTriggered()));
+            this->addAction(guidedaction);
+            guidedaction = new QAction(this);
+            guidedaction->setText("Go To Here Alt (Guided Mode)");
+            connect(guidedaction,SIGNAL(triggered()),this,SLOT(guidedAltActionTriggered()));
+            this->addAction(guidedaction);
+        }
 
         // Connect the waypoint manager / data storage to the UI
         connect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int)));

src/ui/map/QGCMapWidget.h

@@ -40,6 +40,10 @@ signals:
     void waypointChanged(Waypoint* wp);
 
 public slots:
+    /** @brief Action triggered when guided action is selected from the context menu */
+    void guidedActionTriggered();
+    /** @brief Action triggered when guided action is selected from the context menu, allows for altitude selection */
+    bool guidedAltActionTriggered();
     /** @brief Add system to map view */
     void addUAS(UASInterface* uas);
     /** @brief Update the global position of a system */
@@ -126,6 +130,8 @@ protected:
     /** @brief Initialize */
     void showEvent(QShowEvent* event);
     void hideEvent(QHideEvent* event);
+    void mousePressEvent(QMouseEvent *event);
+    void mouseReleaseEvent(QMouseEvent *event);
     void mouseDoubleClickEvent(QMouseEvent* event);
 
     UASWaypointManager* currWPManager; ///< The current waypoint manager
@@ -150,6 +156,8 @@ protected:
     int followUAVID;                    ///< Which UAV should be tracked?
     bool mapInitialized;                ///< Map initialized?
     float homeAltitude;                 ///< Home altitude
+    QPoint mousePressPos;               ///< Mouse position when the button is released.
+    int defaultGuidedAlt;               ///< Default altitude for guided mode
 
 };