MissionItem.cc

/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/

#include <QDebug>
#include <QStringList>

#include "FirmwarePluginManager.h"
#include "JsonHelper.h"
#include "MissionItem.h"
#include "QGCApplication.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),
      _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),
      _sequenceNumber(0), _doJumpId(-1), _isCurrentItem(false) {
  // 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), _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),
      _sequenceNumber(sequenceNumber), _doJumpId(-1),
      _isCurrentItem(isCurrentItem) {
  // 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), _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),
      _sequenceNumber(0), _doJumpId(-1), _isCurrentItem(false) {
  // 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);
  }
}