MissionItem.cc

/****************************************************************************
 *
 * (c) 2009-2020 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.
 *
 ****************************************************************************/


#include <QStringList>
#include <QDebug>

#include "MissionItem.h"
#include "FirmwarePluginManager.h"
#include "QGCApplication.h"
#include "JsonHelper.h"
#include "VisualMissionItem.h"

const char*  MissionItem::_jsonFrameKey =           "frame";
const char*  MissionItem::_jsonCommandKey =         "command";
const char*  MissionItem::_jsonAutoContinueKey =    "autoContinue";
const char*  MissionItem::_jsonCoordinateKey =      "coordinate";
const char*  MissionItem::_jsonParamsKey =          "params";
const char*  MissionItem::_jsonDoJumpIdKey =        "doJumpId";

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

MissionItem::MissionItem(QObject* parent)
    : QObject(parent)
    , _sequenceNumber(0)
    , _doJumpId(-1)
    , _isCurrentItem(false)
    , _autoContinueFact             (0, "AutoContinue",                 FactMetaData::valueTypeUint32)
    , _commandFact                  (0, "",                             FactMetaData::valueTypeUint32)
    , _frameFact                    (0, "",                             FactMetaData::valueTypeUint32)
    , _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)
{
    // 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);

    setAutoContinue(true);

    connect(&_param1Fact, &Fact::rawValueChanged, this, &MissionItem::_param1Changed);
    connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed);
    connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed);
}

MissionItem::MissionItem(int             sequenceNumber,
                         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)
    , _doJumpId(-1)
    , _isCurrentItem(isCurrentItem)
    , _commandFact                  (0, "",                             FactMetaData::valueTypeUint32)
    , _frameFact                    (0, "",                             FactMetaData::valueTypeUint32)
    , _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)
{
    // 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);

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

    _param1Fact.setRawValue(param1);
    _param2Fact.setRawValue(param2);
    _param3Fact.setRawValue(param3);
    _param4Fact.setRawValue(param4);
    _param5Fact.setRawValue(param5);
    _param6Fact.setRawValue(param6);
    _param7Fact.setRawValue(param7);

    connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed);
    connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed);
}

MissionItem::MissionItem(const MissionItem& other, QObject* parent)
    : QObject(parent)
    , _sequenceNumber(0)
    , _doJumpId(-1)
    , _isCurrentItem(false)
    , _commandFact                  (0, "",                             FactMetaData::valueTypeUint32)
    , _frameFact                    (0, "",                             FactMetaData::valueTypeUint32)
    , _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)
{
    // 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);

    *this = other;

    connect(&_param2Fact, &Fact::rawValueChanged, this, &MissionItem::_param2Changed);
    connect(&_param3Fact, &Fact::rawValueChanged, this, &MissionItem::_param3Changed);
}

const MissionItem& MissionItem::operator=(const MissionItem& other)
{
    _doJumpId = other._doJumpId;

    setCommand(other.command());
    setFrame(other.frame());
    setSequenceNumber(other._sequenceNumber);
    setAutoContinue(other.autoContinue());
    setIsCurrentItem(other._isCurrentItem);

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

    return *this;
}

MissionItem::~MissionItem()
{    

}

void MissionItem::save(QJsonObject& json) const
{
    json[VisualMissionItem::jsonTypeKey] = VisualMissionItem::jsonTypeSimpleItemValue;
    json[_jsonFrameKey] = frame();
    json[_jsonCommandKey] = command();
    json[_jsonAutoContinueKey] = autoContinue();
    json[_jsonDoJumpIdKey] = _sequenceNumber;

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

bool MissionItem::load(QTextStream &loadStream)
{
    const QStringList &wpParams = loadStream.readLine().split("\t");
    if (wpParams.size() == 12) {
        setCommand((MAV_CMD)wpParams[3].toInt());   // Has to be first since it triggers defaults to be set, which are then override by below set calls
        setSequenceNumber(wpParams[0].toInt());
        setIsCurrentItem(wpParams[1].toInt() == 1 ? true : false);
        setFrame((MAV_FRAME)wpParams[2].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;
    }

    return false;
}

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::_convertJsonV2ToV3(QJsonObject& json, QString& errorString)
{
    // V2 format: param 5/6/7 stored in GeoCoordinate
    // V3 format: param 5/6/7 stored in params array

    if (!json.contains(_jsonCoordinateKey)) {
        // Already V3 format
        return true;
    }

    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
        { _jsonCoordinateKey, QJsonValue::Array, true },
    };
    if (!JsonHelper::validateKeys(json, keyInfoList, errorString)) {
        return false;
    }

    QGeoCoordinate coordinate;
    if (!JsonHelper::loadGeoCoordinate(json[_jsonCoordinateKey], true /* altitudeRequired */, coordinate, errorString)) {
        return false;
    }

    QJsonArray rgParam = json[_jsonParamsKey].toArray();
    rgParam.append(coordinate.latitude());
    rgParam.append(coordinate.longitude());
    rgParam.append(coordinate.altitude());
    json[_jsonParamsKey] = rgParam;

    json.remove(_jsonCoordinateKey);

    return true;
}

bool MissionItem::load(const QJsonObject& json, int sequenceNumber, QString& errorString)
{
    QJsonObject convertedJson;
    if (!_convertJsonV1ToV2(json, convertedJson, errorString)) {
        return false;
    }
    if (!_convertJsonV2ToV3(convertedJson, 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 },
        { _jsonDoJumpIdKey,                 QJsonValue::Double, false },
    };
    if (!JsonHelper::validateKeys(convertedJson, keyInfoList, errorString)) {
        return false;
    }

    if (convertedJson[VisualMissionItem::jsonTypeKey] != VisualMissionItem::jsonTypeSimpleItemValue) {
        errorString = tr("Type found: %1 must be: %2").arg(convertedJson[VisualMissionItem::jsonTypeKey].toString()).arg(VisualMissionItem::jsonTypeSimpleItemValue);
        return false;
    }

    QJsonArray rgParams = convertedJson[_jsonParamsKey].toArray();
    if (rgParams.count() != 7) {
        errorString = tr("%1 key must contains 7 values").arg(_jsonParamsKey);
        return false;
    }

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

    // Make sure to set these first since they can signal other changes
    setCommand((MAV_CMD)convertedJson[_jsonCommandKey].toInt());
    setFrame((MAV_FRAME)convertedJson[_jsonFrameKey].toInt());

    _doJumpId = -1;
    if (convertedJson.contains(_jsonDoJumpIdKey)) {
        _doJumpId = convertedJson[_jsonDoJumpIdKey].toInt();
    }
    setIsCurrentItem(false);
    setSequenceNumber(sequenceNumber);
    setAutoContinue(convertedJson[_jsonAutoContinueKey].toBool());

    setParam1(JsonHelper::possibleNaNJsonValue(rgParams[0]));
    setParam2(JsonHelper::possibleNaNJsonValue(rgParams[1]));
    setParam3(JsonHelper::possibleNaNJsonValue(rgParams[2]));
    setParam4(JsonHelper::possibleNaNJsonValue(rgParams[3]));
    setParam5(JsonHelper::possibleNaNJsonValue(rgParams[4]));
    setParam6(JsonHelper::possibleNaNJsonValue(rgParams[5]));
    setParam7(JsonHelper::possibleNaNJsonValue(rgParams[6]));

    return true;
}


void MissionItem::setSequenceNumber(int sequenceNumber)
{
    if (_sequenceNumber != sequenceNumber) {
        _sequenceNumber = sequenceNumber;
        emit sequenceNumberChanged(_sequenceNumber);
    }
}

void MissionItem::setCommand(MAV_CMD command)
{
    if ((MAV_CMD)this->command() != command) {
        _commandFact.setRawValue(command);
    }
}

void MissionItem::setFrame(MAV_FRAME frame)
{
    if (this->frame() != frame) {
        _frameFact.setRawValue(frame);
    }
}

void MissionItem::setAutoContinue(bool autoContinue)
{
    if (this->autoContinue() != autoContinue) {
        _autoContinueFact.setRawValue(autoContinue);
    }
}

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

QGeoCoordinate MissionItem::coordinate(void) const
{
    if(!std::isfinite(param5()) || !std::isfinite(param6())) {
        //-- If either of these are NAN, return an invalid (QGeoCoordinate::isValid() == false) coordinate
        return QGeoCoordinate();
    }
    return QGeoCoordinate(param5(), param6(), param7());
}

double MissionItem::specifiedFlightSpeed(void) const
{
    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;
}

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

double MissionItem::specifiedGimbalPitch(void) const
{
    double gimbalPitch = std::numeric_limits<double>::quiet_NaN();

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

    return gimbalPitch;
}

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

    double gimbalPitch = specifiedGimbalPitch();
    if (!qIsNaN(gimbalPitch)) {
        emit specifiedGimbalPitchChanged(gimbalPitch);
    }
}

void MissionItem::_param2Changed(QVariant value)
{
    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);
    }
}