From c2280df3484d2e1cea5f89da4d7df019feda09fe Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Tue, 14 Mar 2017 16:25:54 -0700 Subject: [PATCH] More fun with FW landing pattern geometry Should be all working now except for distance < radius cases. --- .../FWLandingPatternMapVisual.qml | 52 +----- .../FixedWingLandingComplexItem.cc | 157 ++++++++++++------ .../FixedWingLandingComplexItem.h | 57 ++++--- 3 files changed, 141 insertions(+), 125 deletions(-) diff --git a/src/MissionEditor/FWLandingPatternMapVisual.qml b/src/MissionEditor/FWLandingPatternMapVisual.qml index 4fc54b71a..87e2f8d39 100644 --- a/src/MissionEditor/FWLandingPatternMapVisual.qml +++ b/src/MissionEditor/FWLandingPatternMapVisual.qml @@ -26,7 +26,6 @@ Item { property var _itemVisuals: [ ] property var _mouseArea property var _dragAreas: [ ] - property var _loiterTangentCoordinate property var _flightPath readonly property int _flightPathIndex: 0 @@ -86,46 +85,8 @@ Item { } } - function radiansToDegrees(radians) { - return radians * (180.0 / Math.PI) - } - - function calcPointTangentToCircleWithCenter() { - if (_missionItem.landingCoordSet) { - var radius = _missionItem.loiterRadius.value - var loiterPointPixels = map.fromCoordinate(_missionItem.loiterCoordinate, false /* clipToViewport */) - var landPointPixels = map.fromCoordinate(_missionItem.landingCoordinate, false /* clipToViewport */) - - var dxHypotenuse = loiterPointPixels.x - landPointPixels.x - var dyHypotenuse = loiterPointPixels.y - landPointPixels.y - var oppositeLength = radius - var hypotenuseLength = _missionItem.landingCoordinate.distanceTo(_missionItem.loiterCoordinate) - var adjacentLength = Math.sqrt(Math.pow(hypotenuseLength, 2) - Math.pow(oppositeLength, 2)) - var angleToCenterRadians = -Math.atan2(dyHypotenuse, dxHypotenuse) - var angleCenterToTangentRadians = Math.asin(oppositeLength / hypotenuseLength) - var angleToTangentRadians - if (_missionItem.loiterClockwise) { - angleToTangentRadians = angleToCenterRadians - angleCenterToTangentRadians - } else { - angleToTangentRadians = angleToCenterRadians + angleCenterToTangentRadians - } - var angleToTangentDegrees = (radiansToDegrees(angleToTangentRadians) - 90) * -1 - /* - Keep in for debugging for now - console.log("dxHypotenuse", dxHypotenuse) - console.log("dyHypotenuse", dyHypotenuse) - console.log("oppositeLength", oppositeLength) - console.log("hypotenuseLength", hypotenuseLength) - console.log("adjacentLength", adjacentLength) - console.log("angleCenterToTangentRadians", angleCenterToTangentRadians, radiansToDegrees(angleCenterToTangentRadians)) - console.log("angleToCenterRadians", angleToCenterRadians, radiansToDegrees(angleToCenterRadians)) - console.log("angleToTangentDegrees", angleToTangentDegrees) - */ - _loiterTangentCoordinate = _missionItem.landingCoordinate.atDistanceAndAzimuth(adjacentLength, angleToTangentDegrees) - _flightPath = [ _loiterTangentCoordinate, _missionItem.landingCoordinate ] - } else { - _flightPath = undefined - } + function _setFlightPath() { + _flightPath = [ _missionItem.loiterTangentCoordinate, _missionItem.landingCoordinate ] } Component.onCompleted: { @@ -134,10 +95,10 @@ Item { if (_missionItem.isCurrentItem) { showDragAreas() } + _setFlightPath() } else if (_missionItem.isCurrentItem) { showMouseArea() } - calcPointTangentToCircleWithCenter() } Component.onDestruction: { @@ -167,16 +128,15 @@ Item { hideMouseArea() showItemVisuals() showDragAreas() + _setFlightPath() } else if (_missionItem.isCurrentItem) { hideDragAreas() showMouseArea() } - calcPointTangentToCircleWithCenter() } - onLandingCoordinateChanged: calcPointTangentToCircleWithCenter() - onLoiterCoordinateChanged: calcPointTangentToCircleWithCenter() - onLoiterClockwiseChanged: calcPointTangentToCircleWithCenter() + onLandingCoordinateChanged: _setFlightPath() + onLoiterTangentCoordinateChanged: _setFlightPath() } // Mouse area to capture landing point coordindate diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index 68b08916a..e36a56839 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -40,7 +40,7 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje , _dirty(false) , _landingCoordSet(false) , _ignoreRecalcSignals(false) - , _loiterToLandDistanceFact (0, _loiterToLandDistanceName, FactMetaData::valueTypeDouble) + , _landingDistanceFact (0, _loiterToLandDistanceName, FactMetaData::valueTypeDouble) , _loiterAltitudeFact (0, _loiterAltitudeName, FactMetaData::valueTypeDouble) , _loiterRadiusFact (0, _loiterRadiusName, FactMetaData::valueTypeDouble) , _landingHeadingFact (0, _landingHeadingName, FactMetaData::valueTypeDouble) @@ -55,24 +55,29 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/FWLandingPattern.FactMetaData.json"), NULL /* metaDataParent */); } - _loiterToLandDistanceFact.setMetaData (_metaDataMap[_loiterToLandDistanceName]); + _landingDistanceFact.setMetaData (_metaDataMap[_loiterToLandDistanceName]); _loiterAltitudeFact.setMetaData (_metaDataMap[_loiterAltitudeName]); _loiterRadiusFact.setMetaData (_metaDataMap[_loiterRadiusName]); _landingHeadingFact.setMetaData (_metaDataMap[_landingHeadingName]); _landingAltitudeFact.setMetaData (_metaDataMap[_landingAltitudeName]); - _loiterToLandDistanceFact.setRawValue (_loiterToLandDistanceFact.rawDefaultValue()); + _landingDistanceFact.setRawValue (_landingDistanceFact.rawDefaultValue()); _loiterAltitudeFact.setRawValue (_loiterAltitudeFact.rawDefaultValue()); _loiterRadiusFact.setRawValue (_loiterRadiusFact.rawDefaultValue()); _landingHeadingFact.setRawValue (_landingHeadingFact.rawDefaultValue()); _landingAltitudeFact.setRawValue (_landingAltitudeFact.rawDefaultValue()); connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact); - connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact); - 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); + connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact); + + connect(&_landingDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange); + connect(&_landingHeadingFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange); + + connect(&_loiterRadiusFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcFromRadiusChange); + connect(this, &FixedWingLandingComplexItem::loiterClockwiseChanged, this, &FixedWingLandingComplexItem::_recalcFromRadiusChange); + + connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFromCoordinateChange); + connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFromCoordinateChange); } int FixedWingLandingComplexItem::lastSequenceNumber(void) const @@ -168,7 +173,7 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq _landingAltitudeRelative = complexObject[_jsonLandingAltitudeRelativeKey].toBool(); _landingCoordSet = true; - _recalcFactsFromCoords(); + _recalcFromHeadingAndDistanceChange(); return true; } @@ -253,7 +258,7 @@ void FixedWingLandingComplexItem::setLandingCoordinate(const QGeoCoordinate& coo emit landingCoordinateChanged(coordinate); _ignoreRecalcSignals = false; _landingCoordSet = true; - _recalcLoiterCoordFromFacts(); + _recalcFromHeadingAndDistanceChange(); emit landingCoordSetChanged(true); } } @@ -268,29 +273,81 @@ void FixedWingLandingComplexItem::setLoiterCoordinate(const QGeoCoordinate& coor } } -void FixedWingLandingComplexItem::_recalcLoiterCoordFromFacts(void) +double FixedWingLandingComplexItem::_mathematicAngleToHeading(double angle) { - if (!_ignoreRecalcSignals && _landingCoordSet) { - double north, east, down; - QGeoCoordinate tangentOrigin = _landingCoordinate; + double heading = (angle - 90) * -1; + if (heading < 0) { + heading += 360; + } + + return heading; +} + +double FixedWingLandingComplexItem::_headingToMathematicAngle(double heading) +{ + return heading - 90 * -1; +} + +void FixedWingLandingComplexItem::_recalcFromRadiusChange(void) +{ + // Fixed: + // land + // loiter tangent + // distance + // radius + // heading + // Adjusted: + // loiter + + if (!_ignoreRecalcSignals) { + // These are our known values + double radius = _loiterRadiusFact.rawValue().toDouble(); + double landToTangentDistance = _landingDistanceFact.rawValue().toDouble(); + double heading = _landingHeadingFact.rawValue().toDouble(); + + double landToLoiterDistance = qSqrt(qPow(radius, 2) + qPow(landToTangentDistance, 2)); + double angleLoiterToTangent = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? -1 : 1); + + _loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToLoiterDistance, heading + 180 + angleLoiterToTangent); + + _ignoreRecalcSignals = true; + emit loiterCoordinateChanged(_loiterCoordinate); + _ignoreRecalcSignals = false; + } +} - convertGeoToNed(_landingCoordinate, tangentOrigin, &north, &east, &down); +void FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange(void) +{ + // Fixed: + // land + // heading + // distance + // radius + // Adjusted: + // loiter + // loiter tangent - // Heading is from loiter to land, so we need to rotate angle 180 degrees and go the opposite direction + if (!_ignoreRecalcSignals && _landingCoordSet) { + // These are our known values + double radius = _loiterRadiusFact.rawValue().toDouble(); + double landToTangentDistance = _landingDistanceFact.rawValue().toDouble(); double heading = _landingHeadingFact.rawValue().toDouble(); - heading += 180.0; - heading *= -1.0; - QPointF originPoint(east, north); - north += _loiterToLandDistanceFact.rawValue().toDouble(); - QPointF loiterPoint(east, north); - QPointF rotatedLoiterPoint = _rotatePoint(loiterPoint, originPoint, heading); + // Calculate loiter tangent coordinate + _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, heading + 180); + + // Calculate the distance and angle to the loiter coordinate + QGeoCoordinate tangent = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, 0); + QGeoCoordinate loiter = tangent.atDistanceAndAzimuth(radius, 90); + double loiterDistance = _landingCoordinate.distanceTo(loiter); + double loiterAzimuth = _landingCoordinate.azimuthTo(loiter) * (_loiterClockwise ? -1 : 1); - convertNedToGeo(rotatedLoiterPoint.y(), rotatedLoiterPoint.x(), down, tangentOrigin, &_loiterCoordinate); + // Use those values to get the new loiter point which takes heading into acount + _loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(loiterDistance, heading + 180 + loiterAzimuth); _ignoreRecalcSignals = true; + emit loiterTangentCoordinateChanged(_loiterTangentCoordinate); emit loiterCoordinateChanged(_loiterCoordinate); - emit coordinateChanged(_loiterCoordinate); _ignoreRecalcSignals = false; } } @@ -306,42 +363,34 @@ QPointF FixedWingLandingComplexItem::_rotatePoint(const QPointF& point, const QP return rotated; } -void FixedWingLandingComplexItem::_recalcFactsFromCoords(void) +void FixedWingLandingComplexItem::_recalcFromCoordinateChange(void) { - if (!_ignoreRecalcSignals && _landingCoordSet) { - - // Prevent signal recursion - _ignoreRecalcSignals = true; - - // Calc new distance + // Fixed: + // land + // loiter + // radius + // Adjusted: + // loiter tangent + // heading + // distance - double northLand, eastLand, down; - double northLoiter, eastLoiter; - QGeoCoordinate tangentOrigin = _landingCoordinate; - - convertGeoToNed(_landingCoordinate, tangentOrigin, &northLand, &eastLand, &down); - convertGeoToNed(_loiterCoordinate, tangentOrigin, &northLoiter, &eastLoiter, &down); + if (!_ignoreRecalcSignals && _landingCoordSet) { + // These are our known values + double radius = _loiterRadiusFact.rawValue().toDouble(); + double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate); + double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate); - double newDistance = sqrt(pow(eastLoiter - eastLand, 2.0) + pow(northLoiter - northLand, 2.0)); - _loiterToLandDistanceFact.setRawValue(newDistance); + double loiterToTangentAngle = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? 1 : -1); + double landToTangentDistance = qSqrt(qPow(landToLoiterDistance, 2) - qPow(radius, 2)); - // Calc new heading + _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, landToLoiterHeading + loiterToTangentAngle); - QPointF vector(eastLand - eastLoiter, northLand - northLoiter); - double radians = atan2(vector.y(), vector.x()); - double degrees = qRadiansToDegrees(radians); - // Change angle to north up = 0 degrees - degrees -= 90; - // Reverse the angle direction to go from mathematic angle (counter-clockwise) to compass heading (clockwise) - degrees *= -1.0; - // Bring with 0-360 range - if (degrees < 0.0) { - degrees += 360.0; - } else if (degrees > 360.0) { - degrees -= 360.0; - } - _landingHeadingFact.setRawValue(degrees); + double heading = _loiterTangentCoordinate.azimuthTo(_landingCoordinate); + _ignoreRecalcSignals = true; + _landingHeadingFact.setRawValue(heading); + _landingDistanceFact.setRawValue(landToTangentDistance); + emit loiterTangentCoordinateChanged(_loiterTangentCoordinate); _ignoreRecalcSignals = false; } } diff --git a/src/MissionManager/FixedWingLandingComplexItem.h b/src/MissionManager/FixedWingLandingComplexItem.h index e20532e9d..ed9ae9a13 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.h +++ b/src/MissionManager/FixedWingLandingComplexItem.h @@ -24,28 +24,30 @@ class FixedWingLandingComplexItem : public ComplexMissionItem public: FixedWingLandingComplexItem(Vehicle* vehicle, QObject* parent = NULL); - Q_PROPERTY(Fact* loiterAltitude READ loiterAltitude CONSTANT) - Q_PROPERTY(Fact* loiterRadius READ loiterRadius CONSTANT) - Q_PROPERTY(Fact* landingAltitude READ landingAltitude CONSTANT) - Q_PROPERTY(Fact* landingDistance READ landingDistance CONSTANT) - Q_PROPERTY(Fact* landingHeading READ landingHeading CONSTANT) - Q_PROPERTY(bool loiterClockwise MEMBER _loiterClockwise NOTIFY loiterClockwiseChanged) - Q_PROPERTY(bool loiterAltitudeRelative MEMBER _loiterAltitudeRelative NOTIFY loiterAltitudeRelativeChanged) - Q_PROPERTY(bool landingAltitudeRelative MEMBER _landingAltitudeRelative NOTIFY landingAltitudeRelativeChanged) - 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* loiterAltitude (void) { return &_loiterAltitudeFact; } - Fact* loiterRadius (void) { return &_loiterRadiusFact; } - Fact* landingAltitude (void) { return &_landingAltitudeFact; } - Fact* landingDistance (void) { return &_loiterToLandDistanceFact; } - Fact* landingHeading (void) { return &_landingHeadingFact; } - QGeoCoordinate landingCoordinate (void) const { return _landingCoordinate; } - QGeoCoordinate loiterCoordinate (void) const { return _loiterCoordinate; } - - void setLandingCoordinate (const QGeoCoordinate& coordinate); - void setLoiterCoordinate (const QGeoCoordinate& coordinate); + Q_PROPERTY(Fact* loiterAltitude READ loiterAltitude CONSTANT) + Q_PROPERTY(Fact* loiterRadius READ loiterRadius CONSTANT) + Q_PROPERTY(Fact* landingAltitude READ landingAltitude CONSTANT) + Q_PROPERTY(Fact* landingDistance READ landingDistance CONSTANT) + Q_PROPERTY(Fact* landingHeading READ landingHeading CONSTANT) + Q_PROPERTY(bool loiterClockwise MEMBER _loiterClockwise NOTIFY loiterClockwiseChanged) + Q_PROPERTY(bool loiterAltitudeRelative MEMBER _loiterAltitudeRelative NOTIFY loiterAltitudeRelativeChanged) + Q_PROPERTY(bool landingAltitudeRelative MEMBER _landingAltitudeRelative NOTIFY landingAltitudeRelativeChanged) + Q_PROPERTY(QGeoCoordinate loiterCoordinate READ loiterCoordinate WRITE setLoiterCoordinate NOTIFY loiterCoordinateChanged) + Q_PROPERTY(QGeoCoordinate loiterTangentCoordinate READ loiterTangentCoordinate NOTIFY loiterTangentCoordinateChanged) + Q_PROPERTY(QGeoCoordinate landingCoordinate READ landingCoordinate WRITE setLandingCoordinate NOTIFY landingCoordinateChanged) + Q_PROPERTY(bool landingCoordSet MEMBER _landingCoordSet NOTIFY landingCoordSetChanged) + + Fact* loiterAltitude (void) { return &_loiterAltitudeFact; } + Fact* loiterRadius (void) { return &_loiterRadiusFact; } + Fact* landingAltitude (void) { return &_landingAltitudeFact; } + Fact* landingDistance (void) { return &_landingDistanceFact; } + Fact* landingHeading (void) { return &_landingHeadingFact; } + QGeoCoordinate landingCoordinate (void) const { return _landingCoordinate; } + QGeoCoordinate loiterCoordinate (void) const { return _loiterCoordinate; } + QGeoCoordinate loiterTangentCoordinate (void) const { return _loiterTangentCoordinate; } + + void setLandingCoordinate (const QGeoCoordinate& coordinate); + void setLoiterCoordinate (const QGeoCoordinate& coordinate); // Overrides from ComplexMissionItem @@ -84,6 +86,7 @@ public: signals: void loiterCoordinateChanged (QGeoCoordinate coordinate); + void loiterTangentCoordinateChanged (QGeoCoordinate coordinate); void landingCoordinateChanged (QGeoCoordinate coordinate); void landingCoordSetChanged (bool landingCoordSet); void loiterClockwiseChanged (bool loiterClockwise); @@ -91,10 +94,13 @@ signals: void landingAltitudeRelativeChanged (bool loiterAltitudeRelative); private slots: - void _recalcLoiterCoordFromFacts(void); - void _recalcFactsFromCoords(void); + void _recalcFromHeadingAndDistanceChange(void); + void _recalcFromCoordinateChange(void); + void _recalcFromRadiusChange(void); void _updateLoiterCoodinateAltitudeFromFact(void); void _updateLandingCoodinateAltitudeFromFact(void); + double _mathematicAngleToHeading(double angle); + double _headingToMathematicAngle(double heading); private: QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle); @@ -102,11 +108,12 @@ private: int _sequenceNumber; bool _dirty; QGeoCoordinate _loiterCoordinate; + QGeoCoordinate _loiterTangentCoordinate; QGeoCoordinate _landingCoordinate; bool _landingCoordSet; bool _ignoreRecalcSignals; - Fact _loiterToLandDistanceFact; + Fact _landingDistanceFact; Fact _loiterAltitudeFact; Fact _loiterRadiusFact; Fact _landingHeadingFact; -- 2.22.0