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

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

lm's avatar
lm committed
40
UASWaypointManager::UASWaypointManager(UAS* _uas)
41 42 43 44 45 46 47
    : uas(_uas),
      current_retries(0),
      current_wp_id(0),
      current_count(0),
      current_state(WP_IDLE),
      current_partner_systemid(0),
      current_partner_compid(0),
48 49
      currentWaypointEditable(NULL),
      protocol_timer(this)
pixhawk's avatar
pixhawk committed
50
{
51 52
    if (uas)
    {
lm's avatar
lm committed
53 54 55 56 57 58 59 60
        uasid = uas->getUASID();
        connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout()));
        connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleLocalPositionChanged(UASInterface*,double,double,double,quint64)));
        connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleGlobalPositionChanged(UASInterface*,double,double,double,quint64)));
    }
    else
    {
        uasid = 0;
61
    }
pixhawk's avatar
pixhawk committed
62 63 64
}

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

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

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

91 92 93 94 95 96
        current_state = WP_IDLE;
        current_count = 0;
        current_wp_id = 0;
        current_partner_systemid = 0;
        current_partner_compid = 0;
    }
97 98
}

99 100 101 102
void UASWaypointManager::handleLocalPositionChanged(UASInterface* mav, double x, double y, double z, quint64 time)
{
    Q_UNUSED(mav);
    Q_UNUSED(time);
pixhawk's avatar
pixhawk committed
103
    if (waypointsEditable.count() > 0 && currentWaypointEditable && (currentWaypointEditable->getFrame() == MAV_FRAME_LOCAL_NED || currentWaypointEditable->getFrame() == MAV_FRAME_LOCAL_ENU))
104
    {
pixhawk's avatar
pixhawk committed
105 106 107
        double xdiff = x-currentWaypointEditable->getX();
        double ydiff = y-currentWaypointEditable->getY();
        double zdiff = z-currentWaypointEditable->getZ();
108 109 110 111 112 113 114
        double dist = sqrt(xdiff*xdiff + ydiff*ydiff + zdiff*zdiff);
        emit waypointDistanceChanged(dist);
    }
}

void UASWaypointManager::handleGlobalPositionChanged(UASInterface* mav, double lat, double lon, double alt, quint64 time)
{
115 116 117 118 119 120 121 122
    Q_UNUSED(mav);
    Q_UNUSED(time);
    if (waypointsEditable.count() > 0 && currentWaypointEditable && (currentWaypointEditable->getFrame() == MAV_FRAME_GLOBAL || currentWaypointEditable->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT))
    {
        // TODO FIXME Calculate distance
        double dist = 0;
        emit waypointDistanceChanged(dist);
    }
123 124
}

125 126
void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, quint16 count)
{
127
    if (current_state == WP_GETLIST && systemId == current_partner_systemid && compId == current_partner_compid) {
128 129 130
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;

131
        // // qDebug() << "got waypoint count (" << count << ") from ID " << systemId;
pixhawk's avatar
pixhawk committed
132

133
        if (count > 0) {
pixhawk's avatar
pixhawk committed
134 135 136
            current_count = count;
            current_wp_id = 0;
            current_state = WP_GETLIST_GETWPS;
137

pixhawk's avatar
pixhawk committed
138
            sendWaypointRequest(current_wp_id);
139
        } else {
140
            protocol_timer.stop();
pixhawk's avatar
pixhawk committed
141
            emit updateStatusString("done.");
142
            // // qDebug() << "No waypoints on UAS " << systemId;
143 144 145 146 147
            current_state = WP_IDLE;
            current_count = 0;
            current_wp_id = 0;
            current_partner_systemid = 0;
            current_partner_compid = 0;
pixhawk's avatar
pixhawk committed
148
        }
149 150
    } 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);
151
    }
152 153
}

lm's avatar
lm committed
154
void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_mission_item_t *wp)
pixhawk's avatar
pixhawk committed
155
{
156
    if (systemId == current_partner_systemid && compId == current_partner_compid && current_state == WP_GETLIST_GETWPS && wp->seq == current_wp_id) {
157 158 159
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;

160
        if(wp->seq == current_wp_id) {
161
            //// // qDebug() << "Got WP: " << wp->seq << wp->x <<  wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << "Frame:"<< (MAV_FRAME) wp->frame << "Command:" << (MAV_CMD) wp->command;
pixhawk's avatar
pixhawk committed
162 163 164 165 166 167 168 169 170 171 172

            Waypoint *lwp_vo = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command);
            addWaypointViewOnly(lwp_vo);


            if (read_to_edit == true) {
                Waypoint *lwp_ed = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command);
                addWaypointEditable(lwp_ed, false);
                if (wp->current == 1) currentWaypointEditable = lwp_ed;
            }

pixhawk's avatar
pixhawk committed
173 174 175 176

            //get next waypoint
            current_wp_id++;

177
            if(current_wp_id < current_count) {
178
                sendWaypointRequest(current_wp_id);
179
            } else {
180 181
                sendWaypointAck(0);

pixhawk's avatar
pixhawk committed
182
                // all waypoints retrieved, change state to idle
pixhawk's avatar
pixhawk committed
183 184 185 186 187
                current_state = WP_IDLE;
                current_count = 0;
                current_wp_id = 0;
                current_partner_systemid = 0;
                current_partner_compid = 0;
188

pixhawk's avatar
pixhawk committed
189
                protocol_timer.stop();
190
                emit readGlobalWPFromUAS(false);
191
                //if (currentWaypointEditable) emit currentWaypointChanged(currentWaypointEditable->getId());
192 193
                emit updateStatusString("done.");

194
                // // qDebug() << "got all waypoints from ID " << systemId;
pixhawk's avatar
pixhawk committed
195
            }
196
        } else {
197
            emit updateStatusString(tr("Waypoint ID mismatch, rejecting waypoint"));
pixhawk's avatar
pixhawk committed
198
        }
199 200
    } 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);
201
    }
pixhawk's avatar
pixhawk committed
202 203
}

lm's avatar
lm committed
204
void UASWaypointManager::handleWaypointAck(quint8 systemId, quint8 compId, mavlink_mission_ack_t *wpa)
205
{
206 207
    if (systemId == current_partner_systemid && compId == current_partner_compid) {
        if((current_state == WP_SENDLIST || current_state == WP_SENDLIST_SENDWPS) && (current_wp_id == waypoint_buffer.count()-1 && wpa->type == 0)) {
208 209
            //all waypoints sent and ack received
            protocol_timer.stop();
210
            current_state = WP_IDLE;
211
            emit updateStatusString("done.");
212
            // // qDebug() << "sent all waypoints to ID " << systemId;
213
        } else if(current_state == WP_CLEARLIST) {
214 215 216
            protocol_timer.stop();
            current_state = WP_IDLE;
            emit updateStatusString("done.");
217
            // // qDebug() << "cleared waypoint list of ID " << systemId;
218
        }
219 220 221
    }
}

lm's avatar
lm committed
222
void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_mission_request_t *wpr)
223
{
224
    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)))) {
225 226
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
227

228
        if (wpr->seq < waypoint_buffer.count()) {
pixhawk's avatar
pixhawk committed
229 230 231
            current_state = WP_SENDLIST_SENDWPS;
            current_wp_id = wpr->seq;
            sendWaypoint(current_wp_id);
232
        } else {
pixhawk's avatar
pixhawk committed
233 234
            //TODO: Error message or something
        }
235 236
    } 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);
237
    }
238 239
}

lm's avatar
lm committed
240
void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_mission_item_reached_t *wpr)
241
{
lm's avatar
lm committed
242
    if (!uas) return;
243
    if (systemId == uasid) {
pixhawk's avatar
pixhawk committed
244 245
        emit updateStatusString(QString("Reached waypoint %1").arg(wpr->seq));
    }
pixhawk's avatar
pixhawk committed
246 247
}

lm's avatar
lm committed
248
void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_mission_current_t *wpc)
pixhawk's avatar
pixhawk committed
249
{
250
    Q_UNUSED(compId);
lm's avatar
lm committed
251
    if (!uas) return;
252
    if (systemId == uasid) {
253
        // FIXME Petri
254
        if (current_state == WP_SETCURRENT) {
255 256
            protocol_timer.stop();
            current_state = WP_IDLE;
257 258

            // update the local main storage
pixhawk's avatar
pixhawk committed
259 260 261 262 263
            if (wpc->seq < waypointsViewOnly.size()) {
                for(int i = 0; i < waypointsViewOnly.size(); i++) {
                    if (waypointsViewOnly[i]->getId() == wpc->seq) {
                        waypointsViewOnly[i]->setCurrent(true);
                        //currentWaypointEditable = waypoints[i];
264
                    } else {
pixhawk's avatar
pixhawk committed
265
                        waypointsViewOnly[i]->setCurrent(false);
266 267 268
                    }
                }
            }
269
        }
270 271 272
        emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq));
        //emit update to UI widgets
        emit currentWaypointChanged(wpc->seq);
pixhawk's avatar
pixhawk committed
273
    }
pixhawk's avatar
pixhawk committed
274 275
}

276
void UASWaypointManager::notifyOfChangeEditable(Waypoint* wp)
277
{
278
    // // qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId();
279
    // If only one waypoint was changed, emit only WP signal
280
    if (wp != NULL) {
281
        emit waypointEditableChanged(uasid, wp);
282
    } else {
283
        emit waypointEditableListChanged();
284
        emit waypointEditableListChanged(uasid);
285
    }
286 287
}

288 289 290
void UASWaypointManager::notifyOfChangeViewOnly(Waypoint* wp)
{
    if (wp != NULL) {
291
        emit waypointViewOnlyChanged(uasid, wp);
292 293
    } else {
        emit waypointViewOnlyListChanged();
294
        emit waypointViewOnlyListChanged(uasid);
295 296
    }
}
297 298


299
int UASWaypointManager::setCurrentWaypoint(quint16 seq)
pixhawk's avatar
pixhawk committed
300
{
pixhawk's avatar
pixhawk committed
301
    if (seq < waypointsViewOnly.size()) {
302
        if(current_state == WP_IDLE) {
303

304 305 306 307 308 309
            //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;
310
            current_partner_systemid = uasid;
lm's avatar
lm committed
311
            current_partner_compid = MAV_COMP_ID_MISSIONPLANNER;
312 313 314 315 316 317 318 319 320 321 322

            sendWaypointSetCurrent(current_wp_id);

            //emit waypointListChanged();

            return 0;
        }
    }
    return -1;
}

323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341
int UASWaypointManager::setCurrentEditable(quint16 seq)
{
    if (seq < waypointsEditable.size()) {
        if(current_state == WP_IDLE) {
            //update local main storage
            for(int i = 0; i < waypointsEditable.size(); i++) {
                if (waypointsEditable[i]->getId() == seq) {
                    waypointsEditable[i]->setCurrent(true);
                    //currentWaypointEditable = waypoints[i];
                } else {
                    waypointsEditable[i]->setCurrent(false);
                }
            }

            return 0;
        }
    }
    return -1;
}
pixhawk's avatar
pixhawk committed
342 343 344 345

void UASWaypointManager::addWaypointViewOnly(Waypoint *wp)
{
    if (wp)
346
    {
pixhawk's avatar
pixhawk committed
347 348 349 350
        waypointsViewOnly.insert(waypointsViewOnly.size(), wp);
        connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeViewOnly(Waypoint*)));

        emit waypointViewOnlyListChanged();
351
        emit waypointViewOnlyListChanged(uasid);
pixhawk's avatar
pixhawk committed
352 353 354 355
    }
}


pixhawk's avatar
pixhawk committed
356
/**
357
 * @warning Make sure the waypoint stays valid for the whole application lifecycle!
pixhawk's avatar
pixhawk committed
358
 * @param enforceFirstActive Enforces that the first waypoint is set as active
359
 * @see createWaypoint() is more suitable for most use cases
pixhawk's avatar
pixhawk committed
360
 */
361
void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActive)
362
{
lm's avatar
lm committed
363 364
    if (wp)
    {
pixhawk's avatar
pixhawk committed
365 366
        wp->setId(waypointsEditable.size());
        if (enforceFirstActive && waypointsEditable.size() == 0)
367 368
        {
            wp->setCurrent(true);
pixhawk's avatar
pixhawk committed
369
            currentWaypointEditable = wp;
370
        }
pixhawk's avatar
pixhawk committed
371
        waypointsEditable.insert(waypointsEditable.size(), wp);
372
        connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*)));
373

374
        emit waypointEditableListChanged();
375
        emit waypointEditableListChanged(uasid);
376
    }
pixhawk's avatar
pixhawk committed
377 378
}

379 380 381 382 383 384
/**
 * @param enforceFirstActive Enforces that the first waypoint is set as active
 */
Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive)
{
    Waypoint* wp = new Waypoint();
pixhawk's avatar
pixhawk committed
385 386
    wp->setId(waypointsEditable.size());
    if (enforceFirstActive && waypointsEditable.size() == 0)
387 388
    {
        wp->setCurrent(true);
pixhawk's avatar
pixhawk committed
389
        currentWaypointEditable = wp;
390
    }
pixhawk's avatar
pixhawk committed
391
    waypointsEditable.insert(waypointsEditable.size(), wp);
392
    connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*)));
393

394
    emit waypointEditableListChanged();
395
    emit waypointEditableListChanged(uasid);
396 397 398
    return wp;
}

399
int UASWaypointManager::removeWaypoint(quint16 seq)
400
{
pixhawk's avatar
pixhawk committed
401
    if (seq < waypointsEditable.size())
402
    {
pixhawk's avatar
pixhawk committed
403 404
        Waypoint *t = waypointsEditable[seq];
        waypointsEditable.remove(seq);
405
        delete t;
406
        t = NULL;
407

pixhawk's avatar
pixhawk committed
408
        for(int i = seq; i < waypointsEditable.size(); i++)
409
        {
pixhawk's avatar
pixhawk committed
410
            waypointsEditable[i]->setId(i);
411
        }
Alejandro's avatar
Alejandro committed
412

413
        emit waypointEditableListChanged();
414
        emit waypointEditableListChanged(uasid);
415 416 417 418 419
        return 0;
    }
    return -1;
}

420
void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq)
421
{
pixhawk's avatar
pixhawk committed
422
    if (cur_seq != new_seq && cur_seq < waypointsEditable.size() && new_seq < waypointsEditable.size())
423
    {
pixhawk's avatar
pixhawk committed
424
        Waypoint *t = waypointsEditable[cur_seq];
425
        if (cur_seq < new_seq) {
426 427
            for (int i = cur_seq; i < new_seq; i++)
            {
pixhawk's avatar
pixhawk committed
428
                waypointsEditable[i] = waypointsEditable[i+1];
429
            }
430 431 432 433 434
        }
        else
        {
            for (int i = cur_seq; i > new_seq; i--)
            {
pixhawk's avatar
pixhawk committed
435
                waypointsEditable[i] = waypointsEditable[i-1];
436 437
            }
        }
pixhawk's avatar
pixhawk committed
438
        waypointsEditable[new_seq] = t;
439

440
        emit waypointEditableListChanged();
441
        emit waypointEditableListChanged(uasid);
442 443 444
    }
}

445
void UASWaypointManager::saveWaypoints(const QString &saveFile)
446 447 448 449 450 451
{
    QFile file(saveFile);
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
        return;

    QTextStream out(&file);
452 453

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

pixhawk's avatar
pixhawk committed
456
    for (int i = 0; i < waypointsEditable.size(); i++)
457
    {
pixhawk's avatar
pixhawk committed
458 459
        waypointsEditable[i]->setId(i);
        waypointsEditable[i]->save(out);
460 461 462 463
    }
    file.close();
}

464
void UASWaypointManager::loadWaypoints(const QString &loadFile)
465 466 467 468 469
{
    QFile file(loadFile);
    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
        return;

pixhawk's avatar
pixhawk committed
470 471 472
    while(waypointsEditable.size()>0) {
        Waypoint *t = waypointsEditable[0];
        waypointsEditable.remove(0);
473 474 475 476
        delete t;
    }

    QTextStream in(&file);
477 478 479

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

480
    if (!(version.size() == 3 && version[0] == "QGC" && version[1] == "WPL" && version[2] == "120"))
481
    {
482
        emit updateStatusString(tr("The waypoint file is not compatible with the current version of QGroundControl."));
483 484 485 486 487
    }
    else
    {
        while (!in.atEnd())
        {
488
            Waypoint *t = new Waypoint();
489 490
            if(t->load(in))
            {
pixhawk's avatar
pixhawk committed
491 492
                t->setId(waypointsEditable.size());
                waypointsEditable.insert(waypointsEditable.size(), t);
493 494 495
            }
            else
            {
496 497
                emit updateStatusString(tr("The waypoint file is corrupted. Load operation only partly succesful."));
                //MainWindow::instance()->showCriticalMessage(tr("Error loading waypoint file"),);
498 499
                break;
            }
500 501
        }
    }
502

503 504
    file.close();

505
    emit loadWPFile();
506
    emit waypointEditableListChanged();
507
    emit waypointEditableListChanged(uasid);
508 509 510
}

void UASWaypointManager::clearWaypointList()
pixhawk's avatar
pixhawk committed
511
{
512 513
    if(current_state == WP_IDLE)
    {
pixhawk's avatar
pixhawk committed
514
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
515
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
516

517 518
        current_state = WP_CLEARLIST;
        current_wp_id = 0;
519
        current_partner_systemid = uasid;
lm's avatar
lm committed
520
        current_partner_compid = MAV_COMP_ID_MISSIONPLANNER;
pixhawk's avatar
pixhawk committed
521

522
        sendWaypointClearAll();
523 524 525
    }
}

526 527 528 529 530 531
const QVector<Waypoint *> UASWaypointManager::getGlobalFrameWaypointList()
{
    // TODO Keep this global frame list up to date
    // with complete waypoint list
    // instead of filtering on each request
    QVector<Waypoint*> wps;
pixhawk's avatar
pixhawk committed
532
    foreach (Waypoint* wp, waypointsEditable)
533 534 535
    {
        if (wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)
        {
536 537 538 539 540 541
            wps.append(wp);
        }
    }
    return wps;
}

lm's avatar
lm committed
542 543 544 545 546 547
const QVector<Waypoint *> UASWaypointManager::getGlobalFrameAndNavTypeWaypointList()
{
    // TODO Keep this global frame list up to date
    // with complete waypoint list
    // instead of filtering on each request
    QVector<Waypoint*> wps;
pixhawk's avatar
pixhawk committed
548
    foreach (Waypoint* wp, waypointsEditable)
549 550 551
    {
        if ((wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && wp->isNavigationType())
        {
lm's avatar
lm committed
552 553 554 555 556 557
            wps.append(wp);
        }
    }
    return wps;
}

LM's avatar
LM committed
558 559 560 561 562 563
const QVector<Waypoint *> UASWaypointManager::getNavTypeWaypointList()
{
    // TODO Keep this global frame list up to date
    // with complete waypoint list
    // instead of filtering on each request
    QVector<Waypoint*> wps;
pixhawk's avatar
pixhawk committed
564
    foreach (Waypoint* wp, waypointsEditable)
565 566 567
    {
        if (wp->isNavigationType())
        {
LM's avatar
LM committed
568 569 570 571 572 573
            wps.append(wp);
        }
    }
    return wps;
}

574 575
int UASWaypointManager::getIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
576
    return waypointsEditable.indexOf(wp);
577 578
}

579 580
int UASWaypointManager::getGlobalFrameIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
581
    // Search through all waypointsEditable,
582 583
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
584
    foreach (Waypoint* p, waypointsEditable) {
585 586 587 588
        if (p->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)
        {
            if (p == wp)
            {
589 590 591 592 593 594 595 596 597
                return i;
            }
            i++;
        }
    }

    return -1;
}

lm's avatar
lm committed
598 599
int UASWaypointManager::getGlobalFrameAndNavTypeIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
600
    // Search through all waypointsEditable,
lm's avatar
lm committed
601 602
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
603
    foreach (Waypoint* p, waypointsEditable) {
604
        if ((p->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && p->isNavigationType())
605 606 607
        {
            if (p == wp)
            {
lm's avatar
lm committed
608 609 610 611 612 613 614 615 616
                return i;
            }
            i++;
        }
    }

    return -1;
}

LM's avatar
LM committed
617 618
int UASWaypointManager::getNavTypeIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
619
    // Search through all waypointsEditable,
LM's avatar
LM committed
620 621
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
622
    foreach (Waypoint* p, waypointsEditable)
623 624 625 626 627
    {
        if (p->isNavigationType())
        {
            if (p == wp)
            {
LM's avatar
LM committed
628 629 630 631 632 633 634 635 636
                return i;
            }
            i++;
        }
    }

    return -1;
}

637 638
int UASWaypointManager::getGlobalFrameCount()
{
pixhawk's avatar
pixhawk committed
639
    // Search through all waypointsEditable,
640 641
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
642
    foreach (Waypoint* p, waypointsEditable)
643
    {
644 645
        if (p->getFrame() == MAV_FRAME_GLOBAL || p->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)
        {
646 647 648 649 650 651 652
            i++;
        }
    }

    return i;
}

lm's avatar
lm committed
653 654
int UASWaypointManager::getGlobalFrameAndNavTypeCount()
{
pixhawk's avatar
pixhawk committed
655
    // Search through all waypointsEditable,
lm's avatar
lm committed
656 657
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
658
    foreach (Waypoint* p, waypointsEditable) {
659
        if ((p->getFrame() == MAV_FRAME_GLOBAL || p->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && p->isNavigationType())
660
        {
lm's avatar
lm committed
661 662 663 664 665 666 667
            i++;
        }
    }

    return i;
}

LM's avatar
LM committed
668 669
int UASWaypointManager::getNavTypeCount()
{
pixhawk's avatar
pixhawk committed
670
    // Search through all waypointsEditable,
LM's avatar
LM committed
671 672
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
673
    foreach (Waypoint* p, waypointsEditable) {
LM's avatar
LM committed
674 675 676 677 678 679 680 681
        if (p->isNavigationType()) {
            i++;
        }
    }

    return i;
}

682 683
int UASWaypointManager::getLocalFrameCount()
{
pixhawk's avatar
pixhawk committed
684
    // Search through all waypointsEditable,
685 686
    // counting only those in global frame
    int i = 0;
pixhawk's avatar
pixhawk committed
687
    foreach (Waypoint* p, waypointsEditable)
lm's avatar
lm committed
688
    {
689
        if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU)
lm's avatar
lm committed
690
        {
691 692 693 694 695 696 697 698 699
            i++;
        }
    }

    return i;
}

int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
700
    // Search through all waypointsEditable,
701 702
    // counting only those in local frame
    int i = 0;
pixhawk's avatar
pixhawk committed
703
    foreach (Waypoint* p, waypointsEditable)
lm's avatar
lm committed
704 705 706
    {
        if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU)
        {
707 708
            if (p == wp)
            {
709 710 711 712 713 714 715 716 717 718 719
                return i;
            }
            i++;
        }
    }

    return -1;
}

int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp)
{
pixhawk's avatar
pixhawk committed
720
    // Search through all waypointsEditable,
721 722
    // counting only those in mission frame
    int i = 0;
pixhawk's avatar
pixhawk committed
723
    foreach (Waypoint* p, waypointsEditable)
lm's avatar
lm committed
724
    {
725 726 727 728
        if (p->getFrame() == MAV_FRAME_MISSION)
        {
            if (p == wp)
            {
729 730 731 732 733 734 735 736 737
                return i;
            }
            i++;
        }
    }

    return -1;
}

pixhawk's avatar
pixhawk committed
738 739 740 741 742

/**
 * @param readToEdit If true, incoming waypoints will be copied both to "edit"-tab and "view"-tab. Otherwise, only to "view"-tab.
 */
void UASWaypointManager::readWaypoints(bool readToEdit)
743
{
pixhawk's avatar
pixhawk committed
744
    read_to_edit = readToEdit;
745
    emit readGlobalWPFromUAS(true);
746
    if(current_state == WP_IDLE) {
747

748 749 750 751 752 753 754 755 756 757 758 759
        //Clear the old view-list before receiving the new one
        while(waypointsViewOnly.size()>0) {
            delete waypointsViewOnly.back();
            waypointsViewOnly.pop_back();
        }

        //Clear the old edit-list before receiving the new one
        if (read_to_edit == true){
            while(waypointsEditable.size()>0) {
                delete waypointsEditable.back();
                waypointsEditable.pop_back();
            }
760 761
        }

762 763
        protocol_timer.start(PROTOCOL_TIMEOUT_MS);
        current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
764 765 766

        current_state = WP_GETLIST;
        current_wp_id = 0;
767
        current_partner_systemid = uasid;
lm's avatar
lm committed
768
        current_partner_compid = MAV_COMP_ID_MISSIONPLANNER;
pixhawk's avatar
pixhawk committed
769

770
        sendWaypointRequestList();
771

pixhawk's avatar
pixhawk committed
772 773 774
    }
}

775
void UASWaypointManager::writeWaypoints()
pixhawk's avatar
pixhawk committed
776
{
777
    if (current_state == WP_IDLE) {
pixhawk's avatar
pixhawk committed
778
        // Send clear all if count == 0
pixhawk's avatar
pixhawk committed
779
        if (waypointsEditable.count() > 0) {
pixhawk's avatar
pixhawk committed
780
            protocol_timer.start(PROTOCOL_TIMEOUT_MS);
781
            current_retries = PROTOCOL_MAX_RETRIES;
pixhawk's avatar
pixhawk committed
782

pixhawk's avatar
pixhawk committed
783
            current_count = waypointsEditable.count();
pixhawk's avatar
pixhawk committed
784 785
            current_state = WP_SENDLIST;
            current_wp_id = 0;
786
            current_partner_systemid = uasid;
lm's avatar
lm committed
787
            current_partner_compid = MAV_COMP_ID_MISSIONPLANNER;
pixhawk's avatar
pixhawk committed
788 789

            //clear local buffer
pixhawk's avatar
pixhawk committed
790 791 792
            // 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.
793
            while(!waypoint_buffer.empty()) {
pixhawk's avatar
pixhawk committed
794 795 796
                delete waypoint_buffer.back();
                waypoint_buffer.pop_back();
            }
pixhawk's avatar
pixhawk committed
797

798 799
            bool noCurrent = true;

pixhawk's avatar
pixhawk committed
800
            //copy waypoint data to local buffer
801
            for (int i=0; i < current_count; i++) {
lm's avatar
lm committed
802 803 804
                waypoint_buffer.push_back(new mavlink_mission_item_t);
                mavlink_mission_item_t *cur_d = waypoint_buffer.back();
                memset(cur_d, 0, sizeof(mavlink_mission_item_t));   //initialize with zeros
pixhawk's avatar
pixhawk committed
805
                const Waypoint *cur_s = waypointsEditable.at(i);
pixhawk's avatar
pixhawk committed
806 807

                cur_d->autocontinue = cur_s->getAutoContinue();
808
                cur_d->current = cur_s->getCurrent() & noCurrent;   //make sure only one current waypoint is selected, the first selected will be chosen
809 810
                cur_d->param1 = cur_s->getParam1();
                cur_d->param2 = cur_s->getParam2();
lm's avatar
lm committed
811 812
                cur_d->param3 = cur_s->getParam3();
                cur_d->param4 = cur_s->getParam4();
813
                cur_d->frame = cur_s->getFrame();
lm's avatar
lm committed
814
                cur_d->command = cur_s->getAction();
815
                cur_d->seq = i;     // don't read out the sequence number of the waypoint class
pixhawk's avatar
pixhawk committed
816 817 818
                cur_d->x = cur_s->getX();
                cur_d->y = cur_s->getY();
                cur_d->z = cur_s->getZ();
819 820 821

                if (cur_s->getCurrent() && noCurrent)
                    noCurrent = false;
822 823
                if (i == (current_count - 1) && noCurrent == true) //not a single waypoint was set as "current"
                    cur_d->current = true; // set the last waypoint as current. Or should it better be the first waypoint ?
pixhawk's avatar
pixhawk committed
824
            }
825

826 827 828



pixhawk's avatar
pixhawk committed
829
            //send the waypoint count to UAS (this starts the send transaction)
830
            sendWaypointCount();
pixhawk's avatar
pixhawk committed
831
        }
pixhawk's avatar
pixhawk committed
832
    } else if (waypointsEditable.count() == 0) {
833
        sendWaypointClearAll();
834
    } else {
835
        //we're in another transaction, ignore command
836
        qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
837
    }
pixhawk's avatar
pixhawk committed
838 839
}

840 841
void UASWaypointManager::sendWaypointClearAll()
{
lm's avatar
lm committed
842
    if (!uas) return;
843
    mavlink_message_t message;
lm's avatar
lm committed
844
    mavlink_mission_clear_all_t wpca;
845

846
    wpca.target_system = uasid;
lm's avatar
lm committed
847
    wpca.target_component = MAV_COMP_ID_MISSIONPLANNER;
848

849
    emit updateStatusString(QString("Clearing waypoint list..."));
850

lm's avatar
lm committed
851 852 853
    mavlink_msg_mission_clear_all_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpca);
    if (uas) uas->sendMessage(message);
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
854

855
    // // qDebug() << "sent waypoint clear all to ID " << wpca.target_system;
856 857 858 859
}

void UASWaypointManager::sendWaypointSetCurrent(quint16 seq)
{
lm's avatar
lm committed
860
    if (!uas) return;
861
    mavlink_message_t message;
lm's avatar
lm committed
862
    mavlink_mission_set_current_t wpsc;
863

864
    wpsc.target_system = uasid;
lm's avatar
lm committed
865
    wpsc.target_component = MAV_COMP_ID_MISSIONPLANNER;
866 867
    wpsc.seq = seq;

868
    emit updateStatusString(QString("Updating target waypoint..."));
869

lm's avatar
lm committed
870 871 872
    mavlink_msg_mission_set_current_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpsc);
    if (uas) uas->sendMessage(message);
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
873

874
    // // qDebug() << "sent waypoint set current (" << wpsc.seq << ") to ID " << wpsc.target_system;
875 876 877 878
}

void UASWaypointManager::sendWaypointCount()
{
lm's avatar
lm committed
879
    if (!uas) return;
880
    mavlink_message_t message;
lm's avatar
lm committed
881
    mavlink_mission_count_t wpc;
882

883
    wpc.target_system = uasid;
lm's avatar
lm committed
884
    wpc.target_component = MAV_COMP_ID_MISSIONPLANNER;
885 886
    wpc.count = current_count;

887
    // // qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
888
    emit updateStatusString(QString("Starting to transmit waypoints..."));
889

lm's avatar
lm committed
890 891 892
    mavlink_msg_mission_count_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpc);
    if (uas) uas->sendMessage(message);
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
893

894
    // // qDebug() << "sent waypoint count (" << wpc.count << ") to ID " << wpc.target_system;
895 896 897 898
}

void UASWaypointManager::sendWaypointRequestList()
{
lm's avatar
lm committed
899
    if (!uas) return;
900
    mavlink_message_t message;
lm's avatar
lm committed
901
    mavlink_mission_request_list_t wprl;
902

903
    wprl.target_system = uasid;
lm's avatar
lm committed
904
    wprl.target_component = MAV_COMP_ID_MISSIONPLANNER;
905

906
    emit updateStatusString(QString("Requesting waypoint list..."));
907

lm's avatar
lm committed
908 909 910
    mavlink_msg_mission_request_list_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wprl);
    if (uas) uas->sendMessage(message);
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
911

912
    // // qDebug() << "sent waypoint list request to ID " << wprl.target_system;
913 914


915 916
}

917
void UASWaypointManager::sendWaypointRequest(quint16 seq)
pixhawk's avatar
pixhawk committed
918
{
lm's avatar
lm committed
919
    if (!uas) return;
920
    mavlink_message_t message;
lm's avatar
lm committed
921
    mavlink_mission_request_t wpr;
922

923
    wpr.target_system = uasid;
lm's avatar
lm committed
924
    wpr.target_component = MAV_COMP_ID_MISSIONPLANNER;
925 926
    wpr.seq = seq;

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

lm's avatar
lm committed
929 930 931
    mavlink_msg_mission_request_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpr);
    if (uas) uas->sendMessage(message);
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
932

933
    // // qDebug() << "sent waypoint request (" << wpr.seq << ") to ID " << wpr.target_system;
pixhawk's avatar
pixhawk committed
934
}
pixhawk's avatar
pixhawk committed
935

pixhawk's avatar
pixhawk committed
936 937
void UASWaypointManager::sendWaypoint(quint16 seq)
{
lm's avatar
lm committed
938
    if (!uas) return;
pixhawk's avatar
pixhawk committed
939
    mavlink_message_t message;
940
    // // qDebug() <<" WP Buffer count: "<<waypoint_buffer.count();
941

942
    if (seq < waypoint_buffer.count()) {
pixhawk's avatar
pixhawk committed
943

lm's avatar
lm committed
944
        mavlink_mission_item_t *wp;
945

pixhawk's avatar
pixhawk committed
946 947

        wp = waypoint_buffer.at(seq);
948
        wp->target_system = uasid;
lm's avatar
lm committed
949
        wp->target_component = MAV_COMP_ID_MISSIONPLANNER;
pixhawk's avatar
pixhawk committed
950

951
        emit updateStatusString(QString("Sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
952

953
        // // qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system<<" WP Buffer count: "<<waypoint_buffer.count();
pixhawk's avatar
pixhawk committed
954

lm's avatar
lm committed
955 956 957
        mavlink_msg_mission_item_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, wp);
        if (uas) uas->sendMessage(message);
        QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
pixhawk's avatar
pixhawk committed
958
    }
pixhawk's avatar
pixhawk committed
959
}
960 961 962

void UASWaypointManager::sendWaypointAck(quint8 type)
{
lm's avatar
lm committed
963
    if (!uas) return;
964
    mavlink_message_t message;
lm's avatar
lm committed
965
    mavlink_mission_ack_t wpa;
966

967
    wpa.target_system = uasid;
lm's avatar
lm committed
968
    wpa.target_component = MAV_COMP_ID_MISSIONPLANNER;
969 970
    wpa.type = type;

lm's avatar
lm committed
971 972 973
    mavlink_msg_mission_ack_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpa);
    if (uas) uas->sendMessage(message);
    QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
974

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