From 1215c5451a5cbb4f5394c1c176926853e908cda8 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Sun, 23 Jan 2011 19:36:40 +0100 Subject: [PATCH] Added right-click option to select MAV, ensured that waypoints get updated properly on switching MAV --- src/comm/MAVLinkSimulationLink.cc | 2 +- src/comm/MAVLinkSimulationMAV.cc | 19 +++---- src/ui/MapWidget.cc | 20 +++---- src/ui/map/MAV2DIcon.cc | 107 +++++++++++++++++++++++++------------- src/ui/uas/UASView.cc | 6 +++ src/ui/uas/UASView.h | 1 + 6 files changed, 99 insertions(+), 56 deletions(-) diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 03a367f..c8039c5 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -632,7 +632,7 @@ void MAVLinkSimulationLink::mainloop() // HEARTBEAT VEHICLE 2 // Pack message and get size of encoded byte string - messageSize = mavlink_msg_heartbeat_pack(54, componentId, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_ARDUPILOTMEGA); + messageSize = mavlink_msg_heartbeat_pack(54, componentId, &msg, MAV_HELICOPTER, MAV_AUTOPILOT_ARDUPILOTMEGA); // Allocate buffer with packet data bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); //add data into datastream diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index b6e7629..829f571 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -50,6 +50,13 @@ void MAVLinkSimulationMAV::mainloop() // double xNew = // (nextSPX - previousSPX) + if (flying) + { + sys_state = MAV_STATE_ACTIVE; + sys_mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_WAYPOINT; + } + // 1 Hz execution if (timer1Hz <= 0) { @@ -76,17 +83,6 @@ void MAVLinkSimulationMAV::mainloop() //float trueyaw = atan2f(xm, ym); float newYaw = atan2f(xm, ym); - // Choose shortest direction -// if (yaw - newYaw < -180.0f) -// { -// yaw = newYaw + 180.0f; -// } - -// if (yaw - newYaw > 180.0f) -// { -// yaw = newYaw - 180.0f; -// } - if (fabs(yaw - newYaw) < 90) { @@ -152,6 +148,7 @@ void MAVLinkSimulationMAV::mainloop() status.status = sys_state; mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status); + link->sendMAVLinkMessage(&msg); timer10Hz = 5; } diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 528d8d9..7d0913a 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -157,8 +157,8 @@ MapWidget::MapWidget(QWidget *parent) : zoomout->setStyleSheet(buttonStyle); createPath = new QPushButton(QIcon(":/images/actions/go-bottom.svg"), "", this); createPath->setStyleSheet(buttonStyle); - clearTracking = new QPushButton(QIcon(""), "", this); - clearTracking->setStyleSheet(buttonStyle); +// clearTracking = new QPushButton(QIcon(""), "", this); +// clearTracking->setStyleSheet(buttonStyle); followgps = new QPushButton(QIcon(":/images/actions/system-lock-screen.svg"), "", this); followgps->setStyleSheet(buttonStyle); QPushButton* goToButton = new QPushButton(QIcon(""), "T", this); @@ -167,7 +167,7 @@ MapWidget::MapWidget(QWidget *parent) : zoomin->setMaximumWidth(30); zoomout->setMaximumWidth(30); createPath->setMaximumWidth(30); - clearTracking->setMaximumWidth(30); +// clearTracking->setMaximumWidth(30); followgps->setMaximumWidth(30); goToButton->setMaximumWidth(30); @@ -185,7 +185,7 @@ MapWidget::MapWidget(QWidget *parent) : innerlayout->addWidget(zoomout, 1, 0); innerlayout->addWidget(followgps, 2, 0); innerlayout->addWidget(createPath, 3, 0); - innerlayout->addWidget(clearTracking, 4, 0); + //innerlayout->addWidget(clearTracking, 4, 0); // Add spacers to compress buttons on the top left innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 5, 0); innerlayout->addItem(new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Expanding), 0, 1, 0, 7); @@ -692,7 +692,7 @@ void MapWidget::redoWaypoints(int uas) // Get waypoint list for this MAV // Clear all waypoints - clearWaypoints(); + //clearWaypoints(); // Re-add the updated waypoints // } @@ -714,16 +714,17 @@ void MapWidget::activeUASSet(UASInterface* uas) if (uas) { mav = uas; - QColor color = mav->getColor().lighter(100); - color.setAlphaF(0.6); + QColor color = mav->getColor(); + color.setAlphaF(0.9); QPen* pen = new QPen(color); - pen->setWidth(2.0); + pen->setWidth(3.0); mavPens.insert(mav->getUASID(), pen); // FIXME Remove after refactoring waypointPath->setPen(pen); // Delete all waypoints and add waypoint from new system - redoWaypoints(); + //redoWaypoints(); + updateWaypointList(uas->getUASID()); // Connect the waypoint manager / data storage to the UI connect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int))); @@ -731,6 +732,7 @@ void MapWidget::activeUASSet(UASInterface* uas) connect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*))); updateSelectedSystem(mav->getUASID()); + mc->updateRequest(waypointPath->boundingBox().toRect()); } } diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc index e2c1f77..ded3fe2 100644 --- a/src/ui/map/MAV2DIcon.cc +++ b/src/ui/map/MAV2DIcon.cc @@ -90,28 +90,25 @@ void MAV2DIcon::drawIcon(QPen* pen) // QPen selPen(color); // int width = 5; // selPen.setWidth(width); - painter.setPen(Qt::yellow); + QPen pen(Qt::yellow); + pen.setWidth(2); + painter.setPen(pen); 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: case MAV_ICON_AIRPLANE: - case MAV_ICON_QUADROTOR: - case MAV_ICON_ROTARY_WING: - default: { // DRAW AIRPLANE + // Rotate 180 degs more since the icon is oriented downwards + float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f+180.0f; + + painter.rotate(yawRotate); + //qDebug() << "ICON SIZE:" << radius; float iconSize = radius*0.9f; @@ -141,30 +138,70 @@ void MAV2DIcon::drawIcon(QPen* pen) poly.replace(22, QPointF(-0.031250f*iconSize, -0.262500f*iconSize)); poly.replace(23, QPointF(0.000000f*iconSize, -0.312500f*iconSize)); - // // 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(0); -// painter.setPen(pen2); -// } + painter.setBrush(QBrush(iconColor)); + QPen iconPen(Qt::black); + iconPen.setWidthF(1.0f); + painter.setPen(iconPen); + + painter.drawPolygon(poly); + } + break; + case MAV_ICON_QUADROTOR: + { + // QUADROTOR + float iconSize = radius*0.9f; + float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f; + + painter.rotate(yawRotate); + + //qDebug() << "ICON SIZE:" << radius; + + QPointF front(0, 0.2); + front = front *iconSize; + QPointF left(-0.2, 0); + left = left * iconSize; + QPointF right(0.2, 0.0); + right *= iconSize; + QPointF back(0, -0.2); + back *= iconSize; + + QPolygonF poly(0); + + + + painter.setBrush(QBrush(iconColor)); + QPen iconPen(Qt::black); + iconPen.setWidthF(1.0f); + painter.setPen(iconPen); + + painter.drawPolygon(poly); + + painter.drawEllipse(left, radius/4/2, radius/4/2); + painter.drawEllipse(right, radius/4/2, radius/4/2); + painter.drawEllipse(back, radius/4/2, radius/4/2); + + painter.setBrush(Qt::red); + painter.drawEllipse(front, radius/4/2, radius/4/2); + } + break; + case MAV_ICON_ROTARY_WING: + case MAV_ICON_GENERIC: + default: + { + // GENERIC + + float yawRotate = (yaw/(float)M_PI)*180.0f + 180.0f; + + painter.rotate(yawRotate); + + //qDebug() << "ICON SIZE:" << radius; + + float iconSize = radius*0.9f; + QPolygonF poly(3); + poly.replace(0, QPointF(0.0f*iconSize, 0.3f*iconSize)); + poly.replace(1, QPointF(-0.2f*iconSize, -0.2f*iconSize)); + poly.replace(2, QPointF(0.2f*iconSize, -0.2f*iconSize)); + painter.setBrush(QBrush(iconColor)); QPen iconPen(Qt::black); iconPen.setWidthF(1.0f); diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index 836bdd8..1861c92 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -66,6 +66,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : localFrame(false), removeAction(new QAction("Delete this system", this)), renameAction(new QAction("Rename..", this)), + selectAction(new QAction("Select this system", this )), m_ui(new Ui::UASView) { m_ui->setupUi(this); @@ -102,6 +103,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : // Allow to delete this widget connect(removeAction, SIGNAL(triggered()), this, SLOT(deleteLater())); connect(renameAction, SIGNAL(triggered()), this, SLOT(rename())); + connect(selectAction, SIGNAL(triggered()), uas, SLOT(setSelected())); connect(uas, SIGNAL(systemRemoved()), this, SLOT(deleteLater())); // Name changes @@ -135,6 +137,9 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : UASView::~UASView() { delete m_ui; + delete removeAction; + delete renameAction; + delete selectAction; } void UASView::heartbeatTimeout() @@ -413,6 +418,7 @@ void UASView::contextMenuEvent (QContextMenuEvent* event) { menu.addAction(removeAction); } + menu.addAction(selectAction); menu.exec(event->globalPos()); } diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h index 1a62248..c2f76f3 100644 --- a/src/ui/uas/UASView.h +++ b/src/ui/uas/UASView.h @@ -110,6 +110,7 @@ protected: bool localFrame; QAction* removeAction; QAction* renameAction; + QAction* selectAction; static const int updateInterval = 300;