Merged

src/uas/UAS.cc

@@ -1911,25 +1911,6 @@ void UAS::executeCommand(MAV_CMD command)
     sendMessage(msg);
 }
 
-void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component)
-{
-    mavlink_message_t msg;
-    mavlink_command_long_t cmd;
-    cmd.command = (uint8_t)command;
-    cmd.confirmation = confirmation;
-    cmd.param1 = param1;
-    cmd.param2 = param2;
-    cmd.param3 = param3;
-    cmd.param4 = param4;
-    cmd.param5 = 0.0f;
-    cmd.param6 = 0.0f;
-    cmd.param7 = 0.0f;
-    cmd.target_system = uasId;
-    cmd.target_component = component;
-    mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
-    sendMessage(msg);
-}
-
 void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component)
 {
     mavlink_message_t msg;
@@ -2196,7 +2177,7 @@ void UAS::shutdown()
 void UAS::setTargetPosition(float x, float y, float z, float yaw)
 {
     mavlink_message_t msg;
-    mavlink_msg_set_local_position_setpoint_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_FRAME_LOCAL_NED, x, y, z, yaw);
+    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 2, 3, 0, yaw, x, y, z);
     sendMessage(msg);
 }
 

src/uas/UAS.h

@@ -411,8 +411,6 @@ public slots:
     void setUASName(const QString& name);
     /** @brief Executes a command **/
     void executeCommand(MAV_CMD command);
-    /** @brief Executes a command **/
-    void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component);
     /** @brief Executes a command with 7 params */
     void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component);
     /** @brief Set the current battery type and voltages */

src/uas/UASInterface.h

@@ -215,7 +215,7 @@ public slots:
     /** @brief Execute command immediately **/
     virtual void executeCommand(MAV_CMD command) = 0;
     /** @brief Executes a command **/
-    virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, int component) = 0;
+    virtual void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) = 0;
 
     /** @brief Selects the airframe */
     virtual void setAirframe(int airframe) = 0;

src/ui/designer/QGCCommandButton.cc

@@ -79,7 +79,6 @@ QGCCommandButton::~QGCCommandButton()
 void QGCCommandButton::sendCommand()
 {
     if (QGCToolWidgetItem::uas) {
-        // FIXME
         int index = ui->editCommandComboBox->itemData(ui->editCommandComboBox->currentIndex()).toInt();
         MAV_CMD command = static_cast<MAV_CMD>(index);
         int confirm = (ui->editConfirmationCheckBox->isChecked()) ? 1 : 0;
@@ -87,9 +86,12 @@ void QGCCommandButton::sendCommand()
         float param2 = ui->editParam2SpinBox->value();
         float param3 = ui->editParam3SpinBox->value();
         float param4 = ui->editParam4SpinBox->value();
+        float param5 = ui->editParam5SpinBox->value();
+        float param6 = ui->editParam6SpinBox->value();
+        float param7 = ui->editParam7SpinBox->value();
         int component = ui->editComponentSpinBox->value();
 
-        QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, component);
+        QGCToolWidgetItem::uas->executeCommand(command, confirm, param1, param2, param3, param4, param5, param6, param7, component);
         qDebug() << __FILE__ << __LINE__ << "SENDING COMMAND" << index;
     } else {
         qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING";

src/ui/map3D/Pixhawk3DWidget.h

@@ -70,10 +70,12 @@ private slots:
     void recenter(void);
     void toggleFollowCamera(int state);
 
+    void selectTargetHeading(void);
     void selectTarget(void);
+    void setTarget(void);
     void insertWaypoint(void);
-    void moveWaypoint(void);
-    void setWaypoint(void);
+    void moveWaypointPosition(void);
+    void moveWaypointHeading(void);
     void deleteWaypoint(void);
     void setWaypointAltitude(void);
     void clearAllWaypoints(void);
@@ -96,6 +98,7 @@ protected:
         QWidget::hideEvent(event);
         emit visibilityChanged(false);
     }
+    virtual void mouseMoveEvent(QMouseEvent* event);
 
     UASInterface* uas;
 
