Commit 04a00aef authored by DonLakeFlyer's avatar DonLakeFlyer

New Takeoff, Start Mission, Resume Mission

parent 9b913b32
......@@ -133,14 +133,22 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
vehicle->setFlightMode("Land");
}
void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
#if 0
// WIP
void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
{
if (!_armVehicle(vehicle)) {
qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
return;
}
vehicle->sendMavCommand(vehicle->defaultComponentId(),
MAV_CMD_NAV_TAKEOFF,
true, // show error
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
altitudeRel);
2.5);
}
#endif
void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
{
......
......@@ -59,7 +59,10 @@ public:
void pauseVehicle(Vehicle* vehicle) final;
void guidedModeRTL(Vehicle* vehicle) final;
void guidedModeLand(Vehicle* vehicle) final;
void guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) final;
#if 0
// WIP
void guidedModeTakeoff(Vehicle* vehicle) final;
#endif
void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final;
const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
......
......@@ -265,11 +265,10 @@ void FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
}
void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
{
// Not supported by generic vehicle
Q_UNUSED(vehicle);
Q_UNUSED(altitudeRel);
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
}
......@@ -295,6 +294,13 @@ void FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeR
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
}
void FirmwarePlugin::startMission(Vehicle* vehicle)
{
// Not supported by generic vehicle
Q_UNUSED(vehicle);
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
}
const FirmwarePlugin::remapParamNameMajorVersionMap_t& FirmwarePlugin::paramNameRemapMajorVersionMap(void) const
{
static const remapParamNameMajorVersionMap_t remap;
......@@ -436,3 +442,20 @@ bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle)
{
return vehicle->multiRotor() ? false : true;
}
bool FirmwarePlugin::_armVehicle(Vehicle* vehicle)
{
if (!vehicle->armed()) {
vehicle->setArmed(true);
}
// Wait for vehicle to return armed state for 2 seconds
for (int i=0; i<20; i++) {
if (vehicle->armed()) {
break;
}
QGC::SLEEP::msleep(100);
qgcApp()->processEvents(QEventLoop::ExcludeUserInputEvents);
}
return vehicle->armed();
}
......@@ -112,9 +112,11 @@ public:
/// Command vehicle to land at current location
virtual void guidedModeLand(Vehicle* vehicle);
/// Command vehicle to takeoff from current location
/// @param altitudeRel Relative altitude to takeoff to
virtual void guidedModeTakeoff(Vehicle* vehicle, double altitudeRel);
/// Command vehicle to takeoff from current location to a firmware specific height.
virtual void guidedModeTakeoff(Vehicle* vehicle);
/// Command the vehicle to start the mission
virtual void startMission(Vehicle* vehicle);
/// Command vehicle to orbit given center point
/// @param centerCoord Center Coordinates
......@@ -126,6 +128,9 @@ public:
/// Command vehicle to change to the specified relatice altitude
virtual void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel);
/// Returns the flight mode for running missions
virtual QString missionFlightMode(void);
......@@ -273,6 +278,11 @@ public:
// FIXME: Hack workaround for non pluginize FollowMe support
static const char* px4FollowMeFlightMode;
protected:
// Arms the vehicle, waiting for the arm state to change.
// @return: true - vehicle armed, false - vehicle failed to arm
bool _armVehicle(Vehicle* vehicle);
private:
QVariantList _toolBarIndicatorList;
static QVariantList _cameraList; ///< Standard QGC camera list
......
......@@ -350,24 +350,33 @@ void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate&
centerCoord.isValid() ? centerCoord.altitude() : NAN);
}
void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
{
Q_UNUSED(altitudeRel);
QString takeoffAltParam("MIS_TAKEOFF_ALT");
if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known."));
return;
}
if (!vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
qgcApp()->showMessage(tr("Unable to takeoff, MIS_TAKEOFF_ALT parameter missing."));
return;
}
Fact* takeoffAlt = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
if (!_armVehicle(vehicle)) {
qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
return;
}
vehicle->sendMavCommand(vehicle->defaultComponentId(),
MAV_CMD_NAV_TAKEOFF,
true, // show error is fails
-1.0f,
0.0f,
0.0f,
NAN,
NAN,
NAN,
vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel);
true, // show error is fails
-1, // No pitch requested
0, 0, // param 2-4 unused
NAN, NAN, NAN, // No yaw, lat, lon
vehicle->altitudeAMSL()->rawValue().toDouble() + takeoffAlt->rawValue().toDouble());
}
void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
......@@ -412,6 +421,16 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
vehicle->homePosition().altitude() + altitudeRel);
}
void PX4FirmwarePlugin::startMission(Vehicle* vehicle)
{
if (!_armVehicle(vehicle)) {
qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm."));
return;
}
vehicle->setFlightMode(missionFlightMode());
}
void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
{
if (guidedMode) {
......
......@@ -40,10 +40,11 @@ public:
void pauseVehicle (Vehicle* vehicle) override;
void guidedModeRTL (Vehicle* vehicle) override;
void guidedModeLand (Vehicle* vehicle) override;
void guidedModeTakeoff (Vehicle* vehicle, double altitudeRel) override;
void guidedModeTakeoff (Vehicle* vehicle) override;
void guidedModeOrbit (Vehicle* vehicle, const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN) override;
void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override;
void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeRel) override;
void startMission (Vehicle* vehicle) override;
bool isGuidedMode (const Vehicle* vehicle) const override;
int manualControlReservedButtonCount(void) override;
bool supportsManualControl (void) override;
......
......@@ -247,6 +247,7 @@ QGCView {
anchors.bottom: parent.bottom
qgcView: root
useLightColors: isBackgroundDark
missionController: _flightMap.missionController
visible: singleVehicleView.checked
}
......
......@@ -29,11 +29,19 @@ Item {
property bool gotoEnabled: _activeVehicle && _activeVehicle.guidedMode && _activeVehicle.flying
property var qgcView
property bool useLightColors
property var missionController
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
property bool _isSatellite: _mainIsMap ? (_flightMap ? _flightMap.isSatelliteMap : true) : true
property bool _lightWidgetBorders: _isSatellite
// Guided bar properties
property bool _missionAvailable: missionController.containsItems
property bool _missionActive: _activeVehicle ? _activeVehicle.flightMode === _activeVehicle.missionFlightMode : false
property bool _missionInProgress: missionController.missionInProgress
property bool _showEmergenyStop: QGroundControl.corePlugin.options.guidedBarShowEmergencyStop
property bool _showOrbit: QGroundControl.corePlugin.options.guidedBarShowOrbit
readonly property real _margins: ScreenTools.defaultFontPixelHeight * 0.5
QGCMapPalette { id: mapPal; lightColors: useLightColors }
......@@ -222,6 +230,8 @@ Item {
readonly property int confirmRetask: 9
readonly property int confirmOrbit: 10
readonly property int confirmAbort: 11
readonly property int confirmStartMission: 12
readonly property int confirmResumeMission: 13
property int confirmActionCode
property real _showMargin: _margins
......@@ -237,10 +247,11 @@ Item {
_activeVehicle.guidedModeLand()
break;
case confirmTakeoff:
var altitude1 = altitudeSlider.getValue()
if (!isNaN(altitude1)) {
_activeVehicle.guidedModeTakeoff(altitude1)
}
_activeVehicle.guidedModeTakeoff()
break;
case confirmResumeMission:
case confirmStartMission:
_activeVehicle.startMission()
break;
case confirmArm:
_activeVehicle.armed = true
......@@ -299,10 +310,14 @@ Item {
guidedModeConfirm.confirmText = qsTr("STOP ALL MOTORS!")
break;
case confirmTakeoff:
altitudeSlider.visible = true
altitudeSlider.setInitialValueMeters(3)
guidedModeConfirm.confirmText = qsTr("takeoff")
break;
case confirmStartMission:
guidedModeConfirm.confirmText = qsTr("start mission")
break;
case confirmResumeMission:
guidedModeConfirm.confirmText = qsTr("resume mission")
break;
case confirmLand:
guidedModeConfirm.confirmText = qsTr("land")
break;
......@@ -350,33 +365,58 @@ Item {
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: (_activeVehicle && _activeVehicle.armed) ? (_activeVehicle.flying ? qsTr("Emergency Stop") : qsTr("Disarm")) : qsTr("Arm")
visible: _activeVehicle
onClicked: _guidedModeBar.confirmAction(_activeVehicle.armed ? (_activeVehicle.flying ? _guidedModeBar.confirmEmergencyStop : _guidedModeBar.confirmDisarm) : _guidedModeBar.confirmArm)
text: qsTr("Emergency Stop")
visible: _showEmergenyStop && _activeVehicle && _activeVehicle.armed && _activeVehicle.flying
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmEmergencyStop)
}
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: qsTr("Disarm")
visible: _activeVehicle && _activeVehicle.armed && !_activeVehicle.flying
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmDisarm)
}
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: qsTr("RTL")
visible: (_activeVehicle && _activeVehicle.armed) && _activeVehicle.guidedModeSupported && _activeVehicle.flying
visible: _activeVehicle && _activeVehicle.armed && _activeVehicle.guidedModeSupported && _activeVehicle.flying
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmHome)
}
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: (_activeVehicle && _activeVehicle.flying) ? qsTr("Land"): qsTr("Takeoff")
visible: _activeVehicle && _activeVehicle.guidedModeSupported && _activeVehicle.armed
onClicked: _guidedModeBar.confirmAction(_activeVehicle.flying ? _guidedModeBar.confirmLand : _guidedModeBar.confirmTakeoff)
text: qsTr("Takeoff")
visible: _activeVehicle && _activeVehicle.guidedModeSupported && !_activeVehicle.flying && !_activeVehicle.fixedWing
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmTakeoff)
}
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: qsTr("Land")
visible: _activeVehicle && _activeVehicle.guidedModeSupported && _activeVehicle.armed && !_activeVehicle.fixedWing
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmLand)
}
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: qsTr("Start Mission")
visible: _activeVehicle && !_activeVehicle.flying && _missionAvailable
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmStartMission)
}
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: qsTr("Resume Mission")
visible: _activeVehicle && _activeVehicle.guidedModeSupported && !_activeVehicle.flying && _missionAvailable && _missionInProgress
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmResumeMission)
}
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: qsTr("Pause")
visible: (_activeVehicle && _activeVehicle.armed) && _activeVehicle.pauseVehicleSupported && _activeVehicle.flying
onClicked: {
guidedModeHideTimer.restart()
_activeVehicle.pauseVehicle()
}
text: _missionActive ? qsTr("Pause Mission") : qsTr("Pause")
visible: _activeVehicle && _activeVehicle.armed && _activeVehicle.pauseVehicleSupported && _activeVehicle.flying
onClicked: _activeVehicle.pauseVehicle()
}
QGCButton {
......@@ -389,7 +429,7 @@ Item {
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: qsTr("Orbit")
visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.orbitModeSupported && _activeVehicle.armed
visible: _showOrbit && _activeVehicle && _activeVehicle.flying && _activeVehicle.orbitModeSupported && _activeVehicle.armed
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmOrbit)
}
......
......@@ -1238,6 +1238,7 @@ void MissionController::_initAllVisualItems(void)
connect(_visualItems, &QmlObjectListModel::countChanged, this, &MissionController::_updateContainsItems);
emit visualItemsChanged();
emit containsItemsChanged(containsItems());
_visualItems->setDirty(false);
}
......@@ -1503,6 +1504,8 @@ void MissionController::_addMissionSettings(Vehicle* vehicle, QmlObjectListModel
void MissionController::_currentMissionItemChanged(int sequenceNumber)
{
if (!_editMode) {
bool prevMissionInProgress = missionInProgress();
if (!_activeVehicle->firmwarePlugin()->sendHomePositionToVehicle()) {
sequenceNumber++;
}
......@@ -1511,6 +1514,10 @@ void MissionController::_currentMissionItemChanged(int sequenceNumber)
VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
item->setIsCurrentItem(item->sequenceNumber() == sequenceNumber);
}
if (prevMissionInProgress != missionInProgress()) {
emit missionInProgressChanged();
}
}
}
......@@ -1588,3 +1595,8 @@ QStringList MissionController::complexMissionItemNames(void) const
return complexItems;
}
bool MissionController::missionInProgress(void) const
{
return _visualItems && _visualItems->count() > 1 && (!_visualItems->value<VisualMissionItem*>(0)->isCurrentItem() && !_visualItems->value<VisualMissionItem*>(1)->isCurrentItem());
}
......@@ -50,11 +50,12 @@ public:
double gimbalYaw; ///< NaN signals yaw was never changed
} MissionFlightStatus_t;
// Mission settings
Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged)
Q_PROPERTY(QmlObjectListModel* waypointLines READ waypointLines NOTIFY waypointLinesChanged)
Q_PROPERTY(QStringList complexMissionItemNames READ complexMissionItemNames NOTIFY complexMissionItemNamesChanged)
Q_PROPERTY(bool missionInProgress READ missionInProgress NOTIFY missionInProgressChanged) ///< true: Mission sequence is beyond first item
Q_PROPERTY(double missionDistance READ missionDistance NOTIFY missionDistanceChanged)
Q_PROPERTY(double missionTime READ missionTime NOTIFY missionTimeChanged)
Q_PROPERTY(double missionHoverDistance READ missionHoverDistance NOTIFY missionHoverDistanceChanged)
......@@ -110,6 +111,7 @@ public:
QmlObjectListModel* visualItems (void) { return _visualItems; }
QmlObjectListModel* waypointLines (void) { return &_waypointLines; }
QStringList complexMissionItemNames (void) const;
bool missionInProgress (void) const;
double missionDistance (void) const { return _missionFlightStatus.totalDistance; }
double missionTime (void) const { return _missionFlightStatus.totalTime; }
......@@ -131,6 +133,7 @@ signals:
void missionCruiseTimeChanged(void);
void missionMaxTelemetryChanged(double missionMaxTelemetry);
void complexMissionItemNamesChanged(void);
bool missionInProgressChanged(void);
private slots:
void _newMissionItemsAvailableFromVehicle(bool removeAllRequested);
......
......@@ -91,7 +91,7 @@ Rectangle {
MessageDialog {
id: uploadPrompt
title: _activeVehicle ? qsTr("Unsent changes") : qsTr("Unsaved changes")
text: qsTr("You have %1 changes to your mission. Are you sure you want to leave before you %2?").arg(_activeVehicle ? qsTr("unsent") : qsTr("unsaved")).arg(_activeVehicle ? qsTr("send the missoin to the vehicle") : qsTr("save the mission to a file"))
text: qsTr("You have %1 changes to your mission. Are you sure you want to leave before you %2?").arg(_activeVehicle ? qsTr("unsent") : qsTr("unsaved")).arg(_activeVehicle ? qsTr("send the mission to the vehicle") : qsTr("save the mission to a file"))
standardButtons: StandardButton.Yes | StandardButton.No
onNo: visible = false
......
......@@ -1959,14 +1959,19 @@ void Vehicle::guidedModeLand(void)
_firmwarePlugin->guidedModeLand(this);
}
void Vehicle::guidedModeTakeoff(double altitudeRel)
void Vehicle::guidedModeTakeoff(void)
{
if (!guidedModeSupported()) {
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
return;
}
setGuidedMode(true);
_firmwarePlugin->guidedModeTakeoff(this, altitudeRel);
_firmwarePlugin->guidedModeTakeoff(this);
}
void Vehicle::startMission(void)
{
_firmwarePlugin->startMission(this);
}
void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
......
......@@ -379,7 +379,7 @@ public:
Q_INVOKABLE void guidedModeLand(void);
/// Command vehicle to takeoff from current location
Q_INVOKABLE void guidedModeTakeoff(double altitudeRel);
Q_INVOKABLE void guidedModeTakeoff(void);
/// Command vehicle to move to specified location (altitude is included and relative)
Q_INVOKABLE void guidedModeGotoLocation(const QGeoCoordinate& gotoCoord);
......@@ -404,6 +404,8 @@ public:
/// Command vehicle to abort landing
Q_INVOKABLE void abortLanding(double climbOutAltitude);
Q_INVOKABLE void startMission(void);
/// Alter the current mission item on the vehicle
Q_INVOKABLE void setCurrentMissionSequence(int seq);
......
......@@ -36,6 +36,8 @@ public:
Q_PROPERTY(bool showSensorCalibrationOrient READ showSensorCalibrationOrient NOTIFY showSensorCalibrationOrientChanged)
Q_PROPERTY(bool showFirmwareUpgrade READ showFirmwareUpgrade NOTIFY showFirmwareUpgradeChanged)
Q_PROPERTY(QString firmwareUpgradeSingleURL READ firmwareUpgradeSingleURL CONSTANT)
Q_PROPERTY(bool guidedBarShowEmergencyStop READ guidedBarShowEmergencyStop NOTIFY guidedBarShowEmergencyStopChanged)
Q_PROPERTY(bool guidedBarShowOrbit READ guidedBarShowOrbit NOTIFY guidedBarShowOrbitChanged)
/// Should QGC hide its settings menu and colapse it into one single menu (Settings and Vehicle Setup)?
/// @return true if QGC should consolidate both menus into one.
......@@ -63,6 +65,9 @@ public:
virtual bool showFirmwareUpgrade () const { return true; }
virtual bool guidedBarShowEmergencyStop () const { return true; }
virtual bool guidedBarShowOrbit () const { return true; }
/// If returned QString in non-empty it means that firmware upgrade will run in a mode which only
/// supports downloading a single firmware file from the URL. It also supports custom install through
/// the Advanced options.
......@@ -76,6 +81,8 @@ signals:
void showSensorCalibrationAirspeedChanged (bool show);
void showSensorCalibrationOrientChanged (bool show);
void showFirmwareUpgradeChanged (bool show);
void guidedBarShowEmergencyStopChanged (bool show);
void guidedBarShowOrbitChanged (bool show);
private:
CustomInstrumentWidget* _defaultInstrumentWidget;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment