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
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();

386
    emit loadWPFile();
387
    emit waypointListChanged();
388

389 390
}

391

pixhawk's avatar
pixhawk committed
392 393 394 395 396 397 398 399 400 401
void UASWaypointManager::globalAddWaypoint(Waypoint *wp)
{

}

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

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

409 410
        current_state = WP_CLEARLIST;
        current_wp_id = 0;
411 412
        current_partner_systemid = uas.getUASID();
        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
pixhawk's avatar
pixhawk committed
413

414
        sendWaypointClearAll();
415 416 417 418 419 420 421
    }
}

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

429 430
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
431 432 433 434 435 436

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

437
        sendWaypointRequestList();
pixhawk's avatar
pixhawk committed
438 439 440
    }
}

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

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

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

465 466
            bool noCurrent = true;

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

                cur_d->autocontinue = cur_s->getAutoContinue();
476
                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
477 478
                cur_d->orbit = 0;
                cur_d->orbit_direction = 0;
479 480
                cur_d->param1 = cur_s->getOrbit();
                cur_d->param2 = cur_s->getHoldTime();
481
                // TODO: Replace this value depending on the type of waypoint
482 483
                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
484 485 486 487
                cur_d->x = cur_s->getX();
                cur_d->y = cur_s->getY();
                cur_d->z = cur_s->getZ();
                cur_d->yaw = cur_s->getYaw();
488 489 490

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

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

504 505 506 507 508 509 510 511
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;

512
    emit updateStatusString(QString("clearing waypoint list..."));
513 514 515

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

    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;

530
    emit updateStatusString(QString("Updating target waypoint..."));
531 532 533

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

    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;

548
    qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
549
    emit updateStatusString(QString("start transmitting waypoints..."));
550 551 552

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

    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;

566
    emit updateStatusString(QString("requesting waypoint list..."));
567 568 569

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

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

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

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

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

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

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

pixhawk's avatar
pixhawk committed
593 594 595 596 597 598 599 600 601 602 603 604
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;

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

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

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

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);
626
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
627 628 629

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