MissionController.h 42.4 KB
Newer Older
1 2
/****************************************************************************
 *
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
9

10
#pragma once
11

12
#include "PlanElementController.h"
13 14
#include "QmlObjectListModel.h"
#include "Vehicle.h"
15
#include "QGCLoggingCategory.h"
16
#include "KMLPlanDomDocument.h"
17
#include "QGCGeoBoundingCube.h"
18
#include "QGroundControlQmlGlobal.h"
19

20 21
#include <QHash>

22
<<<<<<< HEAD
23 24


25
class CoordinateVector;
26 27 28
=======
class FlightPathSegment;
>>>>>>> upstream_merge
DonLakeFlyer's avatar
DonLakeFlyer committed
29
class VisualMissionItem;
30
class MissionItem;
31
class AppSettings;
32
class MissionManager;
33
class SimpleMissionItem;
34
class ComplexMissionItem;
35 36
class MissionSettingsItem;
class TakeoffMissionItem;
37
class QDomDocument;
38
class PlanViewSettings;
39

40
Q_DECLARE_LOGGING_CATEGORY(MissionControllerLog)
41

42
typedef QPair<VisualMissionItem*,VisualMissionItem*> VisualItemPair;
43
typedef QHash<VisualItemPair, FlightPathSegment*> FlightPathSegmentHashTable;
44

45
class MissionController : public PlanElementController
46 47
{
    Q_OBJECT
48

49
public:
50
    MissionController(PlanMasterController* masterController, QObject* parent = nullptr);
51 52
    ~MissionController();

53
<<<<<<< HEAD
54
    typedef struct _MissionFlightStatus_t {
55
        double  maxTelemetryDistance;
56
        void    dirtyChanged();
57 58 59 60 61 62 63 64 65
        double  totalDistance;
        double  totalTime;
        double  hoverDistance;
        double  hoverTime;
        double  cruiseDistance;
        double  cruiseTime;
        double  cruiseSpeed;
        double  hoverSpeed;
        double  vehicleSpeed;           ///< Either cruise or hover speed based on vehicle type and vtol state
66
        double  vehicleYaw;
67
        double  gimbalYaw;              ///< NaN signals yaw was never changed
68
        double  gimbalPitch;            ///< NaN signals pitch was never changed
69
        int     mAhBattery;             ///< 0 for not available
Don Gagne's avatar
Don Gagne committed
70 71
        double  hoverAmps;              ///< Amp consumption during hover
        double  cruiseAmps;             ///< Amp consumption during cruise
72 73 74 75 76
        double  ampMinutesAvailable;    ///< Amp minutes available from single battery
        double  hoverAmpsTotal;         ///< Total hover amps used
        double  cruiseAmpsTotal;        ///< Total cruise amps used
        int     batteryChangePoint;     ///< -1 for not supported, 0 for not needed
        int     batteriesRequired;      ///< -1 for not supported
DonLakeFlyer's avatar
DonLakeFlyer committed
77 78
    } MissionFlightStatus_t;

79
    Q_PROPERTY(QmlObjectListModel*  visualItems             READ visualItems                NOTIFY visualItemsChanged)
80 81
    Q_PROPERTY(QmlObjectListModel*  waypointLines           READ waypointLines              NOTIFY waypointLinesChanged)        ///< Used by Plan view only for interactive editing
    Q_PROPERTY(QVariantList         waypointPath            READ waypointPath               NOTIFY waypointPathChanged)         ///< Used by Fly view only for static display
82
    Q_PROPERTY(QStringList          complexMissionItemNames READ complexMissionItemNames    NOTIFY complexMissionItemNamesChanged)
83
    Q_PROPERTY(QGeoCoordinate       plannedHomePosition     READ plannedHomePosition        NOTIFY plannedHomePositionChanged)
84

85 86
    Q_PROPERTY(double               progressPct             READ progressPct                NOTIFY progressPctChanged)

87
    Q_PROPERTY(int                  missionItemCount        READ missionItemCount           NOTIFY missionItemCountChanged)     ///< True mission item command count (only valid in Fly View)
88 89
    Q_PROPERTY(int                  currentMissionIndex     READ currentMissionIndex        NOTIFY currentMissionIndexChanged)
    Q_PROPERTY(int                  resumeMissionIndex      READ resumeMissionIndex         NOTIFY resumeMissionIndexChanged)   ///< Returns the item index two which a mission should be resumed. -1 indicates resume mission not available.
90 91
    Q_PROPERTY(double               remainingDistance       READ remainingDistance          NOTIFY remainingDistanceChanged)
    Q_PROPERTY(double               remainingTime           READ remainingTime              NOTIFY remainingTimeChanged)
92

93 94 95
    Q_PROPERTY(int                  currentPlanViewIndex    READ currentPlanViewIndex       NOTIFY currentPlanViewIndexChanged)
    Q_PROPERTY(VisualMissionItem*   currentPlanViewItem     READ currentPlanViewItem        NOTIFY currentPlanViewItemChanged)

96 97 98 99 100 101 102
    Q_PROPERTY(double               missionDistance         READ missionDistance            NOTIFY missionDistanceChanged)
    Q_PROPERTY(double               missionTime             READ missionTime                NOTIFY missionTimeChanged)
    Q_PROPERTY(double               missionHoverDistance    READ missionHoverDistance       NOTIFY missionHoverDistanceChanged)
    Q_PROPERTY(double               missionCruiseDistance   READ missionCruiseDistance      NOTIFY missionCruiseDistanceChanged)
    Q_PROPERTY(double               missionHoverTime        READ missionHoverTime           NOTIFY missionHoverTimeChanged)
    Q_PROPERTY(double               missionCruiseTime       READ missionCruiseTime          NOTIFY missionCruiseTimeChanged)
    Q_PROPERTY(double               missionMaxTelemetry     READ missionMaxTelemetry        NOTIFY missionMaxTelemetryChanged)
103

104 105
    Q_PROPERTY(int                  batteryChangePoint      READ batteryChangePoint         NOTIFY batteryChangePointChanged)
    Q_PROPERTY(int                  batteriesRequired       READ batteriesRequired          NOTIFY batteriesRequiredChanged)
106
    Q_PROPERTY(QGCGeoBoundingCube*  travelBoundingCube      READ travelBoundingCube         NOTIFY missionBoundingCubeChanged)
107

108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162
=======
    typedef struct {
        double                      maxTelemetryDistance;
        double                      totalDistance;
        double                      totalTime;
        double                      hoverDistance;
        double                      hoverTime;
        double                      cruiseDistance;
        double                      cruiseTime;
        int                         mAhBattery;             ///< 0 for not available
        double                      hoverAmps;              ///< Amp consumption during hover
        double                      cruiseAmps;             ///< Amp consumption during cruise
        double                      ampMinutesAvailable;    ///< Amp minutes available from single battery
        double                      hoverAmpsTotal;         ///< Total hover amps used
        double                      cruiseAmpsTotal;        ///< Total cruise amps used
        int                         batteryChangePoint;     ///< -1 for not supported, 0 for not needed
        int                         batteriesRequired;      ///< -1 for not supported
        double                      vehicleYaw;
        double                      gimbalYaw;              ///< NaN signals yaw was never changed
        double                      gimbalPitch;            ///< NaN signals pitch was never changed
        // The following values are the state prior to executing this item
        QGCMAVLink::VehicleClass_t  vtolMode;               ///< Either VehicleClassFixedWing, VehicleClassMultiRotor, VehicleClassGeneric (mode unknown)
        double                      cruiseSpeed;
        double                      hoverSpeed;
        double                      vehicleSpeed;           ///< Either cruise or hover speed based on vehicle type and vtol state
    } MissionFlightStatus_t;

    Q_PROPERTY(QmlObjectListModel*  visualItems                     READ visualItems                    NOTIFY visualItemsChanged)
    Q_PROPERTY(QmlObjectListModel*  simpleFlightPathSegments        READ simpleFlightPathSegments       CONSTANT)                               ///< Used by Plan view only for interactive editing
    Q_PROPERTY(QVariantList         waypointPath                    READ waypointPath                   NOTIFY waypointPathChanged)             ///< Used by Fly view only for static display
    Q_PROPERTY(QmlObjectListModel*  directionArrows                 READ directionArrows                CONSTANT)
    Q_PROPERTY(QmlObjectListModel*  incompleteComplexItemLines      READ incompleteComplexItemLines     CONSTANT)                               ///< Segments which are not yet completed.
    Q_PROPERTY(QStringList          complexMissionItemNames         READ complexMissionItemNames        NOTIFY complexMissionItemNamesChanged)
    Q_PROPERTY(QGeoCoordinate       plannedHomePosition             READ plannedHomePosition            NOTIFY plannedHomePositionChanged)      ///< Includes AMSL altitude
    Q_PROPERTY(QGeoCoordinate       previousCoordinate              MEMBER _previousCoordinate          NOTIFY previousCoordinateChanged)
    Q_PROPERTY(FlightPathSegment*   splitSegment                    MEMBER _splitSegment                NOTIFY splitSegmentChanged)             ///< Segment which show show + split ui element
    Q_PROPERTY(double               progressPct                     READ progressPct                    NOTIFY progressPctChanged)
    Q_PROPERTY(int                  missionItemCount                READ missionItemCount               NOTIFY missionItemCountChanged)         ///< True mission item command count (only valid in Fly View)
    Q_PROPERTY(int                  currentMissionIndex             READ currentMissionIndex            NOTIFY currentMissionIndexChanged)
    Q_PROPERTY(int                  resumeMissionIndex              READ resumeMissionIndex             NOTIFY resumeMissionIndexChanged)       ///< Returns the item index two which a mission should be resumed. -1 indicates resume mission not available.
    Q_PROPERTY(int                  currentPlanViewSeqNum           READ currentPlanViewSeqNum          NOTIFY currentPlanViewSeqNumChanged)
    Q_PROPERTY(int                  currentPlanViewVIIndex          READ currentPlanViewVIIndex         NOTIFY currentPlanViewVIIndexChanged)
    Q_PROPERTY(VisualMissionItem*   currentPlanViewItem             READ currentPlanViewItem            NOTIFY currentPlanViewItemChanged)
    Q_PROPERTY(TakeoffMissionItem*  takeoffMissionItem              READ takeoffMissionItem             NOTIFY takeoffMissionItemChanged)
    Q_PROPERTY(double               missionDistance                 READ missionDistance                NOTIFY missionDistanceChanged)
    Q_PROPERTY(double               missionTime                     READ missionTime                    NOTIFY missionTimeChanged)
    Q_PROPERTY(double               missionHoverDistance            READ missionHoverDistance           NOTIFY missionHoverDistanceChanged)
    Q_PROPERTY(double               missionCruiseDistance           READ missionCruiseDistance          NOTIFY missionCruiseDistanceChanged)
    Q_PROPERTY(double               missionHoverTime                READ missionHoverTime               NOTIFY missionHoverTimeChanged)
    Q_PROPERTY(double               missionCruiseTime               READ missionCruiseTime              NOTIFY missionCruiseTimeChanged)
    Q_PROPERTY(double               missionMaxTelemetry             READ missionMaxTelemetry            NOTIFY missionMaxTelemetryChanged)
    Q_PROPERTY(int                  batteryChangePoint              READ batteryChangePoint             NOTIFY batteryChangePointChanged)
    Q_PROPERTY(int                  batteriesRequired               READ batteriesRequired              NOTIFY batteriesRequiredChanged)
    Q_PROPERTY(QGCGeoBoundingCube*  travelBoundingCube              READ travelBoundingCube             NOTIFY missionBoundingCubeChanged)
>>>>>>> upstream_merge
163 164 165
    Q_PROPERTY(QString              surveyComplexItemName           READ surveyComplexItemName          CONSTANT)
    Q_PROPERTY(QString              corridorScanComplexItemName     READ corridorScanComplexItemName    CONSTANT)
    Q_PROPERTY(QString              structureScanComplexItemName    READ structureScanComplexItemName   CONSTANT)
166 167 168 169 170 171 172 173
    Q_PROPERTY(bool                 onlyInsertTakeoffValid          MEMBER _onlyInsertTakeoffValid      NOTIFY onlyInsertTakeoffValidChanged)
    Q_PROPERTY(bool                 isInsertTakeoffValid            MEMBER _isInsertTakeoffValid        NOTIFY isInsertTakeoffValidChanged)
    Q_PROPERTY(bool                 isInsertLandValid               MEMBER _isInsertLandValid           NOTIFY isInsertLandValidChanged)
    Q_PROPERTY(bool                 isROIActive                     MEMBER _isROIActive                 NOTIFY isROIActiveChanged)
    Q_PROPERTY(bool                 isROIBeginCurrentItem           MEMBER _isROIBeginCurrentItem       NOTIFY isROIBeginCurrentItemChanged)
    Q_PROPERTY(bool                 flyThroughCommandsAllowed       MEMBER _flyThroughCommandsAllowed   NOTIFY flyThroughCommandsAllowedChanged)
    Q_PROPERTY(double               minAMSLAltitude                 MEMBER _minAMSLAltitude             NOTIFY minAMSLAltitudeChanged)          ///< Minimum altitude associated with this mission. Used to calculate percentages for terrain status.
    Q_PROPERTY(double               maxAMSLAltitude                 MEMBER _maxAMSLAltitude             NOTIFY maxAMSLAltitudeChanged)          ///< Maximum altitude associated with this mission. Used to calculate percentages for terrain status.
174

175
<<<<<<< HEAD
Valentin Platzgummer's avatar
Valentin Platzgummer committed
176

177
    Q_INVOKABLE void removeMissionItem(int index);
178

179 180 181
    /// Add a new simple mission item to the list
    ///     @param i: index to insert at
    /// @return Sequence number for new item
182 183 184 185 186 187
    Q_INVOKABLE int insertSimpleMissionItem(const QGeoCoordinate &coordinate, int i);

    /// Add a list of new simple mission item to the list.
    ///     @param i: index to insert at
    /// @return Sequence number of last item.
    Q_INVOKABLE int insertSimpleMissionItems(const QList<QGeoCoordinate> &coordinates, int i);
188

189 190 191 192 193
    /// Add a new simple mission item to the list
    ///     @param i: index to insert at
    /// @return Sequence number for new item
                int insertSimpleMissionItem(const MissionItem &missionItem, int i);

194 195 196
    /// Add a new simple mission item to the list
    ///     @param i: index to insert at
    /// @return Sequence number for new item
197
                int insertSimpleMissionItem(const SimpleMissionItem &missionItem, int i);
198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224
=======
    Q_PROPERTY(QGroundControlQmlGlobal::AltitudeMode globalAltitudeMode         READ globalAltitudeMode         WRITE setGlobalAltitudeMode NOTIFY globalAltitudeModeChanged)   ///< AltitudeModeNone indicates the plan can used mixed modes
    Q_PROPERTY(QGroundControlQmlGlobal::AltitudeMode globalAltitudeModeDefault  READ globalAltitudeModeDefault  NOTIFY globalAltitudeModeChanged)                               ///< Default to use for newly created items

    Q_INVOKABLE void removeVisualItem(int viIndex);

    /// Add a new simple mission item to the list
    ///     @param coordinate: Coordinate for item
    ///     @param visualItemIndex: index to insert at, -1 for end of list
    ///     @param makeCurrentItem: true: Make this item the current item
    /// @return Newly created item
    Q_INVOKABLE VisualMissionItem* insertSimpleMissionItem(QGeoCoordinate coordinate, int visualItemIndex, bool makeCurrentItem = false);

    /// Add a new takeoff item to the list
    ///     @param coordinate: Coordinate for item
    ///     @param visualItemIndex: index to insert at, -1 for end of list
    ///     @param makeCurrentItem: true: Make this item the current item
    /// @return Newly created item
    Q_INVOKABLE VisualMissionItem* insertTakeoffItem(QGeoCoordinate coordinate, int visualItemIndex, bool makeCurrentItem = false);

    /// Add a new land item to the list
    ///     @param coordinate: Coordinate for item
    ///     @param visualItemIndex: index to insert at, -1 for end of list
    ///     @param makeCurrentItem: true: Make this item the current item
    /// @return Newly created item
    Q_INVOKABLE VisualMissionItem* insertLandItem(QGeoCoordinate coordinate, int visualItemIndex, bool makeCurrentItem = false);
>>>>>>> upstream_merge
225

226
    /// Add a new ROI mission item to the list
227 228 229 230 231 232 233 234 235 236 237
    ///     @param coordinate: Coordinate for item
    ///     @param visualItemIndex: index to insert at, -1 for end of list
    ///     @param makeCurrentItem: true: Make this item the current item
    /// @return Newly created item
    Q_INVOKABLE VisualMissionItem*  insertROIMissionItem(QGeoCoordinate coordinate, int visualItemIndex, bool makeCurrentItem = false);

    /// Add a new Cancel ROI mission item to the list
    ///     @param visualItemIndex: index to insert at, -1 for end of list
    ///     @param makeCurrentItem: true: Make this item the current item
    /// @return Newly created item
    Q_INVOKABLE VisualMissionItem*  insertCancelROIMissionItem(int visualItemIndex, bool makeCurrentItem = false);
238

239
    /// Add a new complex mission item to the list
240 241
    ///     @param itemName: Name of complex item to create (from complexMissionItemNames)
    ///     @param mapCenterCoordinate: coordinate for current center of map
242 243 244 245
    ///     @param visualItemIndex: index to insert at, -1 for end of list
    ///     @param makeCurrentItem: true: Make this item the current item
    /// @return Newly created item
    Q_INVOKABLE VisualMissionItem*  insertComplexMissionItem(QString itemName, QGeoCoordinate mapCenterCoordinate, int visualItemIndex, bool makeCurrentItem = false);
246

247 248
    /// Add a new complex mission item to the list
    ///     @param itemName: Name of complex item to create (from complexMissionItemNames)
249
    ///     @param file: kml or shp file to load from shape from
250 251 252 253 254
    ///     @param coordinate: Coordinate for item
    ///     @param visualItemIndex: index to insert at, -1 for end of list
    ///     @param makeCurrentItem: true: Make this item the current item
    /// @return Newly created item
    Q_INVOKABLE VisualMissionItem*  insertComplexMissionItemFromKMLOrSHP(QString itemName, QString file, int visualItemIndex, bool makeCurrentItem = false);
255

256 257
    Q_INVOKABLE void resumeMission(int resumeIndex);

DonLakeFlyer's avatar
DonLakeFlyer committed
258 259 260
    /// Updates the altitudes of the items in the current mission to the new default altitude
    Q_INVOKABLE void applyDefaultMissionAltitude(void);

261 262
    /// Sets a new current mission item (PlanView).
    ///     @param sequenceNumber - index for new item, -1 to clear current item
263 264 265 266 267 268 269 270 271
    Q_INVOKABLE void setCurrentPlanViewSeqNum(int sequenceNumber, bool force);

    enum SendToVehiclePreCheckState {
        SendToVehiclePreCheckStateOk,                       // Ok to send plan to vehicle
        SendToVehiclePreCheckStateNoActiveVehicle,          // There is no active vehicle
        SendToVehiclePreCheckStateFirwmareVehicleMismatch,  // Firmware/Vehicle type for plan mismatch with actual vehicle
        SendToVehiclePreCheckStateActiveMission,            // Vehicle is currently flying a mission
    };
    Q_ENUM(SendToVehiclePreCheckState)
272

273 274 275 276 277 278
    Q_INVOKABLE SendToVehiclePreCheckState sendToVehiclePreCheck(void);

    /// Determines if the mission has all data needed to be saved or sent to the vehicle.
    /// IMPORTANT NOTE: The return value is a VisualMissionItem::ReadForSaveState value. It is an int here to work around
    /// a nightmare of circular header dependency problems.
    int readyForSaveState(void) const;
279

280
    /// sets the command in missionItem to a land command
281
    bool setLandCommand     (SimpleMissionItem &missionItem);
282
    /// sets the command in missionItem to a takeoff command
283
    bool setTakeoffCommand  (SimpleMissionItem &missionItem);
284

Don Gagne's avatar
Don Gagne committed
285 286 287
    /// Sends the mission items to the specified vehicle
    static void sendItemsToVehicle(Vehicle* vehicle, QmlObjectListModel* visualMissionItems);

288 289 290
    static bool convertToMissionItems(QmlObjectListModel* visualMissionItems, QList<MissionItem*>& rgMissionItems, QObject* missionItemParent);


291

292 293 294
    bool loadJsonFile(QFile& file, QString& errorString);
    bool loadTextFile(QFile& file, QString& errorString);

295
    QGCGeoBoundingCube* travelBoundingCube  () { return &_travelBoundingCube; }
296 297
    QGeoCoordinate      takeoffCoordinate   () { return _takeoffCoordinate; }

298
    // Overrides from PlanElementController
299
    bool supported                  (void) const final { return true; }
300
    void start                      (bool flyView) final;
301 302
    void save                       (QJsonObject& json) final;
    bool load                       (const QJsonObject& json, QString& errorString) final;
303 304 305
    void loadFromVehicle            (void) final;
    void sendToVehicle              (void) final;
    void removeAll                  (void) final;
306
    void removeAllFromVehicle       (void) final;
307 308 309
    bool syncInProgress             (void) const final;
    bool dirty                      (void) const final;
    void setDirty                   (bool dirty) final;
310
    bool containsItems              (void) const final;
DonLakeFlyer's avatar
DonLakeFlyer committed
311
    bool showPlanFromManagerVehicle (void) final;
312

313
    // Create KML file
314
    void addMissionToKML(KMLPlanDomDocument& planKML);
315

316 317
    // Property accessors

318
<<<<<<< HEAD
319 320 321 322 323 324 325 326 327 328 329
    QmlObjectListModel* visualItems                     (void) { return _visualItems; }
    QmlObjectListModel* waypointLines                   (void) { return &_waypointLines; }
    QVariantList        waypointPath                    (void) { return _waypointPath; }
    QStringList         complexMissionItemNames         (void) const;
    QGeoCoordinate      plannedHomePosition             (void) const;
    VisualMissionItem*  currentPlanViewItem             (void) const;
    double              progressPct                     (void) const { return _progressPct; }
    QString             surveyComplexItemName           (void) const { return _surveyMissionItemName; }
    QString             circularSurveyComplexItemName   (void) const { return _circularSurveyMissionItemName; }
    QString             corridorScanComplexItemName     (void) const { return patternCorridorScanName; }
    QString             structureScanComplexItemName    (void) const { return patternStructureScanName; }
330

331 332 333 334
    int     missionItemCount            (void) const { return _missionItemCount; }
    int     currentMissionIndex         (void) const;
    int     resumeMissionIndex          (void) const;
    int     currentPlanViewIndex        (void) const;
335 336 337 338 339 340 341
    // distance and time to mission end (this function does not take into account current vehicle position, if useVehiclePosition == false)
    bool    distanceTimeToMissionEnd    (double &remainingDistance, double &remainingTime, int missionIndex, bool useVehiclePosition) const;

    // distance and time to mission end (this function does (!) take into account current vehicle position)
    double  remainingDistance           () const { return _remainingDistance; }
    // distance and time to mission end (this function does (!) take into account current vehicle position)
    double  remainingTime               () const { return _remainingTime; }
342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365
=======
    QmlObjectListModel* visualItems                 (void) { return _visualItems; }
    QmlObjectListModel* simpleFlightPathSegments    (void) { return &_simpleFlightPathSegments; }
    QmlObjectListModel* directionArrows             (void) { return &_directionArrows; }
    QmlObjectListModel* incompleteComplexItemLines  (void) { return &_incompleteComplexItemLines; }
    QVariantList        waypointPath                (void) { return _waypointPath; }
    QStringList         complexMissionItemNames     (void) const;
    QGeoCoordinate      plannedHomePosition         (void) const;
    VisualMissionItem*  currentPlanViewItem         (void) const { return _currentPlanViewItem; }
    TakeoffMissionItem* takeoffMissionItem          (void) const { return _takeoffMissionItem; }
    double              progressPct                 (void) const { return _progressPct; }
    QString             surveyComplexItemName       (void) const;
    QString             corridorScanComplexItemName (void) const;
    QString             structureScanComplexItemName(void) const;
    bool                isInsertTakeoffValid        (void) const;
    double              minAMSLAltitude             (void) const { return _minAMSLAltitude; }
    double              maxAMSLAltitude             (void) const { return _maxAMSLAltitude; }

    int missionItemCount            (void) const { return _missionItemCount; }
    int currentMissionIndex         (void) const;
    int resumeMissionIndex          (void) const;
    int currentPlanViewSeqNum       (void) const { return _currentPlanViewSeqNum; }
    int currentPlanViewVIIndex      (void) const { return _currentPlanViewVIIndex; }
>>>>>>> upstream_merge
366

DonLakeFlyer's avatar
DonLakeFlyer committed
367 368 369 370 371 372 373
    double  missionDistance         (void) const { return _missionFlightStatus.totalDistance; }
    double  missionTime             (void) const { return _missionFlightStatus.totalTime; }
    double  missionHoverDistance    (void) const { return _missionFlightStatus.hoverDistance; }
    double  missionHoverTime        (void) const { return _missionFlightStatus.hoverTime; }
    double  missionCruiseDistance   (void) const { return _missionFlightStatus.cruiseDistance; }
    double  missionCruiseTime       (void) const { return _missionFlightStatus.cruiseTime; }
    double  missionMaxTelemetry     (void) const { return _missionFlightStatus.maxTelemetryDistance; }
374

375

376 377 378
    int  batteryChangePoint         (void) const { return _missionFlightStatus.batteryChangePoint; }    ///< -1 for not supported, 0 for not needed
    int  batteriesRequired          (void) const { return _missionFlightStatus.batteriesRequired; }     ///< -1 for not supported

379 380 381 382 383
    bool isEmpty                    (void) const;

    QGroundControlQmlGlobal::AltitudeMode globalAltitudeMode(void);
    QGroundControlQmlGlobal::AltitudeMode globalAltitudeModeDefault(void);
    void setGlobalAltitudeMode(QGroundControlQmlGlobal::AltitudeMode altMode);
384

385
signals:
386
<<<<<<< HEAD
387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408
    void visualItemsChanged             (void);
    void waypointLinesChanged           (void);
    void waypointPathChanged            (void);
    void newItemsFromVehicle            (void);
    void missionDistanceChanged         (double missionDistance);
    void missionTimeChanged             (void);
    void missionHoverDistanceChanged    (double missionHoverDistance);
    void missionHoverTimeChanged        (void);
    void missionCruiseDistanceChanged   (double missionCruiseDistance);
    void missionCruiseTimeChanged       (void);
    void missionMaxTelemetryChanged     (double missionMaxTelemetry);
    void complexMissionItemNamesChanged (void);
    void resumeMissionIndexChanged      (void);
    void resumeMissionReady             (void);
    void resumeMissionUploadFail        (void);
    void batteryChangePointChanged      (int batteryChangePoint);
    void batteriesRequiredChanged       (int batteriesRequired);
    void plannedHomePositionChanged     (QGeoCoordinate plannedHomePosition);
    void progressPctChanged             (double progressPct);
    void currentMissionIndexChanged     (int currentMissionIndex);
    void currentPlanViewIndexChanged    (void);
    void currentPlanViewItemChanged     (void);
409
    void missionBoundingCubeChanged     (void);
410
    void missionItemCountChanged        (int missionItemCount);
411 412
    void remainingDistanceChanged       (void);
    void remainingTimeChanged           (void);
413

414
private slots:
415
    void _newMissionItemsAvailableFromVehicle(bool removeAllRequested);
416
    void _itemCommandChanged(void);
417
    void _managerVehicleHomePositionChanged(const QGeoCoordinate& homePosition);
418
    void _inProgressChanged(bool inProgress);
419
    void _currentMissionIndexChanged(int sequenceNumber);
420
    void _recalcWaypointLines(void);
DonLakeFlyer's avatar
DonLakeFlyer committed
421
    void _recalcMissionFlightStatus(void);
422
    void _updateContainsItems(void);
423
    void _progressPctChanged(double progressPct);
424
    void _visualItemsDirtyChanged(bool dirty);
425 426
    void _managerSendComplete(bool error);
    void _managerRemoveAllComplete(bool error);
427 428
    void _updateTimeout();
    void _complexBoundingBoxChanged();
429
    void _recalcAll(void);
430
    void _enableDisableRemainingDistTimeCalculation(bool flying);
431

432
    // updates the fields _remainingTime and_remainingDistance (this function does (!) take into account current vehicle position)
433 434
    // This function is supposed to be called by _remainingDistanceTimeTimer::timeout() exclusively. Calling it in a different way might cause
    // undesired behaviour
435
    void _updateRemainingDistanceTime();
436 437

private:
438
    void _init(void);
439
    void _recalcSequence(void);
440
    void _recalcChildItems(void);
441
    void _recalcAllWithClickCoordinate(const QGeoCoordinate &clickCoordinate);
442 443 444
    void _initAllVisualItems(void);
    void _deinitAllVisualItems(void);
    void _initVisualItem(VisualMissionItem* item);
445 446 447
    void _initVisualItemCommon(VisualMissionItem* visualItem); // Don't call this function, it is used by _initSimpleItem and _initComplexItem.
    void _initSimpleItem(SimpleMissionItem* item);
    void _initComplexItem(ComplexMissionItem* item);
448
    void _deinitVisualItem(VisualMissionItem* item);
Don Gagne's avatar
Don Gagne committed
449
    void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle);
450
    void _calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference);
451
    static double _calcDistanceToHome(VisualMissionItem* currentItem, VisualMissionItem* homeItem);
452
    bool _findPreviousAltitude(int newIndex, double* prevAltitude, int* prevAltitudeMode);
Don Gagne's avatar
Don Gagne committed
453 454
    static double _normalizeLat(double lat);
    static double _normalizeLon(double lon);
DonLakeFlyer's avatar
DonLakeFlyer committed
455
    void _addMissionSettings(QmlObjectListModel* visualItems, bool addToCenter);
456 457 458 459
    bool _loadJsonMissionFile(const QByteArray& bytes, QmlObjectListModel* visualItems, QString& errorString);
    bool _loadJsonMissionFileV1(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString);
    bool _loadJsonMissionFileV2(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString);
    bool _loadTextMissionFile(QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString);
460
    int _nextSequenceNumber(void);
461
    void _scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle);
462
    void _setPlannedHomePositionFromFirstCoordinate(const QGeoCoordinate& clickCoordinate);
463 464 465 466
    void _resetMissionFlightStatus(void);
    void _addHoverTime(double hoverTime, double hoverDistance, int waypointIndex);
    void _addCruiseTime(double cruiseTime, double cruiseDistance, int wayPointIndex);
    void _updateBatteryInfo(int waypointIndex);
467
    bool _loadItemsFromJson(const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString);
468
    void _initLoadedVisualItems(QmlObjectListModel* loadedVisualItems);
469
    void _addWaypointLineSegment(CoordVectHashTable& prevItemPairHashTable, VisualItemPair& pair);
470
    void _addTimeDistance(bool vtolInHover, double hoverTime, double cruiseTime, double extraTime, double distance, int seqNum);
471
    int _insertComplexMissionItemWorker(ComplexMissionItem* complexItem, int i);
472
    void _warnIfTerrainFrameUsed(void);
473 474
    void _setRemainingDistance(double dist);
    void _setRemainingTime(double time);
475

476
private:
477
    MissionManager*         _missionManager;
478
    int                     _missionItemCount;
DonLakeFlyer's avatar
DonLakeFlyer committed
479
    QmlObjectListModel*     _visualItems;
480
    MissionSettingsItem*    _settingsItem;
DonLakeFlyer's avatar
DonLakeFlyer committed
481
    QmlObjectListModel      _waypointLines;
482
    QVariantList            _waypointPath;
DonLakeFlyer's avatar
DonLakeFlyer committed
483 484
    CoordVectHashTable      _linesTable;
    bool                    _firstItemsFromVehicle;
DonLakeFlyer's avatar
DonLakeFlyer committed
485
    bool                    _itemsRequested;
486
    bool                    _inRecalcSequence;
DonLakeFlyer's avatar
DonLakeFlyer committed
487 488
    MissionFlightStatus_t   _missionFlightStatus;
    QString                 _surveyMissionItemName;
489
    QString                 _circularSurveyMissionItemName;
490
    AppSettings*            _appSettings;
491
    double                  _progressPct;
492 493
    int                     _currentPlanViewIndex;
    VisualMissionItem*      _currentPlanViewItem;
494
    QTimer                  _updateTimer;
495
    QTimer                  _remainingDistanceTimeTimer;
496
    QGCGeoBoundingCube      _travelBoundingCube;
497
    QGeoCoordinate          _takeoffCoordinate;
498 499
    double                  _remainingTime; // estimated Time until mission end
    double                  _remainingDistance; // estimated Distance to mission end
500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641
=======
    void visualItemsChanged                 (void);
    void waypointPathChanged                (void);
    void splitSegmentChanged                (void);
    void newItemsFromVehicle                (void);
    void missionDistanceChanged             (double missionDistance);
    void missionTimeChanged                 (void);
    void missionHoverDistanceChanged        (double missionHoverDistance);
    void missionHoverTimeChanged            (void);
    void missionCruiseDistanceChanged       (double missionCruiseDistance);
    void missionCruiseTimeChanged           (void);
    void missionMaxTelemetryChanged         (double missionMaxTelemetry);
    void complexMissionItemNamesChanged     (void);
    void resumeMissionIndexChanged          (void);
    void resumeMissionReady                 (void);
    void resumeMissionUploadFail            (void);
    void batteryChangePointChanged          (int batteryChangePoint);
    void batteriesRequiredChanged           (int batteriesRequired);
    void plannedHomePositionChanged         (QGeoCoordinate plannedHomePosition);
    void progressPctChanged                 (double progressPct);
    void currentMissionIndexChanged         (int currentMissionIndex);
    void currentPlanViewSeqNumChanged       (void);
    void currentPlanViewVIIndexChanged      (void);
    void currentPlanViewItemChanged         (void);
    void takeoffMissionItemChanged          (void);
    void missionBoundingCubeChanged         (void);
    void missionItemCountChanged            (int missionItemCount);
    void onlyInsertTakeoffValidChanged      (void);
    void isInsertTakeoffValidChanged        (void);
    void isInsertLandValidChanged           (void);
    void isROIActiveChanged                 (void);
    void isROIBeginCurrentItemChanged       (void);
    void flyThroughCommandsAllowedChanged   (void);
    void previousCoordinateChanged          (void);
    void minAMSLAltitudeChanged             (double minAMSLAltitude);
    void maxAMSLAltitudeChanged             (double maxAMSLAltitude);
    void recalcTerrainProfile               (void);
    void _recalcMissionFlightStatusSignal   (void);
    void _recalcFlightPathSegmentsSignal    (void);
    void globalAltitudeModeChanged          (void);

private slots:
    void _newMissionItemsAvailableFromVehicle   (bool removeAllRequested);
    void _itemCommandChanged                    (void);
    void _inProgressChanged                     (bool inProgress);
    void _currentMissionIndexChanged            (int sequenceNumber);
    void _recalcFlightPathSegments              (void);
    void _recalcMissionFlightStatus             (void);
    void _updateContainsItems                   (void);
    void _progressPctChanged                    (double progressPct);
    void _visualItemsDirtyChanged               (bool dirty);
    void _managerSendComplete                   (bool error);
    void _managerRemoveAllComplete              (bool error);
    void _updateTimeout                         (void);
    void _complexBoundingBoxChanged             (void);
    void _recalcAll                             (void);
    void _managerVehicleChanged                 (Vehicle* managerVehicle);
    void _takeoffItemNotRequiredChanged         (void);

private:
    void                    _init                               (void);
    void                    _recalcSequence                     (void);
    void                    _recalcChildItems                   (void);
    void                    _recalcAllWithCoordinate            (const QGeoCoordinate& coordinate);
    void                    _recalcROISpecialVisuals            (void);
    void                    _initAllVisualItems                 (void);
    void                    _deinitAllVisualItems               (void);
    void                    _initVisualItem                     (VisualMissionItem* item);
    void                    _deinitVisualItem                   (VisualMissionItem* item);
    void                    _setupActiveVehicle                 (Vehicle* activeVehicle, bool forceLoadFromVehicle);
    void                    _calcPrevWaypointValues             (VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference);
    bool                    _findPreviousAltitude               (int newIndex, double* prevAltitude, int* prevAltitudeMode);
    MissionSettingsItem*    _addMissionSettings                 (QmlObjectListModel* visualItems);
    void                    _centerHomePositionOnMissionItems   (QmlObjectListModel* visualItems);
    bool                    _loadJsonMissionFile                (const QByteArray& bytes, QmlObjectListModel* visualItems, QString& errorString);
    bool                    _loadJsonMissionFileV1              (const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString);
    bool                    _loadJsonMissionFileV2              (const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString);
    bool                    _loadTextMissionFile                (QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString);
    int                     _nextSequenceNumber                 (void);
    void                    _scanForAdditionalSettings          (QmlObjectListModel* visualItems, PlanMasterController* masterController);
    void                    _setPlannedHomePositionFromFirstCoordinate(const QGeoCoordinate& clickCoordinate);
    void                    _resetMissionFlightStatus           (void);
    void                    _addHoverTime                       (double hoverTime, double hoverDistance, int waypointIndex);
    void                    _addCruiseTime                      (double cruiseTime, double cruiseDistance, int wayPointIndex);
    void                    _updateBatteryInfo                  (int waypointIndex);
    bool                    _loadItemsFromJson                  (const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString);
    void                    _initLoadedVisualItems              (QmlObjectListModel* loadedVisualItems);
    FlightPathSegment*      _addFlightPathSegment               (FlightPathSegmentHashTable& prevItemPairHashTable, VisualItemPair& pair);
    void                    _addTimeDistance                    (bool vtolInHover, double hoverTime, double cruiseTime, double extraTime, double distance, int seqNum);
    VisualMissionItem*      _insertSimpleMissionItemWorker      (QGeoCoordinate coordinate, MAV_CMD command, int visualItemIndex, bool makeCurrentItem);
    void                    _insertComplexMissionItemWorker     (const QGeoCoordinate& mapCenterCoordinate, ComplexMissionItem* complexItem, int visualItemIndex, bool makeCurrentItem);
    bool                    _isROIBeginItem                     (SimpleMissionItem* simpleItem);
    bool                    _isROICancelItem                    (SimpleMissionItem* simpleItem);
    FlightPathSegment*      _createFlightPathSegmentWorker      (VisualItemPair& pair);
    void                    _allItemsRemoved                    (void);
    void                    _firstItemAdded                     (void);

    static double           _calcDistanceToHome                 (VisualMissionItem* currentItem, VisualMissionItem* homeItem);
    static double           _normalizeLat                       (double lat);
    static double           _normalizeLon                       (double lon);
    static bool             _convertToMissionItems              (QmlObjectListModel* visualMissionItems, QList<MissionItem*>& rgMissionItems, QObject* missionItemParent);

private:
    Vehicle*                    _controllerVehicle =            nullptr;
    Vehicle*                    _managerVehicle =               nullptr;
    MissionManager*             _missionManager =               nullptr;
    int                         _missionItemCount =             0;
    QmlObjectListModel*         _visualItems =                  nullptr;
    MissionSettingsItem*        _settingsItem =                 nullptr;
    PlanViewSettings*           _planViewSettings =             nullptr;
    QmlObjectListModel          _simpleFlightPathSegments;
    QVariantList                _waypointPath;
    QmlObjectListModel          _directionArrows;
    QmlObjectListModel          _incompleteComplexItemLines;
    FlightPathSegmentHashTable  _flightPathSegmentHashTable;
    bool                        _firstItemsFromVehicle =        false;
    bool                        _itemsRequested =               false;
    bool                        _inRecalcSequence =             false;
    MissionFlightStatus_t       _missionFlightStatus;
    AppSettings*                _appSettings =                  nullptr;
    double                      _progressPct =                  0;
    int                         _currentPlanViewSeqNum =        -1;
    int                         _currentPlanViewVIIndex =       -1;
    VisualMissionItem*          _currentPlanViewItem =          nullptr;
    TakeoffMissionItem*         _takeoffMissionItem =           nullptr;
    QTimer                      _updateTimer;
    QGCGeoBoundingCube          _travelBoundingCube;
    QGeoCoordinate              _takeoffCoordinate;
    QGeoCoordinate              _previousCoordinate;
    FlightPathSegment*          _splitSegment =                 nullptr;
    bool                        _onlyInsertTakeoffValid =       true;
    bool                        _isInsertTakeoffValid =         true;
    bool                        _isInsertLandValid =            false;
    bool                        _isROIActive =                  false;
    bool                        _flyThroughCommandsAllowed =    false;
    bool                        _isROIBeginCurrentItem =        false;
    double                      _minAMSLAltitude =              0;
    double                      _maxAMSLAltitude =              0;
    bool                        _missionContainsVTOLTakeoff =   false;

    QGroundControlQmlGlobal::AltitudeMode _globalAltMode = QGroundControlQmlGlobal::AltitudeModeRelative;
>>>>>>> upstream_merge
642

643
    static const char*  _settingsGroup;
644 645

    // Json file keys for persistence
Don Gagne's avatar
Don Gagne committed
646 647
    static const char*  _jsonFileTypeValue;
    static const char*  _jsonFirmwareTypeKey;
648 649 650
    static const char*  _jsonVehicleTypeKey;
    static const char*  _jsonCruiseSpeedKey;
    static const char*  _jsonHoverSpeedKey;
Don Gagne's avatar
Don Gagne committed
651 652 653
    static const char*  _jsonItemsKey;
    static const char*  _jsonPlannedHomePositionKey;
    static const char*  _jsonParamsKey;
654
    static const char*  _jsonGlobalPlanAltitudeModeKey;
Don Gagne's avatar
Don Gagne committed
655 656

    // Deprecated V1 format keys
657
    static const char*  _jsonMavAutopilotKey;
658
    static const char*  _jsonComplexItemsKey;
Don Gagne's avatar
Don Gagne committed
659 660

    static const int    _missionFileVersion;
661
};