UASWaypointManager.cc 19.3 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
            qDebug() << "Got WP: " << 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;
            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);
136
            addWaypoint(lwp);
pixhawk's avatar
pixhawk committed
137 138 139 140 141 142

            //get next waypoint
            current_wp_id++;

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

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

pixhawk's avatar
pixhawk committed
156
                protocol_timer.stop();
157
                emit readGlobalWPFromUAS(false);
158 159
                emit updateStatusString("done.");

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

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

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

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

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

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

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

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

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

272
            //TODO: signal changed waypoint list
273

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

293
void UASWaypointManager::addWaypoint(Waypoint *wp)
294 295 296 297 298
{
    if (wp)
    {
        wp->setId(waypoints.size());
        waypoints.insert(waypoints.size(), wp);
299

300
        emit waypointListChanged();
301
    }
pixhawk's avatar
pixhawk committed
302 303
}

304
int UASWaypointManager::removeWaypoint(quint16 seq)
305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321
{
    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;
}

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

350
void UASWaypointManager::saveWaypoints(const QString &saveFile)
351 352 353 354 355 356 357 358 359 360 361 362 363
{
    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();
}

364
void UASWaypointManager::loadWaypoints(const QString &loadFile)
365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388
{
    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();

389
    emit loadWPFile();
390
    emit waypointListChanged();
391

392 393
}

394

pixhawk's avatar
pixhawk committed
395 396 397 398 399 400 401 402 403 404
void UASWaypointManager::globalAddWaypoint(Waypoint *wp)
{

}

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

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

412 413
        current_state = WP_CLEARLIST;
        current_wp_id = 0;
414 415
        current_partner_systemid = uas.getUASID();
        current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
pixhawk's avatar
pixhawk committed
416

417
        sendWaypointClearAll();
418 419 420 421 422
    }
}

void UASWaypointManager::readWaypoints()
{
423
    emit readGlobalWPFromUAS(true);
424 425
    if(current_state == WP_IDLE)
    {
426 427 428 429 430
        while(waypoints.size()>0)
        {
            Waypoint *t = waypoints.back();
            delete t;
            waypoints.pop_back();
431

432 433
        }

434 435
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
436 437 438 439 440 441

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

442
        sendWaypointRequestList();
443

pixhawk's avatar
pixhawk committed
444 445 446
    }
}

447
void UASWaypointManager::writeWaypoints()
pixhawk's avatar
pixhawk committed
448
{
449 450
    if (current_state == WP_IDLE)
    {
451
        if (waypoints.count() > 0)
452
        {
pixhawk's avatar
pixhawk committed
453
            protocol_timer.start(PROTOCOL_TIMEOUT_MS);
454
            current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
455

456
            current_count = waypoints.count();
pixhawk's avatar
pixhawk committed
457 458 459 460 461 462
            current_state = WP_SENDLIST;
            current_wp_id = 0;
            current_partner_systemid = uas.getUASID();
            current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;

            //clear local buffer
463
            //TODO: Why not replace with waypoint_buffer.clear() ?
pixhawk's avatar
pixhawk committed
464 465 466 467 468
            while(!waypoint_buffer.empty())
            {
                delete waypoint_buffer.back();
                waypoint_buffer.pop_back();
            }
pixhawk's avatar
pixhawk committed
469

470 471
            bool noCurrent = true;

pixhawk's avatar
pixhawk committed
472 473 474 475 476 477
            //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
478
                const Waypoint *cur_s = waypoints.at(i);
pixhawk's avatar
pixhawk committed
479 480

                cur_d->autocontinue = cur_s->getAutoContinue();
481
                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
482 483
                cur_d->orbit = 0;
                cur_d->orbit_direction = 0;
484 485
                cur_d->param1 = cur_s->getOrbit();
                cur_d->param2 = cur_s->getHoldTime();
486 487
                cur_d->frame = cur_s->getFrame();
                cur_d->action = cur_s->getAction();
488
                cur_d->seq = i;     // don't read out the sequence number of the waypoint class
pixhawk's avatar
pixhawk committed
489 490 491 492
                cur_d->x = cur_s->getX();
                cur_d->y = cur_s->getY();
                cur_d->z = cur_s->getZ();
                cur_d->yaw = cur_s->getYaw();
493 494 495

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

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

509 510 511 512 513 514 515 516
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;

517
    emit updateStatusString(QString("clearing waypoint list..."));
518 519 520

    mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca);
    uas.sendMessage(message);
521
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
522 523 524 525 526 527 528 529 530 531 532 533 534

    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;

535
    emit updateStatusString(QString("Updating target waypoint..."));
536 537 538

    mavlink_msg_waypoint_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc);
    uas.sendMessage(message);
539
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
540 541 542 543 544 545 546 547 548 549 550 551 552

    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;

553
    qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
554
    emit updateStatusString(QString("start transmitting waypoints..."));
555 556 557

    mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc);
    uas.sendMessage(message);
558
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
559 560 561 562 563 564 565 566 567 568 569 570

    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;

571
    emit updateStatusString(QString("requesting waypoint list..."));
572 573 574

    mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl);
    uas.sendMessage(message);
575
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
576 577

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


580 581
}

582
void UASWaypointManager::sendWaypointRequest(quint16 seq)
pixhawk's avatar
pixhawk committed
583
{
584 585 586 587 588 589 590
    mavlink_message_t message;
    mavlink_waypoint_request_t wpr;

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

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

593 594
    mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr);
    uas.sendMessage(message);
595
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
596 597

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

pixhawk's avatar
pixhawk committed
600 601 602 603 604 605 606 607 608 609 610 611
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;

612
        emit updateStatusString(QString("sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
pixhawk's avatar
pixhawk committed
613 614 615

        mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp);
        uas.sendMessage(message);
616
        MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
pixhawk's avatar
pixhawk committed
617 618 619

        qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system;
    }
pixhawk's avatar
pixhawk committed
620
}
621 622 623 624 625 626 627 628 629 630 631 632

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);
633
    MG::SLEEP::usleep(PROTOCOL_DELAY_MS * 1000);
634 635 636

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