diff --git a/images/earth.html b/images/earth.html
index e2b789ca313e66b0881ebc0be237b7c70ddd04b8..c5699c596d2fdf2eff7948b77dc4780c19abaad8 100644
--- a/images/earth.html
+++ b/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);
}
diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc
index 4e71b50775cc6d396cfb22cd67e33954666d388f..b054e26bf4cc6dd44914723553c3d352406583c0 100644
--- a/src/comm/MAVLinkSimulationMAV.cc
+++ b/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;
diff --git a/src/comm/MAVLinkSimulationMAV.h b/src/comm/MAVLinkSimulationMAV.h
index 5f92559c93c0e5f562a4ec9645e8897f1bf25b57..2a42598ff571dc09a30ba9bd6f184c041e4bc057 100644
--- a/src/comm/MAVLinkSimulationMAV.h
+++ b/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;
};
diff --git a/src/comm/MAVLinkSimulationWaypointPlanner.cc b/src/comm/MAVLinkSimulationWaypointPlanner.cc
index 0492810fec080ea6579a5ef05746145c737983ca..c4e5f33f8f367184b9348370afed761318a003a0 100644
--- a/src/comm/MAVLinkSimulationWaypointPlanner.cc
+++ b/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
{
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 7484b90c16cf36064049b26c1e8f4f6692adf0ec..563bb86507d92280298c5285b9a70ff581cf2a1d 100644
--- a/src/uas/UAS.cc
+++ b/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)
diff --git a/src/uas/UAS.h b/src/uas/UAS.h
index dfd9d11d4693a7e50fb41fbd2435dde1527e8725..603e55e3c279f63b383785847d10749535e4ab1d 100644
--- a/src/uas/UAS.h
+++ b/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)
diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h
index 514d596a24d7cdd6ced9da27aede6ed239186a55..77f3c00dcf35146f6126369d73aff1da0e84b82c 100644
--- a/src/uas/UASInterface.h
+++ b/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:
diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc
index 68ef921ea2f0eb79b3b4df6927baf5f6c3d15327..e2fd16f48a0f57a2c7bed000b6e48bef31a6019d 100644
--- a/src/uas/UASManager.cc
+++ b/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));
diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc
index 9ce004f7f8cf48aa96fe74bcebaa652b76e22ecd..528d8d96d11a6c87a412598989a98a7e9a934aad 100644
--- a/src/ui/MapWidget.cc
+++ b/src/ui/MapWidget.cc
@@ -36,7 +36,7 @@ MapWidget::MapWidget(QWidget *parent) :
m_ui(new Ui::MapWidget)
{
m_ui->setupUi(this);
- mc = new qmapcontrol::MapControl(QSize(320, 240));
+ mc = new qmapcontrol::MapControl(this->size());
// VISUAL MAP STYLE
QString buttonStyle("QAbstractButton { background-color: rgba(20, 20, 20, 45%); border-color: rgba(10, 10, 10, 50%)} QAbstractButton:checked { border: 2px solid #379AC3; }");
@@ -254,26 +254,26 @@ MapWidget::MapWidget(QWidget *parent) :
void MapWidget::goTo()
{
bool ok;
- QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
- tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
- QString("%1,%2").arg(mc->currentCoordinate().x()).arg(mc->currentCoordinate().y()), &ok);
- if (ok && !text.isEmpty())
- {
- QStringList split = text.split(",");
- if (split.length() == 2)
- {
- bool convert;
- double latitude = split.first().toDouble(&convert);
- ok &= convert;
- double longitude = split.last().toDouble(&convert);
- ok &= convert;
-
- if (ok)
- {
- mc->setView(QPointF(latitude, longitude));
- }
- }
- }
+ QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
+ tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
+ QString("%1,%2").arg(mc->currentCoordinate().x()).arg(mc->currentCoordinate().y()), &ok);
+ if (ok && !text.isEmpty())
+ {
+ QStringList split = text.split(",");
+ if (split.length() == 2)
+ {
+ bool convert;
+ double latitude = split.first().toDouble(&convert);
+ ok &= convert;
+ double longitude = split.last().toDouble(&convert);
+ ok &= convert;
+
+ if (ok)
+ {
+ mc->setView(QPointF(latitude, longitude));
+ }
+ }
+ }
}
@@ -290,7 +290,7 @@ void MapWidget::mapproviderSelected(QAction* action)
l->setMapAdapter(mapadapter);
geomLayer->setMapAdapter(mapadapter);
- mc->updateRequestNew();
+ if (isVisible()) mc->updateRequestNew();
mc->setZoom(zoom);
// yahooActionOverlay->setEnabled(false);
overlay->setVisible(false);
@@ -306,7 +306,7 @@ void MapWidget::mapproviderSelected(QAction* action)
l->setMapAdapter(mapadapter);
geomLayer->setMapAdapter(mapadapter);
- mc->updateRequestNew();
+ if (isVisible()) mc->updateRequestNew();
mc->setZoom(zoom);
// yahooActionOverlay->setEnabled(false);
overlay->setVisible(false);
@@ -321,7 +321,7 @@ void MapWidget::mapproviderSelected(QAction* action)
mapadapter = new qmapcontrol::YahooMapAdapter("us.maps3.yimg.com", "/aerial.maps.yimg.com/png?v=1.7&t=a&s=256&x=%2&y=%3&z=%1");
l->setMapAdapter(mapadapter);
- mc->updateRequestNew();
+ if (isVisible()) mc->updateRequestNew();
mc->setZoom(zoom);
// yahooActionOverlay->setEnabled(true);
}
@@ -333,7 +333,7 @@ void MapWidget::mapproviderSelected(QAction* action)
l->setMapAdapter(mapadapter);
geomLayer->setMapAdapter(mapadapter);
- mc->updateRequestNew();
+ if (isVisible()) mc->updateRequestNew();
mc->setZoom(zoom);
// yahooActionOverlay->setEnabled(false);
overlay->setVisible(false);
@@ -347,7 +347,7 @@ void MapWidget::mapproviderSelected(QAction* action)
l->setMapAdapter(mapadapter);
geomLayer->setMapAdapter(mapadapter);
- mc->updateRequestNew();
+ if (isVisible()) mc->updateRequestNew();
mc->setZoom(zoom);
// yahooActionOverlay->setEnabled(false);
overlay->setVisible(false);
@@ -426,7 +426,7 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina
waypointPath->addPoint(tempPoint);
// Refresh the screen
- mc->updateRequest(tempPoint->boundingBox().toRect());
+ if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect());
}
// emit signal mouse was clicked
@@ -445,10 +445,11 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
if (uas == this->mav->getUASID())
{
int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getIndexOf(wp);
+ if (wpindex == -1) return;
// Create waypoint name
- QString str = QString("%1").arg(wpindex);
+ //QString str = QString("%1").arg(wpindex);
// Check if wp exists yet
- if (!(wps.count() > wpindex))
+ if (!(wpIcons.count() > wpindex))
{
QPointF coordinate;
coordinate.setX(wp->getX());
@@ -494,7 +495,7 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
//point2Find = dynamic_cast (mc->layer("Waypoints")->get_Geometry(wpindex));
//point2Find->setCoordinate(coordinate);
- if (updateView) mc->updateRequest(waypoint->boundingBox().toRect());
+ if (updateView) if (isVisible()) mc->updateRequest(waypoint->boundingBox().toRect());
}
}
}
@@ -537,7 +538,7 @@ void MapWidget::createWaypointGraphAtMap(int id, const QPointF coordinate)
qDebug()<<"Funcion createWaypointGraphAtMap WP= "< x= "<latitude()<<" y= "<longitude();
// Refresh the screen
- mc->updateRequest(tempPoint->boundingBox().toRect());
+ if (isVisible()) if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect());
}
//// // emit signal mouse was clicked
@@ -570,7 +571,7 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
waypointIsDrag = true;
// Refresh the screen
- mc->updateRequest(geom->boundingBox().toRect());
+ if (isVisible()) mc->updateRequest(geom->boundingBox().toRect());
int temp = 0;
@@ -578,40 +579,35 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
bool wpIndexOk;
int index = geom->name().toInt(&wpIndexOk);
- qmapcontrol::Point* point2Find;
- point2Find = wps.at(geom->name().toInt());
+ Waypoint2DIcon* point2Find = dynamic_cast (geom);
- if (point2Find)
+ if (wpIndexOk && point2Find && wps.count() > index)
{
+ // Update visual
point2Find->setCoordinate(coordinate);
+ waypointPath->points().at(index)->setCoordinate(coordinate);
+ if (isVisible()) mc->updateRequest(waypointPath->boundingBox().toRect());
- point2Find = dynamic_cast (geom);
- if (point2Find)
+ // Update waypoint data storage
+ if (mav)
{
- point2Find->setCoordinate(coordinate);
+ QVector wps = mav->getWaypointManager()->getWaypointList();
- if (wpIndexOk)
+ if (wps.size() > index)
{
- mc->updateRequest(point2Find->boundingBox().toRect());
- if (mav)
- {
- QVector wps = mav->getWaypointManager()->getWaypointList();
-
- if (wps.size() > index)
- {
- wps.at(index)->setX(coordinate.x());
- wps.at(index)->setY(coordinate.y());
- mav->getWaypointManager()->notifyOfChange(wps.at(index));
- }
- }
+ wps.at(index)->setX(coordinate.x());
+ wps.at(index)->setY(coordinate.y());
+ mav->getWaypointManager()->notifyOfChange(wps.at(index));
}
- // qDebug() << geom->name();
- temp = geom->get_myIndex();
- //qDebug() << temp;
- emit sendGeometryEndDrag(coordinate,temp);
}
+
+ // qDebug() << geom->name();
+ temp = geom->get_myIndex();
+ //qDebug() << temp;
+ emit sendGeometryEndDrag(coordinate,temp);
}
+ waypointIsDrag = false;
}
void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate)
@@ -670,21 +666,20 @@ void MapWidget::updateWaypointList(int uas)
}
// Delete now unused wps
- if (wps.count() > wpList.count())
+ if (waypointPath->points().count() > wpList.count())
{
- for (int i = wpList.count(); i < wps.count(); ++i)
+ int overSize = waypointPath->points().count() - wpList.count();
+ for (int i = 0; i < overSize; ++i)
{
- wps.removeAt(i);
- mc->layer("Waypoints")->removeGeometry(wpIcons.at(i));
- waypointPath->points().removeAt(i);
- //Point* linesegment = waypointPath->points().at(mav->getWaypointManager()->getWaypointList().indexOf(wp));
-
-
+ wps.removeLast();
+ mc->layer("Waypoints")->removeGeometry(wpIcons.last());
+ wpIcons.removeLast();
+ waypointPath->points().removeLast();
}
}
// Update view
- mc->updateRequest(updateRect);
+ if (isVisible()) mc->updateRequest(updateRect);
}
}
@@ -734,6 +729,21 @@ void MapWidget::activeUASSet(UASInterface* uas)
connect(mav->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
connect(mav->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
connect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(Waypoint*)));
+
+ updateSelectedSystem(mav->getUASID());
+ }
+}
+
+void MapWidget::updateSelectedSystem(int uas)
+{
+ foreach (qmapcontrol::Point* p, uasIcons.values())
+ {
+ MAV2DIcon* icon = dynamic_cast(p);
+ if (icon)
+ {
+ // Set as selected if ids match
+ icon->setSelectedUAS((icon->getUASId() == uas));
+ }
}
}
@@ -787,63 +797,62 @@ void MapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lon,
// Icon
//QPen* pointpen = new QPen(uasColor);
qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__;
- //p = new MAV2DIcon(lat, lon, 20, uas->getUASName(), qmapcontrol::Point::Middle, mavPens.value(uas->getUASID()));
- p = new MAV2DIcon(lat, lon, 50, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
+ p = new MAV2DIcon(uas, 50, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
uasIcons.insert(uas->getUASID(), p);
mc->layer("Waypoints")->addGeometry(p);
// Line
// A QPen also can use transparency
-// QList points;
-// points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
-// QPen* linepen = new QPen(uasColor.darker());
-// linepen->setWidth(2);
+ // QList points;
+ // points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
+ // QPen* linepen = new QPen(uasColor.darker());
+ // linepen->setWidth(2);
-// // Create tracking line string
-// qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen);
-// uasTrails.insert(uas->getUASID(), ls);
+ // // Create tracking line string
+ // qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen);
+ // uasTrails.insert(uas->getUASID(), ls);
-// // Add the LineString to the layer
-// mc->layer("Waypoints")->addGeometry(ls);
+ // // Add the LineString to the layer
+ // mc->layer("Waypoints")->addGeometry(ls);
}
else
{
-// p = dynamic_cast(uasIcons.value(uas->getUASID()));
-// if (p)
-// {
- p = uasIcons.value(uas->getUASID());
- p->setCoordinate(QPointF(lat, lon));
- //p->setYaw(uas->getYaw());
-// }
+ // p = dynamic_cast(uasIcons.value(uas->getUASID()));
+ // if (p)
+ // {
+ p = uasIcons.value(uas->getUASID());
+ p->setCoordinate(QPointF(lat, lon));
+ //p->setYaw(uas->getYaw());
+ // }
// Extend trail
-// uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
+ // uasTrails.value(uas->getUASID())->addPoint(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
}
-mc->updateRequest(p->boundingBox().toRect());
+ if (isVisible()) mc->updateRequest(p->boundingBox().toRect());
- //mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());
+ //if (isVisible()) mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());
- if (uas->getUASID() == this->mav->getUASID())
-{
- // Limit the position update rate
- quint64 currTime = MG::TIME::getGroundTimeNow();
- if (currTime - lastUpdate > 120)
+ if (this->mav && uas->getUASID() == this->mav->getUASID())
{
- lastUpdate = currTime;
- // Sets the view to the interesting area
- if (followgps->isChecked())
+ // Limit the position update rate
+ quint64 currTime = MG::TIME::getGroundTimeNow();
+ if (currTime - lastUpdate > 120)
{
- updatePosition(0, lat, lon);
- }
- else
- {
- // Refresh the screen
- //mc->updateRequestNew();
+ lastUpdate = currTime;
+ // Sets the view to the interesting area
+ if (followgps->isChecked())
+ {
+ updatePosition(0, lat, lon);
+ }
+ else
+ {
+ // Refresh the screen
+ //if (isVisible()) mc->updateRequestNew();
+ }
}
}
}
-}
/**
* Center the view on this position
@@ -953,7 +962,7 @@ void MapWidget::clearWaypoints(int uas)
//waypointPath = new
//mc->layer("Waypoints")->addGeometry(waypointPath);
//wpIndex.clear();
- mc->updateRequest(box);//(waypointPath->boundingBox().toRect());
+ if (isVisible()) mc->updateRequest(box);//(waypointPath->boundingBox().toRect());
if(createPath->isChecked())
{
@@ -975,7 +984,7 @@ void MapWidget::clearPath(int uas)
mc->layer("Tracking")->addGeometry(lsNew);
}
// FIXME update this with update request only for bounding box of trails
- mc->updateRequestNew();//(QRect(0, 0, width(), height()));
+ if (isVisible()) mc->updateRequestNew();//(QRect(0, 0, width(), height()));
}
void MapWidget::updateCameraPosition(double radio, double bearing, QString dir)
@@ -1012,14 +1021,14 @@ void MapWidget::updateCameraPosition(double radio, double bearing, QString dir)
mc->layer("Camera")->addGeometry(camBorder);
// mc->layer("Camera")->addGeometry(camLine);
- mc->updateRequestNew();
+ if (isVisible()) mc->updateRequestNew();
}
else
{
//clear camera borders
mc->layer("Camera")->clearGeometries();
- mc->updateRequestNew();
+ if (isVisible()) mc->updateRequestNew();
}
diff --git a/src/ui/MapWidget.h b/src/ui/MapWidget.h
index f97430bed14c36bcad02dc099f6af8c20d8e70ee..9db98ab77c77bb1a9077382920fea5389467f802 100644
--- a/src/ui/MapWidget.h
+++ b/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);
diff --git a/src/ui/WaypointView.ui b/src/ui/WaypointView.ui
index 5f0188b07d34b890e8437930a88bacdb5724c808..4615d6b48fd6e125a3ef6e668541d412f75a162c 100644
--- a/src/ui/WaypointView.ui
+++ b/src/ui/WaypointView.ui
@@ -235,7 +235,7 @@ QProgressBar::chunk#thrustBar {
- Qt::ClickFocus
+ Qt::WheelFocus
Position X coordinate
@@ -275,7 +275,7 @@ QProgressBar::chunk#thrustBar {
- Qt::StrongFocus
+ Qt::WheelFocus
Position Y coordinate
@@ -309,7 +309,7 @@ QProgressBar::chunk#thrustBar {
- Qt::StrongFocus
+ Qt::WheelFocus
Position Z coordinate (negative)
@@ -340,7 +340,7 @@ QProgressBar::chunk#thrustBar {
- Qt::StrongFocus
+ Qt::WheelFocus
lat
@@ -371,7 +371,7 @@ QProgressBar::chunk#thrustBar {
- Qt::StrongFocus
+ Qt::WheelFocus
lon
diff --git a/src/ui/map/MAV2DIcon.cc b/src/ui/map/MAV2DIcon.cc
index 86a1dc22336bc2da2d7d10a61d6382baad84016e..e2c1f775af129dbbe63f8dacbb6c98959407773d 100644
--- a/src/ui/map/MAV2DIcon.cc
+++ b/src/ui/map/MAV2DIcon.cc
@@ -3,13 +3,16 @@
#include
-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);
}
diff --git a/src/ui/map/MAV2DIcon.h b/src/ui/map/MAV2DIcon.h
index cc764d01c6f932c2b7d3d9f67ed37889676e22c8..64173b9b4c16da2f6481de65d59d25600d657c98 100644
--- a/src/ui/map/MAV2DIcon.h
+++ b/src/ui/map/MAV2DIcon.h
@@ -4,6 +4,8 @@
#include
#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
};
diff --git a/src/ui/map3D/QGCGoogleEarthView.ui b/src/ui/map3D/QGCGoogleEarthView.ui
index 62ed1354b22f3e691b0e054ead26dbf001918605..9fd726a4a071f113cdac3920bc17baffa3dd6acd 100644
--- a/src/ui/map3D/QGCGoogleEarthView.ui
+++ b/src/ui/map3D/QGCGoogleEarthView.ui
@@ -83,10 +83,10 @@
Distance of the chase camera to the MAV
- 30
+ 100
- 10000
+ 20000
3000