zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Fully working waypoint interface, thoroughly tested. Active waypoint setting has to be debugged in the simulator, but works fine on real hardware

QGC4.4
pixhawk 15 years ago
parent
77e27a2ad6
commit
31f97dd6b5
14 changed files with 332 additions and 187 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      images/earth.html
  2. 143
      src/comm/MAVLinkSimulationMAV.cc
  3. 4
      src/comm/MAVLinkSimulationMAV.h
  4. 6
      src/comm/MAVLinkSimulationWaypointPlanner.cc
  5. 11
      src/uas/UAS.cc
  6. 3
      src/uas/UAS.h
  7. 4
      src/uas/UASInterface.h
  8. 1
      src/uas/UASManager.cc
  9. 217
      src/ui/MapWidget.cc
  10. 2
      src/ui/MapWidget.h
  11. 10
      src/ui/WaypointView.ui
  12. 85
      src/ui/map/MAV2DIcon.cc
  13. 19
      src/ui/map/MAV2DIcon.h
  14. 4
      src/ui/map3D/QGCGoogleEarthView.ui

10
images/earth.html
Unescape View File

@ -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 // FIXME Currently invalid conversion from right-handed z-down to z-up frame
planeOrient.setRoll(((roll/M_PI)+1.0)*360.0); planeOrient.setRoll(((roll/M_PI))*180.0+180.0);
planeOrient.setTilt(((pitch/M_PI)+1.0)*360.0); planeOrient.setTilt(((pitch/M_PI))*180.0+180.0);
planeOrient.setHeading(((yaw/M_PI)+1.0)*360.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.setLatitude(lastLat);
planeLoc.setLongitude(lastLon); planeLoc.setLongitude(lastLon);
@ -303,7 +303,7 @@ function updateFollowAircraft()
currView.setRange(currViewRange); currView.setRange(currViewRange);
currView.setTilt(currFollowTilt); currView.setTilt(currFollowTilt);
currView.setHeading(currFollowHeading-90.0); currView.setHeading(currFollowHeading);
ge.getView().setAbstractView(currView); ge.getView().setAbstractView(currView);
} }

143
src/comm/MAVLinkSimulationMAV.cc
Unescape View File

