Skip to content
GitLab
Commit 31f97dd6 authored by pixhawk's avatar pixhawk
Browse files

Fully working waypoint interface, thoroughly tested. Active waypoint setting... 

Fully working waypoint interface, thoroughly tested. Active waypoint setting has to be debugged in the simulator, but works fine on real hardware
parent 77e27a2a
Hide whitespace changes
Inline Side-by-side
images/earth.html
View file @ 31f97dd6
......@@ -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);
}
......
src/comm/MAVLinkSimulationMAV.cc
View file @ 31f97dd6
......@@ -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;
......
src/comm/MAVLinkSimulationMAV.h
View file @ 31f97dd6
......@@ -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;
};
......
src/comm/MAVLinkSimulationWaypointPlanner.cc
View file @ 31f97dd6
......@@ -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
{
......
src/uas/UAS.cc
View file @ 31f97dd6
......@@ -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)
......
src/uas/UAS.h
View file @ 31f97dd6
......@@ -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)
......
src/uas/UASInterface.h
View file @ 31f97dd6
......@@ -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:
......
src/uas/UASManager.cc
View file @ 31f97dd6
......@@ -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));
......
src/ui/MapWidget.cc
View file @ 31f97dd6
......@@ -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 <Point*> (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= "<<str<<" -> x= "<<tempPoint->latitude()<<" y= "<<tempPoint->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 <Waypoint2DIcon*> (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 <qmapcontrol::Point*> (geom);
if (point2Find)
// Update waypoint data storage
if (mav)
{
point2Find->setCoordinate(coordinate);
QVector<Waypoint*> wps = mav->getWaypointManager()->getWaypointList();
if (wpIndexOk)
if (wps.size() > index)
{
mc->updateRequest(point2Find->boundingBox().toRect());
if (mav)
{
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));
}
}
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<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
//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<qmapcontrol::Point*> points;
// points.append(new qmapcontrol::Point(coordinate.x(), coordinate.y()));
// QPen* linepen = new QPen(uasColor.darker());
// linepen->setWidth(2);
// QList<qmapcontrol::Point*> 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<MAV2DIcon*>(uasIcons.value(uas->getUASID()));
// if (p)
// {
p = uasIcons.value(uas->getUASID());
p->setCoordinate(QPointF(lat, lon));
//p->setYaw(uas->getYaw());
// }
// p = dynamic_cast<MAV2DIcon*>(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();
}
......
src/ui/MapWidget.h
View file @ 31f97dd6
......@@ -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);
......
src/ui/WaypointView.ui
View file @ 31f97dd6
......@@ -235,7 +235,7 @@ QProgressBar::chunk#thrustBar {
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
<enum>Qt::WheelFocus</enum>
</property>
<property name="toolTip">
<string>Position X coordinate</string>
......@@ -275,7 +275,7 @@ QProgressBar::chunk#thrustBar {
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
<enum>Qt::WheelFocus</enum>
</property>
<property name="toolTip">
<string>Position Y coordinate</string>
......@@ -309,7 +309,7 @@ QProgressBar::chunk#thrustBar {
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
<enum>Qt::WheelFocus</enum>
</property>
<property name="toolTip">
<string>Position Z coordinate (negative)</string>
......@@ -340,7 +340,7 @@ QProgressBar::chunk#thrustBar {
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
<enum>Qt::WheelFocus</enum>
</property>
<property name="prefix">
<string>lat </string>
......@@ -371,7 +371,7 @@ QProgressBar::chunk#thrustBar {
</sizepolicy>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
<enum>Qt::WheelFocus</enum>
</property>
<property name="prefix">
<string>lon </string>
......
src/ui/map/MAV2DIcon.cc
View file @ 31f97dd6
......@@ -3,13 +3,16 @@
#include <qmath.h>
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);
}
......
src/ui/map/MAV2DIcon.h
View file @ 31f97dd6
......@@ -4,6 +4,8 @@
#include <QGraphicsItem>
#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
};
......
src/ui/map3D/QGCGoogleEarthView.ui
View file @ 31f97dd6
......@@ -83,10 +83,10 @@
<string>Distance of the chase camera to the MAV</string>
</property>
<property name="minimum">
<number>30</number>
<number>100</number>
</property>
<property name="maximum">
<number>10000</number>
<number>20000</number>
</property>
<property name="value">
<number>3000</number>
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment