Skip to content

Q
qgroundcontrol
Commit 4f91c4af authored by Lorenz Meier's avatar Lorenz Meier
Browse files
Options

Checkpoint: Cleaned up a lot withint the WP management infrastructure

parent a13508b3
Expand all Show whitespace changes
Inline Side-by-side
Showing with 313 additions and 194 deletions
src/uas/UASWaypointManager.cc
View file @ 4f91c4af
......@@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project
#include "UASWaypointManager.h"
#include "UAS.h"
#include "mavlink_types.h"
#include "UASManager.h"
#define PROTOCOL_TIMEOUT_MS 2000 ///< maximum time to wait for pending messages until timeout
#define PROTOCOL_DELAY_MS 20 ///< minimum delay between sent messages
......@@ -136,7 +137,7 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
//Clear the old edit-list before receiving the new one
if (read_to_edit == true){
while(waypointsEditable.size()>0) {
while(waypointsEditable.count()>0) {
Waypoint *t = waypointsEditable[0];
waypointsEditable.remove(0);
delete t;
......@@ -331,10 +332,10 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq)
int UASWaypointManager::setCurrentEditable(quint16 seq)
{
if (seq < waypointsEditable.size()) {
if (seq < waypointsEditable.count()) {
if(current_state == WP_IDLE) {
//update local main storage
for(int i = 0; i < waypointsEditable.size(); i++) {
for(int i = 0; i < waypointsEditable.count(); i++) {
if (waypointsEditable[i]->getId() == seq) {
waypointsEditable[i]->setCurrent(true);
} else {
......@@ -370,13 +371,13 @@ void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActi
{
if (wp)
{
wp->setId(waypointsEditable.size());
if (enforceFirstActive && waypointsEditable.size() == 0)
wp->setId(waypointsEditable.count());
if (enforceFirstActive && waypointsEditable.count() == 0)
{
wp->setCurrent(true);
currentWaypointEditable = wp;
}
waypointsEditable.insert(waypointsEditable.size(), wp);
waypointsEditable.insert(waypointsEditable.count(), wp);
connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*)));
emit waypointEditableListChanged();
......@@ -390,13 +391,15 @@ void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActi
Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive)
{
Waypoint* wp = new Waypoint();
wp->setId(waypointsEditable.size());
if (enforceFirstActive && waypointsEditable.size() == 0)
wp->setId(waypointsEditable.count());
wp->setFrame((MAV_FRAME)getFrameRecommendation());
wp->setAltitude(getAltitudeRecommendation());
if (enforceFirstActive && waypointsEditable.count() == 0)
{
wp->setCurrent(true);
currentWaypointEditable = wp;
}
waypointsEditable.insert(waypointsEditable.size(), wp);
waypointsEditable.append(wp);
connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*)));
emit waypointEditableListChanged();
......@@ -406,17 +409,17 @@ Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive)
int UASWaypointManager::removeWaypoint(quint16 seq)
{
if (seq < waypointsEditable.size())
if (seq < waypointsEditable.count())
{
Waypoint *t = waypointsEditable[seq];
if (t->getCurrent() == true) //trying to remove the current waypoint
{
if (seq+1 < waypointsEditable.size()) // setting the next waypoint as current
if (seq+1 < waypointsEditable.count()) // setting the next waypoint as current
{
waypointsEditable[seq+1]->setCurrent(true);
}
else if (seq-1 >= 0) //if deleting the last on the list, then setting the previous waypoint as current
else if (seq-1 >= 0) // if deleting the last on the list, then setting the previous waypoint as current
{
waypointsEditable[seq-1]->setCurrent(true);
}
......@@ -426,7 +429,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
delete t;
t = NULL;
for(int i = seq; i < waypointsEditable.size(); i++)
for(int i = seq; i < waypointsEditable.count(); i++)
{
waypointsEditable[i]->setId(i);
}
......@@ -440,7 +443,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
{
if (cur_seq != new_seq && cur_seq < waypointsEditable.size() && new_seq < waypointsEditable.size())
if (cur_seq != new_seq && cur_seq < waypointsEditable.count() && new_seq < waypointsEditable.count())
{
Waypoint *t = waypointsEditable[cur_seq];
if (cur_seq < new_seq) {
......@@ -477,7 +480,7 @@ void UASWaypointManager::saveWaypoints(const QString &saveFile)
//write the waypoint list version to the first line for compatibility check
out << "QGC WPL 120\r\n";
for (int i = 0; i < waypointsEditable.size(); i++)
for (int i = 0; i < waypointsEditable.count(); i++)
{
waypointsEditable[i]->setId(i);
waypointsEditable[i]->save(out);
......@@ -491,7 +494,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
return;
while(waypointsEditable.size()>0) {
while(waypointsEditable.count()>0) {
Waypoint *t = waypointsEditable[0];
waypointsEditable.remove(0);
delete t;
......@@ -512,8 +515,8 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
Waypoint *t = new Waypoint();
if(t->load(in))
{
t->setId(waypointsEditable.size());
waypointsEditable.insert(waypointsEditable.size(), t);
t->setId(waypointsEditable.count());
waypointsEditable.insert(waypointsEditable.count(), t);
}
else
{
......@@ -532,7 +535,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
void UASWaypointManager::clearWaypointList()
{
if(current_state == WP_IDLE)
if (current_state == WP_IDLE)
{
protocol_timer.start(PROTOCOL_TIMEOUT_MS);
current_retries = PROTOCOL_MAX_RETRIES;
......@@ -779,7 +782,7 @@ void UASWaypointManager::readWaypoints(bool readToEdit)
/* THIS PART WAS MOVED TO handleWaypointCount. THE EDIT-LIST SHOULD NOT BE CLEARED UNLESS THERE IS A RESPONSE FROM UAV.
//Clear the old edit-list before receiving the new one
if (read_to_edit == true){
while(waypointsEditable.size()>0) {
while(waypointsEditable.count()>0) {
Waypoint *t = waypointsEditable[0];
waypointsEditable.remove(0);
delete t;
......@@ -1023,3 +1026,39 @@ void UASWaypointManager::sendWaypointAck(quint8 type)
uas->sendMessage(message);
QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
}
UAS* UASWaypointManager::getUAS() {
return this->uas; ///< Returns the owning UAS
}
float UASWaypointManager::getAltitudeRecommendation()
{
if (waypointsEditable.count() > 0) {
return waypointsEditable.last()->getAltitude();
} else {
return UASManager::instance()->getHomeAltitude() + getHomeAltitudeOffsetDefault();
}
}
int UASWaypointManager::getFrameRecommendation()
{
if (waypointsEditable.count() > 0) {
return static_cast<int>(waypointsEditable.last()->getFrame());
} else {
return MAV_FRAME_GLOBAL;
}
}
float UASWaypointManager::getAcceptanceRadiusRecommendation()
{
if (waypointsEditable.count() > 0) {
return waypointsEditable.last()->getAcceptanceRadius();
} else {
return 10.0f;
}
}
float UASWaypointManager::getHomeAltitudeOffsetDefault()
{
return defaultAltitudeHomeOffset;
}
src/uas/UASWaypointManager.h
View file @ 4f91c4af
......@@ -112,9 +112,11 @@ public:
int getLocalFrameCount(); ///< Get the count of local waypoints in the list
/*@}*/
UAS* getUAS() {
return this->uas; ///< Returns the owning UAS
}
UAS* getUAS();
float getAltitudeRecommendation();
int getFrameRecommendation();
float getAcceptanceRadiusRecommendation();
float getHomeAltitudeOffsetDefault();
private:
/** @name Message send functions */
......@@ -176,6 +178,9 @@ private:
QTimer protocol_timer; ///< Timer to catch timeouts
bool standalone; ///< If standalone is set, do not write to UAS
quint16 uasid;
// XXX export to settings
static const float defaultAltitudeHomeOffset = 30.0f; ///< Altitude offset in meters from home for new waypoints
};
#endif // UASWAYPOINTMANAGER_H
src/ui/WaypointList.cc
View file @ 4f91c4af
This diff is collapsed.
src/ui/WaypointList.h
View file @ 4f91c4af
......@@ -78,8 +78,10 @@ public slots:
void refresh();
/** @brief Add a waypoint to "edit"-tab */
void addEditable();
/** @brief Add a waypoint to "edit"-tab on current MAV position or on generic position (home) */
void addEditable(bool onCurrentPosition);
/** @brief Add a waypoint at the current MAV position */
int addCurrentPositionWaypoint();
void addCurrentPositionWaypoint();
/** @brief Add a waypoint by mouse click over the map */
//Update events
......
src/ui/map/QGCMapWidget.cc
View file @ 4f91c4af
......@@ -17,10 +17,10 @@ QGCMapWidget::QGCMapWidget(QWidget *parent) :
followUAVID(0),
mapInitialized(false),
homeAltitude(0),
uas(0)
uas(NULL)
{
//currWPManager = new UASWaypointManager(NULL); //Create a waypoint manager that is null.
currWPManager = UASManager::instance()->getActiveUASWaypointManager();
waypointLines.insert(0, new QGraphicsItemGroup(map));
connect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int)));
connect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*)));
......@@ -207,6 +207,18 @@ void QGCMapWidget::loadSettings(bool changePosition)
trailInterval = settings.value("TRAIL_INTERVAL", trailInterval).toFloat();
settings.endGroup();
// SET CORRECT MENU CHECKBOXES
// Set the correct trail interval
if (trailType == mapcontrol::UAVTrailType::ByDistance)
{
// XXX
#warning Settings loading for trail type not implemented
}
else if (trailType == mapcontrol::UAVTrailType::ByTimeElapsed)
{
// XXX
}
// SET TRAIL TYPE
foreach (mapcontrol::UAVItem* uav, GetUAVS())
{
......@@ -245,10 +257,6 @@ void QGCMapWidget::storeSettings()
void QGCMapWidget::mouseDoubleClickEvent(QMouseEvent* event)
{
// FIXME HACK!
//if (currEditMode == EDIT_MODE_WAYPOINTS)
{
// If a waypoint manager is available
if (currWPManager)
{
......@@ -259,11 +267,12 @@ void QGCMapWidget::mouseDoubleClickEvent(QMouseEvent* event)
// wp->setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT);
wp->setLatitude(pos.Lat());
wp->setLongitude(pos.Lng());
wp->setAltitude(0);
wp->setFrame((MAV_FRAME)currWPManager->getFrameRecommendation());
wp->setAltitude(currWPManager->getAltitudeRecommendation());
// wp->blockSignals(false);
// currWPManager->notifyOfChangeEditable(wp);
}
}
OPMapWidget::mouseDoubleClickEvent(event);
}
......@@ -276,6 +285,14 @@ void QGCMapWidget::addUAS(UASInterface* uas)
{
connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int)));
if (!waypointLines.value(uas->getUASID(), NULL)) {
waypointLines.insert(uas->getUASID(), new QGraphicsItemGroup(map));
} else {
foreach (QGraphicsItem* item, waypointLines.value(uas->getUASID())->childItems())
{
delete item;
}
}
}
void QGCMapWidget::activeUASSet(UASInterface* uas)
......@@ -294,15 +311,10 @@ void QGCMapWidget::activeUASSet(UASInterface* uas)
disconnect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*)));
}
if (uas)
{
//UASWaypointManager *old = currWPManager;
currWPManager = uas->getWaypointManager();
updateSelectedSystem(uas->getUASID());
followUAVID = uas->getUASID();
updateWaypointList(uas->getUASID());
// Connect the waypoint manager / data storage to the UI
......@@ -310,29 +322,6 @@ void QGCMapWidget::activeUASSet(UASInterface* uas)
connect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*)));
connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*)));
/*if (offlineMode)
{
if (QMessageBox::question(0,"Question","Do you want to append the offline waypoints to the ones currently on the UAV?",QMessageBox::Yes,QMessageBox::No) == QMessageBox::Yes)
{
//Need to transfer all waypoints from the offline mode WPManager to the online mode.
for (int i=0;i<old->getWaypointEditableList().size();i++)
{
Waypoint *wp = currWPManager->createWaypoint();
wp->setLatitude(old->getWaypointEditableList()[i]->getLatitude());
wp->setLongitude(old->getWaypointEditableList()[i]->getLongitude());
wp->setAltitude(old->getWaypointEditableList()[i]->getAltitude());
}
}
offlineMode = false;
old->deleteLater();
}*/
// Delete all waypoints and add waypoint from new system
}
}
/**
......@@ -360,9 +349,17 @@ void QGCMapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lo
newUAV->setParentItem(map);
UAVS.insert(uas->getUASID(), newUAV);
uav = GetUAV(uas->getUASID());
uav->SetTrailTime(1);
uav->SetTrailDistance(5);
uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed);
// Set the correct trail type
uav->SetTrailType(trailType);
// Set the correct trail interval
if (trailType == mapcontrol::UAVTrailType::ByDistance)
{
uav->SetTrailDistance(trailInterval);
}
else if (trailType == mapcontrol::UAVTrailType::ByTimeElapsed)
{
uav->SetTrailTime(trailInterval);
}
}
// Set new lat/lon position of UAV icon
......@@ -551,6 +548,11 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
{
// Block circle updates
Waypoint* wp = iconsToWaypoints.value(waypoint, NULL);
// Delete UI element if wp doesn't exist
if (!wp)
WPDelete(waypoint);
// Protect from vicious double update cycle
if (firingWaypointChange == wp) return;
// Not in cycle, block now from entering it
......@@ -565,8 +567,8 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
wp->setLatitude(pos.Lat());
wp->setLongitude(pos.Lng());
// XXX Magic values
wp->setAltitude(homeAltitude + 50.0f);
wp->setAcceptanceRadius(10.0f);
// wp->setAltitude(homeAltitude + 50.0f);
// wp->setAcceptanceRadius(10.0f);
wp->blockSignals(false);
......@@ -589,13 +591,15 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint)
*/
void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
{
qDebug() << "UPDATING WP FUNCTION CALLED";
qDebug() << __FILE__ << __LINE__ << "UPDATING WP FUNCTION CALLED";
// Source of the event was in this widget, do nothing
if (firingWaypointChange == wp) return;
if (firingWaypointChange == wp) {
return;
}
// Currently only accept waypoint updates from the UAS in focus
// this has to be changed to accept read-only updates from other systems as well.
UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
if (!uasInstance || uasInstance->getWaypointManager() == currWPManager || uas == -1)
if (currWPManager)
{
// Only accept waypoints in global coordinate frame
if (((wp->getFrame() == MAV_FRAME_GLOBAL) || (wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)) && wp->isNavigationType())
......@@ -607,7 +611,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
// as we're only handling global waypoints
int wpindex = currWPManager->getGlobalFrameAndNavTypeIndexOf(wp);
// If not found, return (this should never happen, but helps safety)
if (wpindex == -1) return;
if (wpindex < 0) return;
// Mark this wp as currently edited
firingWaypointChange = wp;
......@@ -686,7 +690,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
// waypoint list. This implies that the coordinate frame of this
// waypoint was changed and the list containing only global
// waypoints was shortened. Thus update the whole list
if (waypointsToIcons.size() > currWPManager->getGlobalFrameAndNavTypeCount())
if (waypointsToIcons.count() > currWPManager->getGlobalFrameAndNavTypeCount())
{
updateWaypointList(uas);
}
......@@ -705,13 +709,23 @@ void QGCMapWidget::updateWaypointList(int uas)
// Currently only accept waypoint updates from the UAS in focus
// this has to be changed to accept read-only updates from other systems as well.
UASInterface* uasInstance = UASManager::instance()->getUASForId(uas);
if ((uasInstance && (uasInstance->getWaypointManager() == currWPManager)) || uas == -1)
if (currWPManager)
{
// ORDER MATTERS HERE!
// TWO LOOPS ARE NEEDED - INFINITY LOOP ELSE
qDebug() << "DELETING NOW OLD WPS";
// Delete connecting waypoint lines
QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
if (group)
{
foreach (QGraphicsItem* item, group->childItems())
{
delete item;
}
}
// Delete first all old waypoints
// this is suboptimal (quadratic, but wps should stay in the sub-100 range anyway)
QVector<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
......@@ -742,16 +756,6 @@ void QGCMapWidget::updateWaypointList(int uas)
if (!waypointsToIcons.contains(wp)) updateWaypoint(uas, wp);
}
// Delete connecting waypoint lines
QGraphicsItemGroup* group = waypointLines.value(uas, NULL);
if (group)
{
foreach (QGraphicsItem* item, group->childItems())
{
delete item;
}
}
// Add line element if this is NOT the first waypoint
mapcontrol::WayPointItem* prevIcon = NULL;
foreach (Waypoint* wp, wps)
......
Markdown is supported
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