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
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.cc
View file @ c2280df3
......@@ -40,7 +40,7 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje
, _dirty(false)
, _landingCoordSet(false)
, _ignoreRecalcSignals(false)
, _loiterToLandDistanceFact (0, _loiterToLandDistanceName, FactMetaData::valueTypeDouble)
, _landingDistanceFact (0, _loiterToLandDistanceName, FactMetaData::valueTypeDouble)
, _loiterAltitudeFact (0, _loiterAltitudeName, FactMetaData::valueTypeDouble)
, _loiterRadiusFact (0, _loiterRadiusName, FactMetaData::valueTypeDouble)
, _landingHeadingFact (0, _landingHeadingName, FactMetaData::valueTypeDouble)
......@@ -55,24 +55,29 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje
_metaDataMap = FactMetaData::createMapFromJsonFile(QStringLiteral(":/json/FWLandingPattern.FactMetaData.json"), NULL /* metaDataParent */);
}
_loiterToLandDistanceFact.setMetaData (_metaDataMap[_loiterToLandDistanceName]);
_landingDistanceFact.setMetaData (_metaDataMap[_loiterToLandDistanceName]);
_loiterAltitudeFact.setMetaData (_metaDataMap[_loiterAltitudeName]);
_loiterRadiusFact.setMetaData (_metaDataMap[_loiterRadiusName]);
_landingHeadingFact.setMetaData (_metaDataMap[_landingHeadingName]);
_landingAltitudeFact.setMetaData (_metaDataMap[_landingAltitudeName]);
_loiterToLandDistanceFact.setRawValue (_loiterToLandDistanceFact.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);
connect(&_loiterToLandDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcLoiterCoordFromFacts);
connect(&_landingHeadingFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcLoiterCoordFromFacts);
connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFactsFromCoords);
connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFactsFromCoords);
connect(&_landingAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLandingCoodinateAltitudeFromFact);
connect(&_landingDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange);
connect(&_landingHeadingFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange);
connect(&_loiterRadiusFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_recalcFromRadiusChange);
connect(this, &FixedWingLandingComplexItem::loiterClockwiseChanged, this, &FixedWingLandingComplexItem::_recalcFromRadiusChange);
connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFromCoordinateChange);
connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_recalcFromCoordinateChange);
}
int FixedWingLandingComplexItem::lastSequenceNumber(void) const
......@@ -168,7 +173,7 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq
_landingAltitudeRelative = complexObject[_jsonLandingAltitudeRelativeKey].toBool();
_landingCoordSet = true;
_recalcFactsFromCoords();
_recalcFromHeadingAndDistanceChange();
return true;
}
......@@ -253,7 +258,7 @@ void FixedWingLandingComplexItem::setLandingCoordinate(const QGeoCoordinate& coo
emit landingCoordinateChanged(coordinate);
_ignoreRecalcSignals = false;
_landingCoordSet = true;
_recalcLoiterCoordFromFacts();
_recalcFromHeadingAndDistanceChange();
emit landingCoordSetChanged(true);
}
}
......@@ -268,29 +273,81 @@ void FixedWingLandingComplexItem::setLoiterCoordinate(const QGeoCoordinate& coor
}
}
void FixedWingLandingComplexItem::_recalcLoiterCoordFromFacts(void)
double FixedWingLandingComplexItem::_mathematicAngleToHeading(double angle)
{
if (!_ignoreRecalcSignals && _landingCoordSet) {
double north, east, down;
QGeoCoordinate tangentOrigin = _landingCoordinate;
double heading = (angle - 90) * -1;
if (heading < 0) {
heading += 360;
}
return heading;
}
double FixedWingLandingComplexItem::_headingToMathematicAngle(double heading)
{
return heading - 90 * -1;
}
void FixedWingLandingComplexItem::_recalcFromRadiusChange(void)
{
// Fixed:
// land
// loiter tangent
// distance
// radius
// heading
// Adjusted:
// loiter
if (!_ignoreRecalcSignals) {
// These are our known values
double radius = _loiterRadiusFact.rawValue().toDouble();
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);
_loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToLoiterDistance, heading + 180 + angleLoiterToTangent);
_ignoreRecalcSignals = true;
emit loiterCoordinateChanged(_loiterCoordinate);
_ignoreRecalcSignals = false;
}
}
convertGeoToNed(_landingCoordinate, tangentOrigin, &north, &east, &down);
void FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange(void)
{
// Fixed:
// land
// heading
// distance
// radius
// Adjusted:
// loiter
// loiter tangent
// Heading is from loiter to land, so we need to rotate angle 180 degrees and go the opposite direction
if (!_ignoreRecalcSignals && _landingCoordSet) {
// These are our known values
double radius = _loiterRadiusFact.rawValue().toDouble();
double landToTangentDistance = _landingDistanceFact.rawValue().toDouble();
double heading = _landingHeadingFact.rawValue().toDouble();
heading += 180.0;
heading *= -1.0;
QPointF originPoint(east, north);
north += _loiterToLandDistanceFact.rawValue().toDouble();
QPointF loiterPoint(east, north);
QPointF rotatedLoiterPoint = _rotatePoint(loiterPoint, originPoint, heading);
// Calculate loiter tangent coordinate
_loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, heading + 180);
// Calculate the distance and angle to the loiter coordinate
QGeoCoordinate tangent = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, 0);
QGeoCoordinate loiter = tangent.atDistanceAndAzimuth(radius, 90);
double loiterDistance = _landingCoordinate.distanceTo(loiter);
double loiterAzimuth = _landingCoordinate.azimuthTo(loiter) * (_loiterClockwise ? -1 : 1);
convertNedToGeo(rotatedLoiterPoint.y(), rotatedLoiterPoint.x(), down, tangentOrigin, &_loiterCoordinate);
// Use those values to get the new loiter point which takes heading into acount
_loiterCoordinate = _landingCoordinate.atDistanceAndAzimuth(loiterDistance, heading + 180 + loiterAzimuth);
_ignoreRecalcSignals = true;
emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
emit loiterCoordinateChanged(_loiterCoordinate);
emit coordinateChanged(_loiterCoordinate);
_ignoreRecalcSignals = false;
}
}
......@@ -306,42 +363,34 @@ QPointF FixedWingLandingComplexItem::_rotatePoint(const QPointF& point, const QP
return rotated;
}
void FixedWingLandingComplexItem::_recalcFactsFromCoords(void)
void FixedWingLandingComplexItem::_recalcFromCoordinateChange(void)
{
if (!_ignoreRecalcSignals && _landingCoordSet) {
// Prevent signal recursion
_ignoreRecalcSignals = true;
// Calc new distance
// Fixed:
// land
// loiter
// radius
// Adjusted:
// loiter tangent
// heading
// distance
double northLand, eastLand, down;
double northLoiter, eastLoiter;
QGeoCoordinate tangentOrigin = _landingCoordinate;
convertGeoToNed(_landingCoordinate, tangentOrigin, &northLand, &eastLand, &down);
convertGeoToNed(_loiterCoordinate, tangentOrigin, &northLoiter, &eastLoiter, &down);
if (!_ignoreRecalcSignals && _landingCoordSet) {
// These are our known values
double radius = _loiterRadiusFact.rawValue().toDouble();
double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate);
double newDistance = sqrt(pow(eastLoiter - eastLand, 2.0) + pow(northLoiter - northLand, 2.0));
_loiterToLandDistanceFact.setRawValue(newDistance);
double loiterToTangentAngle = qRadiansToDegrees(qAsin(radius/landToLoiterDistance)) * (_loiterClockwise ? 1 : -1);
double landToTangentDistance = qSqrt(qPow(landToLoiterDistance, 2) - qPow(radius, 2));
// Calc new heading
_loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, landToLoiterHeading + loiterToTangentAngle);
QPointF vector(eastLand - eastLoiter, northLand - northLoiter);
double radians = atan2(vector.y(), vector.x());
double degrees = qRadiansToDegrees(radians);
// Change angle to north up = 0 degrees
degrees -= 90;
// Reverse the angle direction to go from mathematic angle (counter-clockwise) to compass heading (clockwise)
degrees *= -1.0;
// Bring with 0-360 range
if (degrees < 0.0) {
degrees += 360.0;
} else if (degrees > 360.0) {
degrees -= 360.0;
}
_landingHeadingFact.setRawValue(degrees);
double heading = _loiterTangentCoordinate.azimuthTo(_landingCoordinate);
_ignoreRecalcSignals = true;
_landingHeadingFact.setRawValue(heading);
_landingDistanceFact.setRawValue(landToTangentDistance);
emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
_ignoreRecalcSignals = false;
}
}
......
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