TakeoffMissionItem.cc 6.7 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

#include <QStringList>
#include <QDebug>

#include "TakeoffMissionItem.h"
#include "FirmwarePluginManager.h"
#include "QGCApplication.h"
#include "JsonHelper.h"
#include "MissionCommandTree.h"
#include "MissionCommandUIInfo.h"
#include "QGroundControlQmlGlobal.h"
#include "SettingsManager.h"
21
#include "PlanMasterController.h"
22

23 24
TakeoffMissionItem::TakeoffMissionItem(PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, bool forLoad, QObject* parent)
    : SimpleMissionItem (masterController, flyView, forLoad, parent)
25 26
    , _settingsItem     (settingsItem)
{
27
    _init(forLoad);
28 29
}

30
TakeoffMissionItem::TakeoffMissionItem(MAV_CMD takeoffCmd, PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent)
31
    : SimpleMissionItem (masterController, flyView, false /* forLoad */, parent)
32 33 34
    , _settingsItem     (settingsItem)
{
    setCommand(takeoffCmd);
35
    _init(false /* forLoad */);
36 37
}

38 39
TakeoffMissionItem::TakeoffMissionItem(const MissionItem& missionItem, PlanMasterController* masterController, bool flyView, MissionSettingsItem* settingsItem, QObject* parent)
    : SimpleMissionItem (masterController, flyView, missionItem, parent)
40 41
    , _settingsItem     (settingsItem)
{
42
    _init(false /* forLoad */);
43
    _wizardMode = false;
44 45 46 47 48 49 50
}

TakeoffMissionItem::~TakeoffMissionItem()
{

}

51
void TakeoffMissionItem::_init(bool forLoad)
52 53 54 55 56
{
    _editorQml = QStringLiteral("qrc:/qml/SimpleItemEditor.qml");

    connect(_settingsItem, &MissionSettingsItem::coordinateChanged, this, &TakeoffMissionItem::launchCoordinateChanged);

DonLakeFlyer's avatar
DonLakeFlyer committed
57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72
    if (_flyView) {
        _initLaunchTakeoffAtSameLocation();
        return;
    }

    QGeoCoordinate homePosition = _settingsItem->coordinate();
    if (!homePosition.isValid()) {
        Vehicle* activeVehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
        if (activeVehicle) {
            homePosition = activeVehicle->homePosition();
            if (homePosition.isValid()) {
                _settingsItem->setCoordinate(homePosition);
            }
        }
    }

73 74 75 76 77
    if (forLoad) {
        // Load routines will set the rest up after load
        return;
    }

78 79
    _initLaunchTakeoffAtSameLocation();

DonLakeFlyer's avatar
DonLakeFlyer committed
80
    if (homePosition.isValid() && coordinate().isValid()) {
81
        // Item already fully specified, most likely from mission load from storage
82 83
        _wizardMode = false;
    } else {
DonLakeFlyer's avatar
DonLakeFlyer committed
84 85 86 87 88 89
        if (_launchTakeoffAtSameLocation && homePosition.isValid()) {
            _wizardMode = false;
            SimpleMissionItem::setCoordinate(homePosition);
        } else {
            _wizardMode = true;
        }
90 91
    }

92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108
    setDirty(false);
}

void TakeoffMissionItem::setLaunchTakeoffAtSameLocation(bool launchTakeoffAtSameLocation)
{
    if (launchTakeoffAtSameLocation != _launchTakeoffAtSameLocation) {
        _launchTakeoffAtSameLocation = launchTakeoffAtSameLocation;
        if (_launchTakeoffAtSameLocation) {
            setLaunchCoordinate(coordinate());
        }
        emit launchTakeoffAtSameLocationChanged(_launchTakeoffAtSameLocation);
        setDirty(true);
    }
}

void TakeoffMissionItem::setCoordinate(const QGeoCoordinate& coordinate)
{
109 110 111 112
    if (coordinate != this->coordinate()) {
        SimpleMissionItem::setCoordinate(coordinate);
        if (_launchTakeoffAtSameLocation) {
            _settingsItem->setCoordinate(coordinate);
113 114 115 116 117 118
        }
    }
}

bool TakeoffMissionItem::isTakeoffCommand(MAV_CMD command)
{
119 120 121
    const MissionCommandUIInfo* uiInfo = qgcApp()->toolbox()->missionCommandTree()->getUIInfo(qgcApp()->toolbox()->multiVehicleManager()->offlineEditingVehicle(), command);

    return uiInfo ? uiInfo->isTakeoffCommand() : false;
122 123 124 125
}

void TakeoffMissionItem::_initLaunchTakeoffAtSameLocation(void)
{
126
    if (specifiesCoordinate()) {
127
        if (_controllerVehicle->fixedWing() || _controllerVehicle->vtol()) {
128 129
            setLaunchTakeoffAtSameLocation(false);
        } else {
DoinLakeFlyer's avatar
DoinLakeFlyer committed
130
            // PX4 specifies a coordinate for takeoff even for multi-rotor. But it makes more sense to not have a coordinate
131 132 133 134 135 136 137 138 139
            // from and end user standpoint. So even for PX4 we try to keep launch and takeoff at the same position. Unless the
            // user has moved/loaded launch at a different location than takeoff.
            if (coordinate().isValid() && _settingsItem->coordinate().isValid()) {
                setLaunchTakeoffAtSameLocation(coordinate().latitude() == _settingsItem->coordinate().latitude() && coordinate().longitude() == _settingsItem->coordinate().longitude());
            } else {
                setLaunchTakeoffAtSameLocation(true);
            }

        }
140
    } else {
141
        setLaunchTakeoffAtSameLocation(true);
142 143 144 145 146 147 148 149 150
    }
}

bool TakeoffMissionItem::load(QTextStream &loadStream)
{
    bool success = SimpleMissionItem::load(loadStream);
    if (success) {
        _initLaunchTakeoffAtSameLocation();
    }
151
    _wizardMode = false; // Always be off for loaded items
152 153 154 155 156 157 158 159 160
    return success;
}

bool TakeoffMissionItem::load(const QJsonObject& json, int sequenceNumber, QString& errorString)
{
    bool success = SimpleMissionItem::load(json, sequenceNumber, errorString);
    if (success) {
        _initLaunchTakeoffAtSameLocation();
    }
161
    _wizardMode = false; // Always be off for loaded items
162 163
    return success;
}
164 165 166 167 168 169 170 171 172 173 174 175 176 177

void TakeoffMissionItem::setLaunchCoordinate(const QGeoCoordinate& launchCoordinate)
{
    if (!launchCoordinate.isValid()) {
        return;
    }

    _settingsItem->setCoordinate(launchCoordinate);

    if (!coordinate().isValid()) {
        QGeoCoordinate takeoffCoordinate;
        if (_launchTakeoffAtSameLocation) {
            takeoffCoordinate = launchCoordinate;
        } else {
178 179 180 181
            double distance = 30.0; // Default distance is VTOL transition to takeoff point distance
            if (_controllerVehicle->fixedWing()) {
                double altitude = this->altitude()->rawValue().toDouble();

182
                if (altitudeMode() == QGroundControlQmlGlobal::AltitudeModeRelative) {
183 184 185 186 187 188
                    // Offset for fixed wing climb out of 30 degrees
                    if (altitude != 0.0) {
                        distance = altitude / tan(qDegreesToRadians(30.0));
                    }
                } else {
                    distance = altitude * 1.5;
189 190 191 192 193 194 195
                }
            }
            takeoffCoordinate = launchCoordinate.atDistanceAndAzimuth(distance, 0);
        }
        SimpleMissionItem::setCoordinate(takeoffCoordinate);
    }
}