UASWaypointManager.cc 22.6 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)
        {
pixhawk's avatar
pixhawk committed
141
            qDebug() << "Got WP: " << wp->seq << wp->x <<  wp->y << wp->z << wp->yaw << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action;
142
            Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2, (MAV_FRAME) wp->frame, (MAV_ACTION) wp->action);
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 240 241 242
        if (current_state == WP_SETCURRENT)
        {
            protocol_timer.stop();
            current_state = WP_IDLE;
243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259

            // 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);
                    }
                }
            }

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

268 269
void UASWaypointManager::notifyOfChange(Waypoint* wp)
{
270
    qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId();
271 272 273 274 275 276 277 278 279 280
    // 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());
    }
281 282
}

283
int UASWaypointManager::setCurrentWaypoint(quint16 seq)
pixhawk's avatar
pixhawk committed
284
{
285
    if (seq < waypoints.size())
286
    {
287 288 289 290 291 292 293 294 295 296 297 298 299 300
        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);
                }
            }
301

302
            //TODO: signal changed waypoint list
303

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

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

340
int UASWaypointManager::removeWaypoint(quint16 seq)
341 342 343 344 345 346 347 348 349 350 351 352
{
    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);
        }
        emit waypointListChanged();
353
        emit waypointListChanged(uas.getUASID());
354 355 356 357 358
        return 0;
    }
    return -1;
}

359
void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383
{
    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();
384
        emit waypointListChanged(uas.getUASID());
385 386 387
    }
}

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

    QTextStream out(&file);
395 396 397 398

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

399 400 401 402 403 404 405
    for (int i = 0; i < waypoints.size(); i++)
    {
        waypoints[i]->save(out);
    }
    file.close();
}

406
void UASWaypointManager::loadWaypoints(const QString &loadFile)
407 408 409 410 411 412 413 414 415 416 417 418 419
{
    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);
420 421 422 423

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

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

447 448
    file.close();

449
    emit loadWPFile();
450
    emit waypointListChanged();
451
    emit waypointListChanged(uas.getUASID());
452 453
}

454

455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472
//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
473

474
void UASWaypointManager::clearWaypointList()
pixhawk's avatar
pixhawk committed
475 476 477
{
    if(current_state == WP_IDLE)
    {
pixhawk's avatar
pixhawk committed
478
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
479
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
480

481 482
        current_state = WP_CLEARLIST;
        current_wp_id = 0;
483 484
        current_partner_systemid = uas.getUASID();
        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
pixhawk's avatar
pixhawk committed
485

486
        sendWaypointClearAll();
487 488 489
    }
}

490 491 492 493 494
int UASWaypointManager::getIndexOf(Waypoint* wp)
{
    return waypoints.indexOf(wp);
}

495 496
void UASWaypointManager::readWaypoints()
{
497
    emit readGlobalWPFromUAS(true);
498 499
    if(current_state == WP_IDLE)
    {
500 501
        while(waypoints.size()>0)
        {
502
            delete waypoints.back();
503
            waypoints.pop_back();
504

505 506
        }

507 508
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
509 510 511 512 513 514

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

515
        sendWaypointRequestList();
516

pixhawk's avatar
pixhawk committed
517 518 519
    }
}

520
void UASWaypointManager::writeWaypoints()
pixhawk's avatar
pixhawk committed
521
{
522 523
    if (current_state == WP_IDLE)
    {
pixhawk's avatar
pixhawk committed
524
        // Send clear all if count == 0
525
        if (waypoints.count() > 0)
526
        {
pixhawk's avatar
pixhawk committed
527
            protocol_timer.start(PROTOCOL_TIMEOUT_MS);
528
            current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
529

530
            current_count = waypoints.count();
pixhawk's avatar
pixhawk committed
531 532 533 534 535 536
            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
537 538 539
            // 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
540 541 542 543 544
            while(!waypoint_buffer.empty())
            {
                delete waypoint_buffer.back();
                waypoint_buffer.pop_back();
            }
pixhawk's avatar
pixhawk committed
545

546 547
            bool noCurrent = true;

pixhawk's avatar
pixhawk committed
548 549 550 551 552 553
            //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
554
                const Waypoint *cur_s = waypoints.at(i);
pixhawk's avatar
pixhawk committed
555 556

                cur_d->autocontinue = cur_s->getAutoContinue();
557
                cur_d->current = cur_s->getCurrent() & noCurrent;   //make sure only one current waypoint is selected, the first selected will be chosen
lm's avatar
lm committed
558 559
                cur_d->orbit = 0;
                cur_d->orbit_direction = 0;
560 561
                cur_d->param1 = cur_s->getOrbit();
                cur_d->param2 = cur_s->getHoldTime();
562 563
                cur_d->frame = cur_s->getFrame();
                cur_d->action = cur_s->getAction();
564
                cur_d->seq = i;     // don't read out the sequence number of the waypoint class
pixhawk's avatar
pixhawk committed
565 566 567 568
                cur_d->x = cur_s->getX();
                cur_d->y = cur_s->getY();
                cur_d->z = cur_s->getZ();
                cur_d->yaw = cur_s->getYaw();
569 570 571

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

pixhawk's avatar
pixhawk committed
574
            //send the waypoint count to UAS (this starts the send transaction)
575
            sendWaypointCount();
pixhawk's avatar
pixhawk committed
576
        }
577
    }
578 579 580 581
    else if (waypoints.count() == 0)
    {
        sendWaypointClearAll();
    }
582 583 584 585 586
    else
    {
        //we're in another transaction, ignore command
        qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
    }
pixhawk's avatar
pixhawk committed
587 588
}

589 590 591 592 593 594 595 596
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;

597
    emit updateStatusString(QString("Clearing waypoint list..."));
598 599 600

    mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca);
    uas.sendMessage(message);
601
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
602 603 604 605 606 607 608 609 610 611 612 613 614

    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;

615
    emit updateStatusString(QString("Updating target waypoint..."));
616 617 618

    mavlink_msg_waypoint_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc);
    uas.sendMessage(message);
619
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
620 621 622 623 624 625 626 627 628 629 630 631 632

    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;

633
    qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
634
    emit updateStatusString(QString("Starting to transmit waypoints..."));
635 636 637

    mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc);
    uas.sendMessage(message);
638
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
639 640 641 642 643 644 645 646 647 648 649 650

    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;

651
    emit updateStatusString(QString("Requesting waypoint list..."));
652 653 654

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

    qDebug() << "sent waypoint list request to ID " << wprl.target_system;
658 659


660 661
}

662
void UASWaypointManager::sendWaypointRequest(quint16 seq)
pixhawk's avatar
pixhawk committed
663
{
664 665 666 667 668 669 670
    mavlink_message_t message;
    mavlink_waypoint_request_t wpr;

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

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

673 674
    mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr);
    uas.sendMessage(message);
675
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
676 677

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

pixhawk's avatar
pixhawk committed
680 681 682
void UASWaypointManager::sendWaypoint(quint16 seq)
{
    mavlink_message_t message;
683
    qDebug() <<" WP Buffer count: "<<waypoint_buffer.count();
pixhawk's avatar
pixhawk committed
684 685 686

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

pixhawk's avatar
pixhawk committed
688 689
        mavlink_waypoint_t *wp;

690

pixhawk's avatar
pixhawk committed
691 692 693 694
        wp = waypoint_buffer.at(seq);
        wp->target_system = uas.getUASID();
        wp->target_component = MAV_COMP_ID_WAYPOINTPLANNER;

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

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

pixhawk's avatar
pixhawk committed
699 700
        mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp);
        uas.sendMessage(message);
701
        MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
pixhawk's avatar
pixhawk committed
702 703

    }
pixhawk's avatar
pixhawk committed
704
}
705 706 707 708 709 710 711 712 713 714 715 716

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);
717
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
718 719 720

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