UASWaypointManager.cc 32.8 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

lm's avatar
lm committed
5
(c) 2009, 2010 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

36
#define PROTOCOL_TIMEOUT_MS 2000    ///< maximum time to wait for pending messages until timeout
lm's avatar
lm committed
37 38
#define PROTOCOL_DELAY_MS 20        ///< minimum delay between sent messages
#define PROTOCOL_MAX_RETRIES 5      ///< maximum number of send retries (after timeout)
pixhawk's avatar
pixhawk committed
39

lm's avatar
lm committed
40
UASWaypointManager::UASWaypointManager(UAS* _uas)
41 42 43 44 45 46 47
    : uas(_uas),
      current_retries(0),
      current_wp_id(0),
      current_count(0),
      current_state(WP_IDLE),
      current_partner_systemid(0),
      current_partner_compid(0),
48 49
      currentWaypointEditable(NULL),
      protocol_timer(this)
pixhawk's avatar
pixhawk committed
50
{
51 52
    if (uas)
    {
lm's avatar
lm committed
53 54 55 56 57 58 59 60
        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;
61
    }
pixhawk's avatar
pixhawk committed
62 63
}

LM's avatar
LM committed
64 65 66 67 68
UASWaypointManager::~UASWaypointManager()
{

}

pixhawk's avatar
pixhawk committed
69
void UASWaypointManager::timeout()
70
{
71
    if (current_retries > 0) {
72 73
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries--;
74
        emit updateStatusString(tr("Timeout, retrying (retries left: %1)").arg(current_retries));
75
        // // qDebug() << "Timeout, retrying (retries left:" << current_retries << ")";
76
        if (current_state == WP_GETLIST) {
77
            sendWaypointRequestList();
78
        } else if (current_state == WP_GETLIST_GETWPS) {
79
            sendWaypointRequest(current_wp_id);
80
        } else if (current_state == WP_SENDLIST) {
81
            sendWaypointCount();
82
        } else if (current_state == WP_SENDLIST_SENDWPS) {
83
            sendWaypoint(current_wp_id);
84
        } else if (current_state == WP_CLEARLIST) {
85
            sendWaypointClearAll();
86
        } else if (current_state == WP_SETCURRENT) {
87 88
            sendWaypointSetCurrent(current_wp_id);
        }
89
    } else {
90
        protocol_timer.stop();
pixhawk's avatar
pixhawk committed
91

92
        // // qDebug() << "Waypoint transaction (state=" << current_state << ") timed out going to state WP_IDLE";
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 && compId == current_partner_compid) {
136 137 138
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;

139
        // // qDebug() << "got waypoint count (" << count << ") from ID " << systemId;
pixhawk's avatar
pixhawk committed
140

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

151
        if (count > 0) {
pixhawk's avatar
pixhawk committed
152 153 154 155
            current_count = count;
            current_wp_id = 0;
            current_state = WP_GETLIST_GETWPS;
            sendWaypointRequest(current_wp_id);
156
        } else {
157
            protocol_timer.stop();
pixhawk's avatar
pixhawk committed
158
            emit updateStatusString("done.");
159
            // // qDebug() << "No waypoints on UAS " << systemId;
160 161 162 163 164
            current_state = WP_IDLE;
            current_count = 0;
            current_wp_id = 0;
            current_partner_systemid = 0;
            current_partner_compid = 0;
pixhawk's avatar
pixhawk committed
165
        }
166 167


168 169
    } 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);
170
    }
171 172
}

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

179
        if(wp->seq == current_wp_id) {
180
            //// // qDebug() << "Got WP: " << wp->seq << wp->x <<  wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << "Frame:"<< (MAV_FRAME) wp->frame << "Command:" << (MAV_CMD) wp->command;
pixhawk's avatar
pixhawk committed
181 182 183 184 185 186 187 188 189 190 191

            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
192 193 194 195

            //get next waypoint
            current_wp_id++;

196
            if(current_wp_id < current_count) {
197
                sendWaypointRequest(current_wp_id);
198
            } else {
199 200
                sendWaypointAck(0);

pixhawk's avatar
pixhawk committed
201
                // all waypoints retrieved, change state to idle
pixhawk's avatar
pixhawk committed
202 203 204 205 206
                current_state = WP_IDLE;
                current_count = 0;
                current_wp_id = 0;
                current_partner_systemid = 0;
                current_partner_compid = 0;
207

pixhawk's avatar
pixhawk committed
208
                protocol_timer.stop();
209
                emit readGlobalWPFromUAS(false);
210
                //if (currentWaypointEditable) emit currentWaypointChanged(currentWaypointEditable->getId());
211 212
                emit updateStatusString("done.");

213
                // // qDebug() << "got all waypoints from ID " << systemId;
pixhawk's avatar
pixhawk committed
214
            }
215
        } else {
216
            emit updateStatusString(tr("Waypoint ID mismatch, rejecting waypoint"));
pixhawk's avatar
pixhawk committed
217
        }
218 219
    } 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);
220
    }
pixhawk's avatar
pixhawk committed
221 222
}

lm's avatar
lm committed
223
void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_mission_ack_t *wpa)
224
{
225 226
    if (systemId == current_partner_systemid && compId == current_partner_compid) {
        if((current_state == WP_SENDLIST || current_state == WP_SENDLIST_SENDWPS) && (current_wp_id == waypoint_buffer.count()-1 && wpa->type == 0)) {
227 228
            //all waypoints sent and ack received
            protocol_timer.stop();
229
            current_state = WP_IDLE;
230
            readWaypoints(false); //Update "Onboard Waypoints"-tab immidiately after the waypoint list has been sent.
231
            emit updateStatusString("done.");
232
            // // qDebug() << "sent all waypoints to ID " << systemId;
233
        } else if(current_state == WP_CLEARLIST) {
234 235 236
            protocol_timer.stop();
            current_state = WP_IDLE;
            emit updateStatusString("done.");
237
            // // qDebug() << "cleared waypoint list of ID " << systemId;
238
        }
239 240 241
    }
}

lm's avatar
lm committed
242
void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_mission_request_t *wpr)
243
{
244
    if (systemId == current_partner_systemid && compId == current_partner_compid && ((current_state == WP_SENDLIST && wpr->seq == 0) || (current_state == WP_SENDLIST_SENDWPS && (wpr->seq == current_wp_id || wpr->seq == current_wp_id + 1)))) {
245 246
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
247

248
        if (wpr->seq < waypoint_buffer.count()) {
pixhawk's avatar
pixhawk committed
249 250 251
            current_state = WP_SENDLIST_SENDWPS;
            current_wp_id = wpr->seq;
            sendWaypoint(current_wp_id);
252
        } else {
pixhawk's avatar
pixhawk committed
253 254
            //TODO: Error message or something
        }
255 256
    } 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);
257
    }
258 259
}

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

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

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

297
void UASWaypointManager::notifyOfChangeEditable(Waypoint* wp)
298
{
299
    // // qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId();
300
    // If only one waypoint was changed, emit only WP signal
301
    if (wp != NULL) {
302
        emit waypointEditableChanged(uasid, wp);
303
    } else {
304
        emit waypointEditableListChanged();
305
        emit waypointEditableListChanged(uasid);
306
    }
307 308
}

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


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

325 326 327 328 329 330
            //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;
331
            current_partner_systemid = uasid;
lm's avatar
lm committed
332
            current_partner_compid = MAV_COMP_ID_MISSIONPLANNER;
333 334 335 336 337 338 339 340 341 342 343

            sendWaypointSetCurrent(current_wp_id);

            //emit waypointListChanged();

            return 0;
        }
    }
    return -1;
}

344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362
int UASWaypointManager::setCurrentEditable(quint16 seq)
{
    if (seq < waypointsEditable.size()) {
        if(current_state == WP_IDLE) {
            //update local main storage
            for(int i = 0; i < waypointsEditable.size(); i++) {
                if (waypointsEditable[i]->getId() == seq) {
                    waypointsEditable[i]->setCurrent(true);
                    //currentWaypointEditable = waypoints[i];
                } 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)
    {
pixhawk's avatar
pixhawk committed
386 387
        wp->setId(waypointsEditable.size());
        if (enforceFirstActive && waypointsEditable.size() == 0)
388 389
        {
            wp->setCurrent(true);
pixhawk's avatar
pixhawk committed
390
            currentWaypointEditable = wp;
391
        }
pixhawk's avatar
pixhawk committed
392
        waypointsEditable.insert(waypointsEditable.size(), wp);
393
        connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*)));
394

395
        emit waypointEditableListChanged();
396
        emit waypointEditableListChanged(uasid);
397
    }
pixhawk's avatar
pixhawk committed
398 399
}

400 401 402 403 404 405
/**
 * @param enforceFirstActive Enforces that the first waypoint is set as active
 */
Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive)
{
    Waypoint* wp = new Waypoint();
pixhawk's avatar
pixhawk committed
406 407
    wp->setId(waypointsEditable.size());
    if (enforceFirstActive && waypointsEditable.size() == 0)
408 409
    {
        wp->setCurrent(true);
pixhawk's avatar
pixhawk committed
410
        currentWaypointEditable = wp;
411
    }
pixhawk's avatar
pixhawk committed
412
    waypointsEditable.insert(waypointsEditable.size(), wp);
413
    connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*)));
414

415
    emit waypointEditableListChanged();
416
    emit waypointEditableListChanged(uasid);
417 418 419
    return wp;
}

420
int UASWaypointManager::removeWaypoint(quint16 seq)
421
{
pixhawk's avatar
pixhawk committed
422
    if (seq < waypointsEditable.size())
423
    {
pixhawk's avatar
pixhawk committed
424
        Waypoint *t = waypointsEditable[seq];
425 426 427 428 429 430 431 432 433 434 435 436 437

        if (t->getCurrent() == true) //trying to remove the current waypoint
        {
            if (seq+1 < waypointsEditable.size()) // 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
            {
                waypointsEditable[seq-1]->setCurrent(true);
            }
        }

pixhawk's avatar
pixhawk committed
438
        waypointsEditable.remove(seq);
439
        delete t;
440
        t = NULL;
441

pixhawk's avatar
pixhawk committed
442
        for(int i = seq; i < waypointsEditable.size(); i++)
443
        {
pixhawk's avatar
pixhawk committed
444
            waypointsEditable[i]->setId(i);
445
        }
Alejandro's avatar
Alejandro committed
446

447
        emit waypointEditableListChanged();
448
        emit waypointEditableListChanged(uasid);
449 450 451 452 453
        return 0;
    }
    return -1;
}

454
void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
455
{
pixhawk's avatar
pixhawk committed
456
    if (cur_seq != new_seq && cur_seq < waypointsEditable.size() && new_seq < waypointsEditable.size())
457
    {
pixhawk's avatar
pixhawk committed
458
        Waypoint *t = waypointsEditable[cur_seq];
459
        if (cur_seq < new_seq) {
460 461
            for (int i = cur_seq; i < new_seq; i++)
            {
pixhawk's avatar
pixhawk committed
462
                waypointsEditable[i] = waypointsEditable[i+1];
463
                waypointsEditable[i]->setId(i);
464
            }
465 466 467 468 469
        }
        else
        {
            for (int i = cur_seq; i > new_seq; i--)
            {
pixhawk's avatar
pixhawk committed
470
                waypointsEditable[i] = waypointsEditable[i-1];
471
                waypointsEditable[i]->setId(i);
472 473
            }
        }
pixhawk's avatar
pixhawk committed
474
        waypointsEditable[new_seq] = t;
475
        waypointsEditable[new_seq]->setId(new_seq);
476

477
        emit waypointEditableListChanged();
478
        emit waypointEditableListChanged(uasid);
479 480 481
    }
}

482
void UASWaypointManager::saveWaypoints(const QString &saveFile)
483 484 485 486 487 488
{
    QFile file(saveFile);
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
        return;

    QTextStream out(&file);
489 490

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

pixhawk's avatar
pixhawk committed
493
    for (int i = 0; i < waypointsEditable.size(); i++)
494
    {
pixhawk's avatar
pixhawk committed
495 496
        waypointsEditable[i]->setId(i);
        waypointsEditable[i]->save(out);
497 498 499 500
    }
    file.close();
}

