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.cc

@@ -114,7 +114,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
 
     buildLayout();
 
-    updateHUD(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, "132N");
+    updateHUD(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, "32N");
 
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
             this, SLOT(setActiveUAS(UASInterface*)));
@@ -248,13 +248,15 @@ Pixhawk3DWidget::toggleFollowCamera(int32_t state)
 }
 
 void
-Pixhawk3DWidget::selectTarget(void)
+Pixhawk3DWidget::selectTargetHeading(void)
 {
     if (!uas)
     {
         return;
     }
 
+    osg::Vec2d p;
+
     if (frame == MAV_FRAME_GLOBAL)
     {
         double altitude = uas->getAltitude();
@@ -262,7 +264,7 @@ Pixhawk3DWidget::selectTarget(void)
         std::pair<double,double> cursorWorldCoords =
             getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
 
-        target.set(cursorWorldCoords.first, cursorWorldCoords.second);
+        p.set(cursorWorldCoords.first, cursorWorldCoords.second);
     }
     else if (frame == MAV_FRAME_LOCAL_NED)
     {
@@ -271,12 +273,52 @@ Pixhawk3DWidget::selectTarget(void)
         std::pair<double,double> cursorWorldCoords =
             getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
 
-        target.set(cursorWorldCoords.first, cursorWorldCoords.second);
+        p.set(cursorWorldCoords.first, cursorWorldCoords.second);
     }
 
-    uas->setTargetPosition(target.x(), target.y(), 0.0, 0.0);
+    target.z() = atan2(p.y() - target.y(), p.x() - target.x());
+}
+
+void
+Pixhawk3DWidget::selectTarget(void)
+{
+    if (!uas)
+    {
+        return;
+    }
+
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double altitude = uas->getAltitude();
+
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(),
+                                    altitude);
+
+        target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0);
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        double z = uas->getLocalZ();
+
+        std::pair<double,double> cursorWorldCoords =
+            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z);
+
+        target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0);
+    }
 
     enableTarget = true;
+
+    mode = SELECT_TARGET_HEADING_MODE;
+}
+
+void
+Pixhawk3DWidget::setTarget(void)
+{
+    selectTargetHeading();
+
+    uas->setTargetPosition(target.x(), target.y(), 0.0,
+                           osg::RadiansToDegrees(target.z()));
 }
 
 void
@@ -298,22 +340,23 @@ Pixhawk3DWidget::insertWaypoint(void)
         Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
 
         std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
+            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(),
+                                    altitude);
 
         Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
                          latitude, longitude);
 
-        wp = new Waypoint(0, longitude, latitude, altitude);
+        wp = new Waypoint(0, longitude, latitude, altitude, 0.0, 0.25);
     }
     else if (frame == MAV_FRAME_LOCAL_NED)
     {
         double z = uas->getLocalZ();
 
         std::pair<double,double> cursorWorldCoords =
-            getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+            getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z);
 
         wp = new Waypoint(0, cursorWorldCoords.first,
-                          cursorWorldCoords.second, z);
+                          cursorWorldCoords.second, z, 0.0, 0.25);
     }
 
     if (wp)
@@ -321,17 +364,20 @@ Pixhawk3DWidget::insertWaypoint(void)
         wp->setFrame(frame);
         uas->getWaypointManager()->addWaypointEditable(wp);
     }
-}
 
-void
-Pixhawk3DWidget::moveWaypoint(void)
-{
-    mode = MOVE_WAYPOINT_MODE;
+    selectedWpIndex = wp->getId();
+    mode = MOVE_WAYPOINT_HEADING_MODE;
 }
 
 void
-Pixhawk3DWidget::setWaypoint(void)
+Pixhawk3DWidget::moveWaypointPosition(void)
 {
+    if (mode != MOVE_WAYPOINT_POSITION_MODE)
+    {
+        mode = MOVE_WAYPOINT_POSITION_MODE;
+        return;
+    }
+
     if (!uas)
     {
         return;
@@ -353,12 +399,11 @@ Pixhawk3DWidget::setWaypoint(void)
         std::pair<double,double> cursorWorldCoords =
             getGlobalCursorPosition(getMouseX(), getMouseY(), altitude);
 
-        Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second, utmZone,
-                         latitude, longitude);
+        Imagery::UTMtoLL(cursorWorldCoords.first, cursorWorldCoords.second,
+                         utmZone, latitude, longitude);
 
         waypoint->setX(longitude);
         waypoint->setY(latitude);
-        waypoint->setZ(altitude);
     }
     else if (frame == MAV_FRAME_LOCAL_NED)
     {
@@ -369,10 +414,52 @@ Pixhawk3DWidget::setWaypoint(void)
 
         waypoint->setX(cursorWorldCoords.first);
         waypoint->setY(cursorWorldCoords.second);
-        waypoint->setZ(z);
     }
 }
 
+void
+Pixhawk3DWidget::moveWaypointHeading(void)
+{
+    if (mode != MOVE_WAYPOINT_HEADING_MODE)
+    {
+        mode = MOVE_WAYPOINT_HEADING_MODE;
+        return;
+    }
+
+    if (!uas)
+    {
+        return;
+    }
+
+    const QVector<Waypoint *> waypoints =
+        uas->getWaypointManager()->getWaypointEditableList();
+    Waypoint* waypoint = waypoints.at(selectedWpIndex);
+
+    double x = 0.0, y = 0.0, z = 0.0;
+
+    if (frame == MAV_FRAME_GLOBAL)
+    {
+        double latitude = waypoint->getY();
+        double longitude = waypoint->getX();
+        z = -waypoint->getZ();
+        QString utmZone;
+        Imagery::LLtoUTM(latitude, longitude, x, y, utmZone);
+    }
+    else if (frame == MAV_FRAME_LOCAL_NED)
+    {
+        z = uas->getLocalZ();
+    }
+
+    std::pair<double,double> cursorWorldCoords =
+        getGlobalCursorPosition(getMouseX(), getMouseY(), -z);
+
+    double yaw = atan2(cursorWorldCoords.second - waypoint->getY(),
+                       cursorWorldCoords.first - waypoint->getX());
+    yaw = osg::RadiansToDegrees(yaw);
+
+    waypoint->setYaw(yaw);
+}
+
 void
 Pixhawk3DWidget::deleteWaypoint(void)
 {
@@ -698,19 +785,23 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
 {
     if (event->button() == Qt::LeftButton)
     {
-        if (mode == MOVE_WAYPOINT_MODE)
+        if (mode == SELECT_TARGET_HEADING_MODE)
         {
-            setWaypoint();
-            mode = DEFAULT_MODE;
+            setTarget();
+        }
 
-            return;
+        if (mode != DEFAULT_MODE)
+        {
+            mode = DEFAULT_MODE;
         }
 
         if (event->modifiers() == Qt::ShiftModifier)
         {
-            selectedWpIndex = findWaypoint(event->x(), event->y());
+            selectedWpIndex = findWaypoint(event->pos());
             if (selectedWpIndex == -1)
             {
+                cachedMousePos = event->pos();
+
                 showInsertWaypointMenu(event->globalPos());
             }
             else
@@ -725,6 +816,25 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event)
     Q3DWidget::mousePressEvent(event);
 }
 
+void
+Pixhawk3DWidget::mouseMoveEvent(QMouseEvent* event)
+{
+    if (mode == SELECT_TARGET_HEADING_MODE)
+    {
+        selectTargetHeading();
+    }
+    if (mode == MOVE_WAYPOINT_POSITION_MODE)
+    {
+        moveWaypointPosition();
+    }
+    if (mode == MOVE_WAYPOINT_HEADING_MODE)
+    {
+        moveWaypointHeading();
+    }
+
+    Q3DWidget::mouseMoveEvent(event);
+}
+
 void
 Pixhawk3DWidget::getPose(double& x, double& y, double& z,
                          double& roll, double& pitch, double& yaw,
@@ -927,14 +1037,15 @@ Pixhawk3DWidget::createTarget(void)
 
     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");
+    osg::ref_ptr<osg::Cone> cone = new osg::Cone(osg::Vec3f(0.0f, 0.0f, 0.0f), 0.2f, 0.6f);
+    osg::ref_ptr<osg::ShapeDrawable> coneDrawable = new osg::ShapeDrawable(cone);
+    coneDrawable->setColor(osg::Vec4f(0.0f, 1.0f, 0.0f, 1.0f));
+    coneDrawable->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON);
+    osg::ref_ptr<osg::Geode> coneGeode = new osg::Geode;
+    coneGeode->addDrawable(coneDrawable);
+    coneGeode->setName("Target");
 
-    pat->addChild(sphereGeode);
+    pat->addChild(coneGeode);
 
     return pat;
 }
@@ -1216,8 +1327,18 @@ void
 Pixhawk3DWidget::updateTarget(double robotX, double robotY)
 {
     osg::PositionAttitudeTransform* pat =
-        static_cast<osg::PositionAttitudeTransform*>(targetNode.get());
+        dynamic_cast<osg::PositionAttitudeTransform*>(targetNode.get());
+
     pat->setPosition(osg::Vec3d(target.y() - robotY, target.x() - robotX, 0.0));
+    pat->setAttitude(osg::Quat(target.z() - 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)));
+
+    osg::Geode* geode = dynamic_cast<osg::Geode*>(pat->getChild(0));
+    osg::ShapeDrawable* sd = dynamic_cast<osg::ShapeDrawable*>(geode->getDrawable(0));
+
+
+    sd->setColor(osg::Vec4f(1.0f, 0.8f, 0.0f, 1.0f));
 }
 
 float colormap_jet[128][3] = {
@@ -1451,13 +1572,14 @@ Pixhawk3DWidget::updateObstacles(void)
 #endif
 
 int
-Pixhawk3DWidget::findWaypoint(int mouseX, int mouseY)
+Pixhawk3DWidget::findWaypoint(const QPoint& mousePos)
 {
     if (getSceneData())
     {
         osgUtil::LineSegmentIntersector::Intersections intersections;
 
-        if (computeIntersections(mouseX, height() - mouseY, intersections))
+        if (computeIntersections(mousePos.x(), height() - mousePos.y(),
+                                 intersections))
         {
             for (osgUtil::LineSegmentIntersector::Intersections::iterator
                     it = intersections.begin(); it != intersections.end(); it++)
@@ -1521,7 +1643,10 @@ Pixhawk3DWidget::showEditWaypointMenu(const QPoint &cursorPos)
 
     QString text;
     text = QString("Move waypoint %1").arg(QString::number(selectedWpIndex));
-    menu.addAction(text, this, SLOT(moveWaypoint()));
+    menu.addAction(text, this, SLOT(moveWaypointPosition()));
+
+    text = QString("Change heading of waypoint %1").arg(QString::number(selectedWpIndex));
+    menu.addAction(text, this, SLOT(moveWaypointHeading()));
 
     text = QString("Change altitude of waypoint %1").arg(QString::number(selectedWpIndex));
     menu.addAction(text, this, SLOT(setWaypointAltitude()));

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);
     }
 }