From b7e6281a7efd02f751008985fec682a556983cb8 Mon Sep 17 00:00:00 2001 From: dogmaphobic Date: Thu, 14 Jul 2016 03:11:18 -0400 Subject: [PATCH] Adding Orbit Mode to Guided Bar --- src/FirmwarePlugin/FirmwarePlugin.cc | 6 +++++ src/FirmwarePlugin/FirmwarePlugin.h | 24 +++++++++++-------- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 23 ++++++++++++++++++ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 17 ++++++------- .../FlightDisplayViewWidgets.qml | 23 +++++++++++++++--- src/Vehicle/Vehicle.cc | 9 +++++++ src/Vehicle/Vehicle.h | 7 ++++++ 7 files changed, 88 insertions(+), 21 deletions(-) diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 6338280b7..7e3ffcee1 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -183,6 +183,12 @@ void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) 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 diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index c64cbdc98..72f09b856 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -57,7 +57,7 @@ public: /// key: firmware major version /// value: remapParamNameMinorVersionRemapMap_t entry typedef QMap remapParamNameMajorVersionMap_t; - + /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware. virtual void initializeVehicle(Vehicle* vehicle); @@ -69,18 +69,18 @@ public: /// @return List of VehicleComponents for the specified vehicle. Caller owns returned objects and must /// free when no longer needed. virtual QList componentsForVehicle(AutoPilotPlugin* vehicle); - + /// Returns the list of available flight modes virtual QStringList flightModes(Vehicle* vehicle) { - Q_UNUSED(vehicle); - return QStringList(); - } - + Q_UNUSED(vehicle); + return QStringList(); + } + /// Returns the name for this flight mode. Flight mode names must be human readable as well as audio speakable. /// @param base_mode Base mode from mavlink HEARTBEAT message /// @param custom_mode Custom mode from mavlink HEARTBEAT message virtual QString flightMode(uint8_t base_mode, uint32_t custom_mode) const; - + /// Sets base_mode and custom_mode to specified flight mode. /// @param[out] base_mode Base mode for SET_MODE mavlink message /// @param[out] custom_mode Custom mode for SET_MODE mavlink message @@ -109,6 +109,10 @@ public: /// @param altitudeRel Relative altitude to takeoff to virtual void guidedModeTakeoff(Vehicle* vehicle, double altitudeRel); + /// 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); @@ -122,7 +126,7 @@ public: /// The remainder can be assigned to Vehicle actions. /// @return -1: reserver all buttons, >0 number of buttons to reserve virtual int manualControlReservedButtonCount(void); - + /// Called before any mavlink message is processed by Vehicle such that the firmwre plugin /// can adjust any message characteristics. This is handy to adjust or differences in mavlink /// spec implementations such that the base code can remain mavlink generic. @@ -130,7 +134,7 @@ public: /// @param message[in,out] Mavlink message to adjust if needed. /// @return false: skip message, true: process message virtual bool adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message); - + /// Called before any mavlink message is sent to the Vehicle so plugin can adjust any message characteristics. /// This is handy to adjust or differences in mavlink spec implementations such that the base code can remain /// mavlink generic. @@ -145,7 +149,7 @@ public: /// it, it may or may not return a home position back in position 0. /// false: Do not send first item to vehicle, sequence numbers must be adjusted virtual bool sendHomePositionToVehicle(void); - + /// Returns the parameter that is used to identify the default component virtual QString getDefaultComponentIdParam(void) const { return QString(); } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 9834ba337..cf27bb8ab 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -269,6 +269,29 @@ void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle) vehicle->setFlightMode(landingFlightMode); } +void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude) +{ + //-- If not in "guided" mode, make it so. + if(!isGuidedMode(vehicle)) + setGuidedMode(vehicle, true); + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_message_t msg; + mavlink_command_long_t cmd; + cmd.command = (uint16_t)MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE; + cmd.confirmation = 0; + cmd.param1 = radius; + cmd.param2 = velocity; + cmd.param3 = altitude; + cmd.param4 = NAN; + cmd.param5 = centerCoord.isValid() ? centerCoord.latitude() : NAN; + cmd.param6 = centerCoord.isValid() ? centerCoord.longitude() : NAN; + cmd.param7 = centerCoord.isValid() ? centerCoord.altitude() : NAN; + cmd.target_system = vehicle->id(); + cmd.target_component = vehicle->defaultComponentId(); + mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + vehicle->sendMessageOnPriorityLink(msg); +} + void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) { Q_UNUSED(altitudeRel); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 6a712f96a..23d2804a7 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -34,14 +34,15 @@ public: QStringList flightModes (Vehicle* vehicle) final; QString flightMode (uint8_t base_mode, uint32_t custom_mode) const final; bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final; - void setGuidedMode(Vehicle* vehicle, bool guidedMode) final; - void pauseVehicle(Vehicle* vehicle) final; - void guidedModeRTL(Vehicle* vehicle) final; - void guidedModeLand(Vehicle* vehicle) final; - void guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) final; - void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final; - void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final; - bool isGuidedMode(const Vehicle* vehicle) const; + void setGuidedMode (Vehicle* vehicle, bool guidedMode) final; + void pauseVehicle (Vehicle* vehicle) final; + void guidedModeRTL (Vehicle* vehicle) final; + void guidedModeLand (Vehicle* vehicle) final; + void guidedModeTakeoff (Vehicle* vehicle, double altitudeRel) final; + void guidedModeOrbit (Vehicle* vehicle, const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN) final; + void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final; + void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeRel) final; + bool isGuidedMode (const Vehicle* vehicle) const; int manualControlReservedButtonCount(void) final; void initializeVehicle (Vehicle* vehicle) final; bool sendHomePositionToVehicle (void) final; diff --git a/src/FlightDisplay/FlightDisplayViewWidgets.qml b/src/FlightDisplay/FlightDisplayViewWidgets.qml index cfea5e63e..c8550d846 100644 --- a/src/FlightDisplay/FlightDisplayViewWidgets.qml +++ b/src/FlightDisplay/FlightDisplayViewWidgets.qml @@ -279,10 +279,9 @@ Item { anchors.horizontalCenter: parent.horizontalCenter width: guidedModeColumn.width + (_margins * 2) height: guidedModeColumn.height + (_margins * 2) - radius: _margins - color: _lightWidgetBorders ? qgcPal.mapWidgetBorderLight : qgcPal.mapWidgetBorderDark + radius: ScreenTools.defaultFontPixelHeight * 0.25 + color: _lightWidgetBorders ? Qt.rgba(qgcPal.mapWidgetBorderLight.r, qgcPal.mapWidgetBorderLight.g, qgcPal.mapWidgetBorderLight.b, 0.8) : Qt.rgba(qgcPal.mapWidgetBorderDark.r, qgcPal.mapWidgetBorderDark.g, qgcPal.mapWidgetBorderDark.b, 0.75) visible: _activeVehicle - opacity: 0.9 z: QGroundControl.zOrderWidgets state: "Shown" @@ -340,6 +339,7 @@ Item { readonly property int confirmChangeAlt: 7 readonly property int confirmGoTo: 8 readonly property int confirmRetask: 9 + readonly property int confirmOrbit: 10 property int confirmActionCode property real _showMargin: _margins @@ -381,6 +381,12 @@ Item { case confirmRetask: _activeVehicle.setCurrentMissionSequence(_flightMap._retaskSequence) break; + case confirmOrbit: + //-- All parameters controlled by RC + _activeVehicle.guidedModeOrbit() + //-- Center on current flight map position and orbit with a 50m radius (velocity/direction controlled by the RC) + //_activeVehicle.guidedModeOrbit(QGroundControl.flightMapPosition, 50.0) + break; default: console.warn(qsTr("Internal error: unknown confirmActionCode"), confirmActionCode) } @@ -429,6 +435,9 @@ Item { case confirmRetask: guidedModeConfirm.confirmText = qsTr("active waypoint change") break; + case confirmOrbit: + guidedModeConfirm.confirmText = qsTr("enter orbit mode") + break; } _guidedModeBar.visible = false guidedModeConfirm.visible = true @@ -488,6 +497,14 @@ Item { visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmChangeAlt) } + + QGCButton { + pointSize: _guidedModeBar._fontPointSize + text: qsTr("Orbit") + visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed + onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmOrbit) + } + } // Row } // Column } // Rectangle - Guided mode buttons diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 230fb67b7..3663472a8 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1637,6 +1637,15 @@ void Vehicle::guidedModeChangeAltitude(double altitudeRel) _firmwarePlugin->guidedModeChangeAltitude(this, altitudeRel); } +void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude) +{ + if (!guidedModeSupported()) { + qgcApp()->showMessage(guided_mode_not_supported_by_vehicle); + return; + } + _firmwarePlugin->guidedModeOrbit(this, centerCoord, radius, velocity, altitude); +} + void Vehicle::pauseVehicle(void) { if (!pauseVehicleSupported()) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 07a8f1bb1..ad62013a8 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -349,6 +349,13 @@ public: /// Command vehicle to change to the specified relatice altitude Q_INVOKABLE void guidedModeChangeAltitude(double altitudeRel); + /// Command vehicle to orbit given center point + /// @param centerCoord Center Coordinates + /// @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); + /// Command vehicle to pause at current location. If vehicle supports guide mode, vehicle will be left /// in guided mode after pause. Q_INVOKABLE void pauseVehicle(void); -- 2.22.0