501
void UASWaypointManager::loadWaypoints(const QString &loadFile)
502 503 504 505 506
{
    QFile file(loadFile);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
        return;

pixhawk's avatar
pixhawk committed
507 508 509
    while(waypointsEditable.size()>0) {
        Waypoint *t = waypointsEditable[0];
        waypointsEditable.remove(0);
510 511 512 513
        delete t;
    }

    QTextStream in(&file);
514 515 516

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

517
    if (!(version.size() == 3 && version[0] == "QGC" && version[1] == "WPL" && version[2] == "120"))
518
    {
519
        emit updateStatusString(tr("The waypoint file is not compatible with the current version of QGroundControl."));
520 521 522 523 524
    }
    else
    {
        while (!in.atEnd())
        {
525
            Waypoint *t = new Waypoint();
526 527
            if(t->load(in))
            {
pixhawk's avatar
pixhawk committed
528 529
                t->setId(waypointsEditable.size());
                waypointsEditable.insert(waypointsEditable.size(), t);
530 531 532
            }
            else
            {
533 534
                emit updateStatusString(tr("The waypoint file is corrupted. Load operation only partly succesful."));
                //MainWindow::instance()->showCriticalMessage(tr("Error loading waypoint file"),);
535 536
                break;
            }
537 538
        }
    }
539

540 541
    file.close();

542
    emit loadWPFile();
543
    emit waypointEditableListChanged();
544
    emit waypointEditableListChanged(uasid);
545 546 547
}

void UASWaypointManager::clearWaypointList()
pixhawk's avatar
pixhawk committed
548
{
549 550
    if(current_state == WP_IDLE)
    {
pixhawk's avatar
pixhawk committed
551
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
552
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
553

554 555
        current_state = WP_CLEARLIST;
        current_wp_id = 0;
556
        current_partner_systemid = uasid;
lm's avatar
lm committed
557
        current_partner_compid = MAV_COMP_ID_MISSIONPLANNER;
pixhawk's avatar
pixhawk committed
558

559
        sendWaypointClearAll();
560 561 562
    }
}

563 564 565 566 567 568
const QVector<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;
pixhawk's avatar
pixhawk committed
569
    foreach (Waypoint* wp, waypointsEditable)
570 571 572
    {
        if (wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)
        {
573 574 575 576 577 578
            wps.append(wp);
        }
    }
    return wps;
}

lm's avatar
lm committed
579 580 581 582 583 584
const QVector<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;
pixhawk's avatar
pixhawk committed
585
    foreach (Waypoint* wp, waypointsEditable)
586 587 588
    {
        if ((wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && wp->isNavigationType())
        {
lm's avatar
lm committed
589 590 591 592 593 594
            wps.append(wp);
        }
    }
    return wps;
}

LM's avatar
LM committed
595 596 597 598 599 600
const QVector<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;
pixhawk's avatar
pixhawk committed
601
    foreach (Waypoint* wp, waypointsEditable)
602 603 604
    {
        if (wp->isNavigationType())
        {
LM's avatar
LM committed
605 606 607 608 609 610
            wps.append(wp);
        }
    }
    return wps;
}

611 612
int UASWaypointManager::getIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
613
    return waypointsEditable.indexOf(wp);
614 615
}

616 617
int UASWaypointManager::getGlobalFrameIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
618
    // Search through all waypointsEditable,
619 620
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
621
    foreach (Waypoint* p, waypointsEditable) {
622 623 624 625
        if (p->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)
        {
            if (p == wp)
            {
626 627 628 629 630 631 632 633 634
                return i;
            }
            i++;
        }
    }

    return -1;
}

lm's avatar
lm committed
635 636
int UASWaypointManager::getGlobalFrameAndNavTypeIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
637
    // Search through all waypointsEditable,
lm's avatar
lm committed
638 639
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
640
    foreach (Waypoint* p, waypointsEditable) {
641
        if ((p->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && p->isNavigationType())
642 643 644
        {
            if (p == wp)
            {
lm's avatar
lm committed
645 646 647 648 649 650 651 652 653
                return i;
            }
            i++;
        }
    }

    return -1;
}

LM's avatar
LM committed
654 655
int UASWaypointManager::getNavTypeIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
656
    // Search through all waypointsEditable,
LM's avatar
LM committed
657 658
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
659
    foreach (Waypoint* p, waypointsEditable)
660 661 662 663 664
    {
        if (p->isNavigationType())
        {
            if (p == wp)
            {
LM's avatar
LM committed
665 666 667 668 669 670 671 672 673
                return i;
            }
            i++;
        }
    }

    return -1;
}

674 675
int UASWaypointManager::getGlobalFrameCount()
{
pixhawk's avatar
pixhawk committed
676
    // Search through all waypointsEditable,
677 678
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
679
    foreach (Waypoint* p, waypointsEditable)
680
    {
681 682
        if (p->getFrame() == MAV_FRAME_GLOBAL || p->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)
        {
683 684 685 686 687 688 689
            i++;
        }
    }

    return i;
}

lm's avatar
lm committed
690 691
int UASWaypointManager::getGlobalFrameAndNavTypeCount()
{
pixhawk's avatar
pixhawk committed
692
    // Search through all waypointsEditable,
lm's avatar
lm committed
693 694
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
695
    foreach (Waypoint* p, waypointsEditable) {
696
        if ((p->getFrame() == MAV_FRAME_GLOBAL || p->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && p->isNavigationType())
697
        {
lm's avatar
lm committed
698 699 700 701 702 703 704
            i++;
        }
    }

    return i;
}

LM's avatar
LM committed
705 706
int UASWaypointManager::getNavTypeCount()
{
pixhawk's avatar
pixhawk committed
707
    // Search through all waypointsEditable,
LM's avatar
LM committed
708 709
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
710
    foreach (Waypoint* p, waypointsEditable) {
LM's avatar
LM committed
711 712 713 714 715 716 717 718
        if (p->isNavigationType()) {
            i++;
        }
    }

    return i;
}

719 720
int UASWaypointManager::getLocalFrameCount()
{
pixhawk's avatar
pixhawk committed
721
    // Search through all waypointsEditable,
722 723
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
724
    foreach (Waypoint* p, waypointsEditable)
lm's avatar
lm committed
725
    {
726
        if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU)
lm's avatar
lm committed
727
        {
728 729 730 731 732 733 734 735 736
            i++;
        }
    }

    return i;
}

int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
737
    // Search through all waypointsEditable,
738 739
    // counting only those in local frame
    int i = 0;
pixhawk's avatar
pixhawk committed
740
    foreach (Waypoint* p, waypointsEditable)
lm's avatar
lm committed
741 742 743
    {
        if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU)
        {
744 745
            if (p == wp)
            {
746 747 748 749 750 751 752 753 754 755 756
                return i;
            }
            i++;
        }
    }

    return -1;
}

int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
757
    // Search through all waypointsEditable,
758 759
    // counting only those in mission frame
    int i = 0;
pixhawk's avatar
pixhawk committed
760
    foreach (Waypoint* p, waypointsEditable)
lm's avatar
lm committed
761
    {
762 763 764 765
        if (p->getFrame() == MAV_FRAME_MISSION)
        {
            if (p == wp)
            {
766 767 768 769 770 771 772 773 774
                return i;
            }
            i++;
        }
    }

    return -1;
}

pixhawk's avatar
pixhawk committed
775 776 777 778 779

/**
 * @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)
780
{
pixhawk's avatar
pixhawk committed
781
    read_to_edit = readToEdit;
782
    emit readGlobalWPFromUAS(true);
783
    if(current_state == WP_IDLE) {
784

785

786 787
        //Clear the old view-list before receiving the new one
        while(waypointsViewOnly.size()>0) {
788 789 790
            Waypoint *t = waypointsViewOnly[0];
            waypointsViewOnly.remove(0);
            delete t;
791
        }
792 793
        emit waypointViewOnlyListChanged();
        /* THIS PART WAS MOVED TO handleWaypointCount. THE EDIT-LIST SHOULD NOT BE CLEARED UNLESS THERE IS A RESPONSE FROM UAV.
794 795 796
        //Clear the old edit-list before receiving the new one
        if (read_to_edit == true){
            while(waypointsEditable.size()>0) {
797 798 799
                Waypoint *t = waypointsEditable[0];
                waypointsEditable.remove(0);
                delete t;
800
            }
801
            emit waypointEditableListChanged();
802
        }
803
        */
804 805
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
806 807 808

        current_state = WP_GETLIST;
        current_wp_id = 0;
809
        current_partner_systemid = uasid;
lm's avatar
lm committed
810
        current_partner_compid = MAV_COMP_ID_MISSIONPLANNER;
pixhawk's avatar
pixhawk committed
811

812
        sendWaypointRequestList();
813

pixhawk's avatar
pixhawk committed
814 815 816
    }
}

