MissionSettingsItem.cc 10.8 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")

DonLakeFlyer's avatar
DonLakeFlyer committed
24
const char* MissionSettingsItem::_plannedHomePositionAltitudeName = "PlannedHomePositionAltitude";
25

26
QMap<QString, FactMetaData*> MissionSettingsItem::_metaDataMap;
27

28 29
MissionSettingsItem::MissionSettingsItem(Vehicle* vehicle, bool flyView, QObject* parent)
    : ComplexMissionItem                (vehicle, flyView, parent)
30
    , _plannedHomePositionAltitudeFact  (0, _plannedHomePositionAltitudeName,   FactMetaData::valueTypeDouble)
31 32
    , _cameraSection                    (vehicle)
    , _speedSection                     (vehicle)
33 34 35 36
{
    _editorQml = "qrc:/qml/MissionSettingsEditor.qml";

    if (_metaDataMap.isEmpty()) {
37
        _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/MissionSettings.FactMetaData.json"), nullptr /* metaDataParent */);
38 39 40 41 42 43
    }

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

44 45 46
    _cameraSection.setAvailable(true);
    _speedSection.setAvailable(true);

47
    connect(this,               &MissionSettingsItem::specifyMissionFlightSpeedChanged, this, &MissionSettingsItem::_setDirtyAndUpdateLastSequenceNumber);
48 49
    connect(&_cameraSection,    &CameraSection::itemCountChanged,                       this, &MissionSettingsItem::_setDirtyAndUpdateLastSequenceNumber);
    connect(&_speedSection,     &CameraSection::itemCountChanged,                       this, &MissionSettingsItem::_setDirtyAndUpdateLastSequenceNumber);
50
    connect(this,               &MissionSettingsItem::terrainAltitudeChanged,           this, &MissionSettingsItem::_setHomeAltFromTerrain);
51 52 53 54 55 56
    connect(&_cameraSection,    &CameraSection::dirtyChanged,                           this, &MissionSettingsItem::_sectionDirtyChanged);
    connect(&_speedSection,     &SpeedSection::dirtyChanged,                            this, &MissionSettingsItem::_sectionDirtyChanged);
    connect(&_cameraSection,    &CameraSection::specifiedGimbalYawChanged,              this, &MissionSettingsItem::specifiedGimbalYawChanged);
    connect(&_cameraSection,    &CameraSection::specifiedGimbalPitchChanged,            this, &MissionSettingsItem::specifiedGimbalPitchChanged);
    connect(&_speedSection,     &SpeedSection::specifiedFlightSpeedChanged,             this, &MissionSettingsItem::specifiedFlightSpeedChanged);
    connect(_vehicle,           &Vehicle::homePositionChanged,                          this, &MissionSettingsItem::_updateHomePosition);
57
    connect(&_plannedHomePositionAltitudeFact,  &Fact::rawValueChanged,                 this, &MissionSettingsItem::_updateAltitudeInCoordinate);
58

59
    _updateHomePosition(_vehicle->homePosition());
60 61
}

62
int MissionSettingsItem::lastSequenceNumber(void) const
63
{
64
    int lastSequenceNumber = _sequenceNumber;
65

66 67
    lastSequenceNumber += _cameraSection.itemCount();
    lastSequenceNumber += _speedSection.itemCount();
68 69 70 71

    return lastSequenceNumber;
}

72
void MissionSettingsItem::setDirty(bool dirty)
73 74 75
{
    if (_dirty != dirty) {
        _dirty = dirty;
76 77 78 79
        if (!dirty) {
            _cameraSection.setDirty(false);
            _speedSection.setDirty(false);
        }
80 81 82 83
        emit dirtyChanged(_dirty);
    }
}

84
void MissionSettingsItem::save(QJsonArray&  missionItems)
85
{
86 87 88
    QList<MissionItem*> items;

    appendMissionItems(items, this);
89

90
    // First item should be planned home position, we are not responsible for save/load
91
    // Remaining items we just output as is
92 93
    for (int i=1; i<items.count(); i++) {
        MissionItem* item = items[i];
94 95 96
        QJsonObject saveObject;
        item->save(saveObject);
        missionItems.append(saveObject);
97
        item->deleteLater();
98 99 100
    }
}

101
void MissionSettingsItem::setSequenceNumber(int sequenceNumber)
102 103 104 105 106 107 108 109
{
    if (_sequenceNumber != sequenceNumber) {
        _sequenceNumber = sequenceNumber;
        emit sequenceNumberChanged(sequenceNumber);
        emit lastSequenceNumberChanged(lastSequenceNumber());
    }
}

110
bool MissionSettingsItem::load(const QJsonObject& complexObject, int sequenceNumber, QString& errorString)
111 112 113 114 115 116 117 118
{
    Q_UNUSED(complexObject);
    Q_UNUSED(sequenceNumber);
    Q_UNUSED(errorString);

    return true;
}

119
double MissionSettingsItem::greatestDistanceTo(const QGeoCoordinate &other) const
120 121 122 123 124
{
    Q_UNUSED(other);
    return 0;
}

125
bool MissionSettingsItem::specifiesCoordinate(void) const
126
{
DonLakeFlyer's avatar
DonLakeFlyer committed
127
    return true;
128 129
}

130
void MissionSettingsItem::appendMissionItems(QList<MissionItem*>& items, QObject* missionItemParent)
131 132 133
{
    int seqNum = _sequenceNumber;

134
    // IMPORTANT NOTE: If anything changes here you must also change MissionSettingsItem::scanForMissionSettings
135 136 137 138 139 140 141 142 143

    // 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
144 145
                                        coordinate().latitude(),
                                        coordinate().longitude(),
146 147 148
                                        _plannedHomePositionAltitudeFact.rawValue().toDouble(),
                                        true,                   // autoContinue
                                        false,                  // isCurrentItem
149 150
                                        missionItemParent);
    items.append(item);
151

152 153
    _cameraSection.appendSectionItems(items, missionItemParent, seqNum);
    _speedSection.appendSectionItems(items, missionItemParent, seqNum);
154 155
}

156
bool MissionSettingsItem::addMissionEndAction(QList<MissionItem*>& /*items*/, int /*seqNum*/, QObject* /*missionItemParent*/)
157
{
158
    return false;
159 160
}

161
bool MissionSettingsItem::scanForMissionSettings(QmlObjectListModel* visualItems, int scanIndex)
162
{
163
    bool foundSpeedSection = false;
164 165
    bool foundCameraSection = false;

166
    qCDebug(MissionSettingsComplexItemLog) << "MissionSettingsItem::scanForMissionSettings count:scanIndex" << visualItems->count() << scanIndex;
167 168

    // Scan through the initial mission items for possible mission settings
169 170
    foundCameraSection = _cameraSection.scanForSection(visualItems, scanIndex);
    foundSpeedSection = _speedSection.scanForSection(visualItems, scanIndex);
171

172
    return foundSpeedSection || foundCameraSection;
173 174
}

175
double MissionSettingsItem::complexDistance(void) const
176 177 178 179
{
    return 0;
}

180
void MissionSettingsItem::_setDirty(void)
181 182 183 184
{
    setDirty(true);
}

185
void MissionSettingsItem::_setCoordinateWorker(const QGeoCoordinate& coordinate)
186
{
187 188 189 190 191 192
    if (_plannedHomePositionCoordinate != coordinate) {
        _plannedHomePositionCoordinate = coordinate;
        emit coordinateChanged(coordinate);
        emit exitCoordinateChanged(coordinate);
        _plannedHomePositionAltitudeFact.setRawValue(coordinate.altitude());
    }
193 194
}

195
void MissionSettingsItem::setHomePositionFromVehicle(Vehicle* vehicle)
196
{
197 198 199
    // If the user hasn't moved the planned home position manually we use the value from the vehicle
    if (!_plannedHomePositionMovedByUser) {
        QGeoCoordinate coordinate = vehicle->homePosition();
200
        // ArduPilot tends to send crap home positions at initial vehicle boot, discard them
Don Gagne's avatar
Don Gagne committed
201
        if (coordinate.isValid() && (coordinate.latitude() != 0 || coordinate.longitude() != 0)) {
202 203
            _plannedHomePositionFromVehicle = true;
            _setCoordinateWorker(coordinate);
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
void MissionSettingsItem::setInitialHomePosition(const QGeoCoordinate& coordinate)
{
    _plannedHomePositionMovedByUser = false;
    _plannedHomePositionFromVehicle = false;
    _setCoordinateWorker(coordinate);
}

void MissionSettingsItem::setInitialHomePositionFromUser(const QGeoCoordinate& coordinate)
{
    _plannedHomePositionMovedByUser = true;
    _plannedHomePositionFromVehicle = false;
    _setCoordinateWorker(coordinate);
}


void MissionSettingsItem::setCoordinate(const QGeoCoordinate& coordinate)
{
    if (coordinate != this->coordinate()) {
        // The user is moving the planned home position manually. Stop tracking vehicle home position.
        _plannedHomePositionMovedByUser = true;
        _plannedHomePositionFromVehicle = false;
        _setCoordinateWorker(coordinate);
    }
}

233
void MissionSettingsItem::_setDirtyAndUpdateLastSequenceNumber(void)
234 235 236 237 238
{
    emit lastSequenceNumberChanged(lastSequenceNumber());
    setDirty(true);
}

239
void MissionSettingsItem::_sectionDirtyChanged(bool dirty)
240 241 242 243 244
{
    if (dirty) {
        setDirty(true);
    }
}
DonLakeFlyer's avatar
DonLakeFlyer committed
245

246
double MissionSettingsItem::specifiedGimbalYaw(void)
DonLakeFlyer's avatar
DonLakeFlyer committed
247 248 249
{
    return _cameraSection.specifyGimbal() ? _cameraSection.gimbalYaw()->rawValue().toDouble() : std::numeric_limits<double>::quiet_NaN();
}
250

251 252 253 254 255
double MissionSettingsItem::specifiedGimbalPitch(void)
{
    return _cameraSection.specifyGimbal() ? _cameraSection.gimbalPitch()->rawValue().toDouble() : std::numeric_limits<double>::quiet_NaN();
}

256 257 258 259 260 261 262 263 264 265
void MissionSettingsItem::_updateAltitudeInCoordinate(QVariant value)
{
    double newAltitude = value.toDouble();

    if (!qFuzzyCompare(_plannedHomePositionCoordinate.altitude(), newAltitude)) {
        _plannedHomePositionCoordinate.setAltitude(newAltitude);
        emit coordinateChanged(_plannedHomePositionCoordinate);
        emit exitCoordinateChanged(_plannedHomePositionCoordinate);
    }
}
266 267 268 269 270 271 272 273 274

double MissionSettingsItem::specifiedFlightSpeed(void)
{
    if (_speedSection.specifyFlightSpeed()) {
        return _speedSection.flightSpeed()->rawValue().toDouble();
    } else {
        return std::numeric_limits<double>::quiet_NaN();
    }
}
275

276 277
void MissionSettingsItem::_setHomeAltFromTerrain(double terrainAltitude)
{
278
    if (!_plannedHomePositionFromVehicle && !qIsNaN(terrainAltitude)) {
279 280 281
        _plannedHomePositionAltitudeFact.setRawValue(terrainAltitude);
    }
}
282 283 284

QString MissionSettingsItem::abbreviation(void) const
{
285
    return _flyView ? tr("L") : tr("Launch");
286
}
287 288 289 290 291 292 293

void MissionSettingsItem::_updateHomePosition(const QGeoCoordinate& homePosition)
{
    if (_flyView) {
        setCoordinate(homePosition);
    }
}