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 {
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
View file @ 6ab8e53e
......@@ -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
View file @ 6ab8e53e
......@@ -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);
}
src/MissionManager/FixedWingLandingComplexItem.h
View file @ 6ab8e53e
......@@ -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);
......
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