Merge branch 'master' of git://github.com/pixhawk/qgroundcontrol into dev

src/comm/MAVLinkSimulationLink.cc

@@ -377,20 +377,77 @@ void MAVLinkSimulationLink::mainloop()
         rate10hzCounter = 1;
 
         // Move X Position
-        x += sin(QGC::groundTimeUsecs()) * 0.1f;
-        y += sin(QGC::groundTimeUsecs()) * 0.1f;
-        z += sin(QGC::groundTimeUsecs()) * 0.1f;
+        x += sin(QGC::groundTimeUsecs()*1000) * 0.05f;
+        y += sin(QGC::groundTimeUsecs()) * 0.05f;
+        z += sin(QGC::groundTimeUsecs()) * 0.009f;
+
+        x = (x > 5.0f) ? 5.0f : x;
+        y = (y > 5.0f) ? 5.0f : y;
+        z = (z > 3.0f) ? 3.0f : z;
+
+        x = (x < -5.0f) ? -5.0f : x;
+        y = (y < -5.0f) ? -5.0f : y;
+        z = (z < -3.0f) ? -3.0f : z;
 
-        x = (x > 1.0f) ? 1.0f : x;
-        y = (y > 1.0f) ? 1.0f : y;
-        z = (z > 1.0f) ? 1.0f : z;
         // Send back new setpoint
         mavlink_message_t ret;
         mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, spX, spY, spZ, spYaw);
-        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
+        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
+        //add data into datastream
+        memcpy(stream+streampointer,buffer, bufferlength);
+        streampointer += bufferlength;
+
+        // Send back new position
+        mavlink_msg_local_position_pack(systemId, componentId, &ret, 0, x, y, z, 0, 0, 0);
+        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
+        //add data into datastream
+        memcpy(stream+streampointer,buffer, bufferlength);
+        streampointer += bufferlength;
+
+        // GPS RAW
+        mavlink_msg_gps_raw_pack(systemId, componentId, &ret, 0, 3, 47.376417+x*0.001f, 8.548103+y*0.001f, z, 0, 0, 2.5f, 0.1f);
+        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
         //add data into datastream
         memcpy(stream+streampointer,buffer, bufferlength);
         streampointer += bufferlength;
+
+        // GLOBAL POSITION
+        mavlink_msg_global_position_pack(systemId, componentId, &ret, 0, 3, 47.376417+x*0.001f, 8.548103+y*0.001f, z, 0, 0);
+        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
+        //add data into datastream
+        memcpy(stream+streampointer,buffer, bufferlength);
+        streampointer += bufferlength;
+
+        static int rcCounter = 0;
+        if (rcCounter == 2)
+        {
+            mavlink_rc_channels_t chan;
+            chan.chan1_raw = 1000 + ((int)(fabs(x) * 1000) % 2000);
+            chan.chan2_raw = 1000 + ((int)(fabs(y) * 1000) % 2000);
+            chan.chan3_raw = 1000 + ((int)(fabs(z) * 1000) % 2000);
+            chan.chan4_raw = (chan.chan1_raw + chan.chan2_raw) / 2.0f;
+            chan.chan5_raw = (chan.chan3_raw + chan.chan4_raw) / 2.0f;
+            chan.chan6_raw = (chan.chan3_raw + chan.chan2_raw) / 2.0f;
+            chan.chan7_raw = (chan.chan4_raw + chan.chan2_raw) / 2.0f;
+            chan.chan8_raw = (chan.chan6_raw + chan.chan2_raw) / 2.0f;
+            chan.chan1_255 = ((chan.chan1_raw - 1000)/1000.0f) * 255.0f;
+            chan.chan2_255 = ((chan.chan2_raw - 1000)/1000.0f) * 255.0f;
+            chan.chan3_255 = ((chan.chan3_raw - 1000)/1000.0f) * 255.0f;
+            chan.chan4_255 = ((chan.chan4_raw - 1000)/1000.0f) * 255.0f;
+            chan.chan5_255 = ((chan.chan5_raw - 1000)/1000.0f) * 255.0f;
+            chan.chan6_255 = ((chan.chan6_raw - 1000)/1000.0f) * 255.0f;
+            chan.chan7_255 = ((chan.chan7_raw - 1000)/1000.0f) * 255.0f;
+            chan.chan8_255 = ((chan.chan8_raw - 1000)/1000.0f) * 255.0f;
+            messageSize = mavlink_msg_rc_channels_encode(systemId, componentId, &msg, &chan);
+            // Allocate buffer with packet data
+            bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
+            //add data into datastream
+            memcpy(stream+streampointer,buffer, bufferlength);
+            streampointer += bufferlength;
+            rcCounter = 0;
+        }
+        rcCounter++;
+
     }
 
     // 1 HZ TASKS
