MissionSettingsItem.cc 11.4 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (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.
 *
 ****************************************************************************/

10
#include "MissionSettingsItem.h"
11 12 13 14 15 16 17
#include "JsonHelper.h"
#include "MissionController.h"
#include "QGCGeo.h"
#include "QGroundControlQmlGlobal.h"
#include "SimpleMissionItem.h"
#include "SettingsManager.h"
#include "AppSettings.h"
18
#include "MissionCommandUIInfo.h"
19 20 21 22 23

#include <QPolygonF>

QGC_LOGGING_CATEGORY(MissionSettingsComplexItemLog, "MissionSettingsComplexItemLog")

24
const char* MissionSettingsItem::jsonComplexItemTypeValue = "MissionSettings";
25

DonLakeFlyer's avatar
DonLakeFlyer committed
26
const char* MissionSettingsItem::_plannedHomePositionAltitudeName = "PlannedHomePositionAltitude";
27

28
QMap<QString, FactMetaData*> MissionSettingsItem::_metaDataMap;
29

30 31
MissionSettingsItem::MissionSettingsItem(Vehicle* vehicle, bool flyView, QObject* parent)
    : ComplexMissionItem                (vehicle, flyView, parent)
32
    , _plannedHomePositionAltitudeFact  (0, _plannedHomePositionAltitudeName,   FactMetaData::valueTypeDouble)
33
    , _plannedHomePositionFromVehicle   (false)
34 35 36 37 38
    , _missionEndRTL                    (false)
    , _cameraSection                    (vehicle)
    , _speedSection                     (vehicle)
    , _sequenceNumber                   (0)
    , _dirty                            (false)
39 40 41 42 43 44 45 46 47 48 49
{
    _editorQml = "qrc:/qml/MissionSettingsEditor.qml";

    if (_metaDataMap.isEmpty()) {
        _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/MissionSettings.FactMetaData.json"), NULL /* metaDataParent */);
    }

    _plannedHomePositionAltitudeFact.setMetaData    (_metaDataMap[_plannedHomePositionAltitudeName]);
    _plannedHomePositionAltitudeFact.setRawValue    (_plannedHomePositionAltitudeFact.rawDefaultValue());
    setHomePositionSpecialCase(true);

50 51 52
    _cameraSection.setAvailable(true);
    _speedSection.setAvailable(true);

53
    connect(this,               &MissionSettingsItem::specifyMissionFlightSpeedChanged, this, &MissionSettingsItem::_setDirtyAndUpdateLastSequenceNumber);
54
    connect(this,               &MissionSettingsItem::missionEndRTLChanged,             this, &MissionSettingsItem::_setDirtyAndUpdateLastSequenceNumber);
55 56
    connect(&_cameraSection,    &CameraSection::itemCountChanged,                       this, &MissionSettingsItem::_setDirtyAndUpdateLastSequenceNumber);
    connect(&_speedSection,     &CameraSection::itemCountChanged,                       this, &MissionSettingsItem::_setDirtyAndUpdateLastSequenceNumber);
57

58
    connect(this,               &MissionSettingsItem::terrainAltitudeChanged,           this, &MissionSettingsItem::_setHomeAltFromTerrain);
59

60
    connect(&_plannedHomePositionAltitudeFact,  &Fact::rawValueChanged,                 this, &MissionSettingsItem::_updateAltitudeInCoordinate);
61

62 63
    connect(&_cameraSection,    &CameraSection::dirtyChanged,   this, &MissionSettingsItem::_sectionDirtyChanged);
    connect(&_speedSection,     &SpeedSection::dirtyChanged,    this, &MissionSettingsItem::_sectionDirtyChanged);
DonLakeFlyer's avatar
DonLakeFlyer committed
64

65 66 67
    connect(&_cameraSection,    &CameraSection::specifiedGimbalYawChanged,      this, &MissionSettingsItem::specifiedGimbalYawChanged);
    connect(&_cameraSection,    &CameraSection::specifiedGimbalPitchChanged,    this, &MissionSettingsItem::specifiedGimbalPitchChanged);
    connect(&_speedSection,     &SpeedSection::specifiedFlightSpeedChanged,     this, &MissionSettingsItem::specifiedFlightSpeedChanged);
68 69
}

70
int MissionSettingsItem::lastSequenceNumber(void) const
71
{
72
    int lastSequenceNumber = _sequenceNumber;
73

74 75
    lastSequenceNumber += _cameraSection.itemCount();
    lastSequenceNumber += _speedSection.itemCount();
76 77 78 79

    return lastSequenceNumber;
}

80
void MissionSettingsItem::setDirty(bool dirty)
81 82 83
{
    if (_dirty != dirty) {
        _dirty = dirty;
84 85 86 87
        if (!dirty) {
            _cameraSection.setDirty(false);
            _speedSection.setDirty(false);
        }
88 89 90 91
        emit dirtyChanged(_dirty);
    }
}

92
void MissionSettingsItem::save(QJsonArray&  missionItems)
93
{
94 95 96
    QList<MissionItem*> items;

    appendMissionItems(items, this);
97

Patrick José Pereira's avatar
Patrick José Pereira committed
98
    // First item show be planned home position, we are not responsible for save/load
99
    // Remaining items we just output as is
100 101
    for (int i=1; i<items.count(); i++) {
        MissionItem* item = items[i];
102 103 104
        QJsonObject saveObject;
        item->save(saveObject);
        missionItems.append(saveObject);
105
        item->deleteLater();
106 107 108
    }
}

109
void MissionSettingsItem::setSequenceNumber(int sequenceNumber)
110 111 112 113 114 115 116 117
{
    if (_sequenceNumber != sequenceNumber) {
        _sequenceNumber = sequenceNumber;
        emit sequenceNumberChanged(sequenceNumber);
        emit lastSequenceNumberChanged(lastSequenceNumber());
    }
}

118
bool MissionSettingsItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
119 120 121 122 123 124 125 126
{
    Q_UNUSED(complexObject);
    Q_UNUSED(sequenceNumber);
    Q_UNUSED(errorString);

    return true;
}

127
double MissionSettingsItem::greatestDistanceTo(const QGeoCoordinate &other) const
128 129 130 131 132
{
    Q_UNUSED(other);
    return 0;
}

133
bool MissionSettingsItem::specifiesCoordinate(void) const
134
{
DonLakeFlyer's avatar
DonLakeFlyer committed
135
    return true;
136 137
}

138
void MissionSettingsItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
139 140 141
{
    int seqNum = _sequenceNumber;

142
    // IMPORTANT NOTE: If anything changes here you must also change MissionSettingsItem::scanForMissionSettings
143 144 145 146 147 148 149 150 151

    // Planned home position
    MissionItem* item = new MissionItem(seqNum++,
                                        MAV_CMD_NAV_WAYPOINT,
                                        MAV_FRAME_GLOBAL,
                                        0,                      // Hold time
                                        0,                      // Acceptance radius
                                        0,                      // Not sure?
                                        0,                      // Yaw
152 153
                                        coordinate().latitude(),
                                        coordinate().longitude(),
154 155 156
                                        _plannedHomePositionAltitudeFact.rawValue().toDouble(),
                                        true,                   // autoContinue
                                        false,                  // isCurrentItem
157 158
                                        missionItemParent);
    items.append(item);
159

160 161
    _cameraSection.appendSectionItems(items, missionItemParent, seqNum);
    _speedSection.appendSectionItems(items, missionItemParent, seqNum);
162 163
}

164
bool MissionSettingsItem::addMissionEndAction(QList<MissionItem*>& items, int seqNum, QObject* missionItemParent)
165 166 167
{
    MissionItem* item = NULL;

168
    // IMPORTANT NOTE: If anything changes here you must also change MissionSettingsItem::scanForMissionSettings
169

170
    if (_missionEndRTL) {
171 172 173 174 175 176 177 178 179 180 181 182 183 184 185
        qCDebug(MissionSettingsComplexItemLog) << "Appending end action RTL seqNum" << seqNum;
        item = new MissionItem(seqNum,
                               MAV_CMD_NAV_RETURN_TO_LAUNCH,
                               MAV_FRAME_MISSION,
                               0, 0, 0, 0, 0, 0, 0,        // param 1-7 not used
                               true,                       // autoContinue
                               false,                      // isCurrentItem
                               missionItemParent);
        items.append(item);
        return true;
    } else {
        return false;
    }
}

186
bool MissionSettingsItem::scanForMissionSettings(QmlObjectListModel* visualItems, int scanIndex)
187
{
188
    bool foundSpeedSection = false;
189 190
    bool foundCameraSection = false;

191
    qCDebug(MissionSettingsComplexItemLog) << "MissionSettingsItem::scanForMissionSettings count:scanIndex" << visualItems->count() << scanIndex;
192 193

    // Scan through the initial mission items for possible mission settings
194 195
    foundCameraSection = _cameraSection.scanForSection(visualItems, scanIndex);
    foundSpeedSection = _speedSection.scanForSection(visualItems, scanIndex);
196

197 198 199 200 201 202 203
    // Look at the end of the mission for end actions

    int lastIndex = visualItems->count() - 1;
    SimpleMissionItem* item = visualItems->value<SimpleMissionItem*>(lastIndex);
    if (item) {
        MissionItem& missionItem = item->missionItem();

Don Gagne's avatar
Don Gagne committed
204
        if (missionItem.command() == MAV_CMD_NAV_RETURN_TO_LAUNCH &&
205 206
                missionItem.param1() == 0 && missionItem.param2() == 0 && missionItem.param3() == 0 && missionItem.param4() == 0 && missionItem.param5() == 0 && missionItem.param6() == 0 && missionItem.param7() == 0) {
            qCDebug(MissionSettingsComplexItemLog) << "Scan: Found end action RTL";
207
            _missionEndRTL = true;
208
            visualItems->removeAt(lastIndex)->deleteLater();
209 210 211
        }
    }

212
    return foundSpeedSection || foundCameraSection;
213 214
}

215
double MissionSettingsItem::complexDistance(void) const
216 217 218 219
{
    return 0;
}

220
void MissionSettingsItem::_setDirty(void)
221 222 223 224
{
    setDirty(true);
}

225 226 227 228 229 230
void MissionSettingsItem::setHomePositionFromVehicle(const QGeoCoordinate& coordinate)
{
    _plannedHomePositionFromVehicle = true;
    setCoordinate(coordinate);
}

231
void MissionSettingsItem::setCoordinate(const QGeoCoordinate& coordinate)
232
{
233
    if (_plannedHomePositionCoordinate != coordinate) {
234
        // ArduPilot tends to send crap home positions at initial vehicel boot, discard them
Don Gagne's avatar
Don Gagne committed
235
        if (coordinate.isValid() && (coordinate.latitude() != 0 || coordinate.longitude() != 0)) {
236 237 238 239 240
            _plannedHomePositionCoordinate = coordinate;
            emit coordinateChanged(coordinate);
            emit exitCoordinateChanged(coordinate);
            _plannedHomePositionAltitudeFact.setRawValue(coordinate.altitude());
        }
241 242 243
    }
}

244
void MissionSettingsItem::_setDirtyAndUpdateLastSequenceNumber(void)
245 246 247 248 249
{
    emit lastSequenceNumberChanged(lastSequenceNumber());
    setDirty(true);
}

250
void MissionSettingsItem::_sectionDirtyChanged(bool dirty)
251 252 253 254 255
{
    if (dirty) {
        setDirty(true);
    }
}
DonLakeFlyer's avatar
DonLakeFlyer committed
256

257
double MissionSettingsItem::specifiedGimbalYaw(void)
DonLakeFlyer's avatar
DonLakeFlyer committed
258 259 260
{
    return _cameraSection.specifyGimbal() ? _cameraSection.gimbalYaw()->rawValue().toDouble() : std::numeric_limits<double>::quiet_NaN();
}
261

262 263 264 265 266
double MissionSettingsItem::specifiedGimbalPitch(void)
{
    return _cameraSection.specifyGimbal() ? _cameraSection.gimbalPitch()->rawValue().toDouble() : std::numeric_limits<double>::quiet_NaN();
}

267 268 269 270 271 272 273 274 275 276 277
void MissionSettingsItem::_updateAltitudeInCoordinate(QVariant value)
{
    double newAltitude = value.toDouble();

    if (!qFuzzyCompare(_plannedHomePositionCoordinate.altitude(), newAltitude)) {

        _plannedHomePositionCoordinate.setAltitude(newAltitude);
        emit coordinateChanged(_plannedHomePositionCoordinate);
        emit exitCoordinateChanged(_plannedHomePositionCoordinate);
    }
}
278 279 280 281 282 283 284 285 286

double MissionSettingsItem::specifiedFlightSpeed(void)
{
    if (_speedSection.specifyFlightSpeed()) {
        return _speedSection.flightSpeed()->rawValue().toDouble();
    } else {
        return std::numeric_limits<double>::quiet_NaN();
    }
}
287 288 289 290 291 292 293 294

void MissionSettingsItem::setMissionEndRTL(bool missionEndRTL)
{
    if (missionEndRTL != _missionEndRTL) {
        _missionEndRTL = missionEndRTL;
        emit missionEndRTLChanged(missionEndRTL);
    }
}
295 296 297 298 299 300 301

void MissionSettingsItem::_setHomeAltFromTerrain(double terrainAltitude)
{
    if (!_plannedHomePositionFromVehicle) {
        _plannedHomePositionAltitudeFact.setRawValue(terrainAltitude);
    }
}
302 303 304

QString MissionSettingsItem::abbreviation(void) const
{
305
    return _flyView ? tr("H") : tr("Planned Home");
306
}