Merge branch 'experimental' of pixhawk.ethz.ch:qgroundcontrol into experimental

src/uas/UAS.cc

@@ -1291,6 +1291,19 @@ void UAS::setParameter(int component, QString id, float value)
 }
 
 /**
+ * Sets an action
+ *
+ **/
+void UAS::setAction(MAV_ACTION action)
+{
+    mavlink_message_t msg;
+    mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, action);
+    // Send message twice to increase chance that it reaches its goal
+    sendMessage(msg);
+    sendMessage(msg);
+}
+
+/**
  * Launches the system
  *
  **/

src/uas/UAS.h

@@ -174,6 +174,9 @@ public:
     void setAutopilotType(int apType) { autopilot = apType;}
 
 public slots:
+    /** @brief Sets an action **/
+    void setAction(MAV_ACTION action);
+
     /** @brief Launches the system **/
     void launch();
     /** @brief Write this waypoint to the list of waypoints */

src/uas/UASInterface.h

@@ -162,6 +162,9 @@ public:
 
 public slots:
 
+    /** @brief Sets an action **/
+    virtual void setAction(MAV_ACTION action) = 0;
+
     /** @brief Launches the system/Liftof **/
     virtual void launch() = 0;
     /** @brief Set a new waypoint **/

src/ui/designer/QGCActionButton.cc

@@ -1,13 +1,67 @@
 #include "QGCActionButton.h"
 #include "ui_QGCActionButton.h"
 
+#include "MAVLinkProtocol.h"
+#include "UASManager.h"
+
+const char* kActionLabels[MAV_ACTION_NB] =
+{"HOLD",
+ "START MOTORS",
+ "LAUNCH",
+ "RETURN",
+ "EMERGENCY LAND",
+ "EMERGENCY KILL",
+ "CONFIRM KILL",
+ "CONTINUE",
+ "STOP MOTORS",
+ "HALT",
+ "SHUTDOWN",
+ "REBOOT",
+ "SET MANUAL",
+ "SET AUTO",
+ "READ STORAGE",
+ "WRITE STORAGE",
+ "CALIBRATE RC",
+ "CALIBRATE GYRO",
+ "CALIBRATE MAG",
+ "CALIBRATE ACC",
+ "CALIBRATE PRESSURE",
+ "START REC",
+ "PAUSE REC",
+ "STOP REC",
+ "TAKEOFF",
+ "NAVIGATE",
+ "LAND",
+ "LOITER",
+ "SET ORIGIN",
+ "RELAY ON",
+ "RELAY OFF",
+ "GET IMAGE",
+ "START VIDEO",
+ "STOP VIDEO",
+ "RESET MAP"};
+
 QGCActionButton::QGCActionButton(QWidget *parent) :
     QGCToolWidgetItem(parent),
-    ui(new Ui::QGCActionButton)
+    ui(new Ui::QGCActionButton),
+    uas(NULL)
 {
     ui->setupUi(this);
+
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
+            this, SLOT(setActiveUAS(UASInterface*)));
+
+    connect(ui->actionButton, SIGNAL(clicked()), this, SLOT(sendAction()));
     connect(ui->editFinishButton, SIGNAL(clicked()), this, SLOT(endEditMode()));
+    connect(ui->editButtonName, SIGNAL(textChanged(QString)), this, SLOT(setActionButtonName(QString)));
+    connect(ui->editActionComboBox, SIGNAL(currentIndexChanged(QString)), ui->nameLabel, SLOT(setText(QString)));
     endEditMode();
+
+    // add action labels to combobox
+    for (int i = 0; i < MAV_ACTION_NB; i++)
+    {
+        ui->editActionComboBox->addItem(kActionLabels[i]);
+    }
 }
 
 QGCActionButton::~QGCActionButton()
@@ -15,6 +69,22 @@ QGCActionButton::~QGCActionButton()
     delete ui;
 }
 
+void QGCActionButton::sendAction()
+{
+    if (uas)
+    {
+        MAV_ACTION action = static_cast<MAV_ACTION>(
+                ui->editActionComboBox->currentIndex());
+
+        uas->setAction(action);
+    }
+}
+
+void QGCActionButton::setActionButtonName(QString text)
+{
+    ui->actionButton->setText(text);
+}
+
 void QGCActionButton::startEditMode()
 {
     ui->editActionComboBox->show();
@@ -30,3 +100,8 @@ void QGCActionButton::endEditMode()
     ui->editFinishButton->hide();
     isInEditMode = false;
 }
+
+void QGCActionButton::setActiveUAS(UASInterface *uas)
+{
+    this->uas = uas;
+}

src/ui/designer/QGCActionButton.h

@@ -7,6 +7,8 @@ namespace Ui {
     class QGCActionButton;
 }
 
+class UASInterface;
+
 class QGCActionButton : public QGCToolWidgetItem
 {
     Q_OBJECT
@@ -16,11 +18,17 @@ public:
     ~QGCActionButton();
 
 public slots:
+    void sendAction();
+    void setActionButtonName(QString text);
     void startEditMode();
     void endEditMode();
 
+private slots:
+    void setActiveUAS(UASInterface* uas);
+
 private:
     Ui::QGCActionButton *ui;
+    UASInterface* uas;
 };
 
 #endif // QGCACTIONBUTTON_H

