zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

More fun with FW landing pattern geometry 

Should be all working now except for distance < radius cases.
QGC4.4
DonLakeFlyer 8 years ago
parent
0d0fdbf745
commit
c2280df348
3 changed files with 141 additions and 125 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 52
      src/MissionEditor/FWLandingPatternMapVisual.qml
  2. 157
      src/MissionManager/FixedWingLandingComplexItem.cc
  3. 57
      src/MissionManager/FixedWingLandingComplexItem.h

52
src/MissionEditor/FWLandingPatternMapVisual.qml
Unescape View File

@ -26,7 +26,6 @@ Item { @@ -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 { @@ -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 { @@ -134,10 +95,10 @@ Item {
if (_missionItem.isCurrentItem) {
showDragAreas()
}
_setFlightPath()
} else if (_missionItem.isCurrentItem) {
showMouseArea()
}
calcPointTangentToCircleWithCenter()
}
Component.onDestruction: {
@ -167,16 +128,15 @@ Item { @@ -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

157
src/MissionManager/FixedWingLandingComplexItem.cc
Unescape View File

@ -40,7 +40,7 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, QObje @@ -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 @@ -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 @@ -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 @@ -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 @@ -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 @@ -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;
}
}

57
src/MissionManager/FixedWingLandingComplexItem.h
Unescape View File

@ -24,28 +24,30 @@ class FixedWingLandingComplexItem : public ComplexMissionItem @@ -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: @@ -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: @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save