diff --git a/images/style-mission.css b/images/style-mission.css index 96ac39109a194522f8a35e5d0f4da29d1bbcb2e1..b4b601717240a0a9c10c93192b53d7f7d2a86d6e 100644 --- a/images/style-mission.css +++ b/images/style-mission.css @@ -251,6 +251,21 @@ QSlider::groove:horizontal { margin: -5px 0; /* handle is placed by default on the contents rect of the groove. Expand outside the groove */ border-radius: 3px; } + + QSlider::groove:vertical { + border: 1px solid #999999; + width: 4px; /* the groove expands to the size of the slider by default. by giving it a height, it has a fixed size */ + background: qlineargradient(x1:0, y1:0, x2:1, y2:0, stop:0 #4A4A4F, stop:1 #4A4A4F); + margin: 2px 0; + } + + QSlider::handle:vertical { + background-color: qlineargradient(x1: 0, y1: 0, x2: 1, y2: 0, stop: 0 #232228, stop: 1 #020208); + border: 2px solid #379AC3; + height: 18px; + margin: 0 -5px; /* handle is placed by default on the contents rect of the groove. Expand outside the groove */ + border-radius: 3px; + } QPushButton#forceLandButton { font-weight: bold; diff --git a/lib/QMapControl/src/mapcontrol.cpp b/lib/QMapControl/src/mapcontrol.cpp index 348391ad3d70ec4b20c707fec5e998a585f1711b..3a08f663e25c6b7fd5ce9f55cd0fff38f9dc7557 100644 --- a/lib/QMapControl/src/mapcontrol.cpp +++ b/lib/QMapControl/src/mapcontrol.cpp @@ -377,7 +377,7 @@ namespace qmapcontrol { layermanager->setZoom(zoomlevel); - qDebug() << "MAPCONTROL: Set zoomlevel to:" << zoomlevel << "at " << __FILE__ << __LINE__; + //qDebug() << "MAPCONTROL: Set zoomlevel to:" << zoomlevel << "at " << __FILE__ << __LINE__; update(); } diff --git a/lib/QMapControl/src/mapnetwork.cpp b/lib/QMapControl/src/mapnetwork.cpp index 7fba8ae5306055e103de8e7508b644e97f8f89b5..041ba1c4b8b2466660ac7c80295d21d647d1e857 100644 --- a/lib/QMapControl/src/mapnetwork.cpp +++ b/lib/QMapControl/src/mapnetwork.cpp @@ -93,13 +93,19 @@ namespace qmapcontrol // qDebug() << "Network loaded: " << (loaded); parent->receivedImage(pm, url); } + else if (pm.width() == 0 || pm.height() == 0) + { + // Silently ignore map request for a + // 0xn pixel map + + } else { // QGC FIXME Error is currently undetected // TODO Error is currently undetected //qDebug() << "NETWORK_PIXMAP_ERROR: " << ax; qDebug() << "QMapControl external library: ERROR loading map:" << "width:" << pm.width() << "heigh:" << pm.height() << "at " << __FILE__ << __LINE__; - qDebug() << "HTML ERROR MESSAGE:" << ax << "at " << __FILE__ << __LINE__; + //qDebug() << "HTML ERROR MESSAGE:" << ax << "at " << __FILE__ << __LINE__; } } diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 17c664dcbbf52c5d64f10477d29f2c71a2090e83..aa483e7e98b7046648ffa7619446fb5b00a0f89a 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -159,8 +159,6 @@ void MAVLinkSimulationLink::sendMAVLinkMessage(const mavlink_message_t* msg) readyBuffer.enqueue(*(buf + i)); } // readyBufferMutex.unlock(); - - qDebug() << "SENT MAVLINK MESSAGE FROM SYSTEM" << msg->sysid << "COMP" << msg->compid; } void MAVLinkSimulationLink::enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg) diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 80177415555b3550d60b3f0339c421b7254890fb..6f9b7b9e639ea307856f7c4891fd9a72f1a65cc4 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -48,9 +48,15 @@ void MAVLinkSimulationMAV::mainloop() if (!firstWP) { - x += (nextSPX - previousSPX) * 0.01; - y += (nextSPY - previousSPY) * 0.01; - z += (nextSPZ - previousSPZ) * 0.1; + double xm = (nextSPX - x) * 0.01; + double ym = (nextSPY - y) * 0.01; + double zm = (nextSPZ - z) * 0.1; + + x += xm; + y += ym; + z += zm; + + //if (xm < 0.001) xm } else { diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index d3e53d541f224380d470c92351de6b4eeb8e7d1f..b3d88c30097ae7d7df787ab567f8345fa0865e94 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -180,13 +180,13 @@ void SerialLink::writeBytes(const char* data, qint64 size) // Increase write counter bitsSentTotal += size * 8; - // int i; - // for (i=0; iat(h); quint16 currentPort = ports->at(h); - // QList currentPorts = ports->values(currentHost); - // for (int p = 0; p < currentPorts.size(); p++) - // { - // quint16 currentPort = currentPorts.at(p); - //qDebug() << "Sent message to " << currentHost << ":" << currentPort << "at" << __FILE__ << ":" << __LINE__; - socket->writeDatagram(data, size, currentHost, currentPort); - // } - } - - //if(socket->write(data, size) > 0) { + for (int i=0; iwriteDatagram(data, size, currentHost, currentPort); + } } /** diff --git a/src/input/JoystickInput.cc b/src/input/JoystickInput.cc index 023c97d43966188b151dca7b9e539d7adbf13cd7..a8aaed4aa939a6dc1286794ae84a09cf6fc0afad 100644 --- a/src/input/JoystickInput.cc +++ b/src/input/JoystickInput.cc @@ -52,7 +52,8 @@ JoystickInput::JoystickInput() : thrustAxis(3), xAxis(1), yAxis(0), - yawAxis(2) + yawAxis(2), + joystickName(tr("Unitinialized")) { for (int i = 0; i < 10; i++) { @@ -63,7 +64,7 @@ JoystickInput::JoystickInput() : connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); // Enter main loop - start(); + //start(); } void JoystickInput::setActiveUAS(UASInterface* uas) @@ -88,7 +89,10 @@ void JoystickInput::setActiveUAS(UASInterface* uas) connect(this, SIGNAL(joystickChanged(double,double,double,double,int,int)), tmp, SLOT(setManualControlCommands(double,double,double,double))); connect(this, SIGNAL(buttonPressed(int)), tmp, SLOT(receiveButton(int))); } - + if (!isRunning()) + { + start(); + } } void JoystickInput::init() @@ -118,6 +122,7 @@ void JoystickInput::init() for(int i=0; i < SDL_NumJoysticks(); i++ ) { printf("\t- %s\n", SDL_JoystickName(i)); + joystickName = QString(SDL_JoystickName(i)); } printf("\nOpened %s\n", SDL_JoystickName(defaultIndex)); @@ -125,6 +130,9 @@ void JoystickInput::init() SDL_JoystickEventState(SDL_ENABLE); joystick = SDL_JoystickOpen(defaultIndex); + + // Make sure active UAS is set + setActiveUAS(UASManager::instance()->getActiveUAS()); } /** @@ -282,3 +290,8 @@ void JoystickInput::run() } } + +const QString& JoystickInput::getName() +{ + return joystickName; +} diff --git a/src/input/JoystickInput.h b/src/input/JoystickInput.h index 14ee34cb47674a59b6de2a4cf4bea194c571e34f..786105bba15fa19e5e1a1eabac079109418c02bd 100644 --- a/src/input/JoystickInput.h +++ b/src/input/JoystickInput.h @@ -50,6 +50,8 @@ public: JoystickInput(); void run(); + const QString& getName(); + const double sdlJoystickMin; const double sdlJoystickMax; @@ -68,6 +70,7 @@ protected: int yAxis; int yawAxis; SDL_Event event; + QString joystickName; void init(); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 3ed41e635198bcdc12e0fe03ce73a134e79deadf..8eed5f525aa1b06d4e3fd23720debad657f342d3 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -934,6 +934,7 @@ void UAS::setMode(int mode) { if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_RC_TRAINING) { + this->mode = mode; mavlink_message_t msg; mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode); sendMessage(msg); @@ -1432,15 +1433,19 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double manualYawAngle = yaw * yawScaling; manualThrust = thrust * thrustScaling; - // if(mode == (int)MAV_MODE_MANUAL) - // { - mavlink_message_t message; - mavlink_msg_manual_control_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual); - sendMessage(message); - qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust; + if(mode == (int)MAV_MODE_MANUAL) + { + mavlink_message_t message; + mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual); + sendMessage(message); + qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust; - emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow()); - // } + emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow()); + } + else + { + qDebug() << "JOYSTICK/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send joystick commands first"; + } } int UAS::getSystemType() diff --git a/src/ui/JoystickWidget.cc b/src/ui/JoystickWidget.cc index ae4c577739cfea44ee9eb428dbb0ffd9467d724a..21674253a364a0a035936d79c5ff3750cc11e016 100644 --- a/src/ui/JoystickWidget.cc +++ b/src/ui/JoystickWidget.cc @@ -9,12 +9,13 @@ JoystickWidget::JoystickWidget(JoystickInput* joystick, QWidget *parent) : m_ui->setupUi(this); this->joystick = joystick; - connect(this->joystick, SIGNAL(joystickChanged(double,double,double,double,int,int)), this, SLOT(updateJoystick(double,double,double,double,int,int))); connect(this->joystick, SIGNAL(buttonPressed(int)), this, SLOT(pressKey(int))); // Display the widget this->window()->setWindowTitle(tr("Joystick Settings")); + if (joystick) updateStatus(tr("Found joystick: %1").arg(joystick->getName())); + this->show(); } @@ -69,15 +70,71 @@ void JoystickWidget::setZ(float z) void JoystickWidget::setHat(float x, float y) { qDebug() << __FILE__ << __LINE__ << "HAT X:" << x << "HAT Y:" << y; + updateStatus(tr("Hat position: x: %1, y: %2").arg(x, y)); +} + +void JoystickWidget::clearKeys() +{ + QString colorstyle; + QColor buttonStyleColor = QColor(200, 20, 20); + colorstyle = QString("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: %1;}").arg(buttonStyleColor.name()); + + m_ui->buttonLabel0->setStyleSheet(colorstyle); + m_ui->buttonLabel1->setStyleSheet(colorstyle); + m_ui->buttonLabel2->setStyleSheet(colorstyle); + m_ui->buttonLabel3->setStyleSheet(colorstyle); + m_ui->buttonLabel4->setStyleSheet(colorstyle); + m_ui->buttonLabel5->setStyleSheet(colorstyle); + m_ui->buttonLabel6->setStyleSheet(colorstyle); + m_ui->buttonLabel7->setStyleSheet(colorstyle); + m_ui->buttonLabel8->setStyleSheet(colorstyle); + m_ui->buttonLabel9->setStyleSheet(colorstyle); } void JoystickWidget::pressKey(int key) { QString colorstyle; - QColor heartbeatColor = QColor(20, 200, 20); - colorstyle = colorstyle.sprintf("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X;}", - heartbeatColor.red(), heartbeatColor.green(), heartbeatColor.blue()); - m_ui->button0Label->setStyleSheet(colorstyle); - m_ui->button0Label->setAutoFillBackground(true); + QColor buttonStyleColor = QColor(20, 200, 20); + colorstyle = QString("QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: %1;}").arg(buttonStyleColor.name()); + switch(key) + { + case 0: + m_ui->buttonLabel0->setStyleSheet(colorstyle); + break; + case 1: + m_ui->buttonLabel1->setStyleSheet(colorstyle); + break; + case 2: + m_ui->buttonLabel2->setStyleSheet(colorstyle); + break; + case 3: + m_ui->buttonLabel3->setStyleSheet(colorstyle); + break; + case 4: + m_ui->buttonLabel4->setStyleSheet(colorstyle); + break; + case 5: + m_ui->buttonLabel5->setStyleSheet(colorstyle); + break; + case 6: + m_ui->buttonLabel6->setStyleSheet(colorstyle); + break; + case 7: + m_ui->buttonLabel7->setStyleSheet(colorstyle); + break; + case 8: + m_ui->buttonLabel8->setStyleSheet(colorstyle); + break; + case 9: + m_ui->buttonLabel9->setStyleSheet(colorstyle); + break; + } + QTimer::singleShot(20, this, SLOT(clearKeys())); qDebug() << __FILE__ << __LINE__ << "KEY" << key << " pressed on joystick"; + updateStatus(tr("Key %1 pressed").arg(key)); +} + +void JoystickWidget::updateStatus(const QString& status) +{ + } diff --git a/src/ui/JoystickWidget.h b/src/ui/JoystickWidget.h index 87bd1e5a9ffed4413be82fd48760a3c5b1815fab..858166ebf5e142bb0f926b4efcc4e15aa9b8bea7 100644 --- a/src/ui/JoystickWidget.h +++ b/src/ui/JoystickWidget.h @@ -67,8 +67,12 @@ public: void setZ(float z); /** @brief Hat switch position */ void setHat(float x, float y); + /** @brief Clear keys */ + void clearKeys(); /** @brief Joystick keys, as labeled on the joystick */ void pressKey(int key); + /** @brief Update status string */ + void updateStatus(const QString& status); protected: /** @brief UI change event */ diff --git a/src/ui/JoystickWidget.ui b/src/ui/JoystickWidget.ui index 403cc3dd94738ccb6354a4972bfa4015a4434da1..390736324ab66f2bf28d66ec1d1081a45b430505 100644 --- a/src/ui/JoystickWidget.ui +++ b/src/ui/JoystickWidget.ui @@ -6,153 +6,355 @@ 0 0 - 400 - 300 + 396 + 323 + + + 368 + 274 + + Dialog - - - - 30 - 240 - 341 - 32 - - - - Qt::Horizontal - - - QDialogButtonBox::Cancel|QDialogButtonBox::Ok - - - - - - 350 - 20 - 31 - 181 - - - - 0 - - - Qt::Vertical - - - - - - 160 - 190 - 160 - 17 - - - - 100 - - - Qt::Horizontal - - - - - - 140 - 40 - 17 - 160 - - - - 100 - - - Qt::Vertical - - - - - - 160 - 50 - 41 - 23 - - - - QFrame::Plain - - - true - - - 3 - - - QLCDNumber::Flat - - - - - - 270 - 160 - 41 - 23 - - - - QFrame::Plain - - - true - - - 3 - - - QLCDNumber::Flat - - - - - - 210 - 50 - 101 - 101 - - - - - - - 60 - 20 - 41 - 16 - - - - false - - - QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #FF2222;} - - - + + + 8 - - false + + 8 - + + + + Stick + + + + 6 + + + + + 100 + + + Qt::Horizontal + + + + + + + + 40 + 24 + + + + QFrame::Plain + + + true + + + 3 + + + QLCDNumber::Flat + + + + + + + + + + Y + + + + + + + X + + + + + + + + 0 + 0 + + + + + 40 + 24 + + + + QFrame::Plain + + + true + + + 3 + + + QLCDNumber::Flat + + + + + + + 100 + + + Qt::Vertical + + + + + + + + + + + 60 + 16777215 + + + + Throttle + + + + 2 + + + + + + 40 + 20 + + + + 0 + + + Qt::Vertical + + + + + + + + + + No joystick - connect and restart QGroundControl + + + + + + + + 40 + 16777215 + + + + Buttons + + + + 2 + + + + + false + + + QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #FF2222;} + + + + + + false + + + + + + + false + + + QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #FF2222;} + + + + + + false + + + + + + + false + + + QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #FF2222;} + + + + + + false + + + + + + + false + + + QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #FF2222;} + + + + + + false + + + + + + + false + + + QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #FF2222;} + + + + + + false + + + + + + + false + + + QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #FF2222;} + + + + + + false + + + + + + + false + + + QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #FF2222;} + + + + + + false + + + + + + + false + + + QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #FF2222;} + + + + + + false + + + + + + + false + + + QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #FF2222;} + + + + + + false + + + + + + + false + + + QGroupBox { border: 1px solid #EEEEEE; border-radius: 4px; padding: 0px; margin: 0px; background-color: #FF2222;} + + + + + + false + + + + + + + + + + Qt::Horizontal + + + QDialogButtonBox::Cancel|QDialogButtonBox::Ok + + + + diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 1159ff38310bc86a7ce8878588145c063cb6306c..b09ecf2b7005b0efd7ead161a5f7c39465348e2c 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -202,6 +202,12 @@ MainWindow::MainWindow(QWidget *parent): connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addLink(LinkInterface*))); + // Connect user interface devices + if (!joystick) + { + joystick = new JoystickInput(); + } + // Enable and update view presentView(); } @@ -492,11 +498,6 @@ void MainWindow::buildPxWidgets() // Dialogue widgets //FIXME: free memory in destructor - if (!joystick) - { - joystick = new JoystickInput(); - } - } void MainWindow::buildSlugsWidgets() @@ -1159,14 +1160,20 @@ void MainWindow::connectCommonActions() // Audio output ui.actionMuteAudioOutput->setChecked(GAudioOutput::instance()->isMuted()); connect(ui.actionMuteAudioOutput, SIGNAL(triggered(bool)), GAudioOutput::instance(), SLOT(mute(bool))); + + // User interaction + // NOTE: Joystick thread is not started and + // configuration widget is not instantiated + // unless it is actually used + // so no ressources spend on this. + ui.actionJoystickSettings->setVisible(true); + // Joystick configuration + connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); } void MainWindow::connectPxActions() { - ui.actionJoystickSettings->setVisible(true); - // Joystick configuration - connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); } @@ -1219,7 +1226,15 @@ void MainWindow::showRoadMap() void MainWindow::configure() { - joystickWidget = new JoystickWidget(joystick, this); + if (!joystickWidget) + { + if (!joystick->isRunning()) + { + joystick->start(); + } + joystickWidget = new JoystickWidget(joystick); + } + joystickWidget->show(); } void MainWindow::addLink() diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index ebd66090415f8a76df209254a4bc495281d1dc0f..5808be0b0053bb85beee4a4b984c4d7dceda6936 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -51,7 +51,7 @@ 0 0 800 - 25 + 22 @@ -62,6 +62,7 @@ + @@ -215,7 +216,7 @@ :/images/devices/input-gaming.svg:/images/devices/input-gaming.svg - Joystick settings + Joystick Test true diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 443ee5ff27442d0fdb7c026734319729a28f3d42..fca34aa29264069d612f5b58cbdf888635afbccb 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -469,7 +469,7 @@ void MapWidget::createWaypointGraphAtMap(const QPointF coordinate) qDebug()<<"Funcion createWaypointGraphAtMap WP= "< x= "<latitude()<<" y= "<longitude(); // Refresh the screen - mc->updateRequest(tempPoint->boundingBox().toRect()); + mc->updateRequestNew();//(tempPoint->boundingBox().toRect()); } //// // emit signal mouse was clicked @@ -630,6 +630,8 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon, uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(lat, lon, "")); } + mc->updateRequest(p->boundingBox().toRect()); + //mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect()); @@ -745,7 +747,7 @@ void MapWidget::clearWaypoints() waypointPath->setPoints(wps); mc->layer("Waypoints")->addGeometry(waypointPath); wpIndex.clear(); - mc->updateRequest(waypointPath->boundingBox().toRect()); + mc->updateRequestNew();//(waypointPath->boundingBox().toRect()); if(createPath->isChecked()) { @@ -764,7 +766,7 @@ void MapWidget::clearPath() mc->layer("Tracking")->addGeometry(lsNew); } // FIXME update this with update request only for bounding box of trails - mc->updateRequest(QRect(0, 0, width(), height())); + mc->updateRequestNew();//(QRect(0, 0, width(), height())); } void MapWidget::changeGlobalWaypointPositionBySpinBox(int index, float lat, float lon) @@ -785,7 +787,7 @@ void MapWidget::changeGlobalWaypointPositionBySpinBox(int index, float lat, floa point2Find->setCoordinate(coordinate); // Refresh the screen - mc->updateRequest(point2Find->boundingBox().toRect()); + mc->updateRequestNew();//(point2Find->boundingBox().toRect()); } diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc index e8d52e7758c222049e4a5beb18241e323ffe00b3..8920f76d996e626505b450d55620cd144e1f3630 100644 --- a/src/ui/map/MAV2DIcon.cc +++ b/src/ui/map/MAV2DIcon.cc @@ -40,53 +40,56 @@ void MAV2DIcon::setPen(QPen* pen) void MAV2DIcon::setYaw(float yaw) { this->yaw = yaw; + drawIcon(mypen); } void MAV2DIcon::drawIcon(QPen* pen) { - if (pen) - { - mypixmap = new QPixmap(radius+1, radius+1); - //mypixmap->fill(Qt::transparent); - QPainter painter(mypixmap); - // DRAW MICRO AIR VEHICLE - QPointF p(radius/2, radius/2); + mypixmap = new QPixmap(radius+1, radius+1); + mypixmap->fill(Qt::transparent); + QPainter painter(mypixmap); - float waypointSize = radius; - QPolygonF poly(3); - // Top point - poly.replace(0, QPointF(p.x(), p.y()+waypointSize/2.0f)); - // Right point - poly.replace(1, QPointF(p.x()-waypointSize/2.0f, p.y()-waypointSize/2.0f)); - // Left point - poly.replace(2, QPointF(p.x()+waypointSize/2.0f, p.y() + waypointSize/2.0f)); + // DRAW WAYPOINT + QPointF p(radius/2, radius/2); - // // Select color based on if this is the current waypoint - // if (list.at(i)->getCurrent()) - // { - // color = QGC::colorCyan;//uas->getColor(); - // pen.setWidthF(refLineWidthToPen(0.8f)); - // } - // else - // { - // color = uas->getColor(); - // pen.setWidthF(refLineWidthToPen(0.4f)); - // } + float waypointSize = radius; + QPolygonF poly(4); + // Top point + poly.replace(0, QPointF(p.x(), p.y()-waypointSize/2.0f)); + // Right point + poly.replace(1, QPointF(p.x()+waypointSize/2.0f, p.y())); + // Bottom point + poly.replace(2, QPointF(p.x(), p.y() + waypointSize/2.0f)); + poly.replace(3, QPointF(p.x() - waypointSize/2.0f, p.y())); - //pen.setColor(color); - if (pen) - { - pen->setWidthF(2); - painter.setPen(*pen); - } - else - { - QPen pen2(Qt::yellow); - pen2.setWidth(2); - painter.setPen(pen2); - } - painter.setBrush(Qt::NoBrush); - painter.drawPolygon(poly); +// // Select color based on if this is the current waypoint +// if (list.at(i)->getCurrent()) +// { +// color = QGC::colorCyan;//uas->getColor(); +// pen.setWidthF(refLineWidthToPen(0.8f)); +// } +// else +// { +// color = uas->getColor(); +// pen.setWidthF(refLineWidthToPen(0.4f)); +// } + + //pen.setColor(color); + if (pen) + { + pen->setWidthF(2); + painter.setPen(*pen); + } + else + { + QPen pen2(Qt::red); + pen2.setWidth(2); + painter.setPen(pen2); } + painter.setBrush(Qt::NoBrush); + + float rad = (waypointSize/2.0f) * 0.8 * (1/sqrt(2.0f)); + painter.drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * rad); + painter.drawPolygon(poly); } diff --git a/src/ui/map/Waypoint2DIcon.cc b/src/ui/map/Waypoint2DIcon.cc index 3ccf52a31f4735f227eea1ede1aeb9835de2003e..4afcee1c6057e59b3b86b4352728591fc38417b6 100644 --- a/src/ui/map/Waypoint2DIcon.cc +++ b/src/ui/map/Waypoint2DIcon.cc @@ -35,6 +35,7 @@ void Waypoint2DIcon::setPen(QPen* pen) void Waypoint2DIcon::setYaw(float yaw) { this->yaw = yaw; + drawIcon(mypen); } void Waypoint2DIcon::drawIcon(QPen* pen)