UASWaypointManager.cc 27.3 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
//#include "MainWindow.h"
36

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

41
UASWaypointManager::UASWaypointManager(UAS &_uas)
42 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),
      protocol_timer(this)
pixhawk's avatar
pixhawk committed
50 51 52 53 54
{
    connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout()));
}

void UASWaypointManager::timeout()
55
{
56
    if (current_retries > 0) {
57 58
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries--;
59
        emit updateStatusString(tr("Timeout, retrying (retries left: %1)").arg(current_retries));
60
        // qDebug() << "Timeout, retrying (retries left:" << current_retries << ")";
61
        if (current_state == WP_GETLIST) {
62
            sendWaypointRequestList();
63
        } else if (current_state == WP_GETLIST_GETWPS) {
64
            sendWaypointRequest(current_wp_id);
65
        } else if (current_state == WP_SENDLIST) {
66
            sendWaypointCount();
67
        } else if (current_state == WP_SENDLIST_SENDWPS) {
68
            sendWaypoint(current_wp_id);
69
        } else if (current_state == WP_CLEARLIST) {
70
            sendWaypointClearAll();
71
        } else if (current_state == WP_SETCURRENT) {
72 73
            sendWaypointSetCurrent(current_wp_id);
        }
74
    } else {
75
        protocol_timer.stop();
pixhawk's avatar
pixhawk committed
76

77
        // qDebug() << "Waypoint transaction (state=" << current_state << ") timed out going to state WP_IDLE";
pixhawk's avatar
pixhawk committed
78

79
        emit updateStatusString("Operation timed out.");
pixhawk's avatar
pixhawk committed
80

81 82 83 84 85 86
        current_state = WP_IDLE;
        current_count = 0;
        current_wp_id = 0;
        current_partner_systemid = 0;
        current_partner_compid = 0;
    }
87 88 89 90
}

void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, quint16 count)
{
91
    if (current_state == WP_GETLIST && systemId == current_partner_systemid && compId == current_partner_compid) {
92 93 94
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;

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

97
        if (count > 0) {
pixhawk's avatar
pixhawk committed
98 99 100
            current_count = count;
            current_wp_id = 0;
            current_state = WP_GETLIST_GETWPS;
101

pixhawk's avatar
pixhawk committed
102
            sendWaypointRequest(current_wp_id);
103
        } else {
104
            protocol_timer.stop();
pixhawk's avatar
pixhawk committed
105
            emit updateStatusString("done.");
106
            // qDebug() << "No waypoints on UAS " << systemId;
107 108 109 110 111
            current_state = WP_IDLE;
            current_count = 0;
            current_wp_id = 0;
            current_partner_systemid = 0;
            current_partner_compid = 0;
pixhawk's avatar
pixhawk committed
112
        }
113 114
    } 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);
115
    }
116 117
}

pixhawk's avatar
pixhawk committed
118 119
void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp)
{
120
    if (systemId == current_partner_systemid && compId == current_partner_compid && current_state == WP_GETLIST_GETWPS && wp->seq == current_wp_id) {
121 122 123
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;

124
        if(wp->seq == current_wp_id) {
125
            //// 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;
lm's avatar
lm committed
126
            Waypoint *lwp = 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);
pixhawk's avatar
pixhawk committed
127
            addWaypoint(lwp, false);
pixhawk's avatar
pixhawk committed
128 129 130 131

            //get next waypoint
            current_wp_id++;

132
            if(current_wp_id < current_count) {
133
                sendWaypointRequest(current_wp_id);
134
            } else {
135 136
                sendWaypointAck(0);

pixhawk's avatar
pixhawk committed
137
                // all waypoints retrieved, change state to idle
pixhawk's avatar
pixhawk committed
138 139 140 141 142
                current_state = WP_IDLE;
                current_count = 0;
                current_wp_id = 0;
                current_partner_systemid = 0;
                current_partner_compid = 0;
143

pixhawk's avatar
pixhawk committed
144
                protocol_timer.stop();
145
                emit readGlobalWPFromUAS(false);
146 147
                emit updateStatusString("done.");

148
                // qDebug() << "got all waypoints from ID " << systemId;
pixhawk's avatar
pixhawk committed
149
            }
150
        } else {
151
            emit updateStatusString(tr("Waypoint ID mismatch, rejecting waypoint"));
pixhawk's avatar
pixhawk committed
152
        }
153 154
    } 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);
155
    }
