diff --git a/src/MissionManager/FWLandingPattern.FactMetaData.json b/src/MissionManager/FWLandingPattern.FactMetaData.json index 98b04c7b9ba4dea05979fde0625e6b504866b4e2..37237a956875698b71781a321c14156d6a4866ab 100644 --- a/src/MissionManager/FWLandingPattern.FactMetaData.json +++ b/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 }, diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index e36a568397eb02262d40bcbabcfbb4f66a7cc357..29631be18c8dd2f6d87e6bcdfb965afad55c5e6e 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -40,11 +40,11 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje , _dirty(false) , _landingCoordSet(false) , _ignoreRecalcSignals(false) - , _landingDistanceFact (0, _loiterToLandDistanceName, FactMetaData::valueTypeDouble) - , _loiterAltitudeFact (0, _loiterAltitudeName, FactMetaData::valueTypeDouble) - , _loiterRadiusFact (0, _loiterRadiusName, FactMetaData::valueTypeDouble) - , _landingHeadingFact (0, _landingHeadingName, FactMetaData::valueTypeDouble) - , _landingAltitudeFact (0, _landingAltitudeName, FactMetaData::valueTypeDouble) + , _landingDistanceFact (0, _loiterToLandDistanceName, FactMetaData::valueTypeDouble) + , _loiterAltitudeFact (0, _loiterAltitudeName, FactMetaData::valueTypeDouble) + , _loiterRadiusFact (0, _loiterRadiusName, FactMetaData::valueTypeDouble) + , _landingHeadingFact (0, _landingHeadingName, FactMetaData::valueTypeDouble) + , _landingAltitudeFact (0, _landingAltitudeName, FactMetaData::valueTypeDouble) , _loiterClockwise(true) , _loiterAltitudeRelative(true) , _landingAltitudeRelative(true) @@ -55,17 +55,17 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/FWLandingPattern.FactMetaData.json"), NULL /* metaDataParent */); } - _landingDistanceFact.setMetaData (_metaDataMap[_loiterToLandDistanceName]); - _loiterAltitudeFact.setMetaData (_metaDataMap[_loiterAltitudeName]); - _loiterRadiusFact.setMetaData (_metaDataMap[_loiterRadiusName]); - _landingHeadingFact.setMetaData (_metaDataMap[_landingHeadingName]); - _landingAltitudeFact.setMetaData (_metaDataMap[_landingAltitudeName]); + _landingDistanceFact.setMetaData (_metaDataMap[_loiterToLandDistanceName]); + _loiterAltitudeFact.setMetaData (_metaDataMap[_loiterAltitudeName]); + _loiterRadiusFact.setMetaData (_metaDataMap[_loiterRadiusName]); + _landingHeadingFact.setMetaData (_metaDataMap[_landingHeadingName]); + _landingAltitudeFact.setMetaData (_metaDataMap[_landingAltitudeName]); - _landingDistanceFact.setRawValue (_landingDistanceFact.rawDefaultValue()); - _loiterAltitudeFact.setRawValue (_loiterAltitudeFact.rawDefaultValue()); - _loiterRadiusFact.setRawValue (_loiterRadiusFact.rawDefaultValue()); - _landingHeadingFact.setRawValue (_landingHeadingFact.rawDefaultValue()); - _landingAltitudeFact.setRawValue (_landingAltitudeFact.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); @@ -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,14 +316,27 @@ void FixedWingLandingComplexItem::_recalcFromRadiusChange(void) 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); + double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate); + if (landToLoiterDistance < radius) { + // Degnenerate case: Move tangent to loiter point + _loiterTangentCoordinate = _loiterCoordinate; - _loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToLoiterDistance, heading + 180 + angleLoiterToTangent); + double heading = _landingCoordinate.azimuthTo(_loiterTangentCoordinate); - _ignoreRecalcSignals = true; - emit loiterCoordinateChanged(_loiterCoordinate); - _ignoreRecalcSignals = false; + _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); + + _loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToLoiterDistance, heading + 180 + angleLoiterToTangent); + + _ignoreRecalcSignals = true; + emit loiterCoordinateChanged(_loiterCoordinate); + _ignoreRecalcSignals = false; + } } } @@ -380,10 +404,18 @@ void FixedWingLandingComplexItem::_recalcFromCoordinateChange(void) double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate); double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate); - double loiterToTangentAngle = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? 1 : -1); - double landToTangentDistance = qSqrt(qPow(landToLoiterDistance, 2) - qPow(radius, 2)); + 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); + landToTangentDistance = qSqrt(qPow(landToLoiterDistance, 2) - qPow(radius, 2)); - _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, landToLoiterHeading + loiterToTangentAngle); + _loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, landToLoiterHeading + loiterToTangentAngle); + + } double heading = _loiterTangentCoordinate.azimuthTo(_landingCoordinate); @@ -404,3 +436,8 @@ void FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void) { _landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble()); } + +void FixedWingLandingComplexItem::_setDirty(void) +{ + setDirty(true); +} diff --git a/src/MissionManager/FixedWingLandingComplexItem.h b/src/MissionManager/FixedWingLandingComplexItem.h index ed9ae9a13b451d607b957514c0ea9fb3e0fafe3d..0afc1e96e053765b0591957a13f4151854607941 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.h +++ b/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);