Skip to content

Q
qgroundcontrol
Commit c2280df3 authored by DonLakeFlyer's avatar DonLakeFlyer
Browse files
Options

More fun with FW landing pattern geometry 

Should be all working now except for distance < radius cases.
parent 0d0fdbf7
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 141 additions and 125 deletions
src/MissionEditor/FWLandingPatternMapVisual.qml
View file @ c2280df3
......@@ -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
......
src/MissionManager/FixedWingLandingComplexItem.h
View file @ c2280df3
......@@ -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;
......
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