MissionItem.cc 11.3 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"
18

19 20 21 22 23 24 25 26 27 28 29 30
const char*  MissionItem::_itemType =               "missionItem";
const char*  MissionItem::_jsonTypeKey =            "type";
const char*  MissionItem::_jsonIdKey =              "id";
const char*  MissionItem::_jsonFrameKey =           "frame";
const char*  MissionItem::_jsonCommandKey =         "command";
const char*  MissionItem::_jsonParam1Key =          "param1";
const char*  MissionItem::_jsonParam2Key =          "param2";
const char*  MissionItem::_jsonParam3Key =          "param3";
const char*  MissionItem::_jsonParam4Key =          "param4";
const char*  MissionItem::_jsonAutoContinueKey =    "autoContinue";
const char*  MissionItem::_jsonCoordinateKey =      "coordinate";

31
MissionItem::MissionItem(QObject* parent)
32 33 34 35
    : QObject(parent)
    , _sequenceNumber(0)
    , _isCurrentItem(false)
    , _autoContinueFact             (0, "AutoContinue",                 FactMetaData::valueTypeUint32)
36 37
    , _commandFact                  (0, "",                             FactMetaData::valueTypeUint32)
    , _frameFact                    (0, "",                             FactMetaData::valueTypeUint32)
38 39 40 41 42 43 44 45
    , _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
46 47 48
    // 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);
49 50 51 52

    setAutoContinue(true);
}

53
MissionItem::MissionItem(int             sequenceNumber,
54 55 56 57 58 59 60 61 62 63 64 65 66 67 68
                         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)
    , _isCurrentItem(isCurrentItem)
Don Gagne's avatar
Don Gagne committed
69 70
    , _commandFact                  (0, "",                             FactMetaData::valueTypeUint32)
    , _frameFact                    (0, "",                             FactMetaData::valueTypeUint32)
71 72 73 74 75 76 77 78
    , _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
79 80 81
    // 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);
82 83 84 85 86 87 88 89 90

    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
91 92 93
    _param5Fact.setRawValue(param5);
    _param6Fact.setRawValue(param6);
    _param7Fact.setRawValue(param7);
94 95 96 97
}

MissionItem::MissionItem(const MissionItem& other, QObject* parent)
    : QObject(parent)
98 99
    , _sequenceNumber(0)
    , _isCurrentItem(false)
Don Gagne's avatar
Don Gagne committed
100 101
    , _commandFact                  (0, "",                             FactMetaData::valueTypeUint32)
    , _frameFact                    (0, "",                             FactMetaData::valueTypeUint32)
102 103 104 105 106 107 108
    , _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)
109
{
Don Gagne's avatar
Don Gagne committed
110 111 112
    // 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);
113 114 115 116 117 118 119 120 121 122 123 124

    *this = other;
}

const MissionItem& MissionItem::operator=(const MissionItem& other)
{
    setCommand(other.command());
    setFrame(other.frame());
    setSequenceNumber(other._sequenceNumber);
    setAutoContinue(other.autoContinue());
    setIsCurrentItem(other._isCurrentItem);

Don Gagne's avatar
Don Gagne committed
125 126 127 128 129 130 131
    _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());
132 133 134 135 136 137 138

    return *this;
}
MissionItem::~MissionItem()
{    
}

139
void MissionItem::save(QJsonObject& json) const
140
{
141 142 143 144 145 146 147 148 149 150 151 152 153
    json[_jsonTypeKey] = _itemType;
    json[_jsonIdKey] = sequenceNumber();
    json[_jsonFrameKey] = frame();
    json[_jsonCommandKey] = command();
    json[_jsonParam1Key] = param1();
    json[_jsonParam2Key] = param2();
    json[_jsonParam3Key] = param3();
    json[_jsonParam4Key] = param4();
    json[_jsonAutoContinueKey] = autoContinue();

    QJsonArray coordinateArray;
    coordinateArray << param5() << param6() << param7();
    json[_jsonCoordinateKey] = coordinateArray;
154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173
}

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

175 176 177
    return false;
}

178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193
bool MissionItem::load(const QJsonObject& json, QString& errorString)
{
    QStringList requiredKeys;

    requiredKeys << _jsonTypeKey << _jsonIdKey << _jsonFrameKey << _jsonCommandKey <<
                    _jsonParam1Key << _jsonParam2Key << _jsonParam3Key << _jsonParam4Key <<
                    _jsonAutoContinueKey << _jsonCoordinateKey;
    if (!JsonHelper::validateRequiredKeys(json, requiredKeys, errorString)) {
        return false;
    }

    if (json[_jsonTypeKey] != _itemType) {
        errorString = QString("type found: %1 must be: %2").arg(json[_jsonTypeKey].toString()).arg(_itemType);
        return false;
    }

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

198 199 200 201
    QGeoCoordinate coordinate;
    if (!JsonHelper::toQGeoCoordinate(json[_jsonCoordinateKey], coordinate, true /* altitudeRequired */, errorString)) {
        return false;
    }
202 203 204
    setParam5(coordinate.latitude());
    setParam6(coordinate.longitude());
    setParam7(coordinate.altitude());
205 206 207 208 209 210 211 212 213 214 215 216

    setIsCurrentItem(false);
    setSequenceNumber(json[_jsonIdKey].toInt());
    setParam1(json[_jsonParam1Key].toDouble());
    setParam2(json[_jsonParam2Key].toDouble());
    setParam3(json[_jsonParam3Key].toDouble());
    setParam4(json[_jsonParam4Key].toDouble());
    setAutoContinue(json[_jsonAutoContinueKey].toBool());

    return true;
}

217 218 219

void MissionItem::setSequenceNumber(int sequenceNumber)
{
220 221 222 223
    if (_sequenceNumber != sequenceNumber) {
        _sequenceNumber = sequenceNumber;
        emit sequenceNumberChanged(_sequenceNumber);
    }
224 225 226 227 228
}

void MissionItem::setCommand(MAV_CMD command)
{
    if ((MAV_CMD)this->command() != command) {
Don Gagne's avatar
Don Gagne committed
229
        _commandFact.setRawValue(command);
230 231 232 233 234 235
    }
}

void MissionItem::setFrame(MAV_FRAME frame)
{
    if (this->frame() != frame) {
Don Gagne's avatar
Don Gagne committed
236
        _frameFact.setRawValue(frame);
237 238 239 240 241 242
    }
}

void MissionItem::setAutoContinue(bool autoContinue)
{
    if (this->autoContinue() != autoContinue) {
Don Gagne's avatar
Don Gagne committed
243
        _autoContinueFact.setRawValue(autoContinue);
244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310
    }
}

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

311
QGeoCoordinate MissionItem::coordinate(void) const
312
{
313
    return QGeoCoordinate(param5(), param6(), param7());
314
}