Skip to content

Q
qgroundcontrol
Commit 6ab8e53e authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #4775 from DonLakeFlyer/FWFixes 

Fixed wing landing pattern fixes
parents 528ba79a 8c7177fb
Hide whitespace changes
Inline Side-by-side
Showing with 65 additions and 26 deletions
src/MissionEditor/FWLandingPatternMapVisual.qml
View file @ 6ab8e53e
...@@ -219,7 +219,7 @@ Item { ...@@ -219,7 +219,7 @@ Item {
MapCircle { MapCircle {
z: QGroundControl.zOrderMapItems z: QGroundControl.zOrderMapItems
center: _missionItem.loiterCoordinate center: _missionItem.loiterCoordinate
radius: _missionItem.loiterRadius.value radius: _missionItem.loiterRadius.rawValue
border.width: 2 border.width: 2
border.color: "green" border.color: "green"
color: "transparent" color: "transparent"
......
src/MissionManager/FWLandingPattern.FactMetaData.json
View file @ 6ab8e53e
...@@ -4,6 +4,7 @@ ...@@ -4,6 +4,7 @@
"shortDescription": "Distance between landing and loiter points.", "shortDescription": "Distance between landing and loiter points.",
"type": "double", "type": "double",
"units": "m", "units": "m",
"min": 10,
"decimalPlaces": 1, "decimalPlaces": 1,
"defaultValue": 300.0 "defaultValue": 300.0
}, },
...@@ -30,7 +31,7 @@ ...@@ -30,7 +31,7 @@
"shortDescription": "Loiter radius.", "shortDescription": "Loiter radius.",
"type": "double", "type": "double",
"decimalPlaces": 1, "decimalPlaces": 1,
"min": 0.1, "min": 1,
"units": "m", "units": "m",
"defaultValue": 75.0 "defaultValue": 75.0
}, },
......
src/MissionManager/FixedWingLandingComplexItem.cc
View file @ 6ab8e53e
...@@ -40,11 +40,11 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje ...@@ -40,11 +40,11 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje
, _dirty(false) , _dirty(false)
, _landingCoordSet(false) , _landingCoordSet(false)
, _ignoreRecalcSignals(false) , _ignoreRecalcSignals(false)
, _landingDistanceFact (0, _loiterToLandDistanceName, FactMetaData::valueTypeDouble) , _landingDistanceFact (0, _loiterToLandDistanceName, FactMetaData::valueTypeDouble)
, _loiterAltitudeFact (0, _loiterAltitudeName, FactMetaData::valueTypeDouble) , _loiterAltitudeFact (0, _loiterAltitudeName, FactMetaData::valueTypeDouble)
, _loiterRadiusFact (0, _loiterRadiusName, FactMetaData::valueTypeDouble) , _loiterRadiusFact (0, _loiterRadiusName, FactMetaData::valueTypeDouble)
, _landingHeadingFact (0, _landingHeadingName, FactMetaData::valueTypeDouble) , _landingHeadingFact (0, _landingHeadingName, FactMetaData::valueTypeDouble)
, _landingAltitudeFact (0, _landingAltitudeName, FactMetaData::valueTypeDouble) , _landingAltitudeFact (0, _landingAltitudeName, FactMetaData::valueTypeDouble)
, _loiterClockwise(true) , _loiterClockwise(true)
, _loiterAltitudeRelative(true) , _loiterAltitudeRelative(true)
, _landingAltitudeRelative(true) , _landingAltitudeRelative(true)
...@@ -55,17 +55,17 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje ...@@ -55,17 +55,17 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje
_metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/FWLandingPattern.FactMetaData.json"), NULL /* metaDataParent */); _metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/FWLandingPattern.FactMetaData.json"), NULL /* metaDataParent */);
} }
_landingDistanceFact.setMetaData (_metaDataMap[_loiterToLandDistanceName]); _landingDistanceFact.setMetaData (_metaDataMap[_loiterToLandDistanceName]);
_loiterAltitudeFact.setMetaData (_metaDataMap[_loiterAltitudeName]); _loiterAltitudeFact.setMetaData (_metaDataMap[_loiterAltitudeName]);
_loiterRadiusFact.setMetaData (_metaDataMap[_loiterRadiusName]); _loiterRadiusFact.setMetaData (_metaDataMap[_loiterRadiusName]);
_landingHeadingFact.setMetaData (_metaDataMap[_landingHeadingName]); _landingHeadingFact.setMetaData (_metaDataMap[_landingHeadingName]);
_landingAltitudeFact.setMetaData (_metaDataMap[_landingAltitudeName]); _landingAltitudeFact.setMetaData (_metaDataMap[_landingAltitudeName]);
_landingDistanceFact.setRawValue (_landingDistanceFact.rawDefaultValue()); _landingDistanceFact.setRawValue (_landingDistanceFact.rawDefaultValue());
_loiterAltitudeFact.setRawValue (_loiterAltitudeFact.rawDefaultValue()); _loiterAltitudeFact.setRawValue (_loiterAltitudeFact.rawDefaultValue());
_loiterRadiusFact.setRawValue (_loiterRadiusFact.rawDefaultValue()); _loiterRadiusFact.setRawValue (_loiterRadiusFact.rawDefaultValue());
_landingHeadingFact.setRawValue (_landingHeadingFact.rawDefaultValue()); _landingHeadingFact.setRawValue (_landingHeadingFact.rawDefaultValue());
_landingAltitudeFact.setRawValue (_landingAltitudeFact.rawDefaultValue()); _landingAltitudeFact.setRawValue (_landingAltitudeFact.rawDefaultValue());
connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact); connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact);
connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact); connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact);
...@@ -78,6 +78,17 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje ...@@ -78,6 +78,17 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje
connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFromCoordinateChange); connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFromCoordinateChange);
connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, 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 int FixedWingLandingComplexItem::lastSequenceNumber(void) const
...@@ -305,14 +316,27 @@ void FixedWingLandingComplexItem::_recalcFromRadiusChange(void) ...@@ -305,14 +316,27 @@ void FixedWingLandingComplexItem::_recalcFromRadiusChange(void)
double landToTangentDistance = _landingDistanceFact.rawValue().toDouble(); double landToTangentDistance = _landingDistanceFact.rawValue().toDouble();
double heading = _landingHeadingFact.rawValue().toDouble(); double heading = _landingHeadingFact.rawValue().toDouble();
double landToLoiterDistance = qSqrt(qPow(radius, 2) + qPow(landToTangentDistance, 2)); double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
double angleLoiterToTangent = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? -1 : 1); 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; _ignoreRecalcSignals = true;
emit loiterCoordinateChanged(_loiterCoordinate); _landingHeadingFact.setRawValue(heading);
_ignoreRecalcSignals = false; 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) ...@@ -380,10 +404,18 @@ void FixedWingLandingComplexItem::_recalcFromCoordinateChange(void)
double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate); double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate); double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate);
double loiterToTangentAngle = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? 1 : -1); double landToTangentDistance;
double landToTangentDistance = qSqrt(qPow(landToLoiterDistance, 2) - qPow(radius, 2)); 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); double heading = _loiterTangentCoordinate.azimuthTo(_landingCoordinate);
...@@ -404,3 +436,8 @@ void FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void) ...@@ -404,3 +436,8 @@ void FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact(void)
{ {
_landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble()); _landingCoordinate.setAltitude(_landingAltitudeFact.rawValue().toDouble());
} }
void FixedWingLandingComplexItem::_setDirty(void)
{
setDirty(true);
}
src/MissionManager/FixedWingLandingComplexItem.h
View file @ 6ab8e53e
...@@ -101,6 +101,7 @@ private slots: ...@@ -101,6 +101,7 @@ private slots:
void _updateLandingCoodinateAltitudeFromFact(void); void _updateLandingCoodinateAltitudeFromFact(void);
double _mathematicAngleToHeading(double angle); double _mathematicAngleToHeading(double angle);
double _headingToMathematicAngle(double heading); double _headingToMathematicAngle(double heading);
void _setDirty(void);
private: private:
QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle); QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle);
......
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