From 8438b4d5a3466704b18117fa7f3880b84d1fd1e3 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Thu, 26 Aug 2010 21:44:20 +0200 Subject: [PATCH] Improved simulation, shows off newest features --- src/comm/MAVLinkSimulationLink.cc | 130 ++++++++++++++++++++++++++++-- src/comm/MAVLinkSimulationLink.h | 14 ++-- src/uas/UASInterface.h | 2 +- src/ui/MainWindow.cc | 4 +- src/ui/ObjectDetectionView.cc | 14 ++-- src/ui/ObjectDetectionView.h | 2 +- 6 files changed, 140 insertions(+), 26 deletions(-) diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index a20877d7e..6aeefc451 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -377,20 +377,70 @@ 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; + + // Send back new position + 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; + + 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 +455,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 +542,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 diff --git a/src/comm/MAVLinkSimulationLink.h b/src/comm/MAVLinkSimulationLink.h index ef223aa40..63574b3ad 100644 --- a/src/comm/MAVLinkSimulationLink.h +++ b/src/comm/MAVLinkSimulationLink.h @@ -1,29 +1,29 @@ /*===================================================================== -PIXHAWK Micro Air Vehicle Flying Robotics Toolkit +QGroundControl Open Source Ground Control Station -(c) 2009, 2010 PIXHAWK PROJECT +(c) 2009, 2010 QGROUNDCONTROL PROJECT -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 . + along with QGROUNDCONTROL. If not, see . ======================================================================*/ /** * @file - * @brief Definition of simulated system link + * @brief Definition of MAVLinkSimulationLink * * @author Lorenz Meier * diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 41931ad18..21e67dc7f 100644 --- a/src/uas/UASInterface.h +++ b/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)); diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index c7101a5b2..42c8c1c23 100644 --- a/src/ui/MainWindow.cc +++ b/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); diff --git a/src/ui/ObjectDetectionView.cc b/src/ui/ObjectDetectionView.cc index f5a80e804..03d8c007e 100644 --- a/src/ui/ObjectDetectionView.cc +++ b/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(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); diff --git a/src/ui/ObjectDetectionView.h b/src/ui/ObjectDetectionView.h index d48d0945f..9e88a38ba 100644 --- a/src/ui/ObjectDetectionView.h +++ b/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 */ -- 2.22.0