Skip to content
GitLab
Commit 259403be authored by Lorenz Meier's avatar Lorenz Meier
Browse files

Checkpoint WPs issue-free in operation

parent 4f91c4af
Hide whitespace changes
Inline Side-by-side
src/uas/UASWaypointManager.cc
View file @ 259403be
......@@ -139,7 +139,7 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
if (read_to_edit == true){
while(waypointsEditable.count()>0) {
Waypoint *t = waypointsEditable[0];
waypointsEditable.remove(0);
waypointsEditable.removeAt(0);
delete t;
}
emit waypointEditableListChanged();
......@@ -425,7 +425,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq)
}
}
waypointsEditable.remove(seq);
waypointsEditable.removeAt(seq);
delete t;
t = NULL;
......@@ -496,7 +496,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile)
while(waypointsEditable.count()>0) {
Waypoint *t = waypointsEditable[0];
waypointsEditable.remove(0);
waypointsEditable.removeAt(0);
delete t;
}
......@@ -549,12 +549,12 @@ void UASWaypointManager::clearWaypointList()
}
}
const QVector<Waypoint *> UASWaypointManager::getGlobalFrameWaypointList()
const QList<Waypoint *> UASWaypointManager::getGlobalFrameWaypointList()
{
// TODO Keep this global frame list up to date
// with complete waypoint list
// instead of filtering on each request
QVector<Waypoint*> wps;
QList<Waypoint*> wps;
foreach (Waypoint* wp, waypointsEditable)
{
if (wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)
......@@ -565,12 +565,12 @@ const QVector<Waypoint *> UASWaypointManager::getGlobalFrameWaypointList()
return wps;
}
const QVector<Waypoint *> UASWaypointManager::getGlobalFrameAndNavTypeWaypointList()
const QList<Waypoint *> UASWaypointManager::getGlobalFrameAndNavTypeWaypointList()
{
// TODO Keep this global frame list up to date
// with complete waypoint list
// instead of filtering on each request
QVector<Waypoint*> wps;
QList<Waypoint*> wps;
foreach (Waypoint* wp, waypointsEditable)
{
if ((wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && wp->isNavigationType())
......@@ -581,12 +581,12 @@ const QVector<Waypoint *> UASWaypointManager::getGlobalFrameAndNavTypeWaypointLi
return wps;
}
const QVector<Waypoint *> UASWaypointManager::getNavTypeWaypointList()
const QList<Waypoint *> UASWaypointManager::getNavTypeWaypointList()
{
// TODO Keep this global frame list up to date
// with complete waypoint list
// instead of filtering on each request
QVector<Waypoint*> wps;
QList<Waypoint*> wps;
foreach (Waypoint* wp, waypointsEditable)
{
if (wp->isNavigationType())
......@@ -775,7 +775,7 @@ void UASWaypointManager::readWaypoints(bool readToEdit)
//Clear the old view-list before receiving the new one
while(waypointsViewOnly.size()>0) {
Waypoint *t = waypointsViewOnly[0];
waypointsViewOnly.remove(0);
waypointsViewOnly.removeAt(0);
delete t;
}
emit waypointViewOnlyListChanged();
......
src/uas/UASWaypointManager.h
View file @ 259403be
......@@ -33,7 +33,7 @@ This file is part of the QGROUNDCONTROL project
#define UASWAYPOINTMANAGER_H
#include <QObject>
#include <QVector>
#include <QList>
#include <QTimer>
#include "Waypoint.h"
#include "QGCMAVLink.h"
......@@ -44,7 +44,7 @@ class UASInterface;
* @brief Implementation of the MAVLINK waypoint protocol
*
* This class handles the communication with a waypoint manager on the MAV.
* All waypoints are stored in the QVector waypoints, modifications can be done with the WaypointList widget.
* All waypoints are stored in the QList waypoints, modifications can be done with the WaypointList widget.
* Notice that currently the access to the internal waypoint storage is not guarded nor thread-safe. This works as long as no other widget alters the data.
*
* See http://qgroundcontrol.org/waypoint_protocol for more information about the protocol and the states.
......@@ -91,15 +91,15 @@ public:
/** @name Waypoint list operations */
/*@{*/
const QVector<Waypoint *> &getWaypointEditableList(void) {
const QList<Waypoint *> &getWaypointEditableList(void) {
return waypointsEditable; ///< Returns a const reference to the waypoint list.
}
const QVector<Waypoint *> &getWaypointViewOnlyList(void) {
const QList<Waypoint *> &getWaypointViewOnlyList(void) {
return waypointsViewOnly; ///< Returns a const reference to the waypoint list.
}
const QVector<Waypoint *> getGlobalFrameWaypointList(); ///< Returns a global waypoint list
const QVector<Waypoint *> getGlobalFrameAndNavTypeWaypointList(); ///< Returns a global waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out.
const QVector<Waypoint *> getNavTypeWaypointList(); ///< Returns a waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out.
const QList<Waypoint *> getGlobalFrameWaypointList(); ///< Returns a global waypoint list
const QList<Waypoint *> getGlobalFrameAndNavTypeWaypointList(); ///< Returns a global waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out.
const QList<Waypoint *> getNavTypeWaypointList(); ///< Returns a waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out.
int getIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list
int getGlobalFrameIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list, counting only global waypoints
int getGlobalFrameAndNavTypeIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list, counting only global AND navigation mode waypoints
......@@ -171,10 +171,10 @@ private:
quint8 current_partner_compid; ///< The current protocol communication target component
bool read_to_edit; ///< If true, after readWaypoints() incoming waypoints will be copied both to "edit"-tab and "view"-tab. Otherwise, only to "view"-tab.
QVector<Waypoint *> waypointsViewOnly; ///< local copy of current waypoint list on MAV
QVector<Waypoint *> waypointsEditable; ///< local editable waypoint list
QList<Waypoint *> waypointsViewOnly; ///< local copy of current waypoint list on MAV
QList<Waypoint *> waypointsEditable; ///< local editable waypoint list
Waypoint* currentWaypointEditable; ///< The currently used waypoint
QVector<mavlink_mission_item_t *> waypoint_buffer; ///< buffer for waypoints during communication
QList<mavlink_mission_item_t *> waypoint_buffer; ///< buffer for waypoints during communication
QTimer protocol_timer; ///< Timer to catch timeouts
bool standalone; ///< If standalone is set, do not write to UAS
quint16 uasid;
......
src/ui/WaypointList.cc
View file @ 259403be
......@@ -254,15 +254,15 @@ void WaypointList::addEditable()
void WaypointList::addEditable(bool onCurrentPosition)
{
const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
const QList<Waypoint *> &waypoints = WPM->getWaypointEditableList();
Waypoint *wp;
if (waypoints.size() > 0 &&
if (waypoints.count() > 0 &&
!(onCurrentPosition && uas && (uas->localPositionKnown() || uas->globalPositionKnown())))
{
// Create waypoint with last frame
Waypoint *last = waypoints.at(waypoints.size()-1);
Waypoint *last = waypoints.last();
wp = WPM->createWaypoint();
wp->blockSignals(true);
// wp->blockSignals(true);
MAV_FRAME frame = (MAV_FRAME)last->getFrame();
wp->setFrame(frame);
if (frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_RELATIVE_ALT)
......@@ -280,8 +280,7 @@ void WaypointList::addEditable(bool onCurrentPosition)
wp->setParam1(last->getParam3());
wp->setParam1(last->getParam4());
wp->setAutocontinue(last->getAutoContinue());
wp->blockSignals(false);
// wp->blockSignals(false);
wp->setAction(last->getAction());
// WPM->addWaypointEditable(wp);
}
......@@ -367,61 +366,6 @@ void WaypointList::addCurrentPositionWaypoint() {
addEditable(true);
}
//int WaypointList::addCurrentPositionWaypoint()
//{
// if (uas)
// {
// const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
// Waypoint *wp;
// Waypoint *last = 0;
// if (waypoints.size() > 0)
// {
// last = waypoints.at(waypoints.size()-1);
// }
// if (uas->globalPositionKnown())
// {
// float acceptanceRadiusGlobal = 10.0f;
// float holdTime = 0.0f;
// float yawGlobal = 0.0f;
// if (last)
// {
// acceptanceRadiusGlobal = last->getAcceptanceRadius();
// holdTime = last->getHoldTime();
// yawGlobal = last->getYaw();
// }
// // Create global frame waypoint per default
// wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT);
// WPM->addWaypointEditable(wp);
// updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint"));
// return 0;
// }
// else if (uas->localPositionKnown())
// {
// float acceptanceRadiusLocal = 0.2f;
// float holdTime = 0.5f;
// if (last)
// {
// acceptanceRadiusLocal = last->getAcceptanceRadius();
// holdTime = last->getHoldTime();
// }
// // Create local frame waypoint as second option
// wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, false, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT);
// WPM->addWaypointEditable(wp);
// updateStatusLabel(tr("Added LOCAL (NED) waypoint"));
// return 0;
// }
// else
// {
// // Do nothing
// updateStatusLabel(tr("Not adding waypoint, no position of MAV known yet."));
// return 1;
// }
// }
// return 1;
//}
void WaypointList::updateStatusLabel(const QString &string)
{
// Status label in write widget
......@@ -443,11 +387,11 @@ void WaypointList::changeCurrentWaypoint(quint16 seq)
void WaypointList::currentWaypointEditableChanged(quint16 seq)
{
WPM->setCurrentEditable(seq);
const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
const QList<Waypoint *> &waypoints = WPM->getWaypointEditableList();
if (seq < waypoints.size())
if (seq < waypoints.count())
{
for(int i = 0; i < waypoints.size(); i++)
for(int i = 0; i < waypoints.count(); i++)
{
WaypointEditableView* widget = wpEditableViews.find(waypoints[i]).value();
......@@ -466,11 +410,11 @@ void WaypointList::currentWaypointEditableChanged(quint16 seq)
// Update waypointViews to correctly indicate the new current waypoint
void WaypointList::currentWaypointViewOnlyChanged(quint16 seq)
{
const QVector<Waypoint *> &waypoints = WPM->getWaypointViewOnlyList();
const QList<Waypoint *> &waypoints = WPM->getWaypointViewOnlyList();
if (seq < waypoints.size())
if (seq < waypoints.count())
{
for(int i = 0; i < waypoints.size(); i++)
for(int i = 0; i < waypoints.count(); i++)
{
WaypointViewOnlyView* widget = wpViewOnlyViews.find(waypoints[i]).value();
......@@ -504,7 +448,7 @@ void WaypointList::waypointViewOnlyListChanged()
{
// Prevent updates to prevent visual flicker
this->setUpdatesEnabled(false);
const QVector<Waypoint *> &waypoints = WPM->getWaypointViewOnlyList();
const QList<Waypoint *> &waypoints = WPM->getWaypointViewOnlyList();
if (!wpViewOnlyViews.empty()) {
QMapIterator<Waypoint*,WaypointViewOnlyView*> viewIt(wpViewOnlyViews);
......@@ -513,12 +457,12 @@ void WaypointList::waypointViewOnlyListChanged()
viewIt.next();
Waypoint *cur = viewIt.key();
int i;
for (i = 0; i < waypoints.size(); i++) {
for (i = 0; i < waypoints.count(); i++) {
if (waypoints[i] == cur) {
break;
}
}
if (i == waypoints.size()) {
if (i == waypoints.count()) {
WaypointViewOnlyView* widget = wpViewOnlyViews.find(cur).value();
widget->hide();
viewOnlyListLayout->removeWidget(widget);
......@@ -528,7 +472,7 @@ void WaypointList::waypointViewOnlyListChanged()
}
// then add/update the views for each waypoint in the list
for(int i = 0; i < waypoints.size(); i++) {
for(int i = 0; i < waypoints.count(); i++) {
Waypoint *wp = waypoints[i];
if (!wpViewOnlyViews.contains(wp)) {
WaypointViewOnlyView* wpview = new WaypointViewOnlyView(wp, this);
......@@ -556,7 +500,7 @@ void WaypointList::waypointEditableListChanged()
{
// Prevent updates to prevent visual flicker
this->setUpdatesEnabled(false);
const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
const QList<Waypoint *> &waypoints = WPM->getWaypointEditableList();
if (!wpEditableViews.empty()) {
QMapIterator<Waypoint*,WaypointEditableView*> viewIt(wpEditableViews);
......@@ -565,12 +509,12 @@ void WaypointList::waypointEditableListChanged()
viewIt.next();
Waypoint *cur = viewIt.key();
int i;
for (i = 0; i < waypoints.size(); i++) {
for (i = 0; i < waypoints.count(); i++) {
if (waypoints[i] == cur) {
break;
}
}
if (i == waypoints.size()) {
if (i == waypoints.count()) {
WaypointEditableView* widget = wpEditableViews.find(cur).value();
widget->hide();
editableListLayout->removeWidget(widget);
......@@ -580,7 +524,7 @@ void WaypointList::waypointEditableListChanged()
}
// then add/update the views for each waypoint in the list
for(int i = 0; i < waypoints.size(); i++) {
for(int i = 0; i < waypoints.count(); i++) {
Waypoint *wp = waypoints[i];
if (!wpEditableViews.contains(wp)) {
WaypointEditableView* wpview = new WaypointEditableView(wp, this);
......@@ -610,34 +554,34 @@ void WaypointList::waypointEditableListChanged()
void WaypointList::moveUp(Waypoint* wp)
{
const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
const QList<Waypoint *> &waypoints = WPM->getWaypointEditableList();
//get the current position of wp in the local storage
int i;
for (i = 0; i < waypoints.size(); i++) {
for (i = 0; i < waypoints.count(); i++) {
if (waypoints[i] == wp)
break;
}
// if wp was found and its not the first entry, move it
if (i < waypoints.size() && i > 0) {
if (i < waypoints.count() && i > 0) {
WPM->moveWaypoint(i, i-1);
}
}
void WaypointList::moveDown(Waypoint* wp)
{
const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
const QList<Waypoint *> &waypoints = WPM->getWaypointEditableList();
//get the current position of wp in the local storage
int i;
for (i = 0; i < waypoints.size(); i++) {
for (i = 0; i < waypoints.count(); i++) {
if (waypoints[i] == wp)
break;
}
// if wp was found and its not the last entry, move it
if (i < waypoints.size()-1) {
if (i < waypoints.count()-1) {
WPM->moveWaypoint(i, i+1);
}
}
......@@ -664,72 +608,25 @@ void WaypointList::on_clearWPListButton_clicked()
{
if (uas) {
emit clearPathclicked();
const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++)
const QList<Waypoint *> &waypoints = WPM->getWaypointEditableList();
while(!waypoints.isEmpty()) {
WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value();
widget->remove();
}
}
}
///** @brief The MapWidget informs that a waypoint global was changed on the map */
//void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP)
//{
// if (uas)
// {
// const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
// if (waypoints.size() > 0)
// {
// Waypoint *temp = waypoints.at(indexWP);
// temp->setX(coordinate.x());
// temp->setY(coordinate.y());
// //WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value();
// //widget->updateValues();
// }
// }
//}
///** @brief The MapWidget informs that a waypoint global was changed on the map */
//void WaypointList::waypointGlobalPositionChanged(Waypoint* wp)
//{
// QPointF coordinate;
// coordinate.setX(wp->getX());
// coordinate.setY(wp->getY());
// emit ChangeWaypointGlobalPosition(wp->getId(), coordinate);
//}
void WaypointList::clearWPWidget()
{
// Get list
const QVector<Waypoint *> &waypoints = WPM->getWaypointEditableList();
const QList<Waypoint *> &waypoints = WPM->getWaypointEditableList();
// XXX delete wps as well
// Clear UI elements
while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++)
while(!waypoints.isEmpty()) {
WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value();
widget->remove();
}
}
//void WaypointList::setIsLoadFileWP()
//{
// loadFileGlobalWP = true;
//}
//void WaypointList::setIsReadGlobalWP(bool value)
//{
// // FIXME James Check this
// Q_UNUSED(value);
// // readGlobalWP = value;
//}
src/ui/map/QGCMapWidget.cc
View file @ 259403be
......@@ -634,7 +634,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp)
if (wpindex > 0)
{
// Get predecessor of this WP
QVector<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
QList<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
Waypoint* wp1 = wps.at(wpindex-1);
mapcontrol::WayPointItem* prevIcon = waypointsToIcons.value(wp1, NULL);
// If we got a valid graphics item, continue
......@@ -728,7 +728,7 @@ void QGCMapWidget::updateWaypointList(int uas)
// Delete first all old waypoints
// this is suboptimal (quadratic, but wps should stay in the sub-100 range anyway)
QVector<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
QList<Waypoint* > wps = currWPManager->getGlobalFrameAndNavTypeWaypointList();
foreach (Waypoint* wp, waypointsToIcons.keys())
{
if (!wps.contains(wp))
......
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