Commit 0817980e authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #4603 from DonLakeFlyer/FWPattern

Complex Item: More work in Fixed Wing Landing Pattern
parents 76984650 bd5e0329
......@@ -22,7 +22,7 @@ import QGroundControl.Palette 1.0
// Editor for Fixed Wing Landing Pattern complex mission item
Rectangle {
id: _root
height: visible ? (editorColumn.height + (_margin * 2)) : 0
height: visible ? ((editorColumn.visible ? editorColumn.height : editorColumnNeedLandingPoint.height) + (_margin * 2)) : 0
width: availableWidth
color: qgcPal.windowShadeDark
radius: _radius
......@@ -38,6 +38,7 @@ Rectangle {
anchors.margins: _margin
anchors.left: parent.left
anchors.right: parent.right
visible: missionItem.landingCoordSet
QGCLabel { text: "WIP (NOT FOR REAL FLIGHT!)" }
......@@ -52,4 +53,16 @@ Rectangle {
fact: missionItem.loiterClockwise
}
}
Column {
id: editorColumnNeedLandingPoint
anchors.margins: _margin
anchors.left: parent.left
anchors.right: parent.right
visible: !missionItem.landingCoordSet
QGCLabel { text: "WIP (NOT FOR REAL FLIGHT!)" }
QGCLabel { text: qsTr("Click in map to set landing point.") }
}
}
......@@ -12,6 +12,7 @@ import QtQuick.Controls 1.2
import QtLocation 5.3
import QtPositioning 5.2
import QGroundControl 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Palette 1.0
import QGroundControl.Controls 1.0
......@@ -20,19 +21,193 @@ import QGroundControl.Controls 1.0
Item {
property var map ///< Map control to place item in
property var _missionItem: object
property var _mouseArea
property var _dragLoiter
property var _dragLand
property var _loiterPoint
property var _landPoint
property var _flightPath
function hideItemVisuals() {
if (_flightPath) {
_flightPath.destroy()
_flightPath = undefined
}
if (_loiterPoint) {
_loiterPoint.destroy()
_loiterPoint = undefined
}
if (_landPoint) {
_landPoint.destroy()
_landPoint = undefined
}
}
function showItemVisuals() {
if (!_flightPath) {
_flightPath = flightPathComponent.createObject(map)
map.addMapItem(_flightPath)
}
if (!_loiterPoint) {
_loiterPoint = loiterPointComponent.createObject(map)
map.addMapItem(_loiterPoint)
}
if (!_landPoint) {
_landPoint = landPointComponent.createObject(map)
map.addMapItem(_landPoint)
}
}
function hideMouseArea() {
if (_mouseArea) {
_mouseArea.destroy()
_mouseArea = undefined
}
}
function showMouseArea() {
if (!_mouseArea) {
_mouseArea = mouseAreaComponent.createObject(map)
map.addMapItem(_mouseArea)
}
}
function hideDragAreas() {
console.log("hideDragAreas")
if (_dragLoiter) {
_dragLoiter.destroy()
_dragLoiter = undefined
}
if (_dragLand) {
_dragLand.destroy()
_dragLand = undefined
}
}
function showDragAreas() {
console.log("showDragAreas")
if (!_dragLoiter) {
_dragLoiter = dragAreaComponent.createObject(map, { "dragLoiter": true })
}
if (!_dragLand) {
_dragLand = dragAreaComponent.createObject(map, { "dragLoiter": false })
}
}
Component.onCompleted: {
_flightPath = flightPathComponent.createObject(map)
_loiterPoint = loiterComponent.createObject(map)
map.addMapItem(_flightPath)
map.addMapItem(_loiterPoint)
if (_missionItem.landingCoordSet) {
showItemVisuals()
if (_missionItem.isCurrentItem) {
showDragAreas()
}
} else if (_missionItem.isCurrentItem) {
showMouseArea()
}
}
Component.onDestruction: {
_loiterPoint.destroy()
_flightPath.destroy()
hideDragAreas()
hideMouseArea()
hideItemVisuals()
}
Connections {
target: _missionItem
onIsCurrentItemChanged: {
console.log("onIsCurrentItemChanged", _missionItem.isCurrentItem)
if (_missionItem.isCurrentItem) {
if (_missionItem.landingCoordSet) {
showDragAreas()
} else {
showMouseArea()
}
} else {
hideMouseArea()
hideDragAreas()
}
}
onLandingCoordSetChanged: {
if (_missionItem.landingCoordSet) {
hideMouseArea()
showItemVisuals()
showDragAreas()
} else if (_missionItem.isCurrentItem) {
hideDragAreas()
showMouseArea()
}
}
}
// Mouse area to capture landing point coordindate
Component {
id: mouseAreaComponent
MouseArea {
anchors.fill: map
onClicked: {
var coordinate = map.toCoordinate(Qt.point(mouse.x, mouse.y))
coordinate.latitude = coordinate.latitude.toFixed(_decimalPlaces)
coordinate.longitude = coordinate.longitude.toFixed(_decimalPlaces)
coordinate.altitude = coordinate.altitude.toFixed(_decimalPlaces)
_missionItem.landingCoordinate = coordinate
}
}
}
// Control which is used to drag items
Component {
id: dragAreaComponent
Rectangle {
id: itemDragger
x: mapQuickItem ? (mapQuickItem.x + mapQuickItem.anchorPoint.x - (itemDragger.width / 2)) : 100
y: mapQuickItem ? (mapQuickItem.y + mapQuickItem.anchorPoint.y - (itemDragger.height / 2)) : 100
width: ScreenTools.defaultFontPixelHeight * 2
height: ScreenTools.defaultFontPixelHeight * 2
color: "transparent"
z: QGroundControl.zOrderMapItems + 1 // Above item icons
property bool dragLoiter
property var mapQuickItem: dragLoiter ? _loiterPoint : _landPoint
property bool _preventCoordinateBindingLoop: false
onXChanged: liveDrag()
onYChanged: liveDrag()
function liveDrag() {
if (!itemDragger._preventCoordinateBindingLoop && Drag.active) {
var point = Qt.point(itemDragger.x + (itemDragger.width / 2), itemDragger.y + (itemDragger.height / 2))
var coordinate = map.toCoordinate(point)
itemDragger._preventCoordinateBindingLoop = true
if (dragLoiter) {
coordinate.altitude = _missionItem.loiterCoordinate.altitude
_missionItem.loiterCoordinate = coordinate
} else {
coordinate.altitude = _missionItem.landingCoordinate.altitude
_missionItem.landingCoordinate = coordinate
}
itemDragger._preventCoordinateBindingLoop = false
}
}
Drag.active: itemDrag.drag.active
Drag.hotSpot.x: width / 2
Drag.hotSpot.y: height / 2
MouseArea {
id: itemDrag
anchors.fill: parent
drag.target: parent
drag.minimumX: 0
drag.minimumY: 0
drag.maximumX: itemDragger.parent.width - parent.width
drag.maximumY: itemDragger.parent.height - parent.height
}
}
}
// Flight path
......@@ -40,25 +215,44 @@ Item {
id: flightPathComponent
MapPolyline {
z: QGroundControl.zOrderMapItems - 1 // Under item indicators
line.color: "white"
line.width: 2
path: [ object.loiterCoordinate, object.exitCoordinate ]
path: _missionItem.landingCoordSet ? [ _missionItem.loiterCoordinate, _missionItem.landingCoordinate ] : undefined
}
}
// Loiter point
Component {
id: loiterComponent
id: loiterPointComponent
MapQuickItem {
anchorPoint.x: sourceItem.width / 2
anchorPoint.y: sourceItem.height / 2
coordinate: object.loiterCoordinate
z: QGroundControl.zOrderMapItems
coordinate: _missionItem.loiterCoordinate
sourceItem:
MissionItemIndexLabel {
label: "L"
}
label: "P"
}
}
}
// Land point
Component {
id: landPointComponent
MapQuickItem {
anchorPoint.x: sourceItem.width / 2
anchorPoint.y: sourceItem.height / 2
z: QGroundControl.zOrderMapItems
coordinate: _missionItem.landingCoordinate
sourceItem:
MissionItemIndexLabel {
label: "L"
}
}
}
}
......@@ -454,42 +454,6 @@ QGCView {
}
}
// Add the complex mission item exit coordinates
MapItemView {
model: missionController.complexVisualItems
delegate: exitCoordinateComponent
}
Component {
id: exitCoordinateComponent
MissionItemIndicator {
coordinate: object.exitCoordinate
z: QGroundControl.zOrderMapItems
missionItem: object
sequenceNumber: object.lastSequenceNumber
visible: object.specifiesCoordinate
// These are the non-coordinate child mission items attached to this item
Row {
anchors.top: parent.top
anchors.left: parent.right
Repeater {
model: !object.isSimpleItem ? object.childItems : 0
delegate: MissionItemIndexLabel {
label: object.abbreviation
checked: object.isCurrentItem
z: 2
onClicked: setCurrentItem(object.sequenceNumber)
}
}
}
}
}
// Add the simple mission items to the map
MapItemView {
model: missionController.visualItems
......@@ -502,7 +466,7 @@ QGCView {
MissionItemIndicator {
id: itemIndicator
coordinate: object.coordinate
visible: object.specifiesCoordinate
visible: object.isSimpleItem && object.specifiesCoordinate
z: QGroundControl.zOrderMapItems
missionItem: object
sequenceNumber: object.sequenceNumber
......
......@@ -12,6 +12,7 @@ import QtQuick.Controls 1.2
import QtLocation 5.3
import QtPositioning 5.2
import QGroundControl 1.0
import QGroundControl.ScreenTools 1.0
import QGroundControl.Palette 1.0
import QGroundControl.Controls 1.0
......@@ -20,19 +21,28 @@ import QGroundControl.Controls 1.0
Item {
property var map ///< Map control to place item in
property var _missionItem: object
property var _polygon
property var _grid
property var _entryCoordinate
property var _exitCoordinate
Component.onCompleted: {
_polygon = polygonComponent.createObject(map)
_grid = gridComponent.createObject(map)
_entryCoordinate = entryPointComponent.createObject(map)
_exitCoordinate = exitPointComponent.createObject(map)
map.addMapItem(_polygon)
map.addMapItem(_grid)
map.addMapItem(_entryCoordinate)
map.addMapItem(_exitCoordinate)
}
Component.onDestruction: {
_polygon.destroy()
_grid.destroy()
_entryCoordinate.destroy()
_exitCoordinate.destroy()
}
// Survey area polygon
......@@ -42,7 +52,7 @@ Item {
MapPolygon {
color: "green"
opacity: 0.5
path: object.polygonPath
path: _missionItem.polygonPath
}
}
......@@ -53,7 +63,41 @@ Item {
MapPolyline {
line.color: "white"
line.width: 2
path: object.gridPoints
path: _missionItem.gridPoints
}
}
// Entry point
Component {
id: entryPointComponent
MapQuickItem {
anchorPoint.x: sourceItem.width / 2
anchorPoint.y: sourceItem.height / 2
z: QGroundControl.zOrderMapItems
coordinate: _missionItem.coordinate
sourceItem:
MissionItemIndexLabel {
label: "S"
}
}
}
// Exit point
Component {
id: exitPointComponent
MapQuickItem {
anchorPoint.x: sourceItem.width / 2
anchorPoint.y: sourceItem.height / 2
z: QGroundControl.zOrderMapItems
coordinate: _missionItem.exitCoordinate
sourceItem:
MissionItemIndexLabel {
label: "S"
}
}
}
}
......@@ -27,10 +27,12 @@ const char* FixedWingLandingComplexItem::_loiterClockwiseName = "Clockwi
QMap<QString, FactMetaData*> FixedWingLandingComplexItem::_metaDataMap;
FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QGeoCoordinate mapClickCoordinate, QObject* parent)
FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObject* parent)
: ComplexMissionItem(vehicle, parent)
, _sequenceNumber(0)
, _dirty(false)
, _landingCoordSet(false)
, _ignoreRecalcSignals(false)
, _loiterToLandDistanceFact (0, _loiterToLandDistanceName, FactMetaData::valueTypeDouble)
, _loiterAltitudeFact (0, _loiterAltitudeName, FactMetaData::valueTypeDouble)
, _loiterRadiusFact (0, _loiterRadiusName, FactMetaData::valueTypeDouble)
......@@ -55,15 +57,12 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QGeoC
_loiterClockwiseFact.setRawValue (_loiterClockwiseFact.rawDefaultValue());
_landingHeadingFact.setRawValue (_landingHeadingFact.rawDefaultValue());
connect(&_loiterToLandDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcLoiterPosition);
connect(&_landingHeadingFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcLoiterPosition);
connect(&_loiterToLandDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcLoiterCoordFromFacts);
connect(&_landingHeadingFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcLoiterCoordFromFacts);
connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFactsFromCoords);
connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFactsFromCoords);
_textFieldFacts << QVariant::fromValue(&_loiterToLandDistanceFact) << QVariant::fromValue(&_loiterAltitudeFact) << QVariant::fromValue(&_loiterRadiusFact) << QVariant::fromValue(&_landingHeadingFact);
// Exit coordinate is our land point
_exitCoordinate = mapClickCoordinate;
_recalcLoiterPosition();
}
int FixedWingLandingComplexItem::lastSequenceNumber(void) const
......@@ -72,14 +71,6 @@ int FixedWingLandingComplexItem::lastSequenceNumber(void) const
return _sequenceNumber + 2;
}
void FixedWingLandingComplexItem::setCoordinate(const QGeoCoordinate& coordinate)
{
if (_coordinate != coordinate) {
_coordinate = coordinate;
emit coordinateChanged(_coordinate);
}
}
void FixedWingLandingComplexItem::setDirty(bool dirty)
{
if (_dirty != dirty) {
......@@ -136,14 +127,6 @@ double FixedWingLandingComplexItem::greatestDistanceTo(const QGeoCoordinate &oth
return greatestDistance;
}
void FixedWingLandingComplexItem::_setExitCoordinate(const QGeoCoordinate& coordinate)
{
if (_exitCoordinate != coordinate) {
_exitCoordinate = coordinate;
emit exitCoordinateChanged(coordinate);
}
}
bool FixedWingLandingComplexItem::specifiesCoordinate(void) const
{
return true;
......@@ -184,8 +167,8 @@ QmlObjectListModel* FixedWingLandingComplexItem::getMissionItems(void) const
MAV_CMD_NAV_LAND,
MAV_FRAME_GLOBAL_RELATIVE_ALT,
0.0, 0.0, 0.0, 0.0, // param 1-4
_exitCoordinate.latitude(),
_exitCoordinate.longitude(),
_landingCoordinate.latitude(),
_landingCoordinate.longitude(),
0.0, // altitude
true, // autoContinue
false, // isCurrentItem
......@@ -207,22 +190,54 @@ void FixedWingLandingComplexItem::setCruiseSpeed(double cruiseSpeed)
Q_UNUSED(cruiseSpeed);
}
void FixedWingLandingComplexItem::_recalcLoiterPosition(void)
void FixedWingLandingComplexItem::setLandingCoordinate(const QGeoCoordinate& coordinate)
{
double north, east, down;
QGeoCoordinate tangentOrigin = _exitCoordinate;
if (coordinate != _landingCoordinate) {
_landingCoordinate = coordinate;
if (_landingCoordSet) {
emit exitCoordinateChanged(coordinate);
emit landingCoordinateChanged(coordinate);
} else {
_ignoreRecalcSignals = true;
emit exitCoordinateChanged(coordinate);
emit landingCoordinateChanged(coordinate);
_ignoreRecalcSignals = false;
_landingCoordSet = true;
_recalcLoiterCoordFromFacts();
emit landingCoordSetChanged(true);
}
}
}
convertGeoToNed(_exitCoordinate, tangentOrigin, &north, &east, &down);
void FixedWingLandingComplexItem::setLoiterCoordinate(const QGeoCoordinate& coordinate)
{
if (coordinate != _loiterCoordinate) {
_loiterCoordinate = coordinate;
emit coordinateChanged(coordinate);
emit loiterCoordinateChanged(coordinate);
}
}
QPointF originPoint(east, north);
north += _loiterToLandDistanceFact.rawValue().toDouble();
QPointF loiterPoint(east, north);
QPointF rotatedLoiterPoint = _rotatePoint(loiterPoint, originPoint, _landingHeadingFact.rawValue().toDouble());
void FixedWingLandingComplexItem::_recalcLoiterCoordFromFacts(void)
{
if (!_ignoreRecalcSignals && _landingCoordSet) {
double north, east, down;
QGeoCoordinate tangentOrigin = _landingCoordinate;
convertNedToGeo(rotatedLoiterPoint.y(), rotatedLoiterPoint.x(), down, tangentOrigin, &_loiterCoordinate);
convertGeoToNed(_landingCoordinate, tangentOrigin, &north, &east, &down);
emit loiterCoordinateChanged(_loiterCoordinate);
setCoordinate(_loiterCoordinate);
QPointF originPoint(east, north);
north += _loiterToLandDistanceFact.rawValue().toDouble();
QPointF loiterPoint(east, north);
QPointF rotatedLoiterPoint = _rotatePoint(loiterPoint, originPoint, _landingHeadingFact.rawValue().toDouble());
convertNedToGeo(rotatedLoiterPoint.y(), rotatedLoiterPoint.x(), down, tangentOrigin, &_loiterCoordinate);
_ignoreRecalcSignals = true;
emit loiterCoordinateChanged(_loiterCoordinate);
emit coordinateChanged(_loiterCoordinate);
_ignoreRecalcSignals = false;
}
}
QPointF FixedWingLandingComplexItem::_rotatePoint(const QPointF& point, const QPointF& origin, double angle)
......@@ -235,3 +250,39 @@ QPointF FixedWingLandingComplexItem::_rotatePoint(const QPointF& point, const QP
return rotated;
}
void FixedWingLandingComplexItem::_recalcFactsFromCoords(void)
{
if (!_ignoreRecalcSignals && _landingCoordSet) {
// Prevent signal recursion
_ignoreRecalcSignals = true;
// Calc new distance
double northLand, eastLand, down;
double northLoiter, eastLoiter;
QGeoCoordinate tangentOrigin = _landingCoordinate;
convertGeoToNed(_landingCoordinate, tangentOrigin, &northLand, &eastLand, &down);
convertGeoToNed(_loiterCoordinate, tangentOrigin, &northLoiter, &eastLoiter, &down);
double newDistance = sqrt(pow(eastLoiter - eastLand, 2.0) + pow(northLoiter - northLand, 2.0));
_loiterToLandDistanceFact.setRawValue(newDistance);
// Calc new heading
QPointF vector(eastLoiter - eastLand, northLoiter - northLand);
double radians = atan2(vector.y(), vector.x());
double degrees = qRadiansToDegrees(radians);
degrees -= 90; // north up
if (degrees < 0.0) {
degrees += 360.0;
} else if (degrees > 360.0) {
degrees -= 360.0;
}
_landingHeadingFact.setRawValue(degrees);
_ignoreRecalcSignals = false;
}
}
......@@ -22,13 +22,20 @@ class FixedWingLandingComplexItem : public ComplexMissionItem
Q_OBJECT
public:
FixedWingLandingComplexItem(Vehicle* vehicle, QGeoCoordinate mapClickCoordinate, QObject* parent = NULL);
FixedWingLandingComplexItem(Vehicle* vehicle, QObject* parent = NULL);
Q_PROPERTY(QVariantList textFieldFacts MEMBER _textFieldFacts CONSTANT)
Q_PROPERTY(Fact* loiterClockwise READ loiterClockwise CONSTANT)
Q_PROPERTY(QGeoCoordinate loiterCoordinate MEMBER _loiterCoordinate NOTIFY loiterCoordinateChanged)
Q_PROPERTY(QVariantList textFieldFacts MEMBER _textFieldFacts CONSTANT)
Q_PROPERTY(Fact* loiterClockwise READ loiterClockwise CONSTANT)
Q_PROPERTY(QGeoCoordinate loiterCoordinate READ loiterCoordinate WRITE setLoiterCoordinate NOTIFY loiterCoordinateChanged)
Q_PROPERTY(QGeoCoordinate landingCoordinate READ landingCoordinate WRITE setLandingCoordinate NOTIFY landingCoordinateChanged)
Q_PROPERTY(bool landingCoordSet MEMBER _landingCoordSet NOTIFY landingCoordSetChanged)
Fact* loiterClockwise(void) { return &_loiterClockwiseFact; }
Fact* loiterClockwise (void) { return &_loiterClockwiseFact; }
QGeoCoordinate landingCoordinate (void) const { return _landingCoordinate; }
QGeoCoordinate loiterCoordinate (void) const { return _loiterCoordinate; }
void setLandingCoordinate (const QGeoCoordinate& coordinate);
void setLoiterCoordinate (const QGeoCoordinate& coordinate);
// Overrides from ComplexMissionItem
......@@ -49,8 +56,8 @@ public:
QString commandDescription (void) const final { return "Landing Pattern"; }
QString commandName (void) const final { return "Landing Pattern"; }
QString abbreviation (void) const final { return "L"; }
QGeoCoordinate coordinate (void) const final { return _coordinate; }
QGeoCoordinate exitCoordinate (void) const final { return _exitCoordinate; }
QGeoCoordinate coordinate (void) const final { return _loiterCoordinate; }
QGeoCoordinate exitCoordinate (void) const final { return _landingCoordinate; }
int sequenceNumber (void) const final { return _sequenceNumber; }
double flightSpeed (void) final { return std::numeric_limits<double>::quiet_NaN(); }
......@@ -59,27 +66,30 @@ public:
bool exitCoordinateSameAsEntry (void) const final { return true; }
void setDirty (bool dirty) final;
void setCoordinate (const QGeoCoordinate& coordinate) final;
void setCoordinate (const QGeoCoordinate& coordinate) final { setLoiterCoordinate(coordinate); }
void setSequenceNumber (int sequenceNumber) final;
void save (QJsonObject& saveObject) const final;
static const char* jsonComplexItemTypeValue;
signals:
void loiterCoordinateChanged(QGeoCoordinate coordinate);
void loiterCoordinateChanged (QGeoCoordinate coordinate);
void landingCoordinateChanged (QGeoCoordinate coordinate);
void landingCoordSetChanged (bool landingCoordSet);
private slots:
void _recalcLoiterPosition(void);
void _recalcLoiterCoordFromFacts(void);
void _recalcFactsFromCoords(void);
private:
void _setExitCoordinate(const QGeoCoordinate& coordinate);
QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle);
int _sequenceNumber;
bool _dirty;
QGeoCoordinate _coordinate;
QGeoCoordinate _exitCoordinate;
QGeoCoordinate _loiterCoordinate;
QGeoCoordinate _landingCoordinate;
bool _landingCoordSet;
bool _ignoreRecalcSignals;
Fact _loiterToLandDistanceFact;
Fact _loiterAltitudeFact;
......
......@@ -228,7 +228,7 @@ int MissionController::insertComplexMissionItem(QString itemName, QGeoCoordinate
newItem = new SurveyMissionItem(_activeVehicle, _visualItems);
newItem->setCoordinate(mapCenterCoordinate);
} else if (itemName == _fwLandingMissionItemName) {
newItem = new FixedWingLandingComplexItem(_activeVehicle, mapCenterCoordinate, _visualItems);
newItem = new FixedWingLandingComplexItem(_activeVehicle, _visualItems);
} else {
qWarning() << "Internal error: Unknown complex item:" << itemName;
return sequenceNumber;
......
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