Fully working waypoint interface, thoroughly tested. Active waypoint setting...

Fully working waypoint interface, thoroughly tested. Active waypoint setting has to be debugged in the simulator, but works fine on real hardware

images/earth.html

@@ -251,11 +251,11 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
 	
 	
 	// FIXME Currently invalid conversion from right-handed z-down to z-up frame
-	planeOrient.setRoll(((roll/M_PI)+1.0)*360.0);
-	planeOrient.setTilt(((pitch/M_PI)+1.0)*360.0);
-	planeOrient.setHeading(((yaw/M_PI)+1.0)*360.0);
+	planeOrient.setRoll(((roll/M_PI))*180.0+180.0);
+	planeOrient.setTilt(((pitch/M_PI))*180.0+180.0);
+	planeOrient.setHeading(((yaw/M_PI))*180.0-90.0);
 	
-	currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
+	currFollowHeading = ((yaw/M_PI))*180.0;
     
     planeLoc.setLatitude(lastLat);
     planeLoc.setLongitude(lastLon);
@@ -303,7 +303,7 @@ function updateFollowAircraft()
 
 	currView.setRange(currViewRange);
 	currView.setTilt(currFollowTilt);
-	currView.setHeading(currFollowHeading-90.0);
+	currView.setHeading(currFollowHeading);
 	
 	ge.getView().setAbstractView(currView);
 }

src/comm/MAVLinkSimulationMAV.cc

@@ -4,32 +4,36 @@
 #include "MAVLinkSimulationMAV.h"
 
 MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid) :
-    QObject(parent),
-    link(parent),
-    planner(parent, systemid),
-    systemid(systemid),
-    timer25Hz(0),
-    timer10Hz(0),
-    timer1Hz(0),
-    latitude(47.376389),
-    longitude(8.548056),
-    altitude(0.0),
-    x(8.548056),
-    y(47.376389),
-    z(550),
-    roll(0.0),
-    pitch(0.0),
-    yaw(0.0),
-    globalNavigation(true),
-    firstWP(false),
-    previousSPX(8.548056),
-    previousSPY(47.376389),
-    previousSPZ(550),
-    previousSPYaw(0.0),
-    nextSPX(8.548056),
-    nextSPY(47.376389),
-    nextSPZ(550),
-    nextSPYaw(0.0)
+        QObject(parent),
+        link(parent),
+        planner(parent, systemid),
+        systemid(systemid),
+        timer25Hz(0),
+        timer10Hz(0),
+        timer1Hz(0),
+        latitude(47.376389),
+        longitude(8.548056),
+        altitude(0.0),
+        x(8.548056),
+        y(47.376389),
+        z(550),
+        roll(0.0),
+        pitch(0.0),
+        yaw(0.0),
+        globalNavigation(true),
+        firstWP(false),
+        previousSPX(8.548056),
+        previousSPY(47.376389),
+        previousSPZ(550),
+        previousSPYaw(0.0),
+        nextSPX(8.548056),
+        nextSPY(47.376389),
+        nextSPZ(550),
+        nextSPYaw(0.0),
+        sys_mode(MAV_MODE_READY),
+        sys_state(MAV_STATE_STANDBY),
+        nav_mode(MAV_NAV_GROUNDED),
+        flying(false)
 {
     // Please note: The waypoint planner is running
     connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop()));
@@ -42,9 +46,9 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
 void MAVLinkSimulationMAV::mainloop()
 {
     // Calculate new simulator values
-//    double maxSpeed = 0.0001; // rad/s in earth coordinate frame
+    //    double maxSpeed = 0.0001; // rad/s in earth coordinate frame
 
-//        double xNew = // (nextSPX - previousSPX)
+    //        double xNew = // (nextSPX - previousSPX)
 
     // 1 Hz execution
     if (timer1Hz <= 0)
@@ -61,8 +65,8 @@ void MAVLinkSimulationMAV::mainloop()
     {
         if (!firstWP)
         {
-            double radPer100ms = 0.0002;
-            double altPer100ms = 0.1;
+            double radPer100ms = 0.00006;
+            double altPer100ms = 1.0;
             double xm = (nextSPX - x);
             double ym = (nextSPY - y);
             double zm = (nextSPZ - z);
@@ -71,14 +75,35 @@ void MAVLinkSimulationMAV::mainloop()
 
             //float trueyaw = atan2f(xm, ym);
 
-            yaw = yaw*0.9 + 0.1*atan2f(xm, ym);
+            float newYaw = atan2f(xm, ym);
+            // Choose shortest direction
+//            if (yaw - newYaw < -180.0f)
+//            {
+//                yaw = newYaw + 180.0f;
+//            }
 
-            qDebug() << "SIMULATION MAV: x:" << xm << "y:" << ym << "z:" << zm << "yaw:" << yaw;
+//            if (yaw - newYaw > 180.0f)
+//            {
+//                yaw = newYaw - 180.0f;
+//            }
 
-            if (sqrt(xm*xm+ym*ym) > 0.0001)
+
+            if (fabs(yaw - newYaw) < 90)
+            {
+                yaw = yaw*0.7 + 0.3*newYaw;
+            }
+            else
             {
-                x += cos(yaw)*radPer100ms;
-                y += sin(yaw)*radPer100ms;
+                yaw = newYaw;
+            }
+
+            //qDebug() << "SIMULATION MAV: x:" << xm << "y:" << ym << "z:" << zm << "yaw:" << yaw;
+
+            //if (sqrt(xm*xm+ym*ym) > 0.0000001)
+            if (flying)
+            {
+                x += sin(yaw)*radPer100ms;
+                y += cos(yaw)*radPer100ms;
                 z += altPer100ms*zsign;
             }
 
@@ -94,6 +119,7 @@ void MAVLinkSimulationMAV::mainloop()
         }
 
 
+        // GLOBAL POSITION
         mavlink_message_t msg;
         mavlink_global_position_int_t pos;
         pos.alt = z*1000.0;
@@ -105,15 +131,27 @@ void MAVLinkSimulationMAV::mainloop()
         mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos);
         link->sendMAVLinkMessage(&msg);
         planner.handleMessage(msg);
+
+        // ATTITUDE
         mavlink_attitude_t attitude;
         attitude.roll = 0.0f;
         attitude.pitch = 0.0f;
         attitude.yaw = yaw;
 
-        qDebug() << "YAW" << yaw;
-
         mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude);
         link->sendMAVLinkMessage(&msg);
+
+        // SYSTEM STATUS
+        mavlink_sys_status_t status;
+        status.load = 300;
+        status.motor_block = 1;
+        status.mode = sys_mode;
+        status.nav_mode = nav_mode;
+        status.packet_drop = 0;
+        status.vbat = 10500;
+        status.status = sys_state;
+
+        mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status);
         timer10Hz = 5;
     }
 
@@ -136,6 +174,37 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
     {
     case MAVLINK_MSG_ID_ATTITUDE:
         break;
+    case MAVLINK_MSG_ID_SET_MODE:
+        {
+            mavlink_set_mode_t mode;
+            mavlink_msg_set_mode_decode(&msg, &mode);
+            if (systemid == mode.target) sys_mode = mode.mode;
+        }
+        break;
+    case MAVLINK_MSG_ID_ACTION:
+        {
+            mavlink_action_t action;
+            mavlink_msg_action_decode(&msg, &action);
+            if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU))
+            {
+                switch (action.action)
+                {
+                case MAV_ACTION_LAUNCH:
+                    flying = true;
+                    break;
+                default:
+                    {
+                        mavlink_statustext_t text;
+                        mavlink_message_t r_msg;
+                        sprintf((char*)text.text, "MAV%d ignored unknown action %d", systemid, action.action);
+                        mavlink_msg_statustext_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &text);
+                        link->sendMAVLinkMessage(&r_msg);
+                    }
+                    break;
+                }
+            }
+        }
+        break;
     case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET:
         {
             mavlink_local_position_setpoint_set_t sp;

src/comm/MAVLinkSimulationMAV.h

@@ -49,6 +49,10 @@ protected:
     double nextSPY;
     double nextSPZ;
     double nextSPYaw;
+    uint8_t sys_mode;
+    uint8_t sys_state;
+    uint8_t nav_mode;
+    bool flying;
 
 };
 

src/comm/MAVLinkSimulationWaypointPlanner.cc

@@ -463,7 +463,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
 
         //check for timed-out operations
 
-        qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid;
+        //qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid;
 
         uint64_t now = QGC::groundTimeUsecs()/1000;
         if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE)
@@ -584,7 +584,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
 
                                 // FIXME big hack for simulation!
                                 //float oneDegreeOfLatMeters = 111131.745f;
-                                float orbit = 0.0001;
+                                float orbit = 0.00008;
 
                                 // compare current position (given in message) with current waypoint
                                 //float orbit = wp->param1;
@@ -1009,7 +1009,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
                                         {
                                                 //the last waypoint was reached, if auto continue is
                                                 //activated restart the waypoint list from the beginning
-                                                current_active_wp_id = 1;
+                                                current_active_wp_id = 0;
                                         }
                                         else
                                         {

src/uas/UAS.cc

@@ -146,7 +146,16 @@ void UAS::updateState()
 
 void UAS::setSelected()
 {
-    UASManager::instance()->setActiveUAS(this);
+    if (UASManager::instance()->getActiveUAS() != this)
+    {
+        UASManager::instance()->setActiveUAS(this);
+        emit systemSelected(true);
+    }
+}
+
+bool UAS::getSelected() const
+{
+    return (UASManager::instance()->getActiveUAS() == this);
 }
 
 void UAS::receiveMessageNamedValue(const mavlink_message_t& message)

src/uas/UAS.h

@@ -89,9 +89,10 @@ public:
     double getRoll() const { return roll; }
     double getPitch() const { return pitch; }
     double getYaw() const { return yaw; }
-
+    bool getSelected() const;
 
 friend class UASWaypointManager;
+
 protected: //COMMENTS FOR TEST UNIT
     int uasId;                    ///< Unique system ID
     unsigned char type;           ///< UAS type (from type enum)

src/uas/UASInterface.h

@@ -78,6 +78,8 @@ public:
     virtual double getPitch() const = 0;
     virtual double getYaw() const = 0;
 
+    virtual bool getSelected() const = 0;
+
     /** @brief Get reference to the waypoint manager **/
     virtual UASWaypointManager* getWaypointManager(void) = 0;
 
@@ -419,6 +421,8 @@ signals:
     void heartbeatTimeout(unsigned int ms);
     /** @brief Name of system changed */
     void nameChanged(QString newName);
+    /** @brief System has been selected as focused system */
+    void systemSelected(bool selected);
 
 protected:
 

src/uas/UASManager.cc

@@ -203,6 +203,7 @@ void UASManager::setActiveUAS(UASInterface* uas)
         activeUAS = uas;
         activeUASMutex.unlock();
 
+        activeUAS->setSelected();
         emit activeUASSet(uas);
         emit activeUASSet(uas->getUASID());
         emit activeUASSetListIndex(systems.indexOf(uas));

src/ui/MapWidget.h

@@ -68,6 +68,8 @@ public:
 public slots:
     void addUAS(UASInterface* uas);
     void activeUASSet(UASInterface* uas);
+    /** @brief Update the selected system */
+    void updateSelectedSystem(int uas);
     /** @brief Update the attitude */
     void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec);
     void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec);

src/ui/WaypointView.ui

@@ -235,7 +235,7 @@ QProgressBar::chunk#thrustBar {
          </sizepolicy>
         </property>
         <property name="focusPolicy">
-         <enum>Qt::ClickFocus</enum>
+         <enum>Qt::WheelFocus</enum>
         </property>
         <property name="toolTip">
          <string>Position X coordinate</string>
@@ -275,7 +275,7 @@ QProgressBar::chunk#thrustBar {
          </sizepolicy>
         </property>
         <property name="focusPolicy">
-         <enum>Qt::StrongFocus</enum>
+         <enum>Qt::WheelFocus</enum>
         </property>
         <property name="toolTip">
          <string>Position Y coordinate</string>
@@ -309,7 +309,7 @@ QProgressBar::chunk#thrustBar {
          </sizepolicy>
         </property>
         <property name="focusPolicy">
-         <enum>Qt::StrongFocus</enum>
+         <enum>Qt::WheelFocus</enum>
         </property>
         <property name="toolTip">
          <string>Position Z coordinate (negative)</string>
