Skip to content

Q
qgroundcontrol
Commit 58c0d436 authored by DonLakeFlyer's avatar DonLakeFlyer
Browse files
Options

Mavlink Orbit support

parent 0b0fdaa9
Hide whitespace changes
Inline Side-by-side
Showing with 98 additions and 93 deletions
src/FirmwarePlugin/FirmwarePlugin.cc
View file @ 58c0d436
......@@ -270,12 +270,6 @@ void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel)
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
}
void FirmwarePlugin::guidedModeOrbit(Vehicle* /*vehicle*/, const QGeoCoordinate& /*centerCoord*/, double /*radius*/, double /*velocity*/, double /*altitude*/)
{
// Not supported by generic vehicle
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
}
void FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
{
// Not supported by generic vehicle
......
src/FirmwarePlugin/FirmwarePlugin.h
View file @ 58c0d436
......@@ -136,10 +136,6 @@ public:
/// Command the vehicle to start the mission
virtual void startMission(Vehicle* vehicle);
/// Command vehicle to orbit given center point
/// @param centerCoord Center Coordinates
virtual void guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude);
/// Command vehicle to move to specified location (altitude is included and relative)
virtual void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord);
......
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
View file @ 58c0d436
......@@ -232,7 +232,7 @@ bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities c
{
int available = SetFlightModeCapability | PauseVehicleCapability | GuidedModeCapability;
if (vehicle->multiRotor() || vehicle->vtol()) {
available |= TakeoffVehicleCapability;
available |= TakeoffVehicleCapability | OrbitModeCapability;
}
return (capabilities & available) == capabilities;
......@@ -364,24 +364,6 @@ void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
_setFlightModeAndValidate(vehicle, _landingFlightMode);
}
void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
{
if (!isGuidedMode(vehicle)) {
setGuidedMode(vehicle, true);
}
vehicle->sendMavCommand(vehicle->defaultComponentId(),
MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE,
true, // show error if fails
radius,
velocity,
altitude,
NAN,
centerCoord.isValid() ? centerCoord.latitude() : NAN,
centerCoord.isValid() ? centerCoord.longitude() : NAN,
centerCoord.isValid() ? centerCoord.altitude() : NAN);
}
void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle)
{
Q_UNUSED(vehicleId);
......
src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h
View file @ 58c0d436
......@@ -46,7 +46,6 @@ public:
void guidedModeRTL (Vehicle* vehicle) override;
void guidedModeLand (Vehicle* vehicle) override;
void guidedModeTakeoff (Vehicle* vehicle, double takeoffAltRel) 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;
double minimumTakeoffAltitude (Vehicle* vehicle) override;
......
src/FlightDisplay/FlightDisplayView.qml
View file @ 58c0d436
......@@ -508,11 +508,10 @@ QGCView {
z: _panel.z + 4
title: qsTr("Fly")
maxHeight: (_flightVideo.visible ? _flightVideo.y : parent.height) - toolStrip.y
buttonVisible: [ _useChecklist, _guidedController.showTakeoff || !_guidedController.showLand, _guidedController.showLand && !_guidedController.showTakeoff, true, true, true, _guidedController.smartShotsAvailable ]
buttonEnabled: [ _useChecklist && _activeVehicle, _guidedController.showTakeoff, _guidedController.showLand, _guidedController.showRTL, _guidedController.showPause, _anyActionAvailable, _anySmartShotAvailable ]
buttonVisible: [ _useChecklist, _guidedController.showTakeoff || !_guidedController.showLand, _guidedController.showLand && !_guidedController.showTakeoff, true, true, true ]
buttonEnabled: [ _useChecklist && _activeVehicle, _guidedController.showTakeoff, _guidedController.showLand, _guidedController.showRTL, _guidedController.showPause, _anyActionAvailable ]
property bool _anyActionAvailable: _guidedController.showStartMission || _guidedController.showResumeMission || _guidedController.showChangeAlt || _guidedController.showLandAbort
property bool _anySmartShotAvailable: _guidedController.showOrbit
property var _actionModel: [
{
title: _guidedController.startMissionTitle,
......@@ -545,14 +544,6 @@ QGCView {
visible: _guidedController.showLandAbort
}
]
property var _smartShotModel: [
{
title: _guidedController.orbitTitle,
text: _guidedController.orbitMessage,
action: _guidedController.actionOrbit,
visible: _guidedController.showOrbit
}
]
model: [
{
......@@ -584,28 +575,15 @@ QGCView {
name: qsTr("Action"),
iconSource: "/res/action.svg",
action: -1
},
/*
No firmware support any smart shots yet
{
name: qsTr("Smart"),
iconSource: "/qmlimages/MapCenter.svg",
action: -1
},
*/
}
]
onClicked: {
guidedActionsController.closeAll()
var action = model[index].action
if (action === -1) {
if (index == 5) {
guidedActionList.model = _actionModel
guidedActionList.visible = true
} else if (index == 6) {
guidedActionList.model = _smartShotModel
guidedActionList.visible = true
}
guidedActionList.model = _actionModel
guidedActionList.visible = true
} else {
_guidedController.confirmAction(action)
}
......
src/FlightDisplay/FlightDisplayViewMap.qml
View file @ 58c0d436
......@@ -49,7 +49,7 @@ FlightMap {
property var _rallyPointController: _planMasterController.rallyPointController
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
property var _activeVehicleCoordinate: _activeVehicle ? _activeVehicle.coordinate : QtPositioning.coordinate()
property var _gotoHereCoordinate: QtPositioning.coordinate()
property var _guidedLocationCoordinate: QtPositioning.coordinate()
property real _toolButtonTopMargin: parent.height - ScreenTools.availableHeight + (ScreenTools.defaultFontPixelHeight / 2)
property bool _disableVehicleTracking: false
......@@ -264,39 +264,74 @@ FlightMap {
}
}
// GoTo here waypoint
// Camera trigger points
MapItemView {
model: _activeVehicle ? _activeVehicle.cameraTriggerPoints : 0
delegate: CameraTriggerIndicator {
coordinate: object.coordinate
z: QGroundControl.zOrderTopMost
}
}
// Guided action location
MapQuickItem {
coordinate: _gotoHereCoordinate
visible: _activeVehicle && _activeVehicle.guidedModeSupported && _gotoHereCoordinate.isValid
id: guidedLocationItem
coordinate: _guidedLocationCoordinate
visible: _activeVehicle && _activeVehicle.guidedModeSupported && _guidedLocationCoordinate.isValid
z: QGroundControl.zOrderMapItems
anchorPoint.x: sourceItem.anchorPointX
anchorPoint.y: sourceItem.anchorPointY
property bool gotoLocation: true ///< true: Used for go to location, false: used for orbit
sourceItem: MissionItemIndexLabel {
checked: true
index: -1
label: qsTr("Goto here", "Goto here waypoint")
label: guidedLocationItem.gotoLocation ? qsTr("Goto here", "Goto here waypoint") : qsTr("Orbit here", "Orbit here waypoint")
}
}
// Camera trigger points
MapItemView {
model: _activeVehicle ? _activeVehicle.cameraTriggerPoints : 0
delegate: CameraTriggerIndicator {
coordinate: object.coordinate
z: QGroundControl.zOrderTopMost
}
}
// Handle guided mode clicks
MouseArea {
anchors.fill: parent
Menu {
id: clickMenu
MenuItem {
text: qsTr("Go to location")
visible: guidedActionsController.showGotoLocation
onTriggered: {
guidedLocationItem.gotoLocation = true
guidedActionsController.confirmAction(guidedActionsController.actionGoto, _guidedLocationCoordinate)
}
}
MenuItem {
text: qsTr("Orbit at location")
visible: guidedActionsController.showOrbit
onTriggered: {
guidedLocationItem.gotoLocation = false
guidedActionsController.confirmAction(guidedActionsController.actionOrbit, _guidedLocationCoordinate)
}
}
}
onClicked: {
if (guidedActionsController.showGotoLocation && !guidedActionsController.guidedUIVisible) {
_gotoHereCoordinate = flightMap.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */)
guidedActionsController.confirmAction(guidedActionsController.actionGoto, _gotoHereCoordinate)
if (guidedActionsController.guidedUIVisible || (!guidedActionsController.showGotoLocation && !guidedActionsController.showOrbit)) {
return
}
_guidedLocationCoordinate = flightMap.toCoordinate(Qt.point(mouse.x, mouse.y), false /* clipToViewPort */)
if (guidedActionsController.showGotoLocation && guidedActionsController.showOrbit) {
clickMenu.popup()
} else if (guidedActionsController.showGotoLocation) {
guidedLocationItem.gotoLocation = true
guidedActionsController.confirmAction(guidedActionsController.actionGoto, _guidedLocationCoordinate)
} else if (guidedActionsController.showOrbit) {
guidedLocationItem.gotoLocation = false
guidedActionsController.confirmAction(guidedActionsController.actionOrbit, _guidedLocationCoordinate)
}
}
}
......
src/FlightDisplay/GuidedActionConfirm.qml
View file @ 58c0d436
......@@ -100,12 +100,13 @@ Rectangle {
onAccept: {
_root.visible = false
var altitudeChange = 0
if (altitudeSlider.visible) {
_root.actionData = altitudeSlider.getValue()
altitudeChange = altitudeSlider.getAltitudeChangeValue()
altitudeSlider.visible = false
}
hideTrigger = false
guidedController.executeAction(_root.action, _root.actionData)
guidedController.executeAction(_root.action, _root.actionData, altitudeChange)
}
onReject: {
......
src/FlightDisplay/GuidedActionsController.qml
View file @ 58c0d436
......@@ -62,9 +62,9 @@ Item {
readonly property string landMessage: qsTr("Land the vehicle at the current position.")
readonly property string rtlMessage: qsTr("Return to the home position of the vehicle.")
readonly property string changeAltMessage: qsTr("Change the altitude of the vehicle up or down.")
readonly property string gotoMessage: qsTr("Move the vehicle to the location clicked on the map.")
readonly property string gotoMessage: qsTr("Move the vehicle to the specified location.")
property string setWaypointMessage: qsTr("Adjust current waypoint to %1.").arg(_actionData)
readonly property string orbitMessage: qsTr("Orbit the vehicle around the current location.")
readonly property string orbitMessage: qsTr("Orbit the vehicle around the specified location. Warning: WORK IN PROGRESS!")
readonly property string landAbortMessage: qsTr("Abort the landing sequence.")
readonly property string pauseMessage: qsTr("Pause the vehicle at it's current position, adjusting altitude up or down as needed.")
readonly property string mvPauseMessage: qsTr("Pause all vehicles at their current position.")
......@@ -103,7 +103,7 @@ Item {
property bool showContinueMission: _guidedActionsEnabled && _missionAvailable && !_missionActive && _vehicleFlying && (_currentMissionIndex < missionController.visualItems.count - 1)
property bool showPause: _guidedActionsEnabled && _vehicleArmed && _activeVehicle.pauseVehicleSupported && _vehicleFlying && !_vehiclePaused
property bool showChangeAlt: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive
property bool showOrbit: _guidedActionsEnabled && !_hideOrbit && _vehicleFlying && _activeVehicle.orbitModeSupported && _vehicleArmed && !_missionActive
property bool showOrbit: _guidedActionsEnabled && !_hideOrbit && _vehicleFlying && _activeVehicle.orbitModeSupported && !_missionActive
property bool showLandAbort: _guidedActionsEnabled && _vehicleFlying && _activeVehicle.fixedWing && _vehicleLanding
property bool showGotoLocation: _guidedActionsEnabled && _vehicleFlying
......@@ -190,7 +190,8 @@ Item {
_vehicleWasFlying = true
}
}
property var _actionData
property var _actionData
on_FlightModeChanged: {
_vehiclePaused = _activeVehicle ? _flightMode === _activeVehicle.pauseFlightMode : false
......@@ -299,6 +300,8 @@ Item {
confirmDialog.title = orbitTitle
confirmDialog.message = orbitMessage
confirmDialog.hideTrigger = Qt.binding(function() { return !showOrbit })
altitudeSlider.reset()
altitudeSlider.visible = true
break;
case actionLandAbort:
confirmDialog.title = landAbortTitle
......@@ -335,7 +338,7 @@ Item {
}
// Executes the specified action
function executeAction(actionCode, actionData) {
function executeAction(actionCode, actionData, actionAltitudeChange) {
var i;
var rgVehicle;
switch (actionCode) {
......@@ -346,7 +349,7 @@ Item {
_activeVehicle.guidedModeLand()
break
case actionTakeoff:
_activeVehicle.guidedModeTakeoff(actionData)
_activeVehicle.guidedModeTakeoff(actionAltitudeChange)
break
case actionResumeMission:
case actionResumeMissionUploadFail:
......@@ -376,7 +379,7 @@ Item {
_activeVehicle.emergencyStop()
break
case actionChangeAlt:
_activeVehicle.guidedModeChangeAltitude(actionData)
_activeVehicle.guidedModeChangeAltitude(actionAltitudeChange)
break
case actionGoto:
_activeVehicle.guidedModeGotoLocation(actionData)
......@@ -385,14 +388,14 @@ Item {
_activeVehicle.setCurrentMissionSequence(actionData)
break
case actionOrbit:
_activeVehicle.guidedModeOrbit()
_activeVehicle.guidedModeOrbit(actionData, 10 /* Hacked fixed radius */, _activeVehicle.altitudeAMSL + actionAltitudeChange)
break
case actionLandAbort:
_activeVehicle.abortLanding(50) // hardcoded value for climbOutAltitude that is currently ignored
break
case actionPause:
_activeVehicle.pauseVehicle()
_activeVehicle.guidedModeChangeAltitude(actionData)
_activeVehicle.guidedModeChangeAltitude(actionAltitudeChange)
break
case actionMVPause:
rgVehicle = QGroundControl.multiVehicleManager.vehicles
......
src/FlightDisplay/GuidedAltitudeSlider.qml
View file @ 58c0d436
......@@ -36,7 +36,8 @@ Rectangle {
altField.setToMinimumTakeoff()
}
function getValue() {
/// Returns the user specified change in altitude from the current vehicle altitude
function getAltitudeChangeValue() {
return altField.newAltitudeMeters - _vehicleAltitude
}
......
src/Vehicle/Vehicle.cc
View file @ 58c0d436
......@@ -2751,13 +2751,30 @@ void Vehicle::guidedModeChangeAltitude(double altitudeChange)
_firmwarePlugin->guidedModeChangeAltitude(this, altitudeChange);
}
void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double amslAltitude)
{
if (!orbitModeSupported()) {
qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle."));
return;
}
_firmwarePlugin->guidedModeOrbit(this, centerCoord, radius, velocity, altitude);
double lat, lon, alt;
if (centerCoord.isValid()) {
lat = lon = alt = qQNaN();
} else {
lat = centerCoord.latitude();
lon = centerCoord.longitude();
alt = amslAltitude;
}
sendMavCommand(defaultComponentId(),
MAV_CMD_DO_ORBIT,
true, // show error if fails
radius,
qQNaN(), // Use default velocity
0, // Vehicle points to center
qQNaN(), // reserved
lat, lon, alt);
}
void Vehicle::pauseVehicle(void)
......
src/Vehicle/Vehicle.h
View file @ 58c0d436
......@@ -582,11 +582,10 @@ public:
Q_INVOKABLE void guidedModeChangeAltitude(double altitudeChange);
/// Command vehicle to orbit given center point
/// @param centerCoord Center Coordinates
/// @param centerCoord Orit around this point
/// @param radius Distance from vehicle to centerCoord
/// @param velocity Orbit velocity (positive CW, negative CCW)
/// @param altitude Desired Vehicle Altitude
Q_INVOKABLE void guidedModeOrbit(const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN);
/// @param amslAltitude Desired vehicle altitude
Q_INVOKABLE void guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double amslAltitude);
/// Command vehicle to pause at current location. If vehicle supports guide mode, vehicle will be left
/// in guided mode after pause.
......
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