UASWaypointManager.cc 26.2 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 106
            emit updateStatusString("done.");
            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.");

pixhawk's avatar
pixhawk committed
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 166
            emit updateStatusString("done.");
            qDebug() << "sent all waypoints to ID " << systemId;
167
        } else if(current_state == WP_CLEARLIST) {
168 169 170 171 172
            protocol_timer.stop();
            current_state = WP_IDLE;
            emit updateStatusString("done.");
            qDebug() << "cleared waypoint list of ID " << systemId;
        }
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;
}

472 473 474 475 476
int UASWaypointManager::getIndexOf(Waypoint* wp)
{
    return waypoints.indexOf(wp);
}

477 478 479 480 481
int UASWaypointManager::getGlobalFrameIndexOf(Waypoint* wp)
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
482 483 484
    foreach (Waypoint* p, waypoints) {
        if (p->getFrame() == MAV_FRAME_GLOBAL) {
            if (p == wp) {
485 486 487 488 489 490 491 492 493
                return i;
            }
            i++;
        }
    }

    return -1;
}

lm's avatar
lm committed
494 495 496 497 498
int UASWaypointManager::getGlobalFrameAndNavTypeIndexOf(Waypoint* wp)
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
499 500 501
    foreach (Waypoint* p, waypoints) {
        if (p->getFrame() == MAV_FRAME_GLOBAL && p->isNavigationType()) {
            if (p == wp) {
lm's avatar
lm committed
502 503 504 505 506 507 508 509 510
                return i;
            }
            i++;
        }
    }

    return -1;
}

511 512 513 514 515
int UASWaypointManager::getGlobalFrameCount()
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
516 517
    foreach (Waypoint* p, waypoints) {
        if (p->getFrame() == MAV_FRAME_GLOBAL) {
518 519 520 521 522 523 524
            i++;
        }
    }

    return i;
}

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

    return i;
}

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

    return i;
}

int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp)
{
    // Search through all waypoints,
    // counting only those in local frame
    int i = 0;
558 559 560
    foreach (Waypoint* p, waypoints) {
        if (p->getFrame() == MAV_FRAME_LOCAL) {
            if (p == wp) {
561 562 563 564 565 566 567 568 569 570 571 572 573 574
                return i;
            }
            i++;
        }
    }

    return -1;
}

int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp)
{
    // Search through all waypoints,
    // counting only those in mission frame
    int i = 0;
575 576 577
    foreach (Waypoint* p, waypoints) {
        if (p->getFrame() == MAV_FRAME_MISSION) {
            if (p == wp) {
578 579 580 581 582 583 584 585 586
                return i;
            }
            i++;
        }
    }

    return -1;
}

587 588
void UASWaypointManager::readWaypoints()
{
589
    emit readGlobalWPFromUAS(true);
590 591
    if(current_state == WP_IDLE) {
        while(waypoints.size()>0) {
592
            delete waypoints.back();
593
            waypoints.pop_back();
594

595 596
        }

597 598
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
599 600 601 602 603 604

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

605
        sendWaypointRequestList();
606

pixhawk's avatar
pixhawk committed
607 608 609
    }
}

610
void UASWaypointManager::writeWaypoints()
pixhawk's avatar
pixhawk committed
611
{
612
    if (current_state == WP_IDLE) {
pixhawk's avatar
pixhawk committed
613
        // Send clear all if count == 0
614
        if (waypoints.count() > 0) {
pixhawk's avatar
pixhawk committed
615
            protocol_timer.start(PROTOCOL_TIMEOUT_MS);
616
            current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
617

618
            current_count = waypoints.count();
pixhawk's avatar
pixhawk committed
619 620 621 622 623 624
            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
625 626 627
            // 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.
628
            while(!waypoint_buffer.empty()) {
pixhawk's avatar
pixhawk committed
629 630 631
                delete waypoint_buffer.back();
                waypoint_buffer.pop_back();
            }
pixhawk's avatar
pixhawk committed
632

633 634
            bool noCurrent = true;

pixhawk's avatar
pixhawk committed
635
            //copy waypoint data to local buffer
636
            for (int i=0; i < current_count; i++) {
pixhawk's avatar
pixhawk committed
637 638 639
                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
640
                const Waypoint *cur_s = waypoints.at(i);
pixhawk's avatar
pixhawk committed
641 642

                cur_d->autocontinue = cur_s->getAutoContinue();
643
                cur_d->current = cur_s->getCurrent() & noCurrent;   //make sure only one current waypoint is selected, the first selected will be chosen
644 645
                cur_d->param1 = cur_s->getParam1();
                cur_d->param2 = cur_s->getParam2();
lm's avatar
lm committed
646 647
                cur_d->param3 = cur_s->getParam3();
                cur_d->param4 = cur_s->getParam4();
648
                cur_d->frame = cur_s->getFrame();
lm's avatar
lm committed
649
                cur_d->command = cur_s->getAction();
650
                cur_d->seq = i;     // don't read out the sequence number of the waypoint class
pixhawk's avatar
pixhawk committed
651 652 653
                cur_d->x = cur_s->getX();
                cur_d->y = cur_s->getY();
                cur_d->z = cur_s->getZ();
654 655 656

                if (cur_s->getCurrent() && noCurrent)
                    noCurrent = false;
657 658
                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
659
            }
660

661 662 663



pixhawk's avatar
pixhawk committed
664
            //send the waypoint count to UAS (this starts the send transaction)
665
            sendWaypointCount();
pixhawk's avatar
pixhawk committed
666
        }
667
    } else if (waypoints.count() == 0) {
668
        sendWaypointClearAll();
669
    } else {
670 671 672
        //we're in another transaction, ignore command
        qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
    }
pixhawk's avatar
pixhawk committed
673 674
}

675 676 677 678 679 680 681 682
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;

683
    emit updateStatusString(QString("Clearing waypoint list..."));
684 685 686

    mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca);
    uas.sendMessage(message);
687
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
688 689 690 691 692 693 694 695 696 697 698 699 700

    qDebug() << "sent waypoint clear all to ID " << wpca.target_system;
}

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;

701
    emit updateStatusString(QString("Updating target waypoint..."));
702 703 704

    mavlink_msg_waypoint_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc);
    uas.sendMessage(message);
705
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
706 707 708 709 710 711 712 713 714 715 716 717 718

    qDebug() << "sent waypoint set current (" << wpsc.seq << ") to ID " << wpsc.target_system;
}

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;

719
    qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
720
    emit updateStatusString(QString("Starting to transmit waypoints..."));
721 722 723

    mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc);
    uas.sendMessage(message);
724
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
725 726 727 728 729 730 731 732 733 734 735 736

    qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
}

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;

737
    emit updateStatusString(QString("Requesting waypoint list..."));
738 739 740

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

    qDebug() << "sent waypoint list request to ID " << wprl.target_system;
744 745


746 747
}

748
void UASWaypointManager::sendWaypointRequest(quint16 seq)
pixhawk's avatar
pixhawk committed
749
{
750 751 752 753 754 755 756
    mavlink_message_t message;
    mavlink_waypoint_request_t wpr;

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

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

759 760
    mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr);
    uas.sendMessage(message);
761
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
762 763

    qDebug() << "sent waypoint request (" << wpr.seq << ") to ID " << wpr.target_system;
pixhawk's avatar
pixhawk committed
764
}
pixhawk's avatar
pixhawk committed
765

pixhawk's avatar
pixhawk committed
766 767 768
void UASWaypointManager::sendWaypoint(quint16 seq)
{
    mavlink_message_t message;
769
    qDebug() <<" WP Buffer count: "<<waypoint_buffer.count();
pixhawk's avatar
pixhawk committed
770

771
    if (seq < waypoint_buffer.count()) {
772

pixhawk's avatar
pixhawk committed
773 774
        mavlink_waypoint_t *wp;

775

pixhawk's avatar
pixhawk committed
776 777 778 779
        wp = waypoint_buffer.at(seq);
        wp->target_system = uas.getUASID();
        wp->target_component = MAV_COMP_ID_WAYPOINTPLANNER;

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

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

pixhawk's avatar
pixhawk committed
784 785
        mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp);
        uas.sendMessage(message);
786
        MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
pixhawk's avatar
pixhawk committed
787 788

    }
pixhawk's avatar
pixhawk committed
789
}
790 791 792 793 794 795 796 797 798 799 800 801

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);
802
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
803 804 805

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