MissionItem.cc 15.7 KB
Newer Older
1 2 3 4 5 6 7 8
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
9 10 11 12 13 14 15 16


#include <QStringList>
#include <QDebug>

#include "MissionItem.h"
#include "FirmwarePluginManager.h"
#include "QGCApplication.h"
17
#include "JsonHelper.h"
Don Gagne's avatar
Don Gagne committed
18
#include "VisualMissionItem.h"
19

20 21
const char*  MissionItem::_jsonFrameKey =           "frame";
const char*  MissionItem::_jsonCommandKey =         "command";
Don Gagne's avatar
Don Gagne committed
22 23 24 25 26 27
const char*  MissionItem::_jsonAutoContinueKey =    "autoContinue";
const char*  MissionItem::_jsonCoordinateKey =      "coordinate";
const char*  MissionItem::_jsonParamsKey =          "params";
const char*  MissionItem::_jsonDoJumpIdKey =        "doJumpId";

// Deprecated V1 format keys
28 29 30 31 32
const char*  MissionItem::_jsonParam1Key =          "param1";
const char*  MissionItem::_jsonParam2Key =          "param2";
const char*  MissionItem::_jsonParam3Key =          "param3";
const char*  MissionItem::_jsonParam4Key =          "param4";

33
MissionItem::MissionItem(QObject* parent)
34 35
    : QObject(parent)
    , _sequenceNumber(0)
Don Gagne's avatar
Don Gagne committed
36
    , _doJumpId(-1)
37 38
    , _isCurrentItem(false)
    , _autoContinueFact             (0, "AutoContinue",                 FactMetaData::valueTypeUint32)
39 40
    , _commandFact                  (0, "",                             FactMetaData::valueTypeUint32)
    , _frameFact                    (0, "",                             FactMetaData::valueTypeUint32)
41 42 43 44 45 46 47 48
    , _param1Fact                   (0, "Param1:",                      FactMetaData::valueTypeDouble)
    , _param2Fact                   (0, "Param2:",                      FactMetaData::valueTypeDouble)
    , _param3Fact                   (0, "Param3:",                      FactMetaData::valueTypeDouble)
    , _param4Fact                   (0, "Param4:",                      FactMetaData::valueTypeDouble)
    , _param5Fact                   (0, "Latitude:",                    FactMetaData::valueTypeDouble)
    , _param6Fact                   (0, "Longitude:",                   FactMetaData::valueTypeDouble)
    , _param7Fact                   (0, "Altitude:",                    FactMetaData::valueTypeDouble)
{
Don Gagne's avatar
Don Gagne committed
49 50 51
    // Need a good command and frame before we start passing signals around
    _commandFact.setRawValue(MAV_CMD_NAV_WAYPOINT);
    _frameFact.setRawValue(MAV_FRAME_GLOBAL_RELATIVE_ALT);
52 53

    setAutoContinue(true);
54 55

    connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed);
DonLakeFlyer's avatar
DonLakeFlyer committed
56
    connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed);
57 58
}

59
MissionItem::MissionItem(int             sequenceNumber,
60 61 62 63 64 65 66 67 68 69 70 71 72 73
                         MAV_CMD         command,
                         MAV_FRAME       frame,
                         double          param1,
                         double          param2,
                         double          param3,
                         double          param4,
                         double          param5,
                         double          param6,
                         double          param7,
                         bool            autoContinue,
                         bool            isCurrentItem,
                         QObject*        parent)
    : QObject(parent)
    , _sequenceNumber(sequenceNumber)
Don Gagne's avatar
Don Gagne committed
74
    , _doJumpId(-1)
75
    , _isCurrentItem(isCurrentItem)
Don Gagne's avatar
Don Gagne committed
76 77
    , _commandFact                  (0, "",                             FactMetaData::valueTypeUint32)
    , _frameFact                    (0, "",                             FactMetaData::valueTypeUint32)
78 79 80 81 82 83 84 85
    , _param1Fact                   (0, "Param1:",                      FactMetaData::valueTypeDouble)
    , _param2Fact                   (0, "Param2:",                      FactMetaData::valueTypeDouble)
    , _param3Fact                   (0, "Param3:",                      FactMetaData::valueTypeDouble)
    , _param4Fact                   (0, "Param4:",                      FactMetaData::valueTypeDouble)
    , _param5Fact                   (0, "Lat/X:",                       FactMetaData::valueTypeDouble)
    , _param6Fact                   (0, "Lon/Y:",                       FactMetaData::valueTypeDouble)
    , _param7Fact                   (0, "Alt/Z:",                       FactMetaData::valueTypeDouble)
{
Don Gagne's avatar
Don Gagne committed
86 87 88
    // Need a good command and frame before we start passing signals around
    _commandFact.setRawValue(MAV_CMD_NAV_WAYPOINT);
    _frameFact.setRawValue(MAV_FRAME_GLOBAL_RELATIVE_ALT);
89 90 91 92 93 94 95 96 97

    setCommand(command);
    setFrame(frame);
    setAutoContinue(autoContinue);

    _param1Fact.setRawValue(param1);
    _param2Fact.setRawValue(param2);
    _param3Fact.setRawValue(param3);
    _param4Fact.setRawValue(param4);
Don Gagne's avatar
Don Gagne committed
98 99 100
    _param5Fact.setRawValue(param5);
    _param6Fact.setRawValue(param6);
    _param7Fact.setRawValue(param7);
101 102

    connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed);
DonLakeFlyer's avatar
DonLakeFlyer committed
103
    connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed);
104 105 106 107
}

MissionItem::MissionItem(const MissionItem& other, QObject* parent)
    : QObject(parent)
108
    , _sequenceNumber(0)
Don Gagne's avatar
Don Gagne committed
109
    , _doJumpId(-1)
110
    , _isCurrentItem(false)
Don Gagne's avatar
Don Gagne committed
111 112
    , _commandFact                  (0, "",                             FactMetaData::valueTypeUint32)
    , _frameFact                    (0, "",                             FactMetaData::valueTypeUint32)
113 114 115 116 117 118 119
    , _param1Fact                   (0, "Param1:",                      FactMetaData::valueTypeDouble)
    , _param2Fact                   (0, "Param2:",                      FactMetaData::valueTypeDouble)
    , _param3Fact                   (0, "Param3:",                      FactMetaData::valueTypeDouble)
    , _param4Fact                   (0, "Param4:",                      FactMetaData::valueTypeDouble)
    , _param5Fact                   (0, "Lat/X:",                       FactMetaData::valueTypeDouble)
    , _param6Fact                   (0, "Lon/Y:",                       FactMetaData::valueTypeDouble)
    , _param7Fact                   (0, "Alt/Z:",                       FactMetaData::valueTypeDouble)
120
{
Don Gagne's avatar
Don Gagne committed
121 122 123
    // Need a good command and frame before we start passing signals around
    _commandFact.setRawValue(MAV_CMD_NAV_WAYPOINT);
    _frameFact.setRawValue(MAV_FRAME_GLOBAL_RELATIVE_ALT);
124 125

    *this = other;
126 127

    connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed);
DonLakeFlyer's avatar
DonLakeFlyer committed
128
    connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed);
129 130 131 132
}

const MissionItem& MissionItem::operator=(const MissionItem& other)
{
Don Gagne's avatar
Don Gagne committed
133 134
    _doJumpId = other._doJumpId;

135 136 137 138 139 140
    setCommand(other.command());
    setFrame(other.frame());
    setSequenceNumber(other._sequenceNumber);
    setAutoContinue(other.autoContinue());
    setIsCurrentItem(other._isCurrentItem);

Don Gagne's avatar
Don Gagne committed
141 142 143 144 145 146 147
    _param1Fact.setRawValue(other._param1Fact.rawValue());
    _param2Fact.setRawValue(other._param2Fact.rawValue());
    _param3Fact.setRawValue(other._param3Fact.rawValue());
    _param4Fact.setRawValue(other._param4Fact.rawValue());
    _param5Fact.setRawValue(other._param5Fact.rawValue());
    _param6Fact.setRawValue(other._param6Fact.rawValue());
    _param7Fact.setRawValue(other._param7Fact.rawValue());
148 149 150

    return *this;
}
151

152 153
MissionItem::~MissionItem()
{    
154

155 156
}

157
void MissionItem::save(QJsonObject& json) const
158
{
Don Gagne's avatar
Don Gagne committed
159
    json[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeSimpleItemValue;
160 161 162
    json[_jsonFrameKey] = frame();
    json[_jsonCommandKey] = command();
    json[_jsonAutoContinueKey] = autoContinue();
Don Gagne's avatar
Don Gagne committed
163 164 165 166
    json[_jsonDoJumpIdKey] = _sequenceNumber;

    QJsonArray rgParams =  { param1(), param2(), param3(), param4() };
    json[_jsonParamsKey] = rgParams;
167

Don Gagne's avatar
Don Gagne committed
168 169 170
    QJsonValue coordinateValue;
    JsonHelper::saveGeoCoordinate(QGeoCoordinate(param5(), param6(), param7()), true /* writeAltitude */, coordinateValue);
    json[_jsonCoordinateKey] = coordinateValue;
171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190
}

bool MissionItem::load(QTextStream &loadStream)
{
    const QStringList &wpParams = loadStream.readLine().split("\t");
    if (wpParams.size() == 12) {
        setSequenceNumber(wpParams[0].toInt());
        setIsCurrentItem(wpParams[1].toInt() == 1 ? true : false);
        setFrame((MAV_FRAME)wpParams[2].toInt());
        setCommand((MAV_CMD)wpParams[3].toInt());
        setParam1(wpParams[4].toDouble());
        setParam2(wpParams[5].toDouble());
        setParam3(wpParams[6].toDouble());
        setParam4(wpParams[7].toDouble());
        setParam5(wpParams[8].toDouble());
        setParam6(wpParams[9].toDouble());
        setParam7(wpParams[10].toDouble());
        setAutoContinue(wpParams[11].toInt() == 1 ? true : false);
        return true;
    }
191

192 193 194
    return false;
}

Don Gagne's avatar
Don Gagne committed
195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232
bool MissionItem::_convertJsonV1ToV2(const QJsonObject& json, QJsonObject& v2Json, QString& errorString)
{
    // V1 format type = "missionItem", V2 format type = "MissionItem"
    // V1 format has params in separate param[1-n] keys
    // V2 format has params in params array
    v2Json = json;

    if (json.contains(_jsonParamsKey)) {
        // Already V2 format
        return true;
    }        

    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
        { VisualMissionItem::jsonTypeKey,   QJsonValue::String, true },
        { _jsonParam1Key,                   QJsonValue::Double, true },
        { _jsonParam2Key,                   QJsonValue::Double, true },
        { _jsonParam3Key,                   QJsonValue::Double, true },
        { _jsonParam4Key,                   QJsonValue::Double, true },
    };
    if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) {
        return false;
    }

    if (v2Json[VisualMissionItem::jsonTypeKey].toString() == QStringLiteral("missionItem")) {
        v2Json[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeSimpleItemValue;
    }

    QJsonArray rgParams =  { json[_jsonParam1Key].toDouble(),  json[_jsonParam2Key].toDouble(), json[_jsonParam3Key].toDouble(), json[_jsonParam4Key].toDouble() };
    v2Json[_jsonParamsKey] = rgParams;
    v2Json.remove(_jsonParam1Key);
    v2Json.remove(_jsonParam2Key);
    v2Json.remove(_jsonParam3Key);
    v2Json.remove(_jsonParam4Key);

    return true;
}

bool MissionItem::load(const QJsonObject& json, int sequenceNumber, QString& errorString)
233
{
Don Gagne's avatar
Don Gagne committed
234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250
    QJsonObject v2Json;
    if (!_convertJsonV1ToV2(json, v2Json, errorString)) {
        return false;
    }

    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
        { VisualMissionItem::jsonTypeKey,   QJsonValue::String, true },
        { _jsonFrameKey,                    QJsonValue::Double, true },
        { _jsonCommandKey,                  QJsonValue::Double, true },
        { _jsonParamsKey,                   QJsonValue::Array,  true },
        { _jsonAutoContinueKey,             QJsonValue::Bool,   true },
        { _jsonCoordinateKey,               QJsonValue::Array,  true },
        { _jsonDoJumpIdKey,                 QJsonValue::Double, false },
    };
    if (!JsonHelper::validateKeys(v2Json, keyInfoList, errorString)) {
        return false;
    }
251

Don Gagne's avatar
Don Gagne committed
252 253
    if (v2Json[VisualMissionItem::jsonTypeKey] != VisualMissionItem::jsonTypeSimpleItemValue) {
        errorString = tr("Type found: %1 must be: %2").arg(v2Json[VisualMissionItem::jsonTypeKey].toString()).arg(VisualMissionItem::jsonTypeSimpleItemValue);
254 255 256
        return false;
    }

Don Gagne's avatar
Don Gagne committed
257 258 259
    QJsonArray rgParams = v2Json[_jsonParamsKey].toArray();
    if (rgParams.count() != 4) {
        errorString = tr("%1 key must contains 4 values").arg(_jsonParamsKey);
260 261 262
        return false;
    }

263 264 265 266 267 268 269
    for (int i=0; i<4; i++) {
        if (rgParams[i].type() != QJsonValue::Double && rgParams[i].type() != QJsonValue::Null) {
            errorString = tr("Param %1 incorrect type %2, must be double or null").arg(i+1).arg(rgParams[i].type());
            return false;
        }
    }

270
    // Make sure to set these first since they can signal other changes
Don Gagne's avatar
Don Gagne committed
271 272
    setFrame((MAV_FRAME)v2Json[_jsonFrameKey].toInt());
    setCommand((MAV_CMD)v2Json[_jsonCommandKey].toInt());
273

274
    QGeoCoordinate coordinate;
Don Gagne's avatar
Don Gagne committed
275
    if (!JsonHelper::loadGeoCoordinate(v2Json[_jsonCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
276 277
        return false;
    }
278 279 280
    setParam5(coordinate.latitude());
    setParam6(coordinate.longitude());
    setParam7(coordinate.altitude());
281

Don Gagne's avatar
Don Gagne committed
282 283 284 285
    _doJumpId = -1;
    if (v2Json.contains(_jsonDoJumpIdKey)) {
        _doJumpId = v2Json[_jsonDoJumpIdKey].toInt();
    }
286
    setIsCurrentItem(false);
Don Gagne's avatar
Don Gagne committed
287 288 289
    setSequenceNumber(sequenceNumber);
    setAutoContinue(v2Json[_jsonAutoContinueKey].toBool());

290 291 292 293
    setParam1(JsonHelper::possibleNaNJsonValue(rgParams[0]));
    setParam2(JsonHelper::possibleNaNJsonValue(rgParams[1]));
    setParam3(JsonHelper::possibleNaNJsonValue(rgParams[2]));
    setParam4(JsonHelper::possibleNaNJsonValue(rgParams[3]));
294 295 296 297

    return true;
}

298 299 300

void MissionItem::setSequenceNumber(int sequenceNumber)
{
301 302 303 304
    if (_sequenceNumber != sequenceNumber) {
        _sequenceNumber = sequenceNumber;
        emit sequenceNumberChanged(_sequenceNumber);
    }
305 306 307 308 309
}

void MissionItem::setCommand(MAV_CMD command)
{
    if ((MAV_CMD)this->command() != command) {
Don Gagne's avatar
Don Gagne committed
310
        _commandFact.setRawValue(command);
311 312 313 314 315 316
    }
}

void MissionItem::setFrame(MAV_FRAME frame)
{
    if (this->frame() != frame) {
Don Gagne's avatar
Don Gagne committed
317
        _frameFact.setRawValue(frame);
318 319 320 321 322 323
    }
}

void MissionItem::setAutoContinue(bool autoContinue)
{
    if (this->autoContinue() != autoContinue) {
Don Gagne's avatar
Don Gagne committed
324
        _autoContinueFact.setRawValue(autoContinue);
325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391
    }
}

void MissionItem::setIsCurrentItem(bool isCurrentItem)
{
    if (_isCurrentItem != isCurrentItem) {
        _isCurrentItem = isCurrentItem;
        emit isCurrentItemChanged(isCurrentItem);
    }
}

void MissionItem::setParam1(double param)
{
    if (param1() != param) {
        _param1Fact.setRawValue(param);
    }
}

void MissionItem::setParam2(double param)
{
    if (param2() != param) {
        _param2Fact.setRawValue(param);
    }
}

void MissionItem::setParam3(double param)
{
    if (param3() != param) {
        _param3Fact.setRawValue(param);
    }
}

void MissionItem::setParam4(double param)
{
    if (param4() != param) {
        _param4Fact.setRawValue(param);
    }
}

void MissionItem::setParam5(double param)
{
    if (param5() != param) {
        _param5Fact.setRawValue(param);
    }
}

void MissionItem::setParam6(double param)
{
    if (param6() != param) {
        _param6Fact.setRawValue(param);
    }
}

void MissionItem::setParam7(double param)
{
    if (param7() != param) {
        _param7Fact.setRawValue(param);
    }
}

void MissionItem::setCoordinate(const QGeoCoordinate& coordinate)
{
    setParam5(coordinate.latitude());
    setParam6(coordinate.longitude());
    setParam7(coordinate.altitude());
}

392
QGeoCoordinate MissionItem::coordinate(void) const
393
{
394
    return QGeoCoordinate(param5(), param6(), param7());
395
}
396

DonLakeFlyer's avatar
DonLakeFlyer committed
397
double MissionItem::specifiedFlightSpeed(void) const
398 399 400 401 402 403 404 405 406 407
{
    double flightSpeed = std::numeric_limits<double>::quiet_NaN();

    if (_commandFact.rawValue().toInt() == MAV_CMD_DO_CHANGE_SPEED && _param2Fact.rawValue().toDouble() > 0) {
        flightSpeed = _param2Fact.rawValue().toDouble();
    }

    return flightSpeed;
}

DonLakeFlyer's avatar
DonLakeFlyer committed
408 409 410 411 412 413 414 415 416 417 418
double MissionItem::specifiedGimbalYaw(void) const
{
    double gimbalYaw = std::numeric_limits<double>::quiet_NaN();

    if (_commandFact.rawValue().toInt() == MAV_CMD_DO_MOUNT_CONTROL && _param7Fact.rawValue().toInt() == MAV_MOUNT_MODE_MAVLINK_TARGETING) {
        gimbalYaw = _param3Fact.rawValue().toDouble();
    }

    return gimbalYaw;
}

419 420
void MissionItem::_param2Changed(QVariant value)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
421 422 423 424 425 426 427 428 429 430 431 432 433 434 435
    Q_UNUSED(value);

    double flightSpeed = specifiedFlightSpeed();
    if (!qIsNaN(flightSpeed)) {
        emit specifiedFlightSpeedChanged(flightSpeed);
    }
}

void MissionItem::_param3Changed(QVariant value)
{
    Q_UNUSED(value);

    double gimbalYaw = specifiedGimbalYaw();
    if (!qIsNaN(gimbalYaw)) {
        emit specifiedGimbalYawChanged(gimbalYaw);
436 437
    }
}