UASWaypointManager.cc 24.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
//#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)
pixhawk's avatar
pixhawk committed
42
        : uas(_uas),
43
        current_retries(0),
pixhawk's avatar
pixhawk committed
44 45 46 47
        current_wp_id(0),
        current_count(0),
        current_state(WP_IDLE),
        current_partner_systemid(0),
pixhawk's avatar
pixhawk committed
48 49 50 51 52 53 54
        current_partner_compid(0),
        protocol_timer(this)
{
    connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout()));
}

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

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

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

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

void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, quint16 count)
{
    if (current_state == WP_GETLIST && systemId == current_partner_systemid && compId == current_partner_compid)
    {
107 108 109
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;

110
        qDebug() << "got waypoint count (" << count << ") from ID " << systemId;
pixhawk's avatar
pixhawk committed
111

pixhawk's avatar
pixhawk committed
112 113 114 115 116
        if (count > 0)
        {
            current_count = count;
            current_wp_id = 0;
            current_state = WP_GETLIST_GETWPS;
117

pixhawk's avatar
pixhawk committed
118 119 120 121 122 123 124
            sendWaypointRequest(current_wp_id);
        }
        else
        {
            emit updateStatusString("done.");
            qDebug() << "No waypoints on UAS " << systemId;
        }
125
    }
126 127 128 129
    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);
    }
130 131
}

pixhawk's avatar
pixhawk committed
132 133 134 135
void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp)
{
    if (systemId == current_partner_systemid && compId == current_partner_compid && current_state == WP_GETLIST_GETWPS && wp->seq == current_wp_id)
    {
136 137 138
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;

pixhawk's avatar
pixhawk committed
139 140
        if(wp->seq == current_wp_id)
        {
lm's avatar
lm committed
141 142
            //qDebug() << "Got WP: " << wp->seq << wp->x <<  wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_CMD) wp->command;
            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
143
            addWaypoint(lwp, false);
pixhawk's avatar
pixhawk committed
144 145 146 147 148 149

            //get next waypoint
            current_wp_id++;

            if(current_wp_id < current_count)
            {
150
                sendWaypointRequest(current_wp_id);
pixhawk's avatar
pixhawk committed
151 152 153
            }
            else
            {
154 155
                sendWaypointAck(0);

pixhawk's avatar
pixhawk committed
156
                // all waypoints retrieved, change state to idle
pixhawk's avatar
pixhawk committed
157 158 159 160 161
                current_state = WP_IDLE;
                current_count = 0;
                current_wp_id = 0;
                current_partner_systemid = 0;
                current_partner_compid = 0;
162

pixhawk's avatar
pixhawk committed
163
                protocol_timer.stop();
164
                emit readGlobalWPFromUAS(false);
165 166
                emit updateStatusString("done.");

pixhawk's avatar
pixhawk committed
167
                qDebug() << "got all waypoints from ID " << systemId;
pixhawk's avatar
pixhawk committed
168 169 170 171
            }
        }
        else
        {
172
            emit updateStatusString(tr("Waypoint ID mismatch, rejecting waypoint"));
pixhawk's avatar
pixhawk committed
173 174
        }
    }
175 176 177 178
    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);
    }
pixhawk's avatar
pixhawk committed
179 180
}

181 182
void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa)
{
183
    if (systemId == current_partner_systemid && compId == current_partner_compid)
184
    {
185
        if((current_state == WP_SENDLIST || current_state == WP_SENDLIST_SENDWPS) && (current_wp_id == waypoint_buffer.count()-1 && wpa->type == 0))
186 187 188
        {
            //all waypoints sent and ack received
            protocol_timer.stop();
189
            current_state = WP_IDLE;
190 191 192
            emit updateStatusString("done.");
            qDebug() << "sent all waypoints to ID " << systemId;
        }
193 194 195 196 197 198 199
        else if(current_state == WP_CLEARLIST)
        {
            protocol_timer.stop();
            current_state = WP_IDLE;
            emit updateStatusString("done.");
            qDebug() << "cleared waypoint list of ID " << systemId;
        }
200 201 202
    }
}

pixhawk's avatar
pixhawk committed
203
void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr)
204
{
205
    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))))
pixhawk's avatar
pixhawk committed
206
    {
207 208
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
209

210
        if (wpr->seq < waypoint_buffer.count())
pixhawk's avatar
pixhawk committed
211
        {
pixhawk's avatar
pixhawk committed
212 213 214
            current_state = WP_SENDLIST_SENDWPS;
            current_wp_id = wpr->seq;
            sendWaypoint(current_wp_id);
pixhawk's avatar
pixhawk committed
215 216 217 218 219 220
        }
        else
        {
            //TODO: Error message or something
        }
    }
221 222 223 224
    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);
    }
225 226
}

pixhawk's avatar
pixhawk committed
227
void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr)
228
{
pixhawk's avatar
pixhawk committed
229 230 231 232
    if (systemId == uas.getUASID() && compId == MAV_COMP_ID_WAYPOINTPLANNER)
    {
        emit updateStatusString(QString("Reached waypoint %1").arg(wpr->seq));
    }
pixhawk's avatar
pixhawk committed
233 234
}

235
void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_current_t *wpc)
pixhawk's avatar
pixhawk committed
236
{
pixhawk's avatar
pixhawk committed
237 238
    if (systemId == uas.getUASID() && compId == MAV_COMP_ID_WAYPOINTPLANNER)
    {
239
        // FIXME Petri
240 241 242 243
        if (current_state == WP_SETCURRENT)
        {
            protocol_timer.stop();
            current_state = WP_IDLE;
244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260

            // update the local main storage
            if (wpc->seq < waypoints.size())
            {
                for(int i = 0; i < waypoints.size(); i++)
                {
                    if (waypoints[i]->getId() == wpc->seq)
                    {
                        waypoints[i]->setCurrent(true);
                    }
                    else
                    {
                        waypoints[i]->setCurrent(false);
                    }
                }
            }

261
            //qDebug() << "Updated waypoints list";
262
        }
263 264 265
        emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq));
        //emit update to UI widgets
        emit currentWaypointChanged(wpc->seq);
266
        //qDebug() << "new current waypoint" << wpc->seq;
pixhawk's avatar
pixhawk committed
267
    }
pixhawk's avatar
pixhawk committed
268 269
}

270 271
void UASWaypointManager::notifyOfChange(Waypoint* wp)
{
272
    qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId();
273 274 275 276 277 278 279 280 281 282
    // If only one waypoint was changed, emit only WP signal
    if (wp != NULL)
    {
        emit waypointChanged(uas.getUASID(), wp);
    }
    else
    {
        emit waypointListChanged();
        emit waypointListChanged(uas.getUASID());
    }
283 284
}

285
int UASWaypointManager::setCurrentWaypoint(quint16 seq)
pixhawk's avatar
pixhawk committed
286
{
287
    if (seq < waypoints.size())
288
    {
289 290 291 292 293 294 295 296 297 298 299 300 301 302
        if(current_state == WP_IDLE)
        {
            //update local main storage
            for(int i = 0; i < waypoints.size(); i++)
            {
                if (waypoints[i]->getId() == seq)
                {
                    waypoints[i]->setCurrent(true);
                }
                else
                {
                    waypoints[i]->setCurrent(false);
                }
            }
303

304
            //TODO: signal changed waypoint list
305

306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324
            //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
325 326 327 328
/**
 * @param enforceFirstActive Enforces that the first waypoint is set as active
 */
void UASWaypointManager::addWaypoint(Waypoint *wp, bool enforceFirstActive)
329 330 331 332
{
    if (wp)
    {
        wp->setId(waypoints.size());
pixhawk's avatar
pixhawk committed
333
        if (enforceFirstActive && waypoints.size() == 0) wp->setCurrent(true);
334
        waypoints.insert(waypoints.size(), wp);
335
        connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*)));
336

337
        emit waypointListChanged();
338
        emit waypointListChanged(uas.getUASID());
339
    }
pixhawk's avatar
pixhawk committed
340 341
}

342
int UASWaypointManager::removeWaypoint(quint16 seq)
343 344 345 346 347 348 349 350 351 352 353
{
    if (seq < waypoints.size())
    {
        Waypoint *t = waypoints[seq];
        waypoints.remove(seq);
        delete t;

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

355
        emit waypointListChanged();
356
        emit waypointListChanged(uas.getUASID());
357 358 359 360 361
        return 0;
    }
    return -1;
}

362
void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386
{
    if (cur_seq != new_seq && cur_seq < waypoints.size() && new_seq < waypoints.size())
    {
        Waypoint *t = waypoints[cur_seq];
        if (cur_seq < new_seq)
        {
            for (int i = cur_seq; i < new_seq; i++)
            {
                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
            }
        }
        else
        {
            for (int i = cur_seq; i > new_seq; i--)
            {
                waypoints[i] = waypoints[i-1];
               // waypoints[i]->setId(i);
            }
        }
        waypoints[new_seq] = t;
        //waypoints[new_seq]->setId(new_seq);

        emit waypointListChanged();
387
        emit waypointListChanged(uas.getUASID());
388 389 390
    }
}

391
void UASWaypointManager::saveWaypoints(const QString &saveFile)
392 393 394 395 396 397
{
    QFile file(saveFile);
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
        return;

    QTextStream out(&file);
398 399

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

402 403
    for (int i = 0; i < waypoints.size(); i++)
    {
404
        waypoints[i]->setId(i);
405 406 407 408 409
        waypoints[i]->save(out);
    }
    file.close();
}

410
void UASWaypointManager::loadWaypoints(const QString &loadFile)
411 412 413 414 415 416 417 418 419 420 421 422 423
{
    QFile file(loadFile);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
        return;

    while(waypoints.size()>0)
    {
        Waypoint *t = waypoints[0];
        waypoints.remove(0);
        delete t;
    }

    QTextStream in(&file);
424 425 426

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

lm's avatar
lm committed
427
    if (!(version.size() == 3 && version[0] == "QGC" && version[1] == "WPL" && version[2] == "110"))
428
    {
429 430
        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."));
431 432 433 434
    }
    else
    {
        while (!in.atEnd())
435
        {
436 437 438 439 440 441 442 443
            Waypoint *t = new Waypoint();
            if(t->load(in))
            {
                t->setId(waypoints.size());
                waypoints.insert(waypoints.size(), t);
            }
            else
            {
444 445
                emit updateStatusString(tr("The waypoint file is corrupted. Load operation only partly succesful."));
                //MainWindow::instance()->showCriticalMessage(tr("Error loading waypoint file"),);
446 447
                break;
            }
448 449
        }
    }
450

451 452
    file.close();

453
    emit loadWPFile();
454
    emit waypointListChanged();
455
    emit waypointListChanged(uas.getUASID());
456 457
}

458

459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476
//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
477

478
void UASWaypointManager::clearWaypointList()
pixhawk's avatar
pixhawk committed
479 480 481
{
    if(current_state == WP_IDLE)
    {
pixhawk's avatar
pixhawk committed
482
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
483
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
484

485 486
        current_state = WP_CLEARLIST;
        current_wp_id = 0;
487 488
        current_partner_systemid = uas.getUASID();
        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
pixhawk's avatar
pixhawk committed
489

490
        sendWaypointClearAll();
491 492 493
    }
}

494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509
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;
    foreach (Waypoint* wp, waypoints)
    {
        if (wp->getFrame() == MAV_FRAME_GLOBAL)
        {
            wps.append(wp);
        }
    }
    return wps;
}

510 511 512 513 514
int UASWaypointManager::getIndexOf(Waypoint* wp)
{
    return waypoints.indexOf(wp);
}

515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606
int UASWaypointManager::getGlobalFrameIndexOf(Waypoint* wp)
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
    foreach (Waypoint* p, waypoints)
    {
        if (p->getFrame() == MAV_FRAME_GLOBAL)
        {
            if (p == wp)
            {
                return i;
            }
            i++;
        }
    }

    return -1;
}

int UASWaypointManager::getGlobalFrameCount()
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
    foreach (Waypoint* p, waypoints)
    {
        if (p->getFrame() == MAV_FRAME_GLOBAL)
        {
            i++;
        }
    }

    return i;
}

int UASWaypointManager::getLocalFrameCount()
{
    // Search through all waypoints,
    // counting only those in global frame
    int i = 0;
    foreach (Waypoint* p, waypoints)
    {
        if (p->getFrame() == MAV_FRAME_GLOBAL)
        {
            i++;
        }
    }

    return i;
}

int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp)
{
    // Search through all waypoints,
    // counting only those in local frame
    int i = 0;
    foreach (Waypoint* p, waypoints)
    {
        if (p->getFrame() == MAV_FRAME_LOCAL)
        {
            if (p == wp)
            {
                return i;
            }
            i++;
        }
    }

    return -1;
}

int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp)
{
    // Search through all waypoints,
    // counting only those in mission frame
    int i = 0;
    foreach (Waypoint* p, waypoints)
    {
        if (p->getFrame() == MAV_FRAME_MISSION)
        {
            if (p == wp)
            {
                return i;
            }
            i++;
        }
    }

    return -1;
}

607 608
void UASWaypointManager::readWaypoints()
{
609
    emit readGlobalWPFromUAS(true);
610 611
    if(current_state == WP_IDLE)
    {
612 613
        while(waypoints.size()>0)
        {
614
            delete waypoints.back();
615
            waypoints.pop_back();
616

617 618
        }

619 620
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
621 622 623 624 625 626

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

627
        sendWaypointRequestList();
628

pixhawk's avatar
pixhawk committed
629 630 631
    }
}

632
void UASWaypointManager::writeWaypoints()
pixhawk's avatar
pixhawk committed
633
{
634 635
    if (current_state == WP_IDLE)
    {
pixhawk's avatar
pixhawk committed
636
        // Send clear all if count == 0
637
        if (waypoints.count() > 0)
638
        {
pixhawk's avatar
pixhawk committed
639
            protocol_timer.start(PROTOCOL_TIMEOUT_MS);
640
            current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
641

642
            current_count = waypoints.count();
pixhawk's avatar
pixhawk committed
643 644 645 646 647 648
            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
649 650 651
            // 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.
pixhawk's avatar
pixhawk committed
652 653 654 655 656
            while(!waypoint_buffer.empty())
            {
                delete waypoint_buffer.back();
                waypoint_buffer.pop_back();
            }
pixhawk's avatar
pixhawk committed
657

658 659
            bool noCurrent = true;

pixhawk's avatar
pixhawk committed
660 661 662 663 664 665
            //copy waypoint data to local buffer
            for (int i=0; i < current_count; i++)
            {
                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
666
                const Waypoint *cur_s = waypoints.at(i);
pixhawk's avatar
pixhawk committed
667 668

                cur_d->autocontinue = cur_s->getAutoContinue();
669
                cur_d->current = cur_s->getCurrent() & noCurrent;   //make sure only one current waypoint is selected, the first selected will be chosen
670 671
                cur_d->param1 = cur_s->getParam1();
                cur_d->param2 = cur_s->getParam2();
lm's avatar
lm committed
672 673
                cur_d->param3 = cur_s->getParam3();
                cur_d->param4 = cur_s->getParam4();
674
                cur_d->frame = cur_s->getFrame();
lm's avatar
lm committed
675
                cur_d->command = cur_s->getAction();
676
                cur_d->seq = i;     // don't read out the sequence number of the waypoint class
pixhawk's avatar
pixhawk committed
677 678 679
                cur_d->x = cur_s->getX();
                cur_d->y = cur_s->getY();
                cur_d->z = cur_s->getZ();
680 681 682

                if (cur_s->getCurrent() && noCurrent)
                    noCurrent = false;
pixhawk's avatar
pixhawk committed
683
            }
684

pixhawk's avatar
pixhawk committed
685
            //send the waypoint count to UAS (this starts the send transaction)
686
            sendWaypointCount();
pixhawk's avatar
pixhawk committed
687
        }
688
    }
689 690 691 692
    else if (waypoints.count() == 0)
    {
        sendWaypointClearAll();
    }
693 694 695 696 697
    else
    {
        //we're in another transaction, ignore command
        qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
    }
pixhawk's avatar
pixhawk committed
698 699
}

700 701 702 703 704 705 706 707
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;

708
    emit updateStatusString(QString("Clearing waypoint list..."));
709 710 711

    mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca);
    uas.sendMessage(message);
712
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
713 714 715 716 717 718 719 720 721 722 723 724 725

    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;

726
    emit updateStatusString(QString("Updating target waypoint..."));
727 728 729

    mavlink_msg_waypoint_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc);
    uas.sendMessage(message);
730
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
731 732 733 734 735 736 737 738 739 740 741 742 743

    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;

744
    qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
745
    emit updateStatusString(QString("Starting to transmit waypoints..."));
746 747 748

    mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc);
    uas.sendMessage(message);
749
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
750 751 752 753 754 755 756 757 758 759 760 761

    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;

762
    emit updateStatusString(QString("Requesting waypoint list..."));
763 764 765

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

    qDebug() << "sent waypoint list request to ID " << wprl.target_system;
769 770


771 772
}

773
void UASWaypointManager::sendWaypointRequest(quint16 seq)
pixhawk's avatar
pixhawk committed
774
{
775 776 777 778 779 780 781
    mavlink_message_t message;
    mavlink_waypoint_request_t wpr;

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

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

784 785
    mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr);
    uas.sendMessage(message);
786
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
787 788

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

pixhawk's avatar
pixhawk committed
791 792 793
void UASWaypointManager::sendWaypoint(quint16 seq)
{
    mavlink_message_t message;
794
    qDebug() <<" WP Buffer count: "<<waypoint_buffer.count();
pixhawk's avatar
pixhawk committed
795 796 797

    if (seq < waypoint_buffer.count())
    {
798

pixhawk's avatar
pixhawk committed
799 800
        mavlink_waypoint_t *wp;

801

pixhawk's avatar
pixhawk committed
802 803 804 805
        wp = waypoint_buffer.at(seq);
        wp->target_system = uas.getUASID();
        wp->target_component = MAV_COMP_ID_WAYPOINTPLANNER;

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

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

pixhawk's avatar
pixhawk committed
810 811
        mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp);
        uas.sendMessage(message);
812
        MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
pixhawk's avatar
pixhawk committed
813 814

    }
pixhawk's avatar
pixhawk committed
815
}
816 817 818 819 820 821 822 823 824 825 826 827

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);
828
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
829 830 831

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