pixhawk's avatar
pixhawk committed
156 157
}

158 159
void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa)
{
160 161
    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)) {
162 163
            //all waypoints sent and ack received
            protocol_timer.stop();
164
            current_state = WP_IDLE;
165
            emit updateStatusString("done.");
166
            // qDebug() << "sent all waypoints to ID " << systemId;
167
        } else if(current_state == WP_CLEARLIST) {
168 169 170
            protocol_timer.stop();
            current_state = WP_IDLE;
            emit updateStatusString("done.");
171
            // qDebug() << "cleared waypoint list of ID " << systemId;
172
        }
173 174 175
    }
}

pixhawk's avatar
pixhawk committed
176
void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr)
177
{
178
    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)))) {
179 180
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
181

182
        if (wpr->seq < waypoint_buffer.count()) {
pixhawk's avatar
pixhawk committed
183 184 185
            current_state = WP_SENDLIST_SENDWPS;
            current_wp_id = wpr->seq;
            sendWaypoint(current_wp_id);
186
        } else {
pixhawk's avatar
pixhawk committed
187 188
            //TODO: Error message or something
        }
189 190
    } 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);
191
    }
192 193
}

pixhawk's avatar
pixhawk committed
194
void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr)
195
{
196
    if (systemId == uas.getUASID() && compId == MAV_COMP_ID_WAYPOINTPLANNER) {
pixhawk's avatar
pixhawk committed
197 198
        emit updateStatusString(QString("Reached waypoint %1").arg(wpr->seq));
    }
pixhawk's avatar
pixhawk committed
199 200
}

201
void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_current_t *wpc)
pixhawk's avatar
pixhawk committed
202
{
203
    if (systemId == uas.getUASID() && compId == MAV_COMP_ID_WAYPOINTPLANNER) {
204
        // FIXME Petri
205
        if (current_state == WP_SETCURRENT) {
206 207
            protocol_timer.stop();
            current_state = WP_IDLE;
208 209

            // update the local main storage
210 211 212
            if (wpc->seq < waypoints.size()) {
                for(int i = 0; i < waypoints.size(); i++) {
                    if (waypoints[i]->getId() == wpc->seq) {
213
                        waypoints[i]->setCurrent(true);
214
                    } else {
215 216 217 218 219
                        waypoints[i]->setCurrent(false);
                    }
                }
            }

220
            //// qDebug() << "Updated waypoints list";
221
        }
222 223 224
        emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq));
        //emit update to UI widgets
        emit currentWaypointChanged(wpc->seq);
225
        //// qDebug() << "new current waypoint" << wpc->seq;
pixhawk's avatar
pixhawk committed
226
    }
pixhawk's avatar
pixhawk committed
227 228
}

229 230
void UASWaypointManager::notifyOfChange(Waypoint* wp)
{
231
    // qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId();
232
    // If only one waypoint was changed, emit only WP signal
233
    if (wp != NULL) {
234
        emit waypointChanged(uas.getUASID(), wp);
235
    } else {
236 237 238
        emit waypointListChanged();
        emit waypointListChanged(uas.getUASID());
    }
239 240
}

241
int UASWaypointManager::setCurrentWaypoint(quint16 seq)
pixhawk's avatar
pixhawk committed
242
{
243 244
    if (seq < waypoints.size()) {
        if(current_state == WP_IDLE) {
245
            //update local main storage
246 247
            for(int i = 0; i < waypoints.size(); i++) {
                if (waypoints[i]->getId() == seq) {
248
                    waypoints[i]->setCurrent(true);
249
                } else {
250 251 252
                    waypoints[i]->setCurrent(false);
                }
            }
253

254
            //TODO: signal changed waypoint list
255

256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274
            //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;
            current_partner_systemid = uas.getUASID();
            current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;

            sendWaypointSetCurrent(current_wp_id);

            //emit waypointListChanged();

            return 0;
        }
    }
    return -1;
}

pixhawk's avatar
pixhawk committed
275
/**
276
 * @warning Make sure the waypoint stays valid for the whole application lifecycle!
pixhawk's avatar
pixhawk committed
277
 * @param enforceFirstActive Enforces that the first waypoint is set as active
278
 * @see createWaypoint() is more suitable for most use cases
pixhawk's avatar
pixhawk committed
279 280
 */
void UASWaypointManager::addWaypoint(Waypoint *wp, bool enforceFirstActive)
281
{
282
    if (wp) {
283
        wp->setId(waypoints.size());
pixhawk's avatar
pixhawk committed
284
        if (enforceFirstActive && waypoints.size() == 0) wp->setCurrent(true);
285
        waypoints.insert(waypoints.size(), wp);
286
        connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*)));
287

288
        emit waypointListChanged();
289
        emit waypointListChanged(uas.getUASID());
290
    }
pixhawk's avatar
pixhawk committed
291 292
}

293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308
/**
 * @param enforceFirstActive Enforces that the first waypoint is set as active
 */
Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive)
{
    Waypoint* wp = new Waypoint();
    wp->setId(waypoints.size());
    if (enforceFirstActive && waypoints.size() == 0) wp->setCurrent(true);
    waypoints.insert(waypoints.size(), wp);
    connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*)));

    emit waypointListChanged();
    emit waypointListChanged(uas.getUASID());
    return wp;
}

309
int UASWaypointManager::removeWaypoint(quint16 seq)
310
{
311
    if (seq < waypoints.size()) {
312 313 314
        Waypoint *t = waypoints[seq];
        waypoints.remove(seq);
        delete t;
315
        t = NULL;
316

317
        for(int i = seq; i < waypoints.size(); i++) {
318 319
            waypoints[i]->setId(i);
        }
Alejandro's avatar
Alejandro committed
320

321
        emit waypointListChanged();
322
        emit waypointListChanged(uas.getUASID());
323 324 325 326 327
        return 0;
    }
    return -1;
}

328
void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
329
{
330
    if (cur_seq != new_seq && cur_seq < waypoints.size() && new_seq < waypoints.size()) {
331
        Waypoint *t = waypoints[cur_seq];
332 333
        if (cur_seq < new_seq) {
            for (int i = cur_seq; i < new_seq; i++) {
334 335 336
                waypoints[i] = waypoints[i+1];
                //waypoints[i]->setId(i);       // let the local IDs stay the same so that they can be found more easily by the user
            }
337 338
        } else {
            for (int i = cur_seq; i > new_seq; i--) {
339
                waypoints[i] = waypoints[i-1];
340
                // waypoints[i]->setId(i);
341 342 343 344 345 346
            }
        }
        waypoints[new_seq] = t;
        //waypoints[new_seq]->setId(new_seq);

        emit waypointListChanged();
347
        emit waypointListChanged(uas.getUASID());
348 349 350
    }
}

351
void UASWaypointManager::saveWaypoints(const QString &saveFile)
352 353 354 355 356 357
{
    QFile file(saveFile);
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
        return;

    QTextStream out(&file);
358 359

    //write the waypoint list version to the first line for compatibility check
lm's avatar
lm committed
360
    out << "QGC WPL 110\r\n";
361

362
    for (int i = 0; i < waypoints.size(); i++) {
363
        waypoints[i]->setId(i);
364 365 366 367 368
        waypoints[i]->save(out);
    }
    file.close();
}

369
void UASWaypointManager::loadWaypoints(const QString &loadFile)
370 371 372 373 374
{
    QFile file(loadFile);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
        return;

375
    while(waypoints.size()>0) {
376 377 378 379 380 381
        Waypoint *t = waypoints[0];
        waypoints.remove(0);
        delete t;
    }

    QTextStream in(&file);
382 383 384

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

385
    if (!(version.size() == 3 && version[0] == "QGC" && version[1] == "WPL" && version[2] == "110")) {
386 387
        emit updateStatusString(tr("The waypoint file is not compatible with the current version of QGroundControl."));
        //MainWindow::instance()->showCriticalMessage(tr("Error loading waypoint file"),tr("The waypoint file is not compatible with the current version of QGroundControl."));
388 389
    } else {
        while (!in.atEnd()) {
390
            Waypoint *t = new Waypoint();
391
            if(t->load(in)) {
392 393
                t->setId(waypoints.size());
                waypoints.insert(waypoints.size(), t);
394
            } else {
395 396
                emit updateStatusString(tr("The waypoint file is corrupted. Load operation only partly succesful."));
                //MainWindow::instance()->showCriticalMessage(tr("Error loading waypoint file"),);
397 398
                break;
            }
399 400
        }
    }
401

402 403
    file.close();

404
    emit loadWPFile();
405
    emit waypointListChanged();
406
    emit waypointListChanged(uas.getUASID());
407 408
}

409