@@ -405,6 +462,62 @@ void MAVLinkSimulationLink::mainloop()
         }
         statusCounter++;
 
+        static int detectionCounter = 6;
+        if (detectionCounter % 10 == 0)
+        {
+#ifdef MAVLINK_ENABLED_PIXHAWK_MESSAGES
+            mavlink_pattern_detected_t detected;
+            detected.confidence = 5.0f;
+
+            if (detectionCounter == 10)
+            {
+                char fileName[] = "patterns/face5.png";
+                memcpy(detected.file, fileName, sizeof(fileName));
+                detected.type = 0; // 0: Pattern, 1: Letter
+            }
+            else if (detectionCounter == 20)
+            {
+                char fileName[] = "7";
+                memcpy(detected.file, fileName, sizeof(fileName));
+                detected.type = 1; // 0: Pattern, 1: Letter
+            }
+            else if (detectionCounter == 30)
+            {
+                char fileName[] = "patterns/einstein.bmp";
+                memcpy(detected.file, fileName, sizeof(fileName));
+                detected.type = 0; // 0: Pattern, 1: Letter
+            }
+            else if (detectionCounter == 40)
+            {
+                char fileName[] = "F";
+                memcpy(detected.file, fileName, sizeof(fileName));
+                detected.type = 1; // 0: Pattern, 1: Letter
+            }
+            else if (detectionCounter == 50)
+            {
+                char fileName[] = "patterns/face2.png";
+                memcpy(detected.file, fileName, sizeof(fileName));
+                detected.type = 0; // 0: Pattern, 1: Letter
+            }
+            else if (detectionCounter == 60)
+            {
+                char fileName[] = "H";
+                memcpy(detected.file, fileName, sizeof(fileName));
+                detected.type = 1; // 0: Pattern, 1: Letter
+                detectionCounter = 0;
+            }
+            detected.detected = 1;
+            messageSize = mavlink_msg_pattern_detected_encode(systemId, componentId, &msg, &detected);
+            // Allocate buffer with packet data
+            bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
+            //add data into datastream
+            memcpy(stream+streampointer,buffer, bufferlength);
+            streampointer += bufferlength;
+            //detectionCounter = 0;
+#endif
+        }
+        detectionCounter++;
+
         status.vbat = voltage * 1000; // millivolts
 
         // Pack message and get size of encoded byte string
@@ -436,7 +549,15 @@ void MAVLinkSimulationLink::mainloop()
         // HEARTBEAT
 
         static int typeCounter = 0;
-        uint8_t mavType = typeCounter % (OCU);
+        uint8_t mavType;
+        if (typeCounter < 10)
+        {
+            mavType = MAV_QUADROTOR;
+        }
+        else
+        {
+            mavType = typeCounter % (OCU);
+        }
         typeCounter++;
 
         // Pack message and get size of encoded byte string

