MissionItem.cc 27.8 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36
/*===================================================================
QGroundControl Open Source Ground Control Station

(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL is free software: you can redistribute it and/or modify
    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.

    QGROUNDCONTROL is distributed in the hope that it will be useful,
    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
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief MissionItem class
 *
 *   @author Benjamin Knecht <mavteam@student.ethz.ch>
 *   @author Petri Tanskanen <mavteam@student.ethz.ch>
 *
 */

#include <QStringList>
#include <QDebug>

#include "MissionItem.h"

Don Gagne's avatar
Don Gagne committed
37
QGC_LOGGING_CATEGORY(MissionItemLog, "MissionItemLog")
38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54

QDebug operator<<(QDebug dbg, const MissionItem& missionItem)
{
    QDebugStateSaver saver(dbg);
    dbg.nospace() << "MissionItem(" << missionItem.coordinate() << ")";
    
    return dbg;
}

QDebug operator<<(QDebug dbg, const MissionItem* missionItem)
{
    QDebugStateSaver saver(dbg);
    dbg.nospace() << "MissionItem(" << missionItem->coordinate() << ")";
    
    return dbg;
}

Don Gagne's avatar
Don Gagne committed
55 56 57 58
const MissionItem::MavCmd2Name_t  MissionItem::_rgMavCmd2Name[_cMavCmd2Name] = {
    { MAV_CMD_NAV_WAYPOINT,         "Waypoint" },
    { MAV_CMD_NAV_LOITER_UNLIM,     "Loiter" },
    { MAV_CMD_NAV_LOITER_TURNS,     "Loiter (turns)" },
Don Gagne's avatar
Don Gagne committed
59
    { MAV_CMD_NAV_LOITER_TIME,      "Loiter (seconds)" },
Don Gagne's avatar
Don Gagne committed
60 61 62 63 64 65 66 67 68 69
    { MAV_CMD_NAV_RETURN_TO_LAUNCH, "Return Home" },
    { MAV_CMD_NAV_LAND,             "Land" },
    { MAV_CMD_NAV_TAKEOFF,          "Takeoff" },
    { MAV_CMD_CONDITION_DELAY,      "Delay" },
    { MAV_CMD_DO_JUMP,              "Jump To Command" },
};

MissionItem::MissionItem(QObject*       parent,
                         int            sequenceNumber,
                         QGeoCoordinate coordinate,
Don Gagne's avatar
Don Gagne committed
70
                         int            command,
Don Gagne's avatar
Don Gagne committed
71 72 73 74 75 76
                         double         param1,
                         double         param2,
                         double         param3,
                         double         param4,
                         bool           autocontinue,
                         bool           isCurrentItem,
Don Gagne's avatar
Don Gagne committed
77
                         int            frame)
78
    : QObject(parent)
Don Gagne's avatar
Don Gagne committed
79
    , _sequenceNumber(sequenceNumber)
Don Gagne's avatar
Don Gagne committed
80
    , _frame(-1)    // Forces set of _altitudeRelativeToHomeFact
Don Gagne's avatar
Don Gagne committed
81
    , _command((MavlinkQmlSingleton::Qml_MAV_CMD)command)
82
    , _autocontinue(autocontinue)
Don Gagne's avatar
Don Gagne committed
83
    , _isCurrentItem(isCurrentItem)
84
    , _reachedTime(0)
Don Gagne's avatar
Don Gagne committed
85
    , _yawRadiansFact(NULL)
Don Gagne's avatar
Don Gagne committed
86
    ,_dirty(false)
87
{
88 89 90 91 92 93 94 95
    _latitudeFact                   = new Fact(0, "Latitude:",                      FactMetaData::valueTypeDouble, this);
    _longitudeFact                  = new Fact(0, "Longitude:",                     FactMetaData::valueTypeDouble, this);
    _altitudeFact                   = new Fact(0, "Altitude:",                      FactMetaData::valueTypeDouble, this);
    _yawRadiansFact                 = new Fact(0, "Heading:",                       FactMetaData::valueTypeDouble, this);
    _loiterOrbitRadiusFact          = new Fact(0, "Radius:",                        FactMetaData::valueTypeDouble, this);
    _param1Fact                     = new Fact(0, QString(),                        FactMetaData::valueTypeDouble, this);
    _param2Fact                     = new Fact(0, QString(),                        FactMetaData::valueTypeDouble, this);
    _altitudeRelativeToHomeFact     = new Fact(0, "Altitude is relative to home",   FactMetaData::valueTypeDouble, this);
Don Gagne's avatar
Don Gagne committed
96

97
    setFrame(frame);
Don Gagne's avatar
Don Gagne committed
98
    
99
    setCoordinate(coordinate);
Don Gagne's avatar
Don Gagne committed
100 101 102 103
    setParam1(param1);
    setParam2(param2);
    setYawRadians(param4);
    setLoiterOrbitRadius(param3);
Don Gagne's avatar
Don Gagne committed
104 105 106
    
    // FIXME: Need to fill out more meta data
    
107 108 109 110 111 112 113 114 115 116 117
    FactMetaData* latitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble, _latitudeFact);
    latitudeMetaData->setUnits("deg");
    
    FactMetaData* longitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble, _longitudeFact);
    longitudeMetaData->setUnits("deg");
    
    FactMetaData* altitudeMetaData = new FactMetaData(FactMetaData::valueTypeDouble, _altitudeFact);
    altitudeMetaData->setUnits("meters");
    
    FactMetaData* yawMetaData = new FactMetaData(FactMetaData::valueTypeDouble, _yawRadiansFact);
    yawMetaData->setUnits("deg");
Don Gagne's avatar
Don Gagne committed
118 119
    
    _pitchMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this);
120
    _pitchMetaData->setUnits("deg");
Don Gagne's avatar
Don Gagne committed
121 122 123 124 125 126 127
    
    _acceptanceRadiusMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this);
    _acceptanceRadiusMetaData->setUnits("meters");
    
    _holdTimeMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this);
    _holdTimeMetaData->setUnits("seconds");
    
Don Gagne's avatar
Don Gagne committed
128 129
    FactMetaData* loiterOrbitRadiusMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this);
    loiterOrbitRadiusMetaData->setUnits("meters");
Don Gagne's avatar
Don Gagne committed
130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145
    
    _loiterTurnsMetaData = new FactMetaData(FactMetaData::valueTypeInt32, this);
    _loiterTurnsMetaData->setUnits("count");
    
    _loiterSecondsMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this);
    _loiterSecondsMetaData->setUnits("seconds");
    
    _delaySecondsMetaData = new FactMetaData(FactMetaData::valueTypeDouble, this);
    _delaySecondsMetaData->setUnits("seconds");
    
    _jumpSequenceMetaData = new FactMetaData(FactMetaData::valueTypeInt32, this);
    _jumpSequenceMetaData->setUnits("#");
    
    _jumpRepeatMetaData = new FactMetaData(FactMetaData::valueTypeInt32, this);
    _jumpRepeatMetaData->setUnits("count");
    
146 147 148
    _latitudeFact->setMetaData(latitudeMetaData);
    _longitudeFact->setMetaData(longitudeMetaData);
    _altitudeFact->setMetaData(altitudeMetaData);
Don Gagne's avatar
Don Gagne committed
149 150
    _yawRadiansFact->setMetaData(yawMetaData);
    _loiterOrbitRadiusFact->setMetaData(loiterOrbitRadiusMetaData);
Don Gagne's avatar
Don Gagne committed
151 152 153 154 155 156 157 158 159 160

    // Connect to valueChanged to track dirty state
    connect(_latitudeFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_longitudeFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_altitudeFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_yawRadiansFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_loiterOrbitRadiusFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_param1Fact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_param2Fact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_altitudeRelativeToHomeFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
161 162 163 164 165
    
    // Connect valueChanged signals so we can output coordinateChanged signal
    connect(_latitudeFact, &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged);
    connect(_longitudeFact, &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged);
    connect(_altitudeFact, &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged);
166 167
}

Don Gagne's avatar
Don Gagne committed
168 169
MissionItem::MissionItem(const MissionItem& other, QObject* parent)
    : QObject(parent)
170
{
171 172 173 174 175 176 177 178
    _latitudeFact               = new Fact(this);
    _longitudeFact              = new Fact(this);
    _altitudeFact               = new Fact(this);
    _yawRadiansFact             = new Fact(this);
    _loiterOrbitRadiusFact      = new Fact(this);
    _param1Fact                 = new Fact(this);
    _param2Fact                 = new Fact(this);
    _altitudeRelativeToHomeFact = new Fact(this);
Don Gagne's avatar
Don Gagne committed
179 180 181 182 183 184 185 186 187 188 189
    
    _pitchMetaData = new FactMetaData(this);
    
    _acceptanceRadiusMetaData = new FactMetaData(this);
    _holdTimeMetaData = new FactMetaData(this);
    _loiterTurnsMetaData = new FactMetaData(this);
    _loiterSecondsMetaData = new FactMetaData(this);
    _delaySecondsMetaData = new FactMetaData(this);
    _jumpSequenceMetaData = new FactMetaData(this);
    _jumpRepeatMetaData = new FactMetaData(this);

190 191 192 193 194 195 196 197 198 199 200 201 202 203 204
    // Connect to valueChanged to track dirty state
    connect(_latitudeFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_longitudeFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_altitudeFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_yawRadiansFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_loiterOrbitRadiusFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_param1Fact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_param2Fact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    connect(_altitudeRelativeToHomeFact, &Fact::valueChanged, this, &MissionItem::_factValueChanged);
    
    // Connect valueChanged signals so we can output coordinateChanged signal
    connect(_latitudeFact, &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged);
    connect(_longitudeFact, &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged);
    connect(_altitudeFact, &Fact::valueChanged, this, &MissionItem::_coordinateFactChanged);
    
205 206 207 208 209 210 211 212 213
    *this = other;
}

MissionItem::~MissionItem()
{    
}

const MissionItem& MissionItem::operator=(const MissionItem& other)
{
214 215 216 217 218 219 220
    _sequenceNumber             = other._sequenceNumber;
    _isCurrentItem              = other._isCurrentItem;
    _frame                      = other._frame;
    _command                    = other._command;
    _autocontinue               = other._autocontinue;
    _reachedTime                = other._reachedTime;
    _altitudeRelativeToHomeFact = other._altitudeRelativeToHomeFact;
Don Gagne's avatar
Don Gagne committed
221
    _dirty                      = other._dirty;
Don Gagne's avatar
Don Gagne committed
222
    
223 224 225
    *_latitudeFact              = *other._latitudeFact;
    *_longitudeFact             = *other._longitudeFact;
    *_altitudeFact              = *other._altitudeFact;
Don Gagne's avatar
Don Gagne committed
226 227 228 229 230 231 232 233 234 235 236 237 238 239
    *_yawRadiansFact            = *other._yawRadiansFact;
    *_loiterOrbitRadiusFact     = *other._loiterOrbitRadiusFact;
    *_param1Fact                = *other._param1Fact;
    *_param2Fact                = *other._param2Fact;
    
    *_pitchMetaData             = *other._pitchMetaData;
    *_acceptanceRadiusMetaData  = *other._acceptanceRadiusMetaData;
    *_holdTimeMetaData          = *other._holdTimeMetaData;
    *_loiterTurnsMetaData       = *other._loiterTurnsMetaData;
    *_loiterSecondsMetaData     = *other._loiterSecondsMetaData;
    *_delaySecondsMetaData      = *other._delaySecondsMetaData;
    *_jumpSequenceMetaData      = *other._jumpSequenceMetaData;
    *_jumpRepeatMetaData        = *other._jumpRepeatMetaData;
    
240 241 242 243 244
    return *this;
}

bool MissionItem::isNavigationType()
{
Don Gagne's avatar
Don Gagne committed
245
    return (_command < MavlinkQmlSingleton::MAV_CMD_NAV_LAST);
246 247 248 249 250
}

void MissionItem::save(QTextStream &saveStream)
{
    QString position("%1\t%2\t%3");
Don Gagne's avatar
Don Gagne committed
251 252 253
    position = position.arg(x(), 0, 'g', 18);
    position = position.arg(y(), 0, 'g', 18);
    position = position.arg(z(), 0, 'g', 18);
254
    QString parameters("%1\t%2\t%3\t%4");
Don Gagne's avatar
Don Gagne committed
255
    parameters = parameters.arg(param1(), 0, 'g', 18).arg(param2(), 0, 'g', 18).arg(loiterOrbitRadius(), 0, 'g', 18).arg(yawRadians(), 0, 'g', 18);
256 257
    // FORMAT: <INDEX> <CURRENT WP> <COORD FRAME> <COMMAND> <PARAM1> <PARAM2> <PARAM3> <PARAM4> <PARAM5/X/LONGITUDE> <PARAM6/Y/LATITUDE> <PARAM7/Z/ALTITUDE> <AUTOCONTINUE> <DESCRIPTION>
    // as documented here: http://qgroundcontrol.org/waypoint_protocol
Don Gagne's avatar
Don Gagne committed
258
    saveStream << this->sequenceNumber() << "\t" << this->isCurrentItem() << "\t" << this->frame() << "\t" << this->command() << "\t"  << parameters << "\t" << position  << "\t" << this->autoContinue() << "\r\n"; //"\t" << this->getDescription() << "\r\n";
259 260 261 262 263 264
}

bool MissionItem::load(QTextStream &loadStream)
{
    const QStringList &wpParams = loadStream.readLine().split("\t");
    if (wpParams.size() == 12) {
Don Gagne's avatar
Don Gagne committed
265 266
        setSequenceNumber(wpParams[0].toInt());
        setIsCurrentItem(wpParams[1].toInt() == 1 ? true : false);
267
        setFrame(wpParams[2].toInt());
Don Gagne's avatar
Don Gagne committed
268
        setAction(wpParams[3].toInt());
Don Gagne's avatar
Don Gagne committed
269 270 271
        setParam1(wpParams[4].toDouble());
        setParam2(wpParams[5].toDouble());
        setLoiterOrbitRadius(wpParams[6].toDouble());
Don Gagne's avatar
Don Gagne committed
272
        setYawRadians(wpParams[7].toDouble());
Don Gagne's avatar
Don Gagne committed
273 274 275
        setLatitude(wpParams[8].toDouble());
        setLongitude(wpParams[9].toDouble());
        setAltitude(wpParams[10].toDouble());
276 277 278 279 280 281 282
        _autocontinue = (wpParams[11].toInt() == 1 ? true : false);
        return true;
    }
    return false;
}


Don Gagne's avatar
Don Gagne committed
283
void MissionItem::setSequenceNumber(int sequenceNumber)
284
{
Don Gagne's avatar
Don Gagne committed
285 286
    _sequenceNumber = sequenceNumber;
    emit sequenceNumberChanged(_sequenceNumber);
287 288 289 290 291 292 293
    emit changed(this);
}

void MissionItem::setX(double x)
{
    if (!isinf(x) && !isnan(x) && ((_frame == MAV_FRAME_LOCAL_NED) || (_frame == MAV_FRAME_LOCAL_ENU)))
    {
Don Gagne's avatar
Don Gagne committed
294
        setLatitude(x);
295 296 297 298 299 300 301
    }
}

void MissionItem::setY(double y)
{
    if (!isinf(y) && !isnan(y) && ((_frame == MAV_FRAME_LOCAL_NED) || (_frame == MAV_FRAME_LOCAL_ENU)))
    {
Don Gagne's avatar
Don Gagne committed
302
        setLongitude(y);
303 304 305 306 307 308 309
    }
}

void MissionItem::setZ(double z)
{
    if (!isinf(z) && !isnan(z) && ((_frame == MAV_FRAME_LOCAL_NED) || (_frame == MAV_FRAME_LOCAL_ENU)))
    {
Don Gagne's avatar
Don Gagne committed
310
        setAltitude(z);
311 312 313 314 315
    }
}

void MissionItem::setLatitude(double lat)
{
Don Gagne's avatar
Don Gagne committed
316
    if (_latitudeFact->value().toDouble() != lat)
317
    {
318
        _latitudeFact->setValue(lat);
319
        emit changed(this);
Don Gagne's avatar
Don Gagne committed
320
        emit coordinateChanged(coordinate());
321 322 323 324 325
    }
}

void MissionItem::setLongitude(double lon)
{
Don Gagne's avatar
Don Gagne committed
326
    if (_longitudeFact->value().toDouble() != lon)
327
    {
328
        _longitudeFact->setValue(lon);
329
        emit changed(this);
Don Gagne's avatar
Don Gagne committed
330
        emit coordinateChanged(coordinate());
331 332 333 334 335
    }
}

void MissionItem::setAltitude(double altitude)
{
Don Gagne's avatar
Don Gagne committed
336
    if (_altitudeFact->value().toDouble() != altitude)
337
    {
338
        _altitudeFact->setValue(altitude);
339 340
        emit changed(this);
        emit valueStringsChanged(valueStrings());
Don Gagne's avatar
Don Gagne committed
341
        emit coordinateChanged(coordinate());
342 343 344 345 346
    }
}

void MissionItem::setAction(int /*MAV_CMD*/ action)
{
Don Gagne's avatar
Don Gagne committed
347 348
    if (_command != action) {
        _command = (MavlinkQmlSingleton::Qml_MAV_CMD)action;
349

Don Gagne's avatar
Don Gagne committed
350
        // Fix defaults according to WP type
351

Don Gagne's avatar
Don Gagne committed
352
        if (_command == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) {
353
            // We default to 15 degrees minimum takeoff pitch
Don Gagne's avatar
Don Gagne committed
354
            setParam1(15.0);
355
        }
Don Gagne's avatar
Don Gagne committed
356 357 358 359 360 361 362 363
        
        if (specifiesCoordinate()) {
            if (_frame != MAV_FRAME_GLOBAL && _frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) {
                setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT);
            }
        } else {
            setFrame(MAV_FRAME_MISSION);
        }
364 365 366

        emit changed(this);
        emit commandNameChanged(commandName());
Don Gagne's avatar
Don Gagne committed
367
        emit commandChanged((MavlinkQmlSingleton::Qml_MAV_CMD)_command);
368 369 370 371 372
        emit valueLabelsChanged(valueLabels());
        emit valueStringsChanged(valueStrings());
    }
}

373 374 375 376 377 378 379 380 381
int MissionItem::frame(void) const
{
    if (_altitudeRelativeToHomeFact->value().toBool()) {
        return MAV_FRAME_GLOBAL_RELATIVE_ALT;
    } else {
        return _frame;
    }
}

382 383 384
void MissionItem::setFrame(int /*MAV_FRAME*/ frame)
{
    if (_frame != frame) {
Don Gagne's avatar
Don Gagne committed
385
        _altitudeRelativeToHomeFact->setValue(frame == MAV_FRAME_GLOBAL_RELATIVE_ALT);
386 387 388 389 390 391 392 393 394 395 396 397 398
        _frame = frame;
        emit changed(this);
    }
}

void MissionItem::setAutocontinue(bool autoContinue)
{
    if (_autocontinue != autoContinue) {
        _autocontinue = autoContinue;
        emit changed(this);
    }
}

Don Gagne's avatar
Don Gagne committed
399
void MissionItem::setIsCurrentItem(bool isCurrentItem)
400
{
Don Gagne's avatar
Don Gagne committed
401 402 403
    if (_isCurrentItem != isCurrentItem) {
        _isCurrentItem = isCurrentItem;
        emit isCurrentItemChanged(isCurrentItem);
404 405 406 407 408
    }
}

void MissionItem::setAcceptanceRadius(double radius)
{
Don Gagne's avatar
Don Gagne committed
409
    setParam2(radius);
410 411
}

Don Gagne's avatar
Don Gagne committed
412
void MissionItem::setParam1(double param)
413
{
Don Gagne's avatar
Don Gagne committed
414
    if (param1() != param)
415
    {
Don Gagne's avatar
Don Gagne committed
416
        _param1Fact->setValue(param);
417 418 419 420 421
        emit changed(this);
        emit valueStringsChanged(valueStrings());
    }
}

Don Gagne's avatar
Don Gagne committed
422
void MissionItem::setParam2(double param)
423
{
Don Gagne's avatar
Don Gagne committed
424
    if (param2() != param)
425
    {
Don Gagne's avatar
Don Gagne committed
426
        _param2Fact->setValue(param);
427 428 429 430 431 432 433
        emit valueStringsChanged(valueStrings());
        emit changed(this);
    }
}

void MissionItem::setParam3(double param3)
{
Don Gagne's avatar
Don Gagne committed
434
    setLoiterOrbitRadius(param3);
435 436 437 438
}

void MissionItem::setParam4(double param4)
{
Don Gagne's avatar
Don Gagne committed
439
    setYawRadians(param4);
440 441 442 443
}

void MissionItem::setParam5(double param5)
{
444
    setLatitude(param5);
445 446 447 448
}

void MissionItem::setParam6(double param6)
{
449
    setLongitude(param6);
450 451 452 453
}

void MissionItem::setParam7(double param7)
{
454
    setAltitude(param7);
455 456
}

Don Gagne's avatar
Don Gagne committed
457
void MissionItem::setLoiterOrbitRadius(double radius)
458
{
Don Gagne's avatar
Don Gagne committed
459 460
    if (loiterOrbitRadius() != radius) {
        _loiterOrbitRadiusFact->setValue(radius);
461 462 463 464 465 466 467
        emit valueStringsChanged(valueStrings());
        emit changed(this);
    }
}

void MissionItem::setHoldTime(int holdTime)
{
Don Gagne's avatar
Don Gagne committed
468
    setParam1(holdTime);
469 470 471 472
}

void MissionItem::setHoldTime(double holdTime)
{
Don Gagne's avatar
Don Gagne committed
473
    setParam1(holdTime);
474 475
}

Don Gagne's avatar
Don Gagne committed
476
bool MissionItem::specifiesCoordinate(void) const
477
{
Don Gagne's avatar
Don Gagne committed
478
    switch (_command) {
479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494
        case MAV_CMD_NAV_WAYPOINT:
        case MAV_CMD_NAV_LOITER_UNLIM:
        case MAV_CMD_NAV_LOITER_TURNS:
        case MAV_CMD_NAV_LOITER_TIME:
        case MAV_CMD_NAV_LAND:
        case MAV_CMD_NAV_TAKEOFF:
            return true;
        default:
            return false;
    }
}

QString MissionItem::commandName(void)
{
    QString type;
    
Don Gagne's avatar
Don Gagne committed
495
    switch (_command) {
496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519
        case MAV_CMD_NAV_WAYPOINT:
            type = "Waypoint";
            break;
        case MAV_CMD_NAV_LOITER_UNLIM:
        case MAV_CMD_NAV_LOITER_TURNS:
        case MAV_CMD_NAV_LOITER_TIME:
            type = "Loiter";
            break;
        case MAV_CMD_NAV_RETURN_TO_LAUNCH:
            type = "Return Home";
            break;
        case MAV_CMD_NAV_LAND:
            type = "Land";
            break;
        case MAV_CMD_NAV_TAKEOFF:
            type = "Takeoff";
            break;
        case MAV_CMD_CONDITION_DELAY:
            type = "Delay";
            break;
        case MAV_CMD_DO_JUMP:
            type = "Jump To Command";
            break;
        default:
Don Gagne's avatar
Don Gagne committed
520
            type = QString("Unknown (%1)").arg(_command);
521 522 523 524 525 526 527 528 529 530
            break;
    }
    
    return type;
}

QStringList MissionItem::valueLabels(void)
{
    QStringList labels;
    
Don Gagne's avatar
Don Gagne committed
531
    switch (_command) {
532
        case MAV_CMD_NAV_WAYPOINT:
Don Gagne's avatar
Don Gagne committed
533
            if (frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551
                labels << "Alt (rel):";
            } else {
                labels << "Alt:";
            }
            labels << "Heading:" << "Radius:" << "Hold:";
            break;
        case MAV_CMD_NAV_LOITER_UNLIM:
            labels << "Heading:" << "Radius:";
            break;
        case MAV_CMD_NAV_LOITER_TURNS:
            labels << "Heading:"  << "Radius:"<< "Turns:";
            break;
        case MAV_CMD_NAV_LOITER_TIME:
            labels << "Heading:" << "Radius:" << "Seconds:";
            break;
        case MAV_CMD_NAV_RETURN_TO_LAUNCH:
            break;
        case MAV_CMD_NAV_LAND:
Don Gagne's avatar
Don Gagne committed
552
            if (frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
553 554 555 556 557 558 559
                labels << "Alt (rel):";
            } else {
                labels << "Alt:";
            }
            labels << "Heading:";
            break;
        case MAV_CMD_NAV_TAKEOFF:
Don Gagne's avatar
Don Gagne committed
560
            if (frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) {
561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588
                labels << "Alt (rel):";
            } else {
                labels << "Alt:";
            }
            labels << "Heading:" << "Pitch:";
            break;
        case MAV_CMD_CONDITION_DELAY:
            labels << "Seconds:";
            break;
        case MAV_CMD_DO_JUMP:
            labels << "Jump to:" << "Repeat:";
            break;
        default:
            break;
    }
    
    return labels;
}

QString MissionItem::_oneDecimalString(double value)
{
    return QString("%1").arg(value, 0 /* min field width */, 'f' /* format */, 1 /* precision */);
}

QStringList MissionItem::valueStrings(void)
{
    QStringList list;
    
Don Gagne's avatar
Don Gagne committed
589
    switch (_command) {
590
        case MAV_CMD_NAV_WAYPOINT:
591
            list << _oneDecimalString(_altitudeFact->value().toDouble()) << _oneDecimalString(yawDegrees()) << _oneDecimalString(param2()) << _oneDecimalString(param1());
592 593
            break;
        case MAV_CMD_NAV_LOITER_UNLIM:
Don Gagne's avatar
Don Gagne committed
594
            list << _oneDecimalString(yawRadians() * (180.0 / M_PI)) << _oneDecimalString(loiterOrbitRadius());
595 596
            break;
        case MAV_CMD_NAV_LOITER_TURNS:
Don Gagne's avatar
Don Gagne committed
597
            list << _oneDecimalString(yawRadians() * (180.0 / M_PI)) << _oneDecimalString(loiterOrbitRadius()) << _oneDecimalString(param1());
598 599
            break;
        case MAV_CMD_NAV_LOITER_TIME:
Don Gagne's avatar
Don Gagne committed
600
            list << _oneDecimalString(yawRadians() * (180.0 / M_PI)) << _oneDecimalString(loiterOrbitRadius()) << _oneDecimalString(param1());
601 602 603 604
            break;
        case MAV_CMD_NAV_RETURN_TO_LAUNCH:
            break;
        case MAV_CMD_NAV_LAND:
605
            list << _oneDecimalString(_altitudeFact->value().toDouble()) << _oneDecimalString(yawRadians() * (180.0 / M_PI));
606 607
            break;
        case MAV_CMD_NAV_TAKEOFF:
608
            list << _oneDecimalString(_altitudeFact->value().toDouble()) << _oneDecimalString(yawRadians() * (180.0 / M_PI)) << _oneDecimalString(param1());
609 610
            break;
        case MAV_CMD_CONDITION_DELAY:
Don Gagne's avatar
Don Gagne committed
611
            list << _oneDecimalString(param1());
612 613
            break;
        case MAV_CMD_DO_JUMP:
Don Gagne's avatar
Don Gagne committed
614
            list << _oneDecimalString(param1()) << _oneDecimalString(param2());
615 616 617 618 619 620
            break;
        default:
            break;
    }
    
    return list;
Don Gagne's avatar
Don Gagne committed
621 622 623 624 625 626 627 628 629 630 631 632 633 634 635
}

QStringList MissionItem::commandNames(void) {
    QStringList list;
    
    for (int i=0; i<_cMavCmd2Name; i++) {
        list += _rgMavCmd2Name[i].name;
    }
    
    return list;
}

int MissionItem::commandByIndex(void)
{
    for (int i=0; i<_cMavCmd2Name; i++) {
Don Gagne's avatar
Don Gagne committed
636
        if (_rgMavCmd2Name[i].command == (MAV_CMD)_command) {
Don Gagne's avatar
Don Gagne committed
637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652
            return i;
        }
    }
    
    return -1;
}

void MissionItem::setCommandByIndex(int index)
{
    if (index < 0 || index >= _cMavCmd2Name) {
        qWarning() << "Invalid index" << index;
        return;
    }
    
    setCommand((MavlinkQmlSingleton::Qml_MAV_CMD)_rgMavCmd2Name[index].command);
}
Don Gagne's avatar
Don Gagne committed
653

654
QmlObjectListModel* MissionItem::textFieldFacts(void)
Don Gagne's avatar
Don Gagne committed
655 656 657
{
    QmlObjectListModel* model = new QmlObjectListModel(this);
    
Don Gagne's avatar
Don Gagne committed
658
    switch ((MAV_CMD)_command) {
Don Gagne's avatar
Don Gagne committed
659 660 661 662 663
        case MAV_CMD_NAV_WAYPOINT:
            _param2Fact->_setName("Radius:");
            _param2Fact->setMetaData(_acceptanceRadiusMetaData);
            _param1Fact->_setName("Hold:");
            _param1Fact->setMetaData(_holdTimeMetaData);
664 665 666
            model->append(_latitudeFact);
            model->append(_longitudeFact);
            model->append(_altitudeFact);
Don Gagne's avatar
Don Gagne committed
667
            model->append(_yawRadiansFact);
Don Gagne's avatar
Don Gagne committed
668 669 670 671
            model->append(_param2Fact);
            model->append(_param1Fact);
            break;
        case MAV_CMD_NAV_LOITER_UNLIM:
672 673 674
            model->append(_latitudeFact);
            model->append(_longitudeFact);
            model->append(_altitudeFact);
Don Gagne's avatar
Don Gagne committed
675 676
            model->append(_yawRadiansFact);
            model->append(_loiterOrbitRadiusFact);
Don Gagne's avatar
Don Gagne committed
677 678 679 680
            break;
        case MAV_CMD_NAV_LOITER_TURNS:
            _param1Fact->_setName("Turns:");
            _param1Fact->setMetaData(_loiterTurnsMetaData);
681 682 683
            model->append(_latitudeFact);
            model->append(_longitudeFact);
            model->append(_altitudeFact);
Don Gagne's avatar
Don Gagne committed
684 685
            model->append(_yawRadiansFact);
            model->append(_loiterOrbitRadiusFact);
Don Gagne's avatar
Don Gagne committed
686 687 688 689 690
            model->append(_param1Fact);
            break;
        case MAV_CMD_NAV_LOITER_TIME:
            _param1Fact->_setName("Seconds:");
            _param1Fact->setMetaData(_loiterSecondsMetaData);
691 692 693
            model->append(_latitudeFact);
            model->append(_longitudeFact);
            model->append(_altitudeFact);
Don Gagne's avatar
Don Gagne committed
694 695
            model->append(_yawRadiansFact);
            model->append(_loiterOrbitRadiusFact);
Don Gagne's avatar
Don Gagne committed
696 697 698
            model->append(_param1Fact);
            break;
        case MAV_CMD_NAV_LAND:
699 700 701
            model->append(_latitudeFact);
            model->append(_longitudeFact);
            model->append(_altitudeFact);
Don Gagne's avatar
Don Gagne committed
702
            model->append(_yawRadiansFact);
Don Gagne's avatar
Don Gagne committed
703 704
            break;
        case MAV_CMD_NAV_TAKEOFF:
Don Gagne's avatar
Don Gagne committed
705 706
            _param1Fact->_setName("Pitch:");
            _param1Fact->setMetaData(_pitchMetaData);
707 708 709
            model->append(_latitudeFact);
            model->append(_longitudeFact);
            model->append(_altitudeFact);
Don Gagne's avatar
Don Gagne committed
710 711
            model->append(_yawRadiansFact);
            model->append(_param1Fact);
Don Gagne's avatar
Don Gagne committed
712 713 714 715 716 717 718 719 720 721 722 723 724 725
            break;
        case MAV_CMD_CONDITION_DELAY:
            _param1Fact->_setName("Seconds:");
            _param1Fact->setMetaData(_delaySecondsMetaData);
            model->append(_param1Fact);
            break;
        case MAV_CMD_DO_JUMP:
            _param1Fact->_setName("Seq #:");
            _param1Fact->setMetaData(_jumpSequenceMetaData);
            _param2Fact->_setName("Repeat:");
            _param2Fact->setMetaData(_jumpRepeatMetaData);
            model->append(_param1Fact);
            model->append(_param2Fact);
            break;
Don Gagne's avatar
Don Gagne committed
726 727
        default:
            break;
Don Gagne's avatar
Don Gagne committed
728 729 730 731 732
    }
    
    return model;
}

733
QmlObjectListModel* MissionItem::checkboxFacts(void)
Don Gagne's avatar
Don Gagne committed
734
{
735 736
    QmlObjectListModel* model = new QmlObjectListModel(this);
    
Don Gagne's avatar
Don Gagne committed
737 738
    switch ((MAV_CMD)_command) {
        case MAV_CMD_NAV_WAYPOINT:
739 740
            model->append(_altitudeRelativeToHomeFact);
            break;
Don Gagne's avatar
Don Gagne committed
741
        case MAV_CMD_NAV_LOITER_UNLIM:
742 743
            model->append(_altitudeRelativeToHomeFact);
            break;
Don Gagne's avatar
Don Gagne committed
744
        case MAV_CMD_NAV_LOITER_TURNS:
745 746
            model->append(_altitudeRelativeToHomeFact);
            break;
Don Gagne's avatar
Don Gagne committed
747
        case MAV_CMD_NAV_LOITER_TIME:
748 749
            model->append(_altitudeRelativeToHomeFact);
            break;
Don Gagne's avatar
Don Gagne committed
750
        case MAV_CMD_NAV_RETURN_TO_LAUNCH:
751
            break;
Don Gagne's avatar
Don Gagne committed
752
        case MAV_CMD_NAV_LAND:
753 754
            model->append(_altitudeRelativeToHomeFact);
            break;
Don Gagne's avatar
Don Gagne committed
755
        case MAV_CMD_NAV_TAKEOFF:
756 757
            model->append(_altitudeRelativeToHomeFact);
            break;
Don Gagne's avatar
Don Gagne committed
758
        default:
759
            break;
Don Gagne's avatar
Don Gagne committed
760
    }
761 762
    
    return model;
Don Gagne's avatar
Don Gagne committed
763 764
}

Don Gagne's avatar
Don Gagne committed
765 766
double MissionItem::yawRadians(void) const
{
Don Gagne's avatar
Don Gagne committed
767
    return _yawRadiansFact->value().toDouble();
Don Gagne's avatar
Don Gagne committed
768 769 770 771
}

void MissionItem::setYawRadians(double yaw)
{
Don Gagne's avatar
Don Gagne committed
772
    if (yawRadians() != yaw)
Don Gagne's avatar
Don Gagne committed
773
    {
Don Gagne's avatar
Don Gagne committed
774
        _yawRadiansFact->setValue(yaw);
Don Gagne's avatar
Don Gagne committed
775 776 777 778 779 780 781 782
        emit changed(this);
        emit valueStringsChanged(valueStrings());
    }
}


double MissionItem::yawDegrees(void) const
{
Don Gagne's avatar
Don Gagne committed
783
    return yawRadians() * (180.0 / M_PI);
Don Gagne's avatar
Don Gagne committed
784 785 786 787 788 789 790
}

void MissionItem::setYawDegrees(double yaw)
{
    setYawRadians(yaw * (M_PI / 180.0));
}

791 792 793 794 795 796 797 798 799 800 801
QGeoCoordinate MissionItem::coordinate(void) const
{
    return QGeoCoordinate(latitude(), longitude(), altitude());
}

void MissionItem::setCoordinate(const QGeoCoordinate& coordinate)
{
    setLatitude(coordinate.latitude());
    setLongitude(coordinate.longitude());
    setAltitude(coordinate.altitude());
}
Don Gagne's avatar
Don Gagne committed
802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830

bool MissionItem::canEdit(void)
{
    bool found = false;
    
    for (int i=0; i<_cMavCmd2Name; i++) {
        if (_rgMavCmd2Name[i].command == (MAV_CMD)_command) {
            found = true;
            break;
        }
    }
    
    if (found) {
        if (!_autocontinue) {
            qCDebug(MissionItemLog) << "canEdit false due to _autocontinue != true";
            return false;
        }
        
        if (_frame != MAV_FRAME_GLOBAL && _frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && _frame != MAV_FRAME_MISSION) {
            qCDebug(MissionItemLog) << "canEdit false due unsupported frame type:" << _frame;
            return false;
        }
        
        return true;
    } else {
        qCDebug(MissionItemLog) << "canEdit false due unsupported command:" << _command;
        return false;
    }
}
Don Gagne's avatar
Don Gagne committed
831 832 833 834

void MissionItem::setDirty(bool dirty)
{
    _dirty = dirty;
835 836
    // We want to emit dirtyChanged even if _dirty didn't change. This can be handy signal for
    // any value within the item changing.
Don Gagne's avatar
Don Gagne committed
837 838 839 840 841 842 843 844
    emit dirtyChanged(_dirty);
}

void MissionItem::_factValueChanged(QVariant value)
{
    Q_UNUSED(value);
    setDirty(true);
}
845 846 847 848 849 850

void MissionItem::_coordinateFactChanged(QVariant value)
{
    Q_UNUSED(value);
    emit coordinateChanged(coordinate());
}