@@ -340,7 +340,7 @@ QProgressBar::chunk#thrustBar {
          </sizepolicy>
         </property>
         <property name="focusPolicy">
-         <enum>Qt::StrongFocus</enum>
+         <enum>Qt::WheelFocus</enum>
         </property>
         <property name="prefix">
          <string>lat </string>
@@ -371,7 +371,7 @@ QProgressBar::chunk#thrustBar {
          </sizepolicy>
         </property>
         <property name="focusPolicy">
-         <enum>Qt::StrongFocus</enum>
+         <enum>Qt::WheelFocus</enum>
         </property>
         <property name="prefix">
          <string>lon </string>

src/ui/map/MAV2DIcon.cc

@@ -3,13 +3,16 @@
 
 #include <qmath.h>
 
-MAV2DIcon::MAV2DIcon(qreal x, qreal y, int radius, int type, const QColor& color, QString name, Alignment alignment, QPen* pen)
-    : Point(x, y, name, alignment),
+MAV2DIcon::MAV2DIcon(UASInterface* uas, int radius, int type, const QColor& color, QString name, Alignment alignment, QPen* pen)
+    : Point(uas->getLatitude(), uas->getLongitude(), name, alignment),
     yaw(0.0f),
     radius(radius),
     type(type),
-    iconColor(color)
+    iconColor(color),
+    selected(uas->getSelected()),
+    uasid(uas->getUASID())
 {
+    //connect
     size = QSize(radius, radius);
     drawIcon(pen);
 }