410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427
//void UASWaypointManager::globalAddWaypoint(Waypoint *wp)
//{
//    // FIXME Will be removed
//    Q_UNUSED(wp);
//}

//int UASWaypointManager::globalRemoveWaypoint(quint16 seq)
//{
//    // FIXME Will be removed
//    Q_UNUSED(seq);
//    return 0;
//}

//void UASWaypointManager::waypointUpdated(Waypoint*)
//{
//    // FIXME Add rate limiter
//    emit waypointListChanged(uas.getUASID());
//}
pixhawk's avatar
pixhawk committed
428

429
void UASWaypointManager::clearWaypointList()
pixhawk's avatar
pixhawk committed
430
{
431
    if(current_state == WP_IDLE) {
pixhawk's avatar
pixhawk committed
432
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
433
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
434

435 436
        current_state = WP_CLEARLIST;
        current_wp_id = 0;
437 438
        current_partner_systemid = uas.getUASID();
        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
pixhawk's avatar
pixhawk committed
439

440
        sendWaypointClearAll();
441 442 443
    }
}

444 445 446 447 448 449
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;
450 451
    foreach (Waypoint* wp, waypoints) {
        if (wp->getFrame() == MAV_FRAME_GLOBAL) {
452 453 454 455 456 457
            wps.append(wp);
        }
    }
    return wps;
}

lm's avatar
lm committed
458 459 460 461 462 463
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;
464 465
    foreach (Waypoint* wp, waypoints) {
        if (wp->getFrame() == MAV_FRAME_GLOBAL && wp->isNavigationType()) {
lm's avatar
lm committed
466 467 468 469 470 471
            wps.append(wp);
        }
    }
    return wps;
}

LM's avatar
LM committed
472 473 474 475 476 477 478 479 480 481 482 483 484 485
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;
    foreach (Waypoint* wp, waypoints) {
        if (wp->isNavigationType()) {
            wps.append(wp);
        }
    }
    return wps;
}

486 487 488 489 490
int UASWaypointManager::getIndexOf(Waypoint* wp)
{
    return waypoints.indexOf(wp);
}

491 492 493 494 495
int UASWaypointManager::getGlobalFrameIndexOf(Waypoint* wp)
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
496 497 498
    foreach (Waypoint* p, waypoints) {
        if (p->getFrame() == MAV_FRAME_GLOBAL) {
            if (p == wp) {
499 500 501 502 503 504 505 506 507
                return i;
            }
            i++;
        }
    }

    return -1;
}

lm's avatar
lm committed
508 509 510 511 512
int UASWaypointManager::getGlobalFrameAndNavTypeIndexOf(Waypoint* wp)
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
513 514 515
    foreach (Waypoint* p, waypoints) {
        if (p->getFrame() == MAV_FRAME_GLOBAL && p->isNavigationType()) {
            if (p == wp) {
lm's avatar
lm committed
516 517 518 519 520 521 522 523 524
                return i;
            }
            i++;
        }
    }

    return -1;
}

LM's avatar
LM committed
525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541
int UASWaypointManager::getNavTypeIndexOf(Waypoint* wp)
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
    foreach (Waypoint* p, waypoints) {
        if (p->isNavigationType()) {
            if (p == wp) {
                return i;
            }
            i++;
        }
    }

    return -1;
}

542 543 544 545 546
int UASWaypointManager::getGlobalFrameCount()
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
547 548
    foreach (Waypoint* p, waypoints) {
        if (p->getFrame() == MAV_FRAME_GLOBAL) {
549 550 551 552 553 554 555
            i++;
        }
    }

    return i;
}

lm's avatar
lm committed
556 557 558 559 560
int UASWaypointManager::getGlobalFrameAndNavTypeCount()
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
561 562
    foreach (Waypoint* p, waypoints) {
        if (p->getFrame() == MAV_FRAME_GLOBAL && p->isNavigationType()) {
lm's avatar
lm committed
563 564 565 566 567 568 569
            i++;
        }
    }

    return i;
}

LM's avatar
LM committed
570 571 572 573 574 575 576 577 578 579 580 581 582 583
int UASWaypointManager::getNavTypeCount()
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
    foreach (Waypoint* p, waypoints) {
        if (p->isNavigationType()) {
            i++;
        }
    }

    return i;
}

584 585 586 587 588
int UASWaypointManager::getLocalFrameCount()
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
589 590
    foreach (Waypoint* p, waypoints) {
        if (p->getFrame() == MAV_FRAME_GLOBAL) {
591 592 593 594 595 596 597 598 599 600 601 602
            i++;
        }
    }

    return i;
}

