Merge pull request #4775 from DonLakeFlyer/FWFixes

Fixed wing landing pattern fixes

src/MissionEditor/FWLandingPatternMapVisual.qml

@@ -219,7 +219,7 @@ Item {
         MapCircle {
             z:              QGroundControl.zOrderMapItems
             center:         _missionItem.loiterCoordinate
-            radius:         _missionItem.loiterRadius.value
+            radius:         _missionItem.loiterRadius.rawValue
             border.width:   2
             border.color:   "green"
             color:          "transparent"

src/MissionManager/FWLandingPattern.FactMetaData.json

@@ -4,6 +4,7 @@
     "shortDescription": "Distance between landing and loiter points.",
     "type":             "double",
     "units":            "m",
+    "min":              10,
     "decimalPlaces":    1,
     "defaultValue":     300.0
 },
@@ -30,7 +31,7 @@
     "shortDescription": "Loiter radius.",
     "type":             "double",
     "decimalPlaces":    1,
-    "min":              0.1,
+    "min":              1,
     "units":            "m",
     "defaultValue":     75.0
 },

src/MissionManager/FixedWingLandingComplexItem.cc

@@ -78,6 +78,17 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje
 
     connect(this,                       &FixedWingLandingComplexItem::loiterCoordinateChanged,  this, &FixedWingLandingComplexItem::_recalcFromCoordinateChange);
     connect(this,                       &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFromCoordinateChange);
+
+    connect(&_loiterAltitudeFact,       &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
+    connect(&_landingAltitudeFact,      &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
+    connect(&_landingDistanceFact,      &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
+    connect(&_landingHeadingFact,       &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
+    connect(&_loiterRadiusFact,         &Fact::valueChanged,                                            this, &FixedWingLandingComplexItem::_setDirty);
+    connect(this,                       &FixedWingLandingComplexItem::loiterCoordinateChanged,          this, &FixedWingLandingComplexItem::_setDirty);
+    connect(this,                       &FixedWingLandingComplexItem::landingCoordinateChanged,         this, &FixedWingLandingComplexItem::_setDirty);
+    connect(this,                       &FixedWingLandingComplexItem::loiterClockwiseChanged,           this, &FixedWingLandingComplexItem::_setDirty);
+    connect(this,                       &FixedWingLandingComplexItem::loiterAltitudeRelativeChanged,    this, &FixedWingLandingComplexItem::_setDirty);
+    connect(this,                       &FixedWingLandingComplexItem::landingAltitudeRelativeChanged,   this, &FixedWingLandingComplexItem::_setDirty);
 }
 
 int FixedWingLandingComplexItem::lastSequenceNumber(void) const
@@ -305,6 +316,18 @@ void FixedWingLandingComplexItem::_recalcFromRadiusChange(void)
         double landToTangentDistance = _landingDistanceFact.rawValue().toDouble();
         double heading = _landingHeadingFact.rawValue().toDouble();
 
+        double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
+        if (landToLoiterDistance < radius) {
+            // Degnenerate case: Move tangent to loiter point
+            _loiterTangentCoordinate = _loiterCoordinate;
+
+            double heading = _landingCoordinate.azimuthTo(_loiterTangentCoordinate);
+
+            _ignoreRecalcSignals = true;
+            _landingHeadingFact.setRawValue(heading);
+            emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
+            _ignoreRecalcSignals = false;
+        } else {
             double landToLoiterDistance = qSqrt(qPow(radius, 2) + qPow(landToTangentDistance, 2));
             double angleLoiterToTangent = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? -1 : 1);
 
@@ -314,6 +337,7 @@ void FixedWingLandingComplexItem::_recalcFromRadiusChange(void)
             emit loiterCoordinateChanged(_loiterCoordinate);
             _ignoreRecalcSignals = false;
         }
+    }
 }
 
 void FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange(void)
@@ -380,11 +404,19 @@ void FixedWingLandingComplexItem::_recalcFromCoordinateChange(void)
         double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
         double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate);
 
+        double landToTangentDistance;
+        if (landToLoiterDistance < radius) {
+            // Degenerate case, set tangent to loiter coordinate
+            _loiterTangentCoordinate = _loiterCoordinate;
+            landToTangentDistance = _landingCoordinate.distanceTo(_loiterTangentCoordinate);
+        } else {
             double loiterToTangentAngle = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? 1 : -1);
-        double landToTangentDistance = qSqrt(qPow(landToLoiterDistance, 2) - qPow(radius, 2));
+            landToTangentDistance = qSqrt(qPow(landToLoiterDistance, 2) - qPow(radius, 2));
 
             _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, landToLoiterHeading + loiterToTangentAngle);
 
+        }
+
         double heading = _loiterTangentCoordinate.azimuthTo(_landingCoordinate);
 
         _ignoreRecalcSignals = true;
@@ -404,3 +436,8 @@ void FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void)
 {
     _landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble());
 }
+
+void FixedWingLandingComplexItem::_setDirty(void)
+{
+    setDirty(true);
+}

src/MissionManager/FixedWingLandingComplexItem.h

@@ -101,6 +101,7 @@ private slots:
     void _updateLandingCoodinateAltitudeFromFact(void);
     double _mathematicAngleToHeading(double angle);
     double _headingToMathematicAngle(double heading);
+    void _setDirty(void);
 
 private:
     QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle);