UASWaypointManager.cc 34.9 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2
/*=====================================================================

lm's avatar
lm committed
3
QGroundControl Open Source Ground Control Station
pixhawk's avatar
pixhawk committed
4

Lorenz Meier's avatar
Lorenz Meier committed
5
(c) 2009-2012 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
pixhawk's avatar
pixhawk committed
6

lm's avatar
lm committed
7
This file is part of the QGROUNDCONTROL project
pixhawk's avatar
pixhawk committed
8

lm's avatar
lm committed
9
    QGROUNDCONTROL is free software: you can redistribute it and/or modify
pixhawk's avatar
pixhawk committed
10 11 12 13
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

lm's avatar
lm committed
14
    QGROUNDCONTROL is distributed in the hope that it will be useful,
pixhawk's avatar
pixhawk committed
15 16 17 18 19
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
lm's avatar
lm committed
20
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
pixhawk's avatar
pixhawk committed
21 22 23 24 25 26 27 28 29 30 31

======================================================================*/

/**
 * @file
 *   @brief Implementation of the waypoint protocol handler
 *
 *   @author Petri Tanskanen <mavteam@student.ethz.ch>
 *
 */

32 33
#include "UASWaypointManager.h"
#include "UAS.h"
34
#include "mavlink_types.h"
35
#include "UASManager.h"
36
#include "MainWindow.h"
37

38
#define PROTOCOL_TIMEOUT_MS 2000    ///< maximum time to wait for pending messages until timeout
lm's avatar
lm committed
39 40
#define PROTOCOL_DELAY_MS 20        ///< minimum delay between sent messages
#define PROTOCOL_MAX_RETRIES 5      ///< maximum number of send retries (after timeout)
41
const float UASWaypointManager::defaultAltitudeHomeOffset   = 30.0f;
lm's avatar
lm committed
42
UASWaypointManager::UASWaypointManager(UAS* _uas)
43 44 45 46 47 48 49
    : uas(_uas),
      current_retries(0),
      current_wp_id(0),
      current_count(0),
      current_state(WP_IDLE),
      current_partner_systemid(0),
      current_partner_compid(0),
50 51
      currentWaypointEditable(NULL),
      protocol_timer(this)
pixhawk's avatar
pixhawk committed
52
{
53 54
    if (uas)
    {
lm's avatar
lm committed
55 56 57 58 59 60 61 62
        uasid = uas->getUASID();
        connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout()));
        connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleLocalPositionChanged(UASInterface*,double,double,double,quint64)));
        connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleGlobalPositionChanged(UASInterface*,double,double,double,quint64)));
    }
    else
    {
        uasid = 0;
63
    }
pixhawk's avatar
pixhawk committed
64 65
}

LM's avatar
LM committed
66 67 68 69 70
UASWaypointManager::~UASWaypointManager()
{

}

pixhawk's avatar
pixhawk committed
71
void UASWaypointManager::timeout()
72
{
73
    if (current_retries > 0) {
74 75
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries--;
76
        emit updateStatusString(tr("Timeout, retrying (retries left: %1)").arg(current_retries));
77

78
        if (current_state == WP_GETLIST) {
79
            sendWaypointRequestList();
80
        } else if (current_state == WP_GETLIST_GETWPS) {
81
            sendWaypointRequest(current_wp_id);
82
        } else if (current_state == WP_SENDLIST) {
83
            sendWaypointCount();
84
        } else if (current_state == WP_SENDLIST_SENDWPS) {
85
            sendWaypoint(current_wp_id);
86
        } else if (current_state == WP_CLEARLIST) {
87
            sendWaypointClearAll();
88
        } else if (current_state == WP_SETCURRENT) {
89 90
            sendWaypointSetCurrent(current_wp_id);
        }
91
    } else {
92
        protocol_timer.stop();
pixhawk's avatar
pixhawk committed
93

94
        emit updateStatusString("Operation timed out.");
pixhawk's avatar
pixhawk committed
95

96 97 98 99 100 101
        current_state = WP_IDLE;
        current_count = 0;
        current_wp_id = 0;
        current_partner_systemid = 0;
        current_partner_compid = 0;
    }
102 103
}

104 105 106 107
void UASWaypointManager::handleLocalPositionChanged(UASInterface* mav, double x, double y, double z, quint64 time)
{
    Q_UNUSED(mav);
    Q_UNUSED(time);
pixhawk's avatar
pixhawk committed
108
    if (waypointsEditable.count() > 0 && currentWaypointEditable && (currentWaypointEditable->getFrame() == MAV_FRAME_LOCAL_NED || currentWaypointEditable->getFrame() == MAV_FRAME_LOCAL_ENU))
109
    {
pixhawk's avatar
pixhawk committed
110 111 112
        double xdiff = x-currentWaypointEditable->getX();
        double ydiff = y-currentWaypointEditable->getY();
        double zdiff = z-currentWaypointEditable->getZ();
113 114 115 116 117 118 119
        double dist = sqrt(xdiff*xdiff + ydiff*ydiff + zdiff*zdiff);
        emit waypointDistanceChanged(dist);
    }
}

void UASWaypointManager::handleGlobalPositionChanged(UASInterface* mav, double lat, double lon, double alt, quint64 time)
{
120 121
    Q_UNUSED(mav);
    Q_UNUSED(time);
122 123 124
	Q_UNUSED(alt);
	Q_UNUSED(lon);
	Q_UNUSED(lat);
125 126 127 128 129 130
    if (waypointsEditable.count() > 0 && currentWaypointEditable && (currentWaypointEditable->getFrame() == MAV_FRAME_GLOBAL || currentWaypointEditable->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT))
    {
        // TODO FIXME Calculate distance
        double dist = 0;
        emit waypointDistanceChanged(dist);
    }
131 132
}

133 134
void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, quint16 count)
{
135
    if (current_state == WP_GETLIST && systemId == current_partner_systemid) {
136 137 138
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;

139 140
        //Clear the old edit-list before receiving the new one
        if (read_to_edit == true){
141
            while(waypointsEditable.count()>0) {
142
                Waypoint *t = waypointsEditable[0];
143
                waypointsEditable.removeAt(0);
144 145 146 147 148
                delete t;
            }
            emit waypointEditableListChanged();
        }

149
        if (count > 0) {
pixhawk's avatar
pixhawk committed
150 151 152 153
            current_count = count;
            current_wp_id = 0;
            current_state = WP_GETLIST_GETWPS;
            sendWaypointRequest(current_wp_id);
154
        } else {
155
            protocol_timer.stop();
pixhawk's avatar
pixhawk committed
156
            emit updateStatusString("done.");
157 158 159 160 161
            current_state = WP_IDLE;
            current_count = 0;
            current_wp_id = 0;
            current_partner_systemid = 0;
            current_partner_compid = 0;
pixhawk's avatar
pixhawk committed
162
        }
163 164


165 166
    } else {
        qDebug("Rejecting message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId);
167
    }
168 169
}

lm's avatar
lm committed
170
void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_mission_item_t *wp)
pixhawk's avatar
pixhawk committed
171
{
172
    if (systemId == current_partner_systemid && current_state == WP_GETLIST_GETWPS && wp->seq == current_wp_id) {
173 174 175
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;

176
        if(wp->seq == current_wp_id) {
pixhawk's avatar
pixhawk committed
177 178 179 180 181 182 183 184 185 186 187

            Waypoint *lwp_vo = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command);
            addWaypointViewOnly(lwp_vo);


            if (read_to_edit == true) {
                Waypoint *lwp_ed = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command);
                addWaypointEditable(lwp_ed, false);
                if (wp->current == 1) currentWaypointEditable = lwp_ed;
            }

pixhawk's avatar
pixhawk committed
188 189 190 191

            //get next waypoint
            current_wp_id++;

192
            if(current_wp_id < current_count) {
193
                sendWaypointRequest(current_wp_id);
194
            } else {
195 196
                sendWaypointAck(0);

pixhawk's avatar
pixhawk committed
197
                // all waypoints retrieved, change state to idle
pixhawk's avatar
pixhawk committed
198 199 200 201 202
                current_state = WP_IDLE;
                current_count = 0;
                current_wp_id = 0;
                current_partner_systemid = 0;
                current_partner_compid = 0;
203

pixhawk's avatar
pixhawk committed
204
                protocol_timer.stop();
205
                emit readGlobalWPFromUAS(false);
206 207 208
                QTime time = QTime::currentTime();
                QString timeString = time.toString();
                emit updateStatusString(tr("done. (updated at %1)").arg(timeString));
209

pixhawk's avatar
pixhawk committed
210
            }
211
        } else {
212
            emit updateStatusString(tr("Waypoint ID mismatch, rejecting waypoint"));
pixhawk's avatar
pixhawk committed
213
        }
214 215
    } else {
        qDebug("Rejecting message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId);
216
    }
pixhawk's avatar
pixhawk committed
217 218
}

lm's avatar
lm committed
219
void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_mission_ack_t *wpa)
220
{
221
    if (systemId == current_partner_systemid && (compId == current_partner_compid || compId == MAV_COMP_ID_ALL)) {
222
        if((current_state == WP_SENDLIST || current_state == WP_SENDLIST_SENDWPS) && (current_wp_id == waypoint_buffer.count()-1 && wpa->type == 0)) {
223 224
            //all waypoints sent and ack received
            protocol_timer.stop();
225
            current_state = WP_IDLE;
226
            readWaypoints(false); //Update "Onboard Waypoints"-tab immidiately after the waypoint list has been sent.
227
            emit updateStatusString("done.");
228 229 230 231 232 233 234 235 236 237 238
        } else if((current_state == WP_SENDLIST || current_state == WP_SENDLIST_SENDWPS) && wpa->type != 0) {
            //give up transmitting if a WP is rejected
            if (wpa->type == 1) {
                emit updateStatusString("upload failed: general error");
            } else if (wpa->type == 2) {
                emit updateStatusString("upload failed: coordinate frame unsupported.");
            } else {
                emit updateStatusString("upload failed: other error.");
            }
            protocol_timer.stop();
            current_state = WP_IDLE;
239
        } else if(current_state == WP_CLEARLIST) {
240 241 242 243
            protocol_timer.stop();
            current_state = WP_IDLE;
            emit updateStatusString("done.");
        }
244 245 246
    }
}

lm's avatar
lm committed
247
void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_mission_request_t *wpr)
248
{
249
    if (systemId == current_partner_systemid && ((current_state == WP_SENDLIST && wpr->seq == 0) || (current_state == WP_SENDLIST_SENDWPS && (wpr->seq == current_wp_id || wpr->seq == current_wp_id + 1)))) {
250 251
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
252

253
        if (wpr->seq < waypoint_buffer.count()) {
pixhawk's avatar
pixhawk committed
254 255 256
            current_state = WP_SENDLIST_SENDWPS;
            current_wp_id = wpr->seq;
            sendWaypoint(current_wp_id);
257
        } else {
pixhawk's avatar
pixhawk committed
258 259
            //TODO: Error message or something
        }
260 261
    } else {
        qDebug("Rejecting message, check mismatch: current_state: %d == %d, system id %d == %d, comp id %d == %d", current_state, WP_GETLIST, current_partner_systemid, systemId, current_partner_compid, compId);
262
    }
263 264
}

lm's avatar
lm committed
265
void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_mission_item_reached_t *wpr)
266
{
267
	Q_UNUSED(compId);
lm's avatar
lm committed
268
    if (!uas) return;
269
    if (systemId == uasid) {
pixhawk's avatar
pixhawk committed
270 271
        emit updateStatusString(QString("Reached waypoint %1").arg(wpr->seq));
    }
pixhawk's avatar
pixhawk committed
272 273
}

lm's avatar
lm committed
274
void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_mission_current_t *wpc)
pixhawk's avatar
pixhawk committed
275
{
276
    Q_UNUSED(compId);
lm's avatar
lm committed
277
    if (!uas) return;
278
    if (systemId == uasid) {
279
        // FIXME Petri
280
        if (current_state == WP_SETCURRENT) {
281 282
            protocol_timer.stop();
            current_state = WP_IDLE;
283 284

            // update the local main storage
pixhawk's avatar
pixhawk committed
285 286 287 288
            if (wpc->seq < waypointsViewOnly.size()) {
                for(int i = 0; i < waypointsViewOnly.size(); i++) {
                    if (waypointsViewOnly[i]->getId() == wpc->seq) {
                        waypointsViewOnly[i]->setCurrent(true);
289
                    } else {
pixhawk's avatar
pixhawk committed
290
                        waypointsViewOnly[i]->setCurrent(false);
291 292 293
                    }
                }
            }
294
        }
295 296 297
        emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq));
        //emit update to UI widgets
        emit currentWaypointChanged(wpc->seq);
pixhawk's avatar
pixhawk committed
298
    }
pixhawk's avatar
pixhawk committed
299 300
}

301
void UASWaypointManager::notifyOfChangeEditable(Waypoint* wp)
302
{
303
    // If only one waypoint was changed, emit only WP signal
304
    if (wp != NULL) {
305
        emit waypointEditableChanged(uasid, wp);
306
    } else {
307
        emit waypointEditableListChanged();
308
        emit waypointEditableListChanged(uasid);
309
    }
310 311
}

312 313 314
void UASWaypointManager::notifyOfChangeViewOnly(Waypoint* wp)
{
    if (wp != NULL) {
315
        emit waypointViewOnlyChanged(uasid, wp);
316 317
    } else {
        emit waypointViewOnlyListChanged();
318
        emit waypointViewOnlyListChanged(uasid);
319 320
    }
}
321 322


323
int UASWaypointManager::setCurrentWaypoint(quint16 seq)
pixhawk's avatar
pixhawk committed
324
{
pixhawk's avatar
pixhawk committed
325
    if (seq < waypointsViewOnly.size()) {
326
        if(current_state == WP_IDLE) {
327

328 329 330 331 332 333
            //send change to UAS - important to note: if the transmission fails, we have inconsistencies
            protocol_timer.start(PROTOCOL_TIMEOUT_MS);
            current_retries = PROTOCOL_MAX_RETRIES;

            current_state = WP_SETCURRENT;
            current_wp_id = seq;
334
            current_partner_systemid = uasid;
lm's avatar
lm committed
335
            current_partner_compid = MAV_COMP_ID_MISSIONPLANNER;
336 337 338 339 340 341 342 343 344

            sendWaypointSetCurrent(current_wp_id);

            return 0;
        }
    }
    return -1;
}

345 346
int UASWaypointManager::setCurrentEditable(quint16 seq)
{
347
    if (seq < waypointsEditable.count()) {
348 349
        if(current_state == WP_IDLE) {
            //update local main storage
350
            for(int i = 0; i < waypointsEditable.count(); i++) {
351 352 353 354 355 356 357 358 359 360 361 362
                if (waypointsEditable[i]->getId() == seq) {
                    waypointsEditable[i]->setCurrent(true);
                } else {
                    waypointsEditable[i]->setCurrent(false);
                }
            }

            return 0;
        }
    }
    return -1;
}
pixhawk's avatar
pixhawk committed
363 364 365 366

void UASWaypointManager::addWaypointViewOnly(Waypoint *wp)
{
    if (wp)
367
    {
pixhawk's avatar
pixhawk committed
368 369 370 371
        waypointsViewOnly.insert(waypointsViewOnly.size(), wp);
        connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeViewOnly(Waypoint*)));

        emit waypointViewOnlyListChanged();
372
        emit waypointViewOnlyListChanged(uasid);
pixhawk's avatar
pixhawk committed
373 374 375 376
    }
}


pixhawk's avatar
pixhawk committed
377
/**
378
 * @warning Make sure the waypoint stays valid for the whole application lifecycle!
pixhawk's avatar
pixhawk committed
379
 * @param enforceFirstActive Enforces that the first waypoint is set as active
380
 * @see createWaypoint() is more suitable for most use cases
pixhawk's avatar
pixhawk committed
381
 */
382
void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActive)
383
{
lm's avatar
lm committed
384 385
    if (wp)
    {
386 387 388 389
        // Check if this is the first waypoint in an offline list
        if (waypointsEditable.count() == 0 && uas == NULL)
            MainWindow::instance()->showCriticalMessage(tr("OFFLINE Waypoint Editing Mode"), tr("You are in offline editing mode. Make sure to safe your mission to a file before connecting to a system - you will need to load the file into the system, the offline list will be cleared on connect."));

390 391
        wp->setId(waypointsEditable.count());
        if (enforceFirstActive && waypointsEditable.count() == 0)
392 393
        {
            wp->setCurrent(true);
pixhawk's avatar
pixhawk committed
394
            currentWaypointEditable = wp;
395
        }
396
        waypointsEditable.insert(waypointsEditable.count(), wp);
397
        connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*)));
398

399
        emit waypointEditableListChanged();
400
        emit waypointEditableListChanged(uasid);
401
    }
pixhawk's avatar
pixhawk committed
402 403
}

404 405 406 407 408
/**
 * @param enforceFirstActive Enforces that the first waypoint is set as active
 */
Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive)
{
409 410 411 412
    // Check if this is the first waypoint in an offline list
    if (waypointsEditable.count() == 0 && uas == NULL)
        MainWindow::instance()->showCriticalMessage(tr("OFFLINE Waypoint Editing Mode"), tr("You are in offline editing mode. Make sure to safe your mission to a file before connecting to a system - you will need to load the file into the system, the offline list will be cleared on connect."));

413
    Waypoint* wp = new Waypoint();
414 415 416
    wp->setId(waypointsEditable.count());
    wp->setFrame((MAV_FRAME)getFrameRecommendation());
    wp->setAltitude(getAltitudeRecommendation());
417
    wp->setAcceptanceRadius(getAcceptanceRadiusRecommendation());
418
    if (enforceFirstActive && waypointsEditable.count() == 0)
419 420
    {
        wp->setCurrent(true);
pixhawk's avatar
pixhawk committed
421
        currentWaypointEditable = wp;
422
    }
423
    waypointsEditable.append(wp);
424
    connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*)));
425

426
    emit waypointEditableListChanged();
427
    emit waypointEditableListChanged(uasid);
428 429 430
    return wp;
}

431
int UASWaypointManager::removeWaypoint(quint16 seq)
432
{
433
    if (seq < waypointsEditable.count())
434
    {
pixhawk's avatar
pixhawk committed
435
        Waypoint *t = waypointsEditable[seq];
436 437 438

        if (t->getCurrent() == true) //trying to remove the current waypoint
        {
439
            if (seq+1 < waypointsEditable.count()) // setting the next waypoint as current
440 441 442
            {
                waypointsEditable[seq+1]->setCurrent(true);
            }
443
            else if (seq-1 >= 0) // if deleting the last on the list, then setting the previous waypoint as current
444 445 446 447 448
            {
                waypointsEditable[seq-1]->setCurrent(true);
            }
        }

449
        waypointsEditable.removeAt(seq);
450
        delete t;
451
        t = NULL;
452

453
        for(int i = seq; i < waypointsEditable.count(); i++)
454
        {
pixhawk's avatar
pixhawk committed
455
            waypointsEditable[i]->setId(i);
456
        }
Alejandro's avatar
Alejandro committed
457

458
        emit waypointEditableListChanged();
459
        emit waypointEditableListChanged(uasid);
460 461 462 463 464
        return 0;
    }
    return -1;
}

465
void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
466
{
467
    if (cur_seq != new_seq && cur_seq < waypointsEditable.count() && new_seq < waypointsEditable.count())
468
    {
pixhawk's avatar
pixhawk committed
469
        Waypoint *t = waypointsEditable[cur_seq];
470
        if (cur_seq < new_seq) {
471 472
            for (int i = cur_seq; i < new_seq; i++)
            {
pixhawk's avatar
pixhawk committed
473
                waypointsEditable[i] = waypointsEditable[i+1];
474
                waypointsEditable[i]->setId(i);
475
            }
476 477 478 479 480
        }
        else
        {
            for (int i = cur_seq; i > new_seq; i--)
            {
pixhawk's avatar
pixhawk committed
481
                waypointsEditable[i] = waypointsEditable[i-1];
482
                waypointsEditable[i]->setId(i);
483 484
            }
        }
pixhawk's avatar
pixhawk committed
485
        waypointsEditable[new_seq] = t;
486
        waypointsEditable[new_seq]->setId(new_seq);
487

488
        emit waypointEditableListChanged();
489
        emit waypointEditableListChanged(uasid);
490 491 492
    }
}

493
void UASWaypointManager::saveWaypoints(const QString &saveFile)
494 495 496 497 498 499
{
    QFile file(saveFile);
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
        return;

    QTextStream out(&file);
500 501

    //write the waypoint list version to the first line for compatibility check
502
    out << "QGC WPL 120\r\n";
503

504
    for (int i = 0; i < waypointsEditable.count(); i++)
505
    {
pixhawk's avatar
pixhawk committed
506 507
        waypointsEditable[i]->setId(i);
        waypointsEditable[i]->save(out);
508 509 510 511
    }
    file.close();
}

512
void UASWaypointManager::loadWaypoints(const QString &loadFile)
513 514 515 516 517
{
    QFile file(loadFile);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
        return;

518
    while(waypointsEditable.count()>0) {
pixhawk's avatar
pixhawk committed
519
        Waypoint *t = waypointsEditable[0];
520
        waypointsEditable.removeAt(0);
521 522 523 524
        delete t;
    }

    QTextStream in(&file);
525 526 527

    const QStringList &version = in.readLine().split(" ");

528
    if (!(version.size() == 3 && version[0] == "QGC" && version[1] == "WPL" && version[2] == "120"))
529
    {
530
        emit updateStatusString(tr("The waypoint file is not compatible with the current version of QGroundControl."));
531 532 533 534 535
    }
    else
    {
        while (!in.atEnd())
        {
536
            Waypoint *t = new Waypoint();
537 538
            if(t->load(in))
            {
539 540
                t->setId(waypointsEditable.count());
                waypointsEditable.insert(waypointsEditable.count(), t);
541 542 543
            }
            else
            {
544
                emit updateStatusString(tr("The waypoint file is corrupted. Load operation only partly succesful."));
545 546
                break;
            }
547 548
        }
    }
549

550 551
    file.close();

552
    emit loadWPFile();
553
    emit waypointEditableListChanged();
554
    emit waypointEditableListChanged(uasid);
555 556 557
}

void UASWaypointManager::clearWaypointList()
pixhawk's avatar
pixhawk committed
558
{
559
    if (current_state == WP_IDLE)
560
    {
pixhawk's avatar
pixhawk committed
561
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
562
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
563

564 565
        current_state = WP_CLEARLIST;
        current_wp_id = 0;
566
        current_partner_systemid = uasid;
lm's avatar
lm committed
567
        current_partner_compid = MAV_COMP_ID_MISSIONPLANNER;
pixhawk's avatar
pixhawk committed
568

569
        sendWaypointClearAll();
570 571 572
    }
}

573
const QList<Waypoint *> UASWaypointManager::getGlobalFrameWaypointList()
574 575 576 577
{
    // TODO Keep this global frame list up to date
    // with complete waypoint list
    // instead of filtering on each request
578
    QList<Waypoint*> wps;
pixhawk's avatar
pixhawk committed
579
    foreach (Waypoint* wp, waypointsEditable)
580 581 582
    {
        if (wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)
        {
583 584 585 586 587 588
            wps.append(wp);
        }
    }
    return wps;
}

589
const QList<Waypoint *> UASWaypointManager::getGlobalFrameAndNavTypeWaypointList()
lm's avatar
lm committed
590 591 592 593
{
    // TODO Keep this global frame list up to date
    // with complete waypoint list
    // instead of filtering on each request
594
    QList<Waypoint*> wps;
pixhawk's avatar
pixhawk committed
595
    foreach (Waypoint* wp, waypointsEditable)
596 597 598
    {
        if ((wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && wp->isNavigationType())
        {
lm's avatar
lm committed
599 600 601 602 603 604
            wps.append(wp);
        }
    }
    return wps;
}

605
const QList<Waypoint *> UASWaypointManager::getNavTypeWaypointList()
LM's avatar
LM committed
606 607 608 609
{
    // TODO Keep this global frame list up to date
    // with complete waypoint list
    // instead of filtering on each request
610
    QList<Waypoint*> wps;
pixhawk's avatar
pixhawk committed
611
    foreach (Waypoint* wp, waypointsEditable)
612 613 614
    {
        if (wp->isNavigationType())
        {
LM's avatar
LM committed
615 616 617 618 619 620
            wps.append(wp);
        }
    }
    return wps;
}

621 622
int UASWaypointManager::getIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
623
    return waypointsEditable.indexOf(wp);
624 625
}

626 627
int UASWaypointManager::getGlobalFrameIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
628
    // Search through all waypointsEditable,
629 630
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
631
    foreach (Waypoint* p, waypointsEditable) {
632 633 634 635
        if (p->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)
        {
            if (p == wp)
            {
636 637 638 639 640 641 642 643 644
                return i;
            }
            i++;
        }
    }

    return -1;
}

lm's avatar
lm committed
645 646
int UASWaypointManager::getGlobalFrameAndNavTypeIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
647
    // Search through all waypointsEditable,
lm's avatar
lm committed
648 649
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
650
    foreach (Waypoint* p, waypointsEditable) {
651
        if ((p->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && p->isNavigationType())
652 653 654
        {
            if (p == wp)
            {
lm's avatar
lm committed
655 656 657 658 659 660 661 662 663
                return i;
            }
            i++;
        }
    }

    return -1;
}

LM's avatar
LM committed
664 665
int UASWaypointManager::getNavTypeIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
666
    // Search through all waypointsEditable,
LM's avatar
LM committed
667 668
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
669
    foreach (Waypoint* p, waypointsEditable)
670 671 672 673 674
    {
        if (p->isNavigationType())
        {
            if (p == wp)
            {
LM's avatar
LM committed
675 676 677 678 679 680 681 682 683
                return i;
            }
            i++;
        }
    }

    return -1;
}

684 685
int UASWaypointManager::getGlobalFrameCount()
{
pixhawk's avatar
pixhawk committed
686
    // Search through all waypointsEditable,
687 688
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
689
    foreach (Waypoint* p, waypointsEditable)
690
    {
691 692
        if (p->getFrame() == MAV_FRAME_GLOBAL || p->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)
        {
693 694 695 696 697 698 699
            i++;
        }
    }

    return i;
}

lm's avatar
lm committed
700 701
int UASWaypointManager::getGlobalFrameAndNavTypeCount()
{
pixhawk's avatar
pixhawk committed
702
    // Search through all waypointsEditable,
lm's avatar
lm committed
703 704
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
705
    foreach (Waypoint* p, waypointsEditable) {
706
        if ((p->getFrame() == MAV_FRAME_GLOBAL || p->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && p->isNavigationType())
707
        {
lm's avatar
lm committed
708 709 710 711 712 713 714
            i++;
        }
    }

    return i;
}

LM's avatar
LM committed
715 716
int UASWaypointManager::getNavTypeCount()
{
pixhawk's avatar
pixhawk committed
717
    // Search through all waypointsEditable,
LM's avatar
LM committed
718 719
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
720
    foreach (Waypoint* p, waypointsEditable) {
LM's avatar
LM committed
721 722 723 724 725 726 727 728
        if (p->isNavigationType()) {
            i++;
        }
    }

    return i;
}

729 730
int UASWaypointManager::getLocalFrameCount()
{
pixhawk's avatar
pixhawk committed
731
    // Search through all waypointsEditable,
732 733
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
734
    foreach (Waypoint* p, waypointsEditable)
lm's avatar
lm committed
735
    {
736
        if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU)
lm's avatar
lm committed
737
        {
738 739 740 741 742 743 744 745 746
            i++;
        }
    }

    return i;
}

int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
747
    // Search through all waypointsEditable,
748 749
    // counting only those in local frame
    int i = 0;
pixhawk's avatar
pixhawk committed
750
    foreach (Waypoint* p, waypointsEditable)
lm's avatar
lm committed
751 752 753
    {
        if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU)
        {
754 755
            if (p == wp)
            {
756 757 758 759 760 761 762 763 764 765 766
                return i;
            }
            i++;
        }
    }

    return -1;
}

int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
767
    // Search through all waypointsEditable,
768 769
    // counting only those in mission frame
    int i = 0;
pixhawk's avatar
pixhawk committed
770
    foreach (Waypoint* p, waypointsEditable)
lm's avatar
lm committed
771
    {
772 773 774 775
        if (p->getFrame() == MAV_FRAME_MISSION)
        {
            if (p == wp)
            {
776 777 778 779 780 781 782 783 784
                return i;
            }
            i++;
        }
    }

    return -1;
}

pixhawk's avatar
pixhawk committed
785 786 787 788 789

/**
 * @param readToEdit If true, incoming waypoints will be copied both to "edit"-tab and "view"-tab. Otherwise, only to "view"-tab.
 */
void UASWaypointManager::readWaypoints(bool readToEdit)
790
{
pixhawk's avatar
pixhawk committed
791
    read_to_edit = readToEdit;
792
    emit readGlobalWPFromUAS(true);
793
    if(current_state == WP_IDLE) {
794

795

796 797
        //Clear the old view-list before receiving the new one
        while(waypointsViewOnly.size()>0) {
798
            Waypoint *t = waypointsViewOnly[0];
799
            waypointsViewOnly.removeAt(0);
800
            delete t;
801
        }
802 803
        emit waypointViewOnlyListChanged();
        /* THIS PART WAS MOVED TO handleWaypointCount. THE EDIT-LIST SHOULD NOT BE CLEARED UNLESS THERE IS A RESPONSE FROM UAV.
804 805
        //Clear the old edit-list before receiving the new one
        if (read_to_edit == true){
806
            while(waypointsEditable.count()>0) {
807 808 809
                Waypoint *t = waypointsEditable[0];
                waypointsEditable.remove(0);
                delete t;
810
            }
811
            emit waypointEditableListChanged();
812
        }
813
        */
814 815
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
816 817 818

        current_state = WP_GETLIST;
        current_wp_id = 0;
819
        current_partner_systemid = uasid;
lm's avatar
lm committed
820
        current_partner_compid = MAV_COMP_ID_MISSIONPLANNER;
pixhawk's avatar
pixhawk committed
821

822
        sendWaypointRequestList();
823

pixhawk's avatar
pixhawk committed
824 825
    }
}
826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859
bool UASWaypointManager::guidedModeSupported()
{
    return (uas->getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA);
}

void UASWaypointManager::goToWaypoint(Waypoint *wp)
{
    //Don't try to send a guided mode message to an AP that does not support it.
    if (uas->getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA)
    {
        mavlink_mission_item_t mission;
        memset(&mission, 0, sizeof(mavlink_mission_item_t));   //initialize with zeros
        //const Waypoint *cur_s = waypointsEditable.at(i);

        mission.autocontinue = 0;
        mission.current = 2; //2 for guided mode
        mission.param1 = wp->getParam1();
        mission.param2 = wp->getParam2();
        mission.param3 = wp->getParam3();
        mission.param4 = wp->getParam4();
        mission.frame = wp->getFrame();
        mission.command = wp->getAction();
        mission.seq = 0;     // don't read out the sequence number of the waypoint class
        mission.x = wp->getX();
        mission.y = wp->getY();
        mission.z = wp->getZ();
        mavlink_message_t message;
        mission.target_system = uasid;
        mission.target_component = MAV_COMP_ID_MISSIONPLANNER;
        mavlink_msg_mission_item_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &mission);
        uas->sendMessage(message);
        QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
    }
}
pixhawk's avatar
pixhawk committed
860

861
void UASWaypointManager::writeWaypoints()
pixhawk's avatar
pixhawk committed
862
{
863
    if (current_state == WP_IDLE) {
pixhawk's avatar
pixhawk committed
864
        // Send clear all if count == 0
pixhawk's avatar
pixhawk committed
865
        if (waypointsEditable.count() > 0) {
pixhawk's avatar
pixhawk committed
866
            protocol_timer.start(PROTOCOL_TIMEOUT_MS);
867
            current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
868

pixhawk's avatar
pixhawk committed
869
            current_count = waypointsEditable.count();
pixhawk's avatar
pixhawk committed
870 871
            current_state = WP_SENDLIST;
            current_wp_id = 0;
872
            current_partner_systemid = uasid;
lm's avatar
lm committed
873
            current_partner_compid = MAV_COMP_ID_MISSIONPLANNER;
pixhawk's avatar
pixhawk committed
874 875

            //clear local buffer
pixhawk's avatar
pixhawk committed
876 877 878
            // Why not replace with waypoint_buffer.clear() ?
            // because this will lead to memory leaks, the waypoint-structs
            // have to be deleted, clear() would only delete the pointers.
879
            while(!waypoint_buffer.empty()) {
pixhawk's avatar
pixhawk committed
880 881 882
                delete waypoint_buffer.back();
                waypoint_buffer.pop_back();
            }
pixhawk's avatar
pixhawk committed
883

884 885
            bool noCurrent = true;

pixhawk's avatar
pixhawk committed
886
            //copy waypoint data to local buffer
887
            for (int i=0; i < current_count; i++) {
lm's avatar
lm committed
888 889 890
                waypoint_buffer.push_back(new mavlink_mission_item_t);
                mavlink_mission_item_t *cur_d = waypoint_buffer.back();
                memset(cur_d, 0, sizeof(mavlink_mission_item_t));   //initialize with zeros
pixhawk's avatar
pixhawk committed
891
                const Waypoint *cur_s = waypointsEditable.at(i);
pixhawk's avatar
pixhawk committed
892 893

                cur_d->autocontinue = cur_s->getAutoContinue();
894
                cur_d->current = cur_s->getCurrent() & noCurrent;   //make sure only one current waypoint is selected, the first selected will be chosen
895 896
                cur_d->param1 = cur_s->getParam1();
                cur_d->param2 = cur_s->getParam2();
lm's avatar
lm committed
897 898
                cur_d->param3 = cur_s->getParam3();
                cur_d->param4 = cur_s->getParam4();
899
                cur_d->frame = cur_s->getFrame();
lm's avatar
lm committed
900
                cur_d->command = cur_s->getAction();
901
                cur_d->seq = i;     // don't read out the sequence number of the waypoint class
pixhawk's avatar
pixhawk committed
902 903 904
                cur_d->x = cur_s->getX();
                cur_d->y = cur_s->getY();
                cur_d->z = cur_s->getZ();
905 906 907

                if (cur_s->getCurrent() && noCurrent)
                    noCurrent = false;
908 909
                if (i == (current_count - 1) && noCurrent == true) //not a single waypoint was set as "current"
                    cur_d->current = true; // set the last waypoint as current. Or should it better be the first waypoint ?
pixhawk's avatar
pixhawk committed
910
            }
911

912 913 914



pixhawk's avatar
pixhawk committed
915
            //send the waypoint count to UAS (this starts the send transaction)
916
            sendWaypointCount();
Lorenz Meier's avatar
Lorenz Meier committed
917
        } else if (waypointsEditable.count() == 0)
918
        {
919
            clearWaypointList();
920
        }
Lorenz Meier's avatar
Lorenz Meier committed
921 922 923
    }
    else
    {
924
        //we're in another transaction, ignore command
925
        qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
926
    }
pixhawk's avatar
pixhawk committed
927 928
}

929 930
void UASWaypointManager::sendWaypointClearAll()
{
lm's avatar
lm committed
931
    if (!uas) return;
932
    mavlink_message_t message;
lm's avatar
lm committed
933
    mavlink_mission_clear_all_t wpca;
934

935
    wpca.target_system = uasid;
lm's avatar
lm committed
936
    wpca.target_component = MAV_COMP_ID_MISSIONPLANNER;
937

938
    emit updateStatusString(QString("Clearing waypoint list..."));
939

lm's avatar
lm committed
940
    mavlink_msg_mission_clear_all_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpca);