src/ui/designer/QGCActionButton.ui

@@ -21,13 +21,6 @@
      </property>
     </widget>
    </item>
-   <item row="1" column="2">
-    <widget class="QPushButton" name="actionButton">
-     <property name="text">
-      <string>Button name</string>
-     </property>
-    </widget>
-   </item>
    <item row="2" column="0">
     <widget class="QComboBox" name="editActionComboBox"/>
    </item>
@@ -59,38 +52,29 @@
      </property>
     </widget>
    </item>
+   <item row="1" column="2">
+    <widget class="QPushButton" name="actionButton">
+     <property name="text">
+      <string>Button name</string>
+     </property>
+    </widget>
+   </item>
   </layout>
  </widget>
  <resources/>
  <connections>
   <connection>
-   <sender>editButtonName</sender>
-   <signal>textChanged(QString)</signal>
-   <receiver>actionButton</receiver>
-   <slot>setWindowTitle(QString)</slot>
-   <hints>
-    <hint type="sourcelabel">
-     <x>310</x>
-     <y>22</y>
-    </hint>
-    <hint type="destinationlabel">
-     <x>310</x>
-     <y>55</y>
-    </hint>
-   </hints>
-  </connection>
-  <connection>
    <sender>editNameLabel</sender>
    <signal>textChanged(QString)</signal>
    <receiver>nameLabel</receiver>
    <slot>setText(QString)</slot>
    <hints>
     <hint type="sourcelabel">
-     <x>116</x>
+     <x>114</x>
      <y>22</y>
     </hint>
     <hint type="destinationlabel">
-     <x>116</x>
+     <x>114</x>
      <y>55</y>
     </hint>
    </hints>

src/ui/map3D/Pixhawk3DWidget.cc

@@ -56,6 +56,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
      , displayRGBD2D(false)
      , displayRGBD3D(false)
      , enableRGBDColor(true)
+     , enableTarget(false)
      , followCamera(true)
      , enableFreenect(false)
      , frame(MAV_FRAME_GLOBAL)
@@ -87,6 +88,10 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
     waypointGroupNode->init();
     rollingMap->addChild(waypointGroupNode);
 
+    // generate target model
+    targetNode = createTarget();
+    rollingMap->addChild(targetNode);
+
 #ifdef QGC_LIBFREENECT_ENABLED
     freenect.reset(new Freenect());
     enableFreenect = freenect->init();
@@ -237,6 +242,36 @@ Pixhawk3DWidget::toggleFollowCamera(int32_t state)
 }
 
 void
+Pixhawk3DWidget::selectTarget(void)
+{
+    if (uas)
+    {
+        if (frame == MAV_FRAME_GLOBAL)
+        {
+            double altitude = uas->getAltitude();
+
+            std::pair<double,double> cursorWorldCoords =
+                    getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+
+            target.set(cursorWorldCoords.first, cursorWorldCoords.second);
+        }
+        else if (frame == MAV_FRAME_LOCAL)
+        {
+            double z = uas->getLocalZ();
+
+            std::pair<double,double> cursorWorldCoords =
+                    getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+
+            target.set(cursorWorldCoords.first, cursorWorldCoords.second);
+        }
+
+        uas->setTargetPosition(target.x(), target.y(), 0.0, 0.0);
+
+        enableTarget = true;
+    }
+}
+
+void
 Pixhawk3DWidget::insertWaypoint(void)
 {
     if (uas)
@@ -539,8 +574,6 @@ Pixhawk3DWidget::display(void)
         lastRobotZ = robotZ;
 
         recenterCamera(robotY, robotX, -robotZ);
-
-        return;
     }
 
     if (followCamera)
@@ -572,6 +605,11 @@ Pixhawk3DWidget::display(void)
         updateWaypoints();
     }
 
+    if (enableTarget)
+    {
+        updateTarget(robotX, robotY);
+    }
+
 #ifdef QGC_LIBFREENECT_ENABLED
     if (enableFreenect && (displayRGBD2D || displayRGBD3D))
     {
@@ -586,6 +624,7 @@ Pixhawk3DWidget::display(void)
     rollingMap->setChildValue(trailNode, displayTrail);
     rollingMap->setChildValue(mapNode, displayImagery);
     rollingMap->setChildValue(waypointGroupNode, displayWaypoints);
+    rollingMap->setChildValue(targetNode, enableTarget);
     if (enableFreenect)
     {
         egocentricMap->setChildValue(rgbd3DNode, displayRGBD3D);
@@ -843,6 +882,26 @@ Pixhawk3DWidget::createRGBD3D(void)
     return geode;
 }
 
+osg::ref_ptr<osg::Node>
+Pixhawk3DWidget::createTarget(void)
+{
+    osg::ref_ptr<osg::PositionAttitudeTransform> pat =
+            new osg::PositionAttitudeTransform;
+
+    pat->setPosition(osg::Vec3d(0.0, 0.0, 0.0));
+
+    osg::ref_ptr<osg::Sphere> sphere = new osg::Sphere(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.1f);
+    osg::ref_ptr<osg::ShapeDrawable> sphereDrawable = new osg::ShapeDrawable(sphere);
+    sphereDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f));
+    osg::ref_ptr<osg::Geode> sphereGeode = new osg::Geode;
+    sphereGeode->addDrawable(sphereDrawable);
+    sphereGeode->setName("Target");
+
+    pat->addChild(sphereGeode);
+
+    return pat;
+}
+
 void
 Pixhawk3DWidget::setupHUD(void)
 {
@@ -1118,6 +1177,14 @@ Pixhawk3DWidget::updateWaypoints(void)
     waypointGroupNode->update(frame, uas);
 }
 
+void
+Pixhawk3DWidget::updateTarget(double robotX, double robotY)
+{
+    osg::PositionAttitudeTransform* pat =
+            static_cast<osg::PositionAttitudeTransform*>(targetNode.get());
+    pat->setPosition(osg::Vec3d(target.y() - robotY, target.x() - robotX, 0.0));
+}
+
 float colormap_jet[128][3] =
 {
                 {0.0f,0.0f,0.53125f},
@@ -1336,7 +1403,6 @@ Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
                     std::string nodeName = it->nodePath[i]->getName();
                     if (nodeName.substr(0, 2).compare("wp") == 0)
                     {
-                        qDebug() << nodeName.c_str() << "Got!!";
                         return atoi(nodeName.substr(2).c_str());
                     }
                 }
@@ -1347,12 +1413,40 @@ Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
     return -1;
 }
 
+bool
+Pixhawk3DWidget::findTarget(int mouseX, int mouseY)
+{
+    if (getSceneData() != NULL)
+    {
+        osgUtil::LineSegmentIntersector::Intersections intersections;
+
+        if (computeIntersections(mouseX, height() - mouseY, intersections))
+        {
+            for (osgUtil::LineSegmentIntersector::Intersections::iterator
+                 it = intersections.begin(); it != intersections.end(); it++)
+            {
+                for (uint i = 0 ; i < it->nodePath.size(); ++i)
+                {
+                    std::string nodeName = it->nodePath[i]->getName();
+                    if (nodeName.compare("Target") == 0)
+                    {
+                        return true;
+                    }
+                }
+            }
+        }
+    }
+
+    return false;
+}
+
 void
 Pixhawk3DWidget::showInsertWaypointMenu(const QPoint &cursorPos)
 {
     QMenu menu;
     menu.addAction("Insert new waypoint", this, SLOT(insertWaypoint()));
     menu.addAction("Clear all waypoints", this, SLOT(clearAllWaypoints()));
+    menu.addAction("Select target", this, SLOT(selectTarget()));
     menu.exec(cursorPos);
 }
 

src/ui/map3D/Pixhawk3DWidget.h

@@ -71,6 +71,7 @@ private slots:
     void recenter(void);
     void toggleFollowCamera(int state);
 
+    void selectTarget(void);
     void insertWaypoint(void);
     void moveWaypoint(void);
     void setWaypoint(void);
@@ -101,6 +102,7 @@ private:
     osg::ref_ptr<osg::Geode> createTrail(void);
     osg::ref_ptr<Imagery> createMap(void);
     osg::ref_ptr<osg::Geode> createRGBD3D(void);
+    osg::ref_ptr<osg::Node> createTarget(void);
 
     void setupHUD(void);
     void resizeHUD(void);
@@ -112,11 +114,13 @@ private:
     void updateImagery(double originX, double originY, double originZ,
                        const QString& zone);
     void updateWaypoints(void);
+    void updateTarget(double robotX, double robotY);
 #ifdef QGC_LIBFREENECT_ENABLED
     void updateRGBD(void);
 #endif
 
     int findWaypoint(int mouseX, int mouseY);
+    bool findTarget(int mouseX, int mouseY);
     void showInsertWaypointMenu(const QPoint& cursorPos);
     void showEditWaypointMenu(const QPoint& cursorPos);
 
@@ -134,6 +138,7 @@ private:
     bool displayRGBD2D;
     bool displayRGBD3D;
     bool enableRGBDColor;
+    bool enableTarget;
 
     bool followCamera;
 
@@ -154,6 +159,7 @@ private:
     osg::ref_ptr<osg::DrawArrays> trailDrawArrays;
     osg::ref_ptr<Imagery> mapNode;
     osg::ref_ptr<WaypointGroupNode> waypointGroupNode;
+    osg::ref_ptr<osg::Node> targetNode;
     osg::ref_ptr<osg::Geode> rgbd3DNode;
 #ifdef QGC_LIBFREENECT_ENABLED
     QScopedPointer<Freenect> freenect;
@@ -163,6 +169,7 @@ private:
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;
 
     MAV_FRAME frame;
+    osg::Vec2d target;
     double lastRobotX, lastRobotY, lastRobotZ;
 };