@@ -18,7 +21,9 @@ MAV2DIcon::MAV2DIcon(qreal x, qreal y, QString name, Alignment alignment, QPen*
     : Point(x, y, name, alignment),
     radius(20),
     type(0),
-    iconColor(Qt::yellow)
+    iconColor(Qt::yellow),
+    selected(false),
+    uasid(0)
 {
     size = QSize(radius, radius);
     drawIcon(pen);
@@ -35,6 +40,12 @@ void MAV2DIcon::setPen(QPen* pen)
     drawIcon(pen);
 }
 
+void MAV2DIcon::setSelectedUAS(bool selected)
+{
+    this->selected = selected;
+    drawIcon(mypen);
+}
+
 /**
  * Yaw changes of less than ±15 degrees are ignored.
  *
@@ -43,22 +54,54 @@ void MAV2DIcon::setPen(QPen* pen)
 void MAV2DIcon::setYaw(float yaw)
 {
     //qDebug() << "MAV2Icon" << yaw;
-    //float diff = fabs(yaw - this->yaw);
-//    while (diff > M_PI)
-//    {
-//        diff -= M_PI;
-//    }
-
-//    if (diff > 0.25)
-//    {
+    float diff = fabs(yaw - this->yaw);
+    while (diff > M_PI)
+    {
+        diff -= M_PI;
+    }
+
+    if (diff > 0.1)
+    {
         this->yaw = yaw;
         drawIcon(mypen);
-//    }
+    }
 }
 
 void MAV2DIcon::drawIcon(QPen* pen)
 {
     Q_UNUSED(pen);
+    if (!mypixmap) mypixmap = new QPixmap(radius+1, radius+1);
+    mypixmap->fill(Qt::transparent);
+    QPainter painter(mypixmap);
+    painter.setRenderHint(QPainter::TextAntialiasing);
+    painter.setRenderHint(QPainter::Antialiasing);
+    painter.setRenderHint(QPainter::HighQualityAntialiasing);
+
+    // Rotate by yaw
+    painter.translate(radius/2, radius/2);
+
+    // Draw selected indicator
+    if (selected)
+    {
+//        qDebug() << "SYSTEM IS NOW SELECTED";
+//        QColor color(Qt::yellow);
+//        color.setAlpha(0.3f);
+        painter.setBrush(Qt::NoBrush);
+//        QPen selPen(color);
+//        int width = 5;
+//        selPen.setWidth(width);
+        painter.setPen(Qt::yellow);
+        painter.drawEllipse(QPoint(0, 0), radius/2-1, radius/2-1);
+        //qDebug() << "Painting ellipse" << radius/2-width << width;
+        //selPen->deleteLater();
+    }
+
+    float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f+180.0f;
+
+    painter.rotate(yawRotate);
+
+    if (this->name() == "1") qDebug() << "uas icon" << name() << "yaw in:" << yaw << "rotate:" << yawRotate;
+
     switch (type)
     {
     case MAV_ICON_GENERIC:
@@ -67,20 +110,9 @@ void MAV2DIcon::drawIcon(QPen* pen)
     case MAV_ICON_ROTARY_WING:
     default:
         {
-            mypixmap = new QPixmap(radius+1, radius+1);
-            mypixmap->fill(Qt::transparent);
-            QPainter painter(mypixmap);
-            painter.setRenderHint(QPainter::TextAntialiasing);
-            painter.setRenderHint(QPainter::Antialiasing);
-            painter.setRenderHint(QPainter::HighQualityAntialiasing);
-
-            // Rotate by yaw
-            painter.translate(radius/2, radius/2);
-            painter.rotate(((yaw/(float)M_PI)+1.0f)*360.0f);
-
             // DRAW AIRPLANE
 
-            qDebug() << "ICON SIZE:" << radius;
+            //qDebug() << "ICON SIZE:" << radius;
 
             float iconSize = radius*0.9f;
             QPolygonF poly(24);
@@ -134,6 +166,9 @@ void MAV2DIcon::drawIcon(QPen* pen)
 //                painter.setPen(pen2);
 //            }
             painter.setBrush(QBrush(iconColor));
+            QPen iconPen(Qt::black);
+            iconPen.setWidthF(1.0f);
+            painter.setPen(iconPen);
 
             painter.drawPolygon(poly);
         }

src/ui/map/MAV2DIcon.h

@@ -4,6 +4,8 @@
 #include <QGraphicsItem>
 #include "qmapcontrol.h"
 
+#include "UASInterface.h"
+
 class MAV2DIcon : public qmapcontrol::Point
 {
 public:
@@ -14,27 +16,29 @@ public:
         MAV_ICON_QUADROTOR,
         MAV_ICON_ROTARY_WING
     } MAV_ICON_TYPE;
+
+    //!
     /*!
      *
      * @param x longitude
      * @param y latitude
+     * @param radius the radius of the circle
      * @param name name of the circle point
      * @param alignment alignment (Middle or TopLeft)
      * @param pen QPen for drawing
      */
-    MAV2DIcon(qreal x, qreal y, QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
+    MAV2DIcon(UASInterface* uas, int radius = 10, int type=0, const QColor& color=QColor(Qt::red), QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
 
-    //!
     /*!
      *
      * @param x longitude
      * @param y latitude
-     * @param radius the radius of the circle
      * @param name name of the circle point
      * @param alignment alignment (Middle or TopLeft)
      * @param pen QPen for drawing
      */
-    MAV2DIcon(qreal x, qreal y, int radius = 10, int type=0, const QColor& color=QColor(Qt::red), QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
+    MAV2DIcon(qreal x, qreal y, QString name = QString(), Alignment alignment = Middle, QPen* pen=0);
+
     virtual ~MAV2DIcon();
 
     //! sets the QPen which is used for drawing the circle
@@ -45,8 +49,13 @@ public:
      */
     virtual void setPen(QPen* pen);
 
+    /** @brief Mark this system as selected */
+    void setSelectedUAS(bool selected);
     void setYaw(float yaw);
 
+    /** @brief Get system id */
+    int getUASId() const { return uasid; }
+
     void drawIcon(QPen* pen);
 
 protected:
@@ -54,6 +63,8 @@ protected:
     int radius;     ///< Radius / width of the icon
     int type;       ///< Type of aircraft: 0: generic, 1: airplane, 2: quadrotor, 3-n: rotary wing
     QColor iconColor; ///< Color to be used for the icon
+    bool selected;  ///< Wether this is the system currently in focus
+    int uasid;      ///< ID of tracked system
 
 };
 

src/ui/map3D/QGCGoogleEarthView.ui

@@ -83,10 +83,10 @@
       <string>Distance of the chase camera to the MAV</string>
      </property>
      <property name="minimum">
-      <number>30</number>
+      <number>100</number>
      </property>
      <property name="maximum">
-      <number>10000</number>
+      <number>20000</number>
      </property>
      <property name="value">
       <number>3000</number>