@ -4,32 +4,36 @@
#include "MAVLinkSimulationMAV.h" #include "MAVLinkSimulationMAV.h"
MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid) : MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid) :
QObject(parent), QObject(parent),
link(parent), link(parent),
planner(parent, systemid), planner(parent, systemid),
systemid(systemid), systemid(systemid),
timer25Hz(0), timer25Hz(0),
timer10Hz(0), timer10Hz(0),
timer1Hz(0), timer1Hz(0),
latitude(47.376389), latitude(47.376389),
longitude(8.548056), longitude(8.548056),
altitude(0.0), altitude(0.0),
x(8.548056), x(8.548056),
y(47.376389), y(47.376389),
z(550), z(550),
roll(0.0), roll(0.0),
pitch(0.0), pitch(0.0),
yaw(0.0), yaw(0.0),
globalNavigation(true), globalNavigation(true),
firstWP(false), firstWP(false),
previousSPX(8.548056), previousSPX(8.548056),
previousSPY(47.376389), previousSPY(47.376389),
previousSPZ(550), previousSPZ(550),
previousSPYaw(0.0), previousSPYaw(0.0),
nextSPX(8.548056), nextSPX(8.548056),
nextSPY(47.376389), nextSPY(47.376389),
nextSPZ(550), nextSPZ(550),
nextSPYaw(0.0) 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 // Please note: The waypoint planner is running
connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop())); connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop()));
@ -42,9 +46,9 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
void MAVLinkSimulationMAV::mainloop() void MAVLinkSimulationMAV::mainloop()
{ {
// Calculate new simulator values // 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 // 1 Hz execution
if (timer1Hz <= 0) if (timer1Hz <= 0)
@ -61,8 +65,8 @@ void MAVLinkSimulationMAV::mainloop()
{ {
if (!firstWP) if (!firstWP)
{ {
double radPer100ms = 0.0002; double radPer100ms = 0.00006;
double altPer100ms = 0.1; double altPer100ms = 1.0;
double xm = (nextSPX - x); double xm = (nextSPX - x);
double ym = (nextSPY - y); double ym = (nextSPY - y);
double zm = (nextSPZ - z); double zm = (nextSPZ - z);
@ -71,14 +75,35 @@ void MAVLinkSimulationMAV::mainloop()
//float trueyaw = atan2f(xm, ym); //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; yaw = newYaw;
y += sin(yaw)*radPer100ms; }
//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; z += altPer100ms*zsign;
} }
@ -94,6 +119,7 @@ void MAVLinkSimulationMAV::mainloop()
} }
// GLOBAL POSITION
mavlink_message_t msg; mavlink_message_t msg;
mavlink_global_position_int_t pos; mavlink_global_position_int_t pos;
pos.alt = z*1000.0; 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); mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos);
link->sendMAVLinkMessage(&msg); link->sendMAVLinkMessage(&msg);
planner.handleMessage(msg); planner.handleMessage(msg);
// ATTITUDE
mavlink_attitude_t attitude; mavlink_attitude_t attitude;
attitude.roll = 0.0f; attitude.roll = 0.0f;
attitude.pitch = 0.0f; attitude.pitch = 0.0f;
attitude.yaw = yaw; attitude.yaw = yaw;
qDebug() << "YAW" << yaw;
mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude); mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude);
link->sendMAVLinkMessage(&msg); 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; timer10Hz = 5;
} }
@ -136,6 +174,37 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
{ {
case MAVLINK_MSG_ID_ATTITUDE: case MAVLINK_MSG_ID_ATTITUDE:
break; 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: case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET:
{ {
mavlink_local_position_setpoint_set_t sp; mavlink_local_position_setpoint_set_t sp;

4
src/comm/MAVLinkSimulationMAV.h
Unescape View File

@ -49,6 +49,10 @@ protected:
double nextSPY; double nextSPY;
double nextSPZ; double nextSPZ;
double nextSPYaw; double nextSPYaw;
uint8_t sys_mode;
uint8_t sys_state;
uint8_t nav_mode;
bool flying;
}; };

6
src/comm/MAVLinkSimulationWaypointPlanner.cc
Unescape View File

@ -463,7 +463,7 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
//check for timed-out operations //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; uint64_t now = QGC::groundTimeUsecs()/1000;
if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE) 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! // FIXME big hack for simulation!
//float oneDegreeOfLatMeters = 111131.745f; //float oneDegreeOfLatMeters = 111131.745f;
float orbit = 0.0001; float orbit = 0.00008;
// compare current position (given in message) with current waypoint // compare current position (given in message) with current waypoint
//float orbit = wp->param1; //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 //the last waypoint was reached, if auto continue is
//activated restart the waypoint list from the beginning //activated restart the waypoint list from the beginning
current_active_wp_id = 1; current_active_wp_id = 0;
} }
else else
{ {

11
src/uas/UAS.cc
Unescape View File

@ -146,7 +146,16 @@ void UAS::updateState()
void UAS::setSelected() 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) void UAS::receiveMessageNamedValue(const mavlink_message_t& message)

3
src/uas/UAS.h
Unescape View File

@ -89,9 +89,10 @@ public:
double getRoll() const { return roll; } double getRoll() const { return roll; }
double getPitch() const { return pitch; } double getPitch() const { return pitch; }
double getYaw() const { return yaw; } double getYaw() const { return yaw; }
bool getSelected() const;
friend class UASWaypointManager; friend class UASWaypointManager;
protected: //COMMENTS FOR TEST UNIT protected: //COMMENTS FOR TEST UNIT
int uasId; ///< Unique system ID int uasId; ///< Unique system ID
unsigned char type; ///< UAS type (from type enum) unsigned char type; ///< UAS type (from type enum)

4
src/uas/UASInterface.h
Unescape View File

@ -78,6 +78,8 @@ public:
virtual double getPitch() const = 0; virtual double getPitch() const = 0;
virtual double getYaw() const = 0; virtual double getYaw() const = 0;
virtual bool getSelected() const = 0;
/** @brief Get reference to the waypoint manager **/ /** @brief Get reference to the waypoint manager **/
virtual UASWaypointManager* getWaypointManager(void) = 0; virtual UASWaypointManager* getWaypointManager(void) = 0;
@ -419,6 +421,8 @@ signals:
void heartbeatTimeout(unsigned int ms); void heartbeatTimeout(unsigned int ms);
/** @brief Name of system changed */ /** @brief Name of system changed */
void nameChanged(QString newName); void nameChanged(QString newName);
/** @brief System has been selected as focused system */
void systemSelected(bool selected);
protected: protected:

1
src/uas/UASManager.cc
Unescape View File

@ -203,6 +203,7 @@ void UASManager::setActiveUAS(UASInterface* uas)
activeUAS = uas; activeUAS = uas;
activeUASMutex.unlock(); activeUASMutex.unlock();
activeUAS->setSelected();
emit activeUASSet(uas); emit activeUASSet(uas);
emit activeUASSet(uas->getUASID()); emit activeUASSet(uas->getUASID());
emit activeUASSetListIndex(systems.indexOf(uas)); emit activeUASSetListIndex(systems.indexOf(uas));

217
src/ui/MapWidget.cc
Unescape View File

@ -36,7 +36,7 @@ MapWidget::MapWidget(QWidget *parent) :
m_ui(new Ui::MapWidget) m_ui(new Ui::MapWidget)
{ {
m_ui->setupUi(this); m_ui->setupUi(this);
mc = new qmapcontrol::MapControl(QSize(320, 240)); mc = new qmapcontrol::MapControl(this->size());
// VISUAL MAP STYLE // 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; }"); 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() void MapWidget::goTo()
{ {
bool ok; bool ok;
QString text = QInputDialog::getText(this, tr("Please enter coordinates"), QString text = QInputDialog::getText(this, tr("Please enter coordinates"),
tr("Coordinates (Lat,Lon):"), QLineEdit::Normal, tr("Coordinates (Lat,Lon):"), QLineEdit::Normal,
QString("%1,%2").arg(mc->currentCoordinate().x()).arg(mc->currentCoordinate().y()), &ok); QString("%1,%2").arg(mc->currentCoordinate().x()).arg(mc->currentCoordinate().y()), &ok);
if (ok && !text.isEmpty()) if (ok && !text.isEmpty())
{ {
QStringList split = text.split(","); QStringList split = text.split(",");
if (split.length() == 2) if (split.length() == 2)
{ {
bool convert; bool convert;
double latitude = split.first().toDouble(&convert); double latitude = split.first().toDouble(&convert);
ok &= convert; ok &= convert;
double longitude = split.last().toDouble(&convert); double longitude = split.last().toDouble(&convert);
ok &= convert; ok &= convert;
if (ok) if (ok)
{ {
mc->setView(QPointF(latitude, longitude)); mc->setView(QPointF(latitude, longitude));
} }
} }
} }
} }
@ -290,7 +290,7 @@ void MapWidget::mapproviderSelected(QAction* action)
l->setMapAdapter(mapadapter); l->setMapAdapter(mapadapter);
geomLayer->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter);
mc->updateRequestNew(); if (isVisible()) mc->updateRequestNew();
mc->setZoom(zoom); mc->setZoom(zoom);
// yahooActionOverlay->setEnabled(false); // yahooActionOverlay->setEnabled(false);
overlay->setVisible(false); overlay->setVisible(false);
@ -306,7 +306,7 @@ void MapWidget::mapproviderSelected(QAction* action)
l->setMapAdapter(mapadapter); l->setMapAdapter(mapadapter);
geomLayer->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter);
mc->updateRequestNew(); if (isVisible()) mc->updateRequestNew();
mc->setZoom(zoom); mc->setZoom(zoom);
// yahooActionOverlay->setEnabled(false); // yahooActionOverlay->setEnabled(false);
overlay->setVisible(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"); 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); l->setMapAdapter(mapadapter);
mc->updateRequestNew(); if (isVisible()) mc->updateRequestNew();
mc->setZoom(zoom); mc->setZoom(zoom);
// yahooActionOverlay->setEnabled(true); // yahooActionOverlay->setEnabled(true);
} }
@ -333,7 +333,7 @@ void MapWidget::mapproviderSelected(QAction* action)
l->setMapAdapter(mapadapter); l->setMapAdapter(mapadapter);
geomLayer->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter);
mc->updateRequestNew(); if (isVisible()) mc->updateRequestNew();
mc->setZoom(zoom); mc->setZoom(zoom);
// yahooActionOverlay->setEnabled(false); // yahooActionOverlay->setEnabled(false);
overlay->setVisible(false); overlay->setVisible(false);
@ -347,7 +347,7 @@ void MapWidget::mapproviderSelected(QAction* action)
l->setMapAdapter(mapadapter); l->setMapAdapter(mapadapter);
geomLayer->setMapAdapter(mapadapter); geomLayer->setMapAdapter(mapadapter);
mc->updateRequestNew(); if (isVisible()) mc->updateRequestNew();
mc->setZoom(zoom); mc->setZoom(zoom);
// yahooActionOverlay->setEnabled(false); // yahooActionOverlay->setEnabled(false);
overlay->setVisible(false); overlay->setVisible(false);
@ -426,7 +426,7 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina
waypointPath->addPoint(tempPoint); waypointPath->addPoint(tempPoint);
// Refresh the screen // Refresh the screen
mc->updateRequest(tempPoint->boundingBox().toRect()); if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect());
} }
// emit signal mouse was clicked // emit signal mouse was clicked
@ -445,10 +445,11 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
if (uas == this->mav->getUASID()) if (uas == this->mav->getUASID())
{ {
int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getIndexOf(wp); int wpindex = UASManager::instance()->getUASForId(uas)->getWaypointManager()->getIndexOf(wp);
if (wpindex == -1) return;
// Create waypoint name // Create waypoint name
QString str = QString("%1").arg(wpindex); //QString str = QString("%1").arg(wpindex);
// Check if wp exists yet // Check if wp exists yet
if (!(wps.count() > wpindex)) if (!(wpIcons.count() > wpindex))
{ {
QPointF coordinate; QPointF coordinate;
coordinate.setX(wp->getX()); coordinate.setX(wp->getX());
@ -494,7 +495,7 @@ void MapWidget::updateWaypoint(int uas, Waypoint* wp, bool updateView)
//point2Find = dynamic_cast <Point*> (mc->layer("Waypoints")->get_Geometry(wpindex)); //point2Find = dynamic_cast <Point*> (mc->layer("Waypoints")->get_Geometry(wpindex));
//point2Find->setCoordinate(coordinate); //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= "<<str<<" -> x= "<<tempPoint->latitude()<<" y= "<<tempPoint->longitude(); qDebug()<<"Funcion createWaypointGraphAtMap WP= "<<str<<" -> x= "<<tempPoint->latitude()<<" y= "<<tempPoint->longitude();
// Refresh the screen // Refresh the screen
mc->updateRequest(tempPoint->boundingBox().toRect()); if (isVisible()) if (isVisible()) mc->updateRequest(tempPoint->boundingBox().toRect());
} }
//// // emit signal mouse was clicked //// // emit signal mouse was clicked
@ -570,7 +571,7 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
waypointIsDrag = true; waypointIsDrag = true;
// Refresh the screen // Refresh the screen
mc->updateRequest(geom->boundingBox().toRect()); if (isVisible()) mc->updateRequest(geom->boundingBox().toRect());
int temp = 0; int temp = 0;
@ -578,40 +579,35 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate)
bool wpIndexOk; bool wpIndexOk;
int index = geom->name().toInt(&wpIndexOk); int index = geom->name().toInt(&wpIndexOk);
qmapcontrol::Point* point2Find; Waypoint2DIcon* point2Find = dynamic_cast <Waypoint2DIcon*> (geom);
point2Find = wps.at(geom->name().toInt());
if (point2Find) if (wpIndexOk && point2Find && wps.count() > index)
{ {
// Update visual
point2Find->setCoordinate(coordinate); point2Find->setCoordinate(coordinate);
waypointPath->points().at(index)->setCoordinate(coordinate);
if (isVisible()) mc->updateRequest(waypointPath->boundingBox().toRect());
point2Find = dynamic_cast <qmapcontrol::Point*> (geom); // Update waypoint data storage
if (point2Find) if (mav)
{ {
point2Find->setCoordinate(coordinate); QVector<Waypoint*> wps = mav->getWaypointManager()->getWaypointList();
if (wpIndexOk) if (wps.size() > index)
{ {
mc->updateRequest(point2Find->boundingBox().toRect()); wps.at(index)->setX(coordinate.x());
if (mav) wps.at(index)->setY(coordinate.y());
{ mav->getWaypointManager()->notifyOfChange(wps.at(index));
QVector<Waypoint*> 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));
}
}
} }
// 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) void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate)
@ -670,21 +666,20 @@ void MapWidget::updateWaypointList(int uas)
} }
// Delete now unused wps // 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); wps.removeLast();
mc->layer("Waypoints")->removeGeometry(wpIcons.at(i)); mc->layer("Waypoints")->removeGeometry(wpIcons.last());
waypointPath->points().removeAt(i); wpIcons.removeLast();
//Point* linesegment = waypointPath->points().at(mav->getWaypointManager()->getWaypointList().indexOf(wp)); waypointPath->points().removeLast();
} }
} }
// Update view // 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(waypointListChanged(int)), this, SLOT(updateWaypointList(int)));
connect(mav->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); connect(mav->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
connect(this, SIGNAL(waypointCreated(Waypoint*)), mav->getWaypointManager(), SLOT(addWaypoint(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<MAV2DIcon*>(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 // Icon
//QPen* pointpen = new QPen(uasColor); //QPen* pointpen = new QPen(uasColor);
qDebug() << "2D MAP: ADDING" << uas->getUASName() << __FILE__ << __LINE__; 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(uas, 50, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
p = new MAV2DIcon(lat, lon, 50, uas->getSystemType(), uas->getColor(), QString("%1").arg(uas->getUASID()), qmapcontrol::Point::Middle);
uasIcons.insert(uas->getUASID(), p); uasIcons.insert(uas->getUASID(), p);
mc->layer("Waypoints")->addGeometry(p); mc->layer("Waypoints")->addGeometry(p);
// Line // Line
// A QPen also can use transparency // A QPen also can use transparency
// QList<qmapcontrol::Point*> points; // QList<qmapcontrol::Point*> points;
// points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y())); // points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
// QPen* linepen = new QPen(uasColor.darker()); // QPen* linepen = new QPen(uasColor.darker());
// linepen->setWidth(2); // linepen->setWidth(2);
// // Create tracking line string // // Create tracking line string
// qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen); // qmapcontrol::LineString* ls = new qmapcontrol::LineString(points, QString("%1").arg(uas->getUASID()), linepen);
// uasTrails.insert(uas->getUASID(), ls); // uasTrails.insert(uas->getUASID(), ls);
// // Add the LineString to the layer // // Add the LineString to the layer
// mc->layer("Waypoints")->addGeometry(ls); // mc->layer("Waypoints")->addGeometry(ls);
} }
else else
{ {
// p = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID())); // p = dynamic_cast<MAV2DIcon*>(uasIcons.value(uas->getUASID()));
// if (p) // if (p)
// { // {
p = uasIcons.value(uas->getUASID()); p = uasIcons.value(uas->getUASID());
p->setCoordinate(QPointF(lat, lon)); p->setCoordinate(QPointF(lat, lon));
//p->setYaw(uas->getYaw()); //p->setYaw(uas->getYaw());
// } // }
// Extend trail // 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()) if (this->mav && uas->getUASID() == this->mav->getUASID())
{
// Limit the position update rate
quint64 currTime = MG::TIME::getGroundTimeNow();
if (currTime - lastUpdate > 120)
{ {
lastUpdate = currTime; // Limit the position update rate
// Sets the view to the interesting area quint64 currTime = MG::TIME::getGroundTimeNow();
if (followgps->isChecked()) if (currTime - lastUpdate > 120)
{ {
updatePosition(0, lat, lon); lastUpdate = currTime;
} // Sets the view to the interesting area
else if (followgps->isChecked())
{ {
// Refresh the screen updatePosition(0, lat, lon);
//mc->updateRequestNew(); }
else
{
// Refresh the screen
//if (isVisible()) mc->updateRequestNew();
}
} }
} }
} }
}
/** /**
* Center the view on this position * Center the view on this position
@ -953,7 +962,7 @@ void MapWidget::clearWaypoints(int uas)
//waypointPath = new //waypointPath = new
//mc->layer("Waypoints")->addGeometry(waypointPath); //mc->layer("Waypoints")->addGeometry(waypointPath);
//wpIndex.clear(); //wpIndex.clear();
mc->updateRequest(box);//(waypointPath->boundingBox().toRect()); if (isVisible()) mc->updateRequest(box);//(waypointPath->boundingBox().toRect());
if(createPath->isChecked()) if(createPath->isChecked())
{ {
@ -975,7 +984,7 @@ void MapWidget::clearPath(int uas)
mc->layer("Tracking")->addGeometry(lsNew); mc->layer("Tracking")->addGeometry(lsNew);
} }
// FIXME update this with update request only for bounding box of trails // 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) 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(camBorder);
// mc->layer("Camera")->addGeometry(camLine); // mc->layer("Camera")->addGeometry(camLine);
mc->updateRequestNew(); if (isVisible()) mc->updateRequestNew();
} }
else else
{ {
//clear camera borders //clear camera borders
mc->layer("Camera")->clearGeometries(); mc->layer("Camera")->clearGeometries();
mc->updateRequestNew(); if (isVisible()) mc->updateRequestNew();
} }

2
src/ui/MapWidget.h
Unescape View File

@ -68,6 +68,8 @@ public:
public slots: public slots:
void addUAS(UASInterface* uas); void addUAS(UASInterface* uas);
void activeUASSet(UASInterface* uas); void activeUASSet(UASInterface* uas);
/** @brief Update the selected system */
void updateSelectedSystem(int uas);
/** @brief Update the attitude */ /** @brief Update the attitude */
void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec); void updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec);
void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec); void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec);

10
src/ui/WaypointView.ui
Unescape View File

@ -235,7 +235,7 @@ QProgressBar::chunk#thrustBar {
</sizepolicy> </sizepolicy>
</property> </property>
<property name="focusPolicy"> <property name="focusPolicy">
<enum>Qt::ClickFocus</enum> <enum>Qt::WheelFocus</enum>
</property> </property>
<property name="toolTip"> <property name="toolTip">
<string>Position X coordinate</string> <string>Position X coordinate</string>
@ -275,7 +275,7 @@ QProgressBar::chunk#thrustBar {
</sizepolicy> </sizepolicy>
</property> </property>
<property name="focusPolicy"> <property name="focusPolicy">
<enum>Qt::StrongFocus</enum> <enum>Qt::WheelFocus</enum>
</property> </property>
<property name="toolTip"> <property name="toolTip">
<string>Position Y coordinate</string> <string>Position Y coordinate</string>
@ -309,7 +309,7 @@ QProgressBar::chunk#thrustBar {
</sizepolicy> </sizepolicy>
</property> </property>
<property name="focusPolicy"> <property name="focusPolicy">
<enum>Qt::StrongFocus</enum> <enum>Qt::WheelFocus</enum>
</property> </property>
<property name="toolTip"> <property name="toolTip">
<string>Position Z coordinate (negative)</string> <string>Position Z coordinate (negative)</string>
@ -340,7 +340,7 @@ QProgressBar::chunk#thrustBar {
</sizepolicy> </sizepolicy>
</property> </property>
<property name="focusPolicy"> <property name="focusPolicy">
<enum>Qt::StrongFocus</enum> <enum>Qt::WheelFocus</enum>
</property> </property>
<property name="prefix"> <property name="prefix">
<string>lat </string> <string>lat </string>
@ -371,7 +371,7 @@ QProgressBar::chunk#thrustBar {
</sizepolicy> </sizepolicy>
</property> </property>
<property name="focusPolicy"> <property name="focusPolicy">
<enum>Qt::StrongFocus</enum> <enum>Qt::WheelFocus</enum>
</property> </property>
<property name="prefix"> <property name="prefix">
<string>lon </string> <string>lon </string>

85
src/ui/map/MAV2DIcon.cc
Unescape View File

@ -3,13 +3,16 @@
#include <qmath.h> #include <qmath.h>
MAV2DIcon::MAV2DIcon(qreal x, qreal y, int radius, int type, const QColor& color, QString name, Alignment alignment, QPen* pen) MAV2DIcon::MAV2DIcon(UASInterface* uas, int radius, int type, const QColor& color, QString name, Alignment alignment, QPen* pen)
: Point(x, y, name, alignment), : Point(uas->getLatitude(), uas->getLongitude(), name, alignment),
yaw(0.0f), yaw(0.0f),
radius(radius), radius(radius),
type(type), type(type),
iconColor(color) iconColor(color),
selected(uas->getSelected()),
uasid(uas->getUASID())
{ {
//connect
size = QSize(radius, radius); size = QSize(radius, radius);
drawIcon(pen); drawIcon(pen);
} }
@ -18,7 +21,9 @@ MAV2DIcon::MAV2DIcon(qreal x, qreal y, QString name, Alignment alignment, QPen*
: Point(x, y, name, alignment), : Point(x, y, name, alignment),
radius(20), radius(20),
type(0), type(0),
iconColor(Qt::yellow) iconColor(Qt::yellow),
selected(false),
uasid(0)
{ {
size = QSize(radius, radius); size = QSize(radius, radius);
drawIcon(pen); drawIcon(pen);
@ -35,6 +40,12 @@ void MAV2DIcon::setPen(QPen* pen)
drawIcon(pen); drawIcon(pen);
} }
void MAV2DIcon::setSelectedUAS(bool selected)
{
this->selected = selected;
drawIcon(mypen);
}
/** /**
* Yaw changes of less than ±15 degrees are ignored. * Yaw changes of less than ±15 degrees are ignored.
* *
@ -43,22 +54,54 @@ void MAV2DIcon::setPen(QPen* pen)
void MAV2DIcon::setYaw(float yaw) void MAV2DIcon::setYaw(float yaw)
{ {
//qDebug() << "MAV2Icon" << yaw; //qDebug() << "MAV2Icon" << yaw;
//float diff = fabs(yaw - this->yaw); float diff = fabs(yaw - this->yaw);
// while (diff > M_PI) while (diff > M_PI)
// { {
// diff -= M_PI; diff -= M_PI;
// } }
// if (diff > 0.25) if (diff > 0.1)
// { {
this->yaw = yaw; this->yaw = yaw;
drawIcon(mypen); drawIcon(mypen);
// } }
} }
void MAV2DIcon::drawIcon(QPen* pen) void MAV2DIcon::drawIcon(QPen* pen)
{ {
Q_UNUSED(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) switch (type)
{ {
case MAV_ICON_GENERIC: case MAV_ICON_GENERIC:
@ -67,20 +110,9 @@ void MAV2DIcon::drawIcon(QPen* pen)
case MAV_ICON_ROTARY_WING: case MAV_ICON_ROTARY_WING:
default: 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 // DRAW AIRPLANE
qDebug() << "ICON SIZE:" << radius; //qDebug() << "ICON SIZE:" << radius;
float iconSize = radius*0.9f; float iconSize = radius*0.9f;
QPolygonF poly(24); QPolygonF poly(24);
@ -134,6 +166,9 @@ void MAV2DIcon::drawIcon(QPen* pen)
// painter.setPen(pen2); // painter.setPen(pen2);
// } // }
painter.setBrush(QBrush(iconColor)); painter.setBrush(QBrush(iconColor));
QPen iconPen(Qt::black);
iconPen.setWidthF(1.0f);
painter.setPen(iconPen);
painter.drawPolygon(poly); painter.drawPolygon(poly);
} }

19
src/ui/map/MAV2DIcon.h
Unescape View File

@ -4,6 +4,8 @@
#include <QGraphicsItem> #include <QGraphicsItem>
#include "qmapcontrol.h" #include "qmapcontrol.h"
#include "UASInterface.h"
class MAV2DIcon : public qmapcontrol::Point class MAV2DIcon : public qmapcontrol::Point
{ {
public: public:
@ -14,27 +16,29 @@ public:
MAV_ICON_QUADROTOR, MAV_ICON_QUADROTOR,
MAV_ICON_ROTARY_WING MAV_ICON_ROTARY_WING
} MAV_ICON_TYPE; } MAV_ICON_TYPE;
//!
/*! /*!
* *
* @param x longitude * @param x longitude
* @param y latitude * @param y latitude
* @param radius the radius of the circle
* @param name name of the circle point * @param name name of the circle point
* @param alignment alignment (Middle or TopLeft) * @param alignment alignment (Middle or TopLeft)
* @param pen QPen for drawing * @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 x longitude
* @param y latitude * @param y latitude
* @param radius the radius of the circle
* @param name name of the circle point * @param name name of the circle point
* @param alignment alignment (Middle or TopLeft) * @param alignment alignment (Middle or TopLeft)
* @param pen QPen for drawing * @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(); virtual ~MAV2DIcon();
//! sets the QPen which is used for drawing the circle //! sets the QPen which is used for drawing the circle
@ -45,8 +49,13 @@ public:
*/ */
virtual void setPen(QPen* pen); virtual void setPen(QPen* pen);
/** @brief Mark this system as selected */
void setSelectedUAS(bool selected);
void setYaw(float yaw); void setYaw(float yaw);
/** @brief Get system id */
int getUASId() const { return uasid; }
void drawIcon(QPen* pen); void drawIcon(QPen* pen);
protected: protected:
@ -54,6 +63,8 @@ protected:
int radius; ///< Radius / width of the icon int radius; ///< Radius / width of the icon
int type; ///< Type of aircraft: 0: generic, 1: airplane, 2: quadrotor, 3-n: rotary wing int type; ///< Type of aircraft: 0: generic, 1: airplane, 2: quadrotor, 3-n: rotary wing
QColor iconColor; ///< Color to be used for the icon 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
}; };

4
src/ui/map3D/QGCGoogleEarthView.ui
Unescape View File

@ -83,10 +83,10 @@
<string>Distance of the chase camera to the MAV</string> <string>Distance of the chase camera to the MAV</string>
</property> </property>
<property name="minimum"> <property name="minimum">
<number>30</number> <number>100</number>
</property> </property>
<property name="maximum"> <property name="maximum">
<number>10000</number> <number>20000</number>
</property> </property>
<property name="value"> <property name="value">
<number>3000</number> <number>3000</number>

Write Preview
Loading…
Cancel
Save