817
void UASWaypointManager::writeWaypoints()
pixhawk's avatar
pixhawk committed
818
{
819
    if (current_state == WP_IDLE) {
pixhawk's avatar
pixhawk committed
820
        // Send clear all if count == 0
pixhawk's avatar
pixhawk committed
821
        if (waypointsEditable.count() > 0) {
pixhawk's avatar
pixhawk committed
822
            protocol_timer.start(PROTOCOL_TIMEOUT_MS);
823
            current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
824

pixhawk's avatar
pixhawk committed
825
            current_count = waypointsEditable.count();
pixhawk's avatar
pixhawk committed
826 827
            current_state = WP_SENDLIST;
            current_wp_id = 0;
828
            current_partner_systemid = uasid;
lm's avatar
lm committed
829
            current_partner_compid = MAV_COMP_ID_MISSIONPLANNER;
pixhawk's avatar
pixhawk committed
830 831

            //clear local buffer
pixhawk's avatar
pixhawk committed
832 833 834
            // 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.
835
            while(!waypoint_buffer.empty()) {
pixhawk's avatar
pixhawk committed
836 837 838
                delete waypoint_buffer.back();
                waypoint_buffer.pop_back();
            }
pixhawk's avatar
pixhawk committed
839

840 841
            bool noCurrent = true;

pixhawk's avatar
pixhawk committed
842
            //copy waypoint data to local buffer
843
            for (int i=0; i < current_count; i++) {
lm's avatar
lm committed
844 845 846
                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
847
                const Waypoint *cur_s = waypointsEditable.at(i);
pixhawk's avatar
pixhawk committed
848 849

                cur_d->autocontinue = cur_s->getAutoContinue();
850
                cur_d->current = cur_s->getCurrent() & noCurrent;   //make sure only one current waypoint is selected, the first selected will be chosen
851 852
                cur_d->param1 = cur_s->getParam1();
                cur_d->param2 = cur_s->getParam2();
lm's avatar
lm committed
853 854
                cur_d->param3 = cur_s->getParam3();
                cur_d->param4 = cur_s->getParam4();
855
                cur_d->frame = cur_s->getFrame();
lm's avatar
lm committed
856
                cur_d->command = cur_s->getAction();
857
                cur_d->seq = i;     // don't read out the sequence number of the waypoint class
pixhawk's avatar
pixhawk committed
858 859 860
                cur_d->x = cur_s->getX();
                cur_d->y = cur_s->getY();
                cur_d->z = cur_s->getZ();
861 862 863

                if (cur_s->getCurrent() && noCurrent)
                    noCurrent = false;
864 865
                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
866
            }
867

868 869 870



pixhawk's avatar
pixhawk committed
871
            //send the waypoint count to UAS (this starts the send transaction)
872
            sendWaypointCount();
pixhawk's avatar
pixhawk committed
873
        }
pixhawk's avatar
pixhawk committed
874
    } else if (waypointsEditable.count() == 0) {
875
        sendWaypointClearAll();
876
    } else {
877
        //we're in another transaction, ignore command
878
        qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
879
    }
pixhawk's avatar
pixhawk committed
880 881
}

882 883
void UASWaypointManager::sendWaypointClearAll()
{
lm's avatar
lm committed
884
    if (!uas) return;
885
    mavlink_message_t message;
lm's avatar
lm committed
886
    mavlink_mission_clear_all_t wpca;
887

888
    wpca.target_system = uasid;
lm's avatar
lm committed
889
    wpca.target_component = MAV_COMP_ID_MISSIONPLANNER;
890

891
    emit updateStatusString(QString("Clearing waypoint list..."));
892

lm's avatar
lm committed
893 894 895
    mavlink_msg_mission_clear_all_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpca);
    if (uas) uas->sendMessage(message);
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
896

897
    // // qDebug() << "sent waypoint clear all to ID " << wpca.target_system;
898 899 900 901
}

void UASWaypointManager::sendWaypointSetCurrent(quint16 seq)
{
lm's avatar
lm committed
902
    if (!uas) return;
903
    mavlink_message_t message;
lm's avatar
lm committed
904
    mavlink_mission_set_current_t wpsc;
905

906
    wpsc.target_system = uasid;
lm's avatar
lm committed
907
    wpsc.target_component = MAV_COMP_ID_MISSIONPLANNER;
908 909
    wpsc.seq = seq;

910
    emit updateStatusString(QString("Updating target waypoint..."));
911

lm's avatar
lm committed
912 913 914
    mavlink_msg_mission_set_current_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpsc);
    if (uas) uas->sendMessage(message);
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
915

916
    // // qDebug() << "sent waypoint set current (" << wpsc.seq << ") to ID " << wpsc.target_system;
917 918 919 920
}

void UASWaypointManager::sendWaypointCount()
{
lm's avatar
lm committed
921
    if (!uas) return;
922
    mavlink_message_t message;
lm's avatar
lm committed
923
    mavlink_mission_count_t wpc;
924

925
    wpc.target_system = uasid;
lm's avatar
lm committed
926
    wpc.target_component = MAV_COMP_ID_MISSIONPLANNER;
927 928
    wpc.count = current_count;

929
    // // qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
930
    emit updateStatusString(QString("Starting to transmit waypoints..."));
931

lm's avatar
lm committed
932 933 934
    mavlink_msg_mission_count_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpc);
    if (uas) uas->sendMessage(message);
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
935

936
    // // qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
937 938 939 940
}

void UASWaypointManager::sendWaypointRequestList()
{
lm's avatar
lm committed
941
    if (!uas) return;
942
    mavlink_message_t message;
lm's avatar
lm committed
943
    mavlink_mission_request_list_t wprl;
944

945
    wprl.target_system = uasid;
lm's avatar
lm committed
946
    wprl.target_component = MAV_COMP_ID_MISSIONPLANNER;
947

948
    emit updateStatusString(QString("Requesting waypoint list..."));
949

lm's avatar
lm committed
950 951 952
    mavlink_msg_mission_request_list_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wprl);
    if (uas) uas->sendMessage(message);
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
953

954
    // // qDebug() << "sent waypoint list request to ID " << wprl.target_system;
955 956


957 958
}

959
void UASWaypointManager::sendWaypointRequest(quint16 seq)
pixhawk's avatar
pixhawk committed
960
{
lm's avatar
lm committed
961
    if (!uas) return;
962
    mavlink_message_t message;
lm's avatar
lm committed
963
    mavlink_mission_request_t wpr;
964

965
    wpr.target_system = uasid;
lm's avatar
lm committed
966
    wpr.target_component = MAV_COMP_ID_MISSIONPLANNER;
967 968
    wpr.seq = seq;

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

lm's avatar
lm committed
971 972 973
    mavlink_msg_mission_request_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpr);
    if (uas) uas->sendMessage(message);
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
974

975
    // // qDebug() << "sent waypoint request (" << wpr.seq << ") to ID " << wpr.target_system;
pixhawk's avatar
pixhawk committed
976
}
pixhawk's avatar
pixhawk committed
977

pixhawk's avatar
pixhawk committed
978 979
void UASWaypointManager::sendWaypoint(quint16 seq)
{
lm's avatar
lm committed
980
    if (!uas) return;
pixhawk's avatar
pixhawk committed
981
    mavlink_message_t message;
982
    // // qDebug() <<" WP Buffer count: "<<waypoint_buffer.count();
983

984
    if (seq < waypoint_buffer.count()) {
pixhawk's avatar
pixhawk committed
985

lm's avatar
lm committed
986
        mavlink_mission_item_t *wp;
987

pixhawk's avatar
pixhawk committed
988 989

        wp = waypoint_buffer.at(seq);
990
        wp->target_system = uasid;
lm's avatar
lm committed
991
        wp->target_component = MAV_COMP_ID_MISSIONPLANNER;
pixhawk's avatar
pixhawk committed
992

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

995
        // // qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system<<" WP Buffer count: "<<waypoint_buffer.count();
pixhawk's avatar
pixhawk committed
996

lm's avatar
lm committed
997 998 999
        mavlink_msg_mission_item_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, wp);
        if (uas) uas->sendMessage(message);
        QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
pixhawk's avatar
pixhawk committed
1000
    }
pixhawk's avatar
pixhawk committed
1001
}
1002 1003 1004

void UASWaypointManager::sendWaypointAck(quint8 type)
{
lm's avatar
lm committed
1005
    if (!uas) return;
1006
    mavlink_message_t message;
lm's avatar
lm committed
1007
    mavlink_mission_ack_t wpa;
1008

1009
    wpa.target_system = uasid;
lm's avatar
lm committed
1010
    wpa.target_component = MAV_COMP_ID_MISSIONPLANNER;
1011 1012
    wpa.type = type;

lm's avatar
lm committed
1013 1014 1015
    mavlink_msg_mission_ack_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpa);
    if (uas) uas->sendMessage(message);
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
1016

1017
    // // qDebug() << "sent waypoint ack (" << wpa.type << ") to ID " << wpa.target_system;
1018
}