@@ -134,14 +137,16 @@ private:
     void updateObstacles(void);
 #endif
 
-    int findWaypoint(int mouseX, int mouseY);
+    int findWaypoint(const QPoint& mousePos);
     bool findTarget(int mouseX, int mouseY);
     void showInsertWaypointMenu(const QPoint& cursorPos);
     void showEditWaypointMenu(const QPoint& cursorPos);
 
     enum Mode {
         DEFAULT_MODE,
-        MOVE_WAYPOINT_MODE
+        MOVE_WAYPOINT_POSITION_MODE,
+        MOVE_WAYPOINT_HEADING_MODE,
+        SELECT_TARGET_HEADING_MODE
     };
     Mode mode;
     int selectedWpIndex;
@@ -184,7 +189,8 @@ private:
     QVector< osg::ref_ptr<osg::Node> > vehicleModels;
 
     MAV_FRAME frame;
-    osg::Vec2d target;
+    osg::Vec3d target;
+    QPoint cachedMousePos;
     double lastRobotX, lastRobotY, lastRobotZ;
 };
 

src/ui/map3D/Q3DWidget.cc

@@ -73,6 +73,7 @@ Q3DWidget::Q3DWidget(QWidget* parent)
     setThreadingModel(osgViewer::Viewer::SingleThreaded);
 
     setFocusPolicy(Qt::StrongFocus);
+    setMouseTracking(true);
 }
 
 Q3DWidget::~Q3DWidget()

src/ui/map3D/WaypointGroupNode.cc

@@ -89,8 +89,42 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
 
         double wpX, wpY, wpZ;
         getPosition(wp, wpX, wpY, wpZ);
+        double wpYaw = osg::DegreesToRadians(wp->getYaw());
 
+        osg::ref_ptr<osg::Group> group = new osg::Group;
+
+        // cone indicates waypoint orientation
         osg::ref_ptr<osg::ShapeDrawable> sd = new osg::ShapeDrawable;
+        double coneRadius = wp->getAcceptanceRadius() / 2.0;
+        osg::ref_ptr<osg::Cone> cone =
+            new osg::Cone(osg::Vec3d(wpZ, 0.0, 0.0),
+                          coneRadius, wp->getAcceptanceRadius() * 2.0);
+
+        sd->setShape(cone);
+        sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+
+        if (wp->getCurrent())
+        {
+            sd->setColor(osg::Vec4(1.0f, 0.3f, 0.3f, 0.5f));
+        }
+        else
+        {
+            sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
+        }
+
+        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+        geode->addDrawable(sd);
+
+        osg::ref_ptr<osg::PositionAttitudeTransform> pat =
+            new osg::PositionAttitudeTransform;
+        pat->addChild(geode);
+        pat->setAttitude(osg::Quat(wpYaw - M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
+                                   M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
+                                   0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));
+        group->addChild(pat);
+
+        // cylinder indicates waypoint position
+        sd = new osg::ShapeDrawable;
         osg::ref_ptr<osg::Cylinder> cylinder =
             new osg::Cylinder(osg::Vec3d(0.0, 0.0, -wpZ / 2.0),
                               wp->getAcceptanceRadius(),
@@ -108,12 +142,13 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
             sd->setColor(osg::Vec4(0.0f, 1.0f, 0.0f, 0.5f));
         }
 
-        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
+        geode = new osg::Geode;
         geode->addDrawable(sd);
+        group->addChild(geode);
 
         char wpLabel[10];
         sprintf(wpLabel, "wp%d", i);
-        geode->setName(wpLabel);
+        group->setName(wpLabel);
 
         if (i < list.size() - 1)
         {
@@ -143,15 +178,13 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas)
             geode->addDrawable(geometry);
         }
 
-        osg::ref_ptr<osg::PositionAttitudeTransform> pat =
-            new osg::PositionAttitudeTransform;
-
+        pat = new osg::PositionAttitudeTransform;
         pat->setPosition(osg::Vec3d(wpY - robotY,
                                     wpX - robotX,
                                     robotZ));
 
         addChild(pat);
-        pat->addChild(geode);
+        pat->addChild(group);
     }
 }