UASWaypointManager.cc 19.1 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 34
#include "UASWaypointManager.h"
#include "UAS.h"

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

39
UASWaypointManager::UASWaypointManager(UAS &_uas)
pixhawk's avatar
pixhawk committed
40
        : uas(_uas),
41
        current_retries(0),
pixhawk's avatar
pixhawk committed
42 43 44 45
        current_wp_id(0),
        current_count(0),
        current_state(WP_IDLE),
        current_partner_systemid(0),
pixhawk's avatar
pixhawk committed
46 47 48 49 50 51 52
        current_partner_compid(0),
        protocol_timer(this)
{
    connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout()));
}

void UASWaypointManager::timeout()
53
{
54 55 56 57 58 59 60 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
    if (current_retries > 0)
    {
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries--;
        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
87

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

90
        emit updateStatusString("Operation timed out.");
pixhawk's avatar
pixhawk committed
91

92 93 94 95 96 97
        current_state = WP_IDLE;
        current_count = 0;
        current_wp_id = 0;
        current_partner_systemid = 0;
        current_partner_compid = 0;
    }
98 99 100 101 102 103
}

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

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

pixhawk's avatar
pixhawk committed
109 110 111 112 113
        if (count > 0)
        {
            current_count = count;
            current_wp_id = 0;
            current_state = WP_GETLIST_GETWPS;
114

pixhawk's avatar
pixhawk committed
115 116 117 118 119 120 121
            sendWaypointRequest(current_wp_id);
        }
        else
        {
            emit updateStatusString("done.");
            qDebug() << "No waypoints on UAS " << systemId;
        }
122 123 124
    }
}

pixhawk's avatar
pixhawk committed
125 126 127 128
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)
    {
129 130 131
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;

pixhawk's avatar
pixhawk committed
132 133
        if(wp->seq == current_wp_id)
        {
134 135
            Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2);
            localAddWaypoint(lwp);
pixhawk's avatar
pixhawk committed
136 137 138 139 140 141

            //get next waypoint
            current_wp_id++;

            if(current_wp_id < current_count)
            {
142
                sendWaypointRequest(current_wp_id);
pixhawk's avatar
pixhawk committed
143 144 145
            }
            else
            {
146 147
                sendWaypointAck(0);

pixhawk's avatar
pixhawk committed
148
                // all waypoints retrieved, change state to idle
pixhawk's avatar
pixhawk committed
149 150 151 152 153
                current_state = WP_IDLE;
                current_count = 0;
                current_wp_id = 0;
                current_partner_systemid = 0;
                current_partner_compid = 0;
154

pixhawk's avatar
pixhawk committed
155
                protocol_timer.stop();
156 157
                emit updateStatusString("done.");

pixhawk's avatar
pixhawk committed
158
                qDebug() << "got all waypoints from ID " << systemId;
pixhawk's avatar
pixhawk committed
159 160 161 162
            }
        }
        else
        {
pixhawk's avatar
pixhawk committed
163
            //TODO: error handling
pixhawk's avatar
pixhawk committed
164 165 166 167
        }
    }
}

168 169
void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa)
{
170
    if (systemId == current_partner_systemid && compId == current_partner_compid)
171
    {
172
        if((current_state == WP_SENDLIST || current_state == WP_SENDLIST_SENDWPS) && (current_wp_id == waypoint_buffer.count()-1 && wpa->type == 0))
173 174 175
        {
            //all waypoints sent and ack received
            protocol_timer.stop();
176
            current_state = WP_IDLE;
177 178 179
            emit updateStatusString("done.");
            qDebug() << "sent all waypoints to ID " << systemId;
        }
180 181 182 183 184 185 186
        else if(current_state == WP_CLEARLIST)
        {
            protocol_timer.stop();
            current_state = WP_IDLE;
            emit updateStatusString("done.");
            qDebug() << "cleared waypoint list of ID " << systemId;
        }
187 188 189
    }
}

pixhawk's avatar
pixhawk committed
190
void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr)
191
{
192
    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
193
    {
194 195
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
196

197
        if (wpr->seq < waypoint_buffer.count())
pixhawk's avatar
pixhawk committed
198
        {
pixhawk's avatar
pixhawk committed
199 200 201
            current_state = WP_SENDLIST_SENDWPS;
            current_wp_id = wpr->seq;
            sendWaypoint(current_wp_id);
pixhawk's avatar
pixhawk committed
202 203 204 205 206 207
        }
        else
        {
            //TODO: Error message or something
        }
    }
208 209
}

pixhawk's avatar
pixhawk committed
210
void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr)
211
{
pixhawk's avatar
pixhawk committed
212 213 214 215
    if (systemId == uas.getUASID() && compId == MAV_COMP_ID_WAYPOINTPLANNER)
    {
        emit updateStatusString(QString("Reached waypoint %1").arg(wpr->seq));
    }
pixhawk's avatar
pixhawk committed
216 217
}

218
void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_current_t *wpc)
pixhawk's avatar
pixhawk committed
219
{
pixhawk's avatar
pixhawk committed
220 221
    if (systemId == uas.getUASID() && compId == MAV_COMP_ID_WAYPOINTPLANNER)
    {
222 223 224 225
        if (current_state == WP_SETCURRENT)
        {
            protocol_timer.stop();
            current_state = WP_IDLE;
226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242

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

243
            emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq));
244 245
            //emit update to UI widgets
            emit currentWaypointChanged(wpc->seq);
246
        }
247
        qDebug() << "new current waypoint" << wpc->seq;
pixhawk's avatar
pixhawk committed
248
    }
pixhawk's avatar
pixhawk committed
249 250
}

251
int UASWaypointManager::setCurrentWaypoint(quint16 seq)
pixhawk's avatar
pixhawk committed
252
{
253
    if (seq < waypoints.size())
254
    {
255 256 257 258 259 260 261 262 263 264 265 266 267 268
        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);
                }
            }
269

270
            //TODO: signal changed waypoint list
271

272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297
            //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;
}

void UASWaypointManager::localAddWaypoint(Waypoint *wp)
{
    if (wp)
    {
        wp->setId(waypoints.size());
        waypoints.insert(waypoints.size(), wp);
        emit waypointListChanged();
298
    }
pixhawk's avatar
pixhawk committed
299 300
}

301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388
int UASWaypointManager::localRemoveWaypoint(quint16 seq)
{
    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();
        return 0;
    }
    return -1;
}

void UASWaypointManager::localMoveWaypoint(quint16 cur_seq, quint16 new_seq)
{
    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();
    }
}

void UASWaypointManager::localSaveWaypoints(const QString &saveFile)
{
    QFile file(saveFile);
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
        return;

    QTextStream out(&file);
    for (int i = 0; i < waypoints.size(); i++)
    {
        waypoints[i]->save(out);
    }
    file.close();
}

void UASWaypointManager::localLoadWaypoints(const QString &loadFile)
{
    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);
    while (!in.atEnd())
    {
        Waypoint *t = new Waypoint();
        if(t->load(in))
        {
            t->setId(waypoints.size());
            waypoints.insert(waypoints.size(), t);
        }
    }
    file.close();

    emit waypointListChanged();
}

pixhawk's avatar
pixhawk committed
389 390 391 392 393 394 395 396 397 398
void UASWaypointManager::globalAddWaypoint(Waypoint *wp)
{

}

int UASWaypointManager::globalRemoveWaypoint(quint16 seq)
{
    return 0;
}

399
void UASWaypointManager::clearWaypointList()
pixhawk's avatar
pixhawk committed
400 401 402
{
    if(current_state == WP_IDLE)
    {
pixhawk's avatar
pixhawk committed
403
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
404
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
405

406 407
        current_state = WP_CLEARLIST;
        current_wp_id = 0;
408 409
        current_partner_systemid = uas.getUASID();
        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
pixhawk's avatar
pixhawk committed
410

411
        sendWaypointClearAll();
412 413 414 415 416 417 418
    }
}

void UASWaypointManager::readWaypoints()
{
    if(current_state == WP_IDLE)
    {
419 420 421 422 423 424 425
        while(waypoints.size()>0)
        {
            Waypoint *t = waypoints.back();
            delete t;
            waypoints.pop_back();
        }

426 427
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
428 429 430 431 432 433

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

434
        sendWaypointRequestList();
pixhawk's avatar
pixhawk committed
435 436 437
    }
}

438
void UASWaypointManager::writeWaypoints()
pixhawk's avatar
pixhawk committed
439
{
440
    qDebug() << "+++++++++++++++++++>>>> Entro Funcion Write WP";
441 442
    if (current_state == WP_IDLE)
    {
443
        if (waypoints.count() > 0)
444
        {
pixhawk's avatar
pixhawk committed
445
            protocol_timer.start(PROTOCOL_TIMEOUT_MS);
446
            current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
447

448
            current_count = waypoints.count();
pixhawk's avatar
pixhawk committed
449 450 451 452 453 454
            current_state = WP_SENDLIST;
            current_wp_id = 0;
            current_partner_systemid = uas.getUASID();
            current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;

            //clear local buffer
455
            //TODO: Why not replace with waypoint_buffer.clear() ?
pixhawk's avatar
pixhawk committed
456 457 458 459 460
            while(!waypoint_buffer.empty())
            {
                delete waypoint_buffer.back();
                waypoint_buffer.pop_back();
            }
pixhawk's avatar
pixhawk committed
461

462 463
            bool noCurrent = true;

pixhawk's avatar
pixhawk committed
464 465 466 467 468 469
            //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
470
                const Waypoint *cur_s = waypoints.at(i);
pixhawk's avatar
pixhawk committed
471 472

                cur_d->autocontinue = cur_s->getAutoContinue();
473
                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
474 475
                cur_d->orbit = 0;
                cur_d->orbit_direction = 0;
476 477
                cur_d->param1 = cur_s->getOrbit();
                cur_d->param2 = cur_s->getHoldTime();
478
                // TODO: Replace this value depending on the type of waypoint
479 480
                cur_d->type = 1;    //FIXME: we only use local waypoints at the moment
                cur_d->seq = i;     // don't read out the sequence number of the waypoint class
pixhawk's avatar
pixhawk committed
481 482 483 484
                cur_d->x = cur_s->getX();
                cur_d->y = cur_s->getY();
                cur_d->z = cur_s->getZ();
                cur_d->yaw = cur_s->getYaw();
485 486 487

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

pixhawk's avatar
pixhawk committed
490
            //send the waypoint count to UAS (this starts the send transaction)
491
            sendWaypointCount();
pixhawk's avatar
pixhawk committed
492
        }
493 494 495 496 497 498
    }
    else
    {
        //we're in another transaction, ignore command
        qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
    }
pixhawk's avatar
pixhawk committed
499 500
}

501 502 503 504 505 506 507 508
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;

509
    emit updateStatusString(QString("clearing waypoint list..."));
510 511 512

    mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca);
    uas.sendMessage(message);
513
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
514 515 516 517 518 519 520 521 522 523 524 525 526

    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;

527
    emit updateStatusString(QString("Updating target waypoint..."));
528 529 530

    mavlink_msg_waypoint_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc);
    uas.sendMessage(message);
531
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
532 533 534 535 536 537 538 539 540 541 542 543 544

    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;

545
    qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
546
    emit updateStatusString(QString("start transmitting waypoints..."));
547 548 549

    mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc);
    uas.sendMessage(message);
550
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
551 552 553 554 555 556 557 558 559 560 561 562

    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;

563
    emit updateStatusString(QString("requesting waypoint list..."));
564 565 566

    mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl);
    uas.sendMessage(message);
567
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
568 569 570 571

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

572
void UASWaypointManager::sendWaypointRequest(quint16 seq)
pixhawk's avatar
pixhawk committed
573
{
574 575 576 577 578 579 580
    mavlink_message_t message;
    mavlink_waypoint_request_t wpr;

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

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

583 584
    mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr);
    uas.sendMessage(message);
585
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
586 587

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

pixhawk's avatar
pixhawk committed
590 591 592 593 594 595 596 597 598 599 600 601
void UASWaypointManager::sendWaypoint(quint16 seq)
{
    mavlink_message_t message;

    if (seq < waypoint_buffer.count())
    {
        mavlink_waypoint_t *wp;

        wp = waypoint_buffer.at(seq);
        wp->target_system = uas.getUASID();
        wp->target_component = MAV_COMP_ID_WAYPOINTPLANNER;

602
        emit updateStatusString(QString("sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
pixhawk's avatar
pixhawk committed
603 604 605

        mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp);
        uas.sendMessage(message);
606
        MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
pixhawk's avatar
pixhawk committed
607 608 609

        qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system;
    }
pixhawk's avatar
pixhawk committed
610
}
611 612 613 614 615 616 617 618 619 620 621 622

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);
623
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
624 625 626

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