int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp)
{
    // Search through all waypoints,
    // counting only those in local frame
    int i = 0;
603 604 605
    foreach (Waypoint* p, waypoints) {
        if (p->getFrame() == MAV_FRAME_LOCAL) {
            if (p == wp) {
606 607 608 609 610 611 612 613 614 615 616 617 618 619
                return i;
            }
            i++;
        }
    }

    return -1;
}

int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp)
{
    // Search through all waypoints,
    // counting only those in mission frame
    int i = 0;
620 621 622
    foreach (Waypoint* p, waypoints) {
        if (p->getFrame() == MAV_FRAME_MISSION) {
            if (p == wp) {
623 624 625 626 627 628 629 630 631
                return i;
            }
            i++;
        }
    }

    return -1;
}

632 633
void UASWaypointManager::readWaypoints()
{
634
    emit readGlobalWPFromUAS(true);
635 636
    if(current_state == WP_IDLE) {
        while(waypoints.size()>0) {
637
            delete waypoints.back();
638
            waypoints.pop_back();
639

640 641
        }

642 643
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
644 645 646 647 648 649

        current_state = WP_GETLIST;
        current_wp_id = 0;
        current_partner_systemid = uas.getUASID();
        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;

650
        sendWaypointRequestList();
651

pixhawk's avatar
pixhawk committed
652 653 654
    }
}

655
void UASWaypointManager::writeWaypoints()
pixhawk's avatar
pixhawk committed
656
{
657
    if (current_state == WP_IDLE) {
pixhawk's avatar
pixhawk committed
658
        // Send clear all if count == 0
659
        if (waypoints.count() > 0) {
pixhawk's avatar
pixhawk committed
660
            protocol_timer.start(PROTOCOL_TIMEOUT_MS);
661
            current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
662

663
            current_count = waypoints.count();
pixhawk's avatar
pixhawk committed
664 665 666 667 668 669
            current_state = WP_SENDLIST;
            current_wp_id = 0;
            current_partner_systemid = uas.getUASID();
            current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;

            //clear local buffer
pixhawk's avatar
pixhawk committed
670 671 672
            // 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.
673
            while(!waypoint_buffer.empty()) {
pixhawk's avatar
pixhawk committed
674 675 676
                delete waypoint_buffer.back();
                waypoint_buffer.pop_back();
            }
pixhawk's avatar
pixhawk committed
677

678 679
            bool noCurrent = true;

pixhawk's avatar
pixhawk committed
680
            //copy waypoint data to local buffer
681
            for (int i=0; i < current_count; i++) {
pixhawk's avatar
pixhawk committed
682 683 684
                waypoint_buffer.push_back(new mavlink_waypoint_t);
                mavlink_waypoint_t *cur_d = waypoint_buffer.back();
                memset(cur_d, 0, sizeof(mavlink_waypoint_t));   //initialize with zeros
685
                const Waypoint *cur_s = waypoints.at(i);
pixhawk's avatar
pixhawk committed
686 687

                cur_d->autocontinue = cur_s->getAutoContinue();
688
                cur_d->current = cur_s->getCurrent() & noCurrent;   //make sure only one current waypoint is selected, the first selected will be chosen
689 690
                cur_d->param1 = cur_s->getParam1();
                cur_d->param2 = cur_s->getParam2();
lm's avatar
lm committed
691 692
                cur_d->param3 = cur_s->getParam3();
                cur_d->param4 = cur_s->getParam4();
693
                cur_d->frame = cur_s->getFrame();
lm's avatar
lm committed
694
                cur_d->command = cur_s->getAction();
695
                cur_d->seq = i;     // don't read out the sequence number of the waypoint class
pixhawk's avatar
pixhawk committed
696 697 698
                cur_d->x = cur_s->getX();
                cur_d->y = cur_s->getY();
                cur_d->z = cur_s->getZ();
699 700 701

                if (cur_s->getCurrent() && noCurrent)
                    noCurrent = false;
702 703
                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
704
            }
705

706 707 708



pixhawk's avatar
pixhawk committed
709
            //send the waypoint count to UAS (this starts the send transaction)
710
            sendWaypointCount();
pixhawk's avatar
pixhawk committed
711
        }
712
    } else if (waypoints.count() == 0) {
713
        sendWaypointClearAll();
714
    } else {
715
        //we're in another transaction, ignore command
716
        // qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
717
    }
pixhawk's avatar
pixhawk committed
718 719
}

720 721 722 723 724 725 726 727
void UASWaypointManager::sendWaypointClearAll()
{
    mavlink_message_t message;
    mavlink_waypoint_clear_all_t wpca;

    wpca.target_system = uas.getUASID();
    wpca.target_component = MAV_COMP_ID_WAYPOINTPLANNER;

728
    emit updateStatusString(QString("Clearing waypoint list..."));
729 730 731

    mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca);
    uas.sendMessage(message);
732
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
733

734
    // qDebug() << "sent waypoint clear all to ID " << wpca.target_system;
735 736 737 738 739 740 741 742 743 744 745
}

void UASWaypointManager::sendWaypointSetCurrent(quint16 seq)
{
    mavlink_message_t message;
    mavlink_waypoint_set_current_t wpsc;

    wpsc.target_system = uas.getUASID();
    wpsc.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
    wpsc.seq = seq;

746
    emit updateStatusString(QString("Updating target waypoint..."));
747 748 749

    mavlink_msg_waypoint_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc);
    uas.sendMessage(message);
750
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
751

752
    // qDebug() << "sent waypoint set current (" << wpsc.seq << ") to ID " << wpsc.target_system;
753 754 755 756 757 758 759 760 761 762 763
}

void UASWaypointManager::sendWaypointCount()
{
    mavlink_message_t message;
    mavlink_waypoint_count_t wpc;

    wpc.target_system = uas.getUASID();
    wpc.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
    wpc.count = current_count;

764
    // qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
765
    emit updateStatusString(QString("Starting to transmit waypoints..."));
766 767 768

    mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc);
    uas.sendMessage(message);
769
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
770

771
    // qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
772 773 774 775 776 777 778 779 780 781
}

void UASWaypointManager::sendWaypointRequestList()
{
    mavlink_message_t message;
    mavlink_waypoint_request_list_t wprl;

    wprl.target_system = uas.getUASID();
    wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER;

782
    emit updateStatusString(QString("Requesting waypoint list..."));
783 784 785

    mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl);
    uas.sendMessage(message);
786
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
787

788
    // qDebug() << "sent waypoint list request to ID " << wprl.target_system;
789 790


791 792
}

793
void UASWaypointManager::sendWaypointRequest(quint16 seq)
pixhawk's avatar
pixhawk committed
794
{
795 796 797 798 799 800 801
    mavlink_message_t message;
    mavlink_waypoint_request_t wpr;

    wpr.target_system = uas.getUASID();
    wpr.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
    wpr.seq = seq;

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

804 805
    mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr);
    uas.sendMessage(message);
806
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
807

808
    // qDebug() << "sent waypoint request (" << wpr.seq << ") to ID " << wpr.target_system;
pixhawk's avatar
pixhawk committed
809
}
pixhawk's avatar
pixhawk committed
810

pixhawk's avatar
pixhawk committed
811 812 813
void UASWaypointManager::sendWaypoint(quint16 seq)
{
    mavlink_message_t message;
814
    // qDebug() <<" WP Buffer count: "<<waypoint_buffer.count();
pixhawk's avatar
pixhawk committed
815

816
    if (seq < waypoint_buffer.count()) {
817

pixhawk's avatar
pixhawk committed
818 819
        mavlink_waypoint_t *wp;

820

pixhawk's avatar
pixhawk committed
821 822 823 824
        wp = waypoint_buffer.at(seq);
        wp->target_system = uas.getUASID();
        wp->target_component = MAV_COMP_ID_WAYPOINTPLANNER;

825
        emit updateStatusString(QString("Sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
pixhawk's avatar
pixhawk committed
826

827
        // qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system<<" WP Buffer count: "<<waypoint_buffer.count();
828

pixhawk's avatar
pixhawk committed
829 830
        mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp);
        uas.sendMessage(message);
831
        MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
pixhawk's avatar
pixhawk committed
832 833

    }
pixhawk's avatar
pixhawk committed
834
}
835 836 837 838 839 840 841 842 843 844 845 846

void UASWaypointManager::sendWaypointAck(quint8 type)
{
    mavlink_message_t message;
    mavlink_waypoint_ack_t wpa;

    wpa.target_system = uas.getUASID();
    wpa.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
    wpa.type = type;

    mavlink_msg_waypoint_ack_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpa);
    uas.sendMessage(message);
847
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
848

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