src/comm/MAVLinkSimulationLink.h

 /*=====================================================================
 
-PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
+QGroundControl Open Source Ground Control Station
 
-(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>
+(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 
-This file is part of the PIXHAWK project
+This file is part of the QGROUNDCONTROL project
 
-    PIXHAWK is free software: you can redistribute it and/or modify
+    QGROUNDCONTROL is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation, either version 3 of the License, or
     (at your option) any later version.
 
-    PIXHAWK is distributed in the hope that it will be useful,
+    QGROUNDCONTROL is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.
 
     You should have received a copy of the GNU General Public License
-    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
+    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
 
 ======================================================================*/
 
 /**
  * @file
- *   @brief Definition of simulated system link
+ *   @brief Definition of MAVLinkSimulationLink
  *
  *   @author Lorenz Meier <mavteam@student.ethz.ch>
  *

src/uas/UASInterface.h

@@ -111,8 +111,8 @@ public:
             ///> Color map for plots, includes 20 colors
             ///> Map will start from beginning when the first 20 colors are exceeded
 
-            colors.append(QColor(203,254,121));
             colors.append(QColor(231,72,28));
+            colors.append(QColor(203,254,121));
             colors.append(QColor(161,252,116));
             colors.append(QColor(232,33,47));
             colors.append(QColor(116,251,110));

src/ui/MainWindow.cc

@@ -127,7 +127,7 @@ void MainWindow::buildWidgets()
   list        = new UASListWidget(this);
   waypoints   = new WaypointList(this, NULL);
   info        = new UASInfoWidget(this);
-  detection   = new ObjectDetectionView("patterns", this);
+  detection   = new ObjectDetectionView("images/patterns", this);
   hud         = new HUD(640, 480, this);
   debugConsole= new DebugConsole(this);
   map         = new MapWidget(this);
@@ -563,7 +563,7 @@ void MainWindow::loadPixhawkView()
     container7->setWidget(debugConsole);
     addDockWidget(Qt::BottomDockWidgetArea, container7);
 
-    // DEBUG CONSOLE
+    // RADIO CONTROL VIEW
     QDockWidget* rcContainer = new QDockWidget(tr("Radio Control"), this);
     rcContainer->setWidget(rcView);
     addDockWidget(Qt::BottomDockWidgetArea, rcContainer);

src/ui/ObjectDetectionView.cc

@@ -94,7 +94,7 @@ void ObjectDetectionView::newPattern(int uasId, QString patternPath, float confi
         if (!patternList.contains(patternPath))
         {
             // Emit audio message on detection
-            if (detected) GAudioOutput::instance()->say("System " + QString::number(uasId) + " detected pattern " + QString(patternPath.split("/").last()).split(".").first());
+            if (detected) GAudioOutput::instance()->say("System " + QString::number(uasId) + " detected pattern " + QString(patternPath.split("/", QString::SkipEmptyParts).last()).split(".", QString::SkipEmptyParts).first());
 
             patternList.insert(patternPath, Pattern(patternPath, confidence));
         }
@@ -117,7 +117,7 @@ void ObjectDetectionView::newPattern(int uasId, QString patternPath, float confi
             m_ui->listWidget->addItem(pattern.name + separator + "(" + QString::number(pattern.count) + ")" + separator + QString::number(pattern.confidence));
 
         // load image
-        QString filePath = MG::DIR::getSupportFilesDirectory() + "/" + patternFolder + "/" + patternPath.split("/").last();
+        QString filePath = MG::DIR::getSupportFilesDirectory() + "/" + patternFolder + "/" + patternPath.split("/", QString::SkipEmptyParts).last();
         QPixmap image = QPixmap(filePath);
         if (image.width() > image.height())
             image = image.scaledToWidth(m_ui->imageLabel->width());
@@ -126,8 +126,8 @@ void ObjectDetectionView::newPattern(int uasId, QString patternPath, float confi
         m_ui->imageLabel->setPixmap(image);
 
         // set textlabel
-        QString patternName = patternPath.split("/").last(); // Remove preceding folder names
-        patternName = patternName.split(".").first(); // Remove file ending
+        QString patternName = patternPath.split("/", QString::SkipEmptyParts).last(); // Remove preceding folder names
+        patternName = patternName.split(".", QString::SkipEmptyParts).first(); // Remove file ending
         m_ui->nameLabel->setText("Pattern: " + patternName);
     }
 }
@@ -204,9 +204,9 @@ void ObjectDetectionView::takeAction()
     QAction* act = dynamic_cast<QAction*>(sender());
     if (act)
     {
-        QString patternPath = act->text().trimmed().split(separator).first(); // Remove additional information
-        QString patternName = patternPath.split("//").last(); // Remove preceding folder names
-        patternName = patternName.split(".").first(); // Remove file ending
+        QString patternPath = act->text().trimmed().split(separator, QString::SkipEmptyParts).first(); // Remove additional information
+        QString patternName = patternPath.split("//", QString::SkipEmptyParts).last(); // Remove preceding folder names
+        patternName = patternName.split(".", QString::SkipEmptyParts).first(); // Remove file ending
 
         // Set name and label
         m_ui->nameLabel->setText(patternName);

src/ui/ObjectDetectionView.h

@@ -62,7 +62,7 @@ class ObjectDetectionView : public QWidget {
     };
 
 public:
-    explicit ObjectDetectionView(QString folder="patterns", QWidget *parent = 0);
+    explicit ObjectDetectionView(QString folder="images/patterns", QWidget *parent = 0);
     virtual ~ObjectDetectionView();
 
     /** @brief Resize widget contents */