941

942
    uas->sendMessage(message);
lm's avatar
lm committed
943
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
944 945 946 947
}

void UASWaypointManager::sendWaypointSetCurrent(quint16 seq)
{
lm's avatar
lm committed
948
    if (!uas) return;
949
    mavlink_message_t message;
lm's avatar
lm committed
950
    mavlink_mission_set_current_t wpsc;
951

952
    wpsc.target_system = uasid;
lm's avatar
lm committed
953
    wpsc.target_component = MAV_COMP_ID_MISSIONPLANNER;
954 955
    wpsc.seq = seq;

956
    emit updateStatusString(QString("Updating target waypoint..."));
957

lm's avatar
lm committed
958
    mavlink_msg_mission_set_current_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpsc);
959
    uas->sendMessage(message);
lm's avatar
lm committed
960
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
961 962 963 964
}

void UASWaypointManager::sendWaypointCount()
{
lm's avatar
lm committed
965
    if (!uas) return;
966
    mavlink_message_t message;
lm's avatar
lm committed
967
    mavlink_mission_count_t wpc;
968

969
    wpc.target_system = uasid;
lm's avatar
lm committed
970
    wpc.target_component = MAV_COMP_ID_MISSIONPLANNER;
971 972
    wpc.count = current_count;

973
    emit updateStatusString(QString("Starting to transmit waypoints..."));
974

lm's avatar
lm committed
975
    mavlink_msg_mission_count_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpc);
976
    uas->sendMessage(message);
lm's avatar
lm committed
977
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
978 979 980 981
}

void UASWaypointManager::sendWaypointRequestList()
{
lm's avatar
lm committed
982
    if (!uas) return;
983
    mavlink_message_t message;
lm's avatar
lm committed
984
    mavlink_mission_request_list_t wprl;
985

986
    wprl.target_system = uasid;
lm's avatar
lm committed
987
    wprl.target_component = MAV_COMP_ID_MISSIONPLANNER;
988

989 990 991
    QString statusMsg(tr("Requesting waypoint list..."));
    qDebug() << __FILE__ << __LINE__ << statusMsg;
    emit updateStatusString(statusMsg);
992

lm's avatar
lm committed
993
    mavlink_msg_mission_request_list_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wprl);
994
    uas->sendMessage(message);
lm's avatar
lm committed
995
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
996 997
}

998
void UASWaypointManager::sendWaypointRequest(quint16 seq)
pixhawk's avatar
pixhawk committed
999
{
lm's avatar
lm committed
1000
    if (!uas) return;
1001
    mavlink_message_t message;
lm's avatar
lm committed
1002
    mavlink_mission_request_t wpr;
1003

1004
    wpr.target_system = uasid;
lm's avatar
lm committed
1005
    wpr.target_component = MAV_COMP_ID_MISSIONPLANNER;
1006 1007
    wpr.seq = seq;

1008
    emit updateStatusString(QString("Retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count));
pixhawk's avatar
pixhawk committed
1009

lm's avatar
lm committed
1010
    mavlink_msg_mission_request_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpr);
1011
    uas->sendMessage(message);
lm's avatar
lm committed
1012
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
pixhawk's avatar
pixhawk committed
1013
}
pixhawk's avatar
pixhawk committed
1014

pixhawk's avatar
pixhawk committed
1015 1016
void UASWaypointManager::sendWaypoint(quint16 seq)
{
lm's avatar
lm committed
1017
    if (!uas) return;
pixhawk's avatar
pixhawk committed
1018
    mavlink_message_t message;
1019

1020
    if (seq < waypoint_buffer.count()) {
pixhawk's avatar
pixhawk committed
1021

lm's avatar
lm committed
1022
        mavlink_mission_item_t *wp;
1023

pixhawk's avatar
pixhawk committed
1024 1025

        wp = waypoint_buffer.at(seq);
1026
        wp->target_system = uasid;
lm's avatar
lm committed
1027
        wp->target_component = MAV_COMP_ID_MISSIONPLANNER;
pixhawk's avatar
pixhawk committed
1028

1029
        emit updateStatusString(QString("Sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
1030

pixhawk's avatar
pixhawk committed
1031

lm's avatar
lm committed
1032
        mavlink_msg_mission_item_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, wp);
1033
        uas->sendMessage(message);
lm's avatar
lm committed
1034
        QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
pixhawk's avatar
pixhawk committed
1035
    }
pixhawk's avatar
pixhawk committed
1036
}
1037 1038 1039

void UASWaypointManager::sendWaypointAck(quint8 type)
{
lm's avatar
lm committed
1040
    if (!uas) return;
1041
    mavlink_message_t message;
lm's avatar
lm committed
1042
    mavlink_mission_ack_t wpa;
1043

1044
    wpa.target_system = uasid;
lm's avatar
lm committed
1045
    wpa.target_component = MAV_COMP_ID_MISSIONPLANNER;
1046 1047
    wpa.type = type;

lm's avatar
lm committed
1048
    mavlink_msg_mission_ack_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpa);
1049
    uas->sendMessage(message);