Skip to content

Q
qgroundcontrol
Unverified Commit 828bd7ae authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

More refactoring to LandingComplexItem base class (#9057)

parent 5d3e0e5d
Hide whitespace changes
Inline Side-by-side
Showing with 155 additions and 264 deletions
src/MissionManager/FixedWingLandingComplexItem.cc
View file @ 828bd7ae
......@@ -64,18 +64,11 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(PlanMasterController* m
_editorQml = "qrc:/qml/FWLandingPatternEditor.qml";
_isIncomplete = false;
_init();
connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact);
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);
connect(&_glideSlopeFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_glideSlopeChanged);
connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_signalLastSequenceNumberChanged);
......@@ -513,125 +506,6 @@ 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 = _landingCoordinate.distanceTo(_loiterCoordinate);
if (landToLoiterDistance < radius) {
// Degnenerate case: Move tangent to loiter point
_loiterTangentCoordinate = _loiterCoordinate;
double heading = _landingCoordinate.azimuthTo(_loiterTangentCoordinate);
_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);
_loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
_ignoreRecalcSignals = true;
emit loiterCoordinateChanged(_loiterCoordinate);
emit coordinateChanged(_loiterCoordinate);
_ignoreRecalcSignals = false;
}
}
}
void FixedWingLandingComplexItem::_recalcFromHeadingAndDistanceChange(void)
{
// Fixed:
// land
// heading
// distance
// radius
// Adjusted:
// loiter
// loiter tangent
// glide slope
if (!_ignoreRecalcSignals && _landingCoordSet) {
// These are our known values
double radius = _loiterRadiusFact.rawValue().toDouble();
double landToTangentDistance = _landingDistanceFact.rawValue().toDouble();
double heading = _landingHeadingFact.rawValue().toDouble();
// Heading is from loiter to land, hence +180
_loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, heading + 180);
// Loiter coord is 90 degrees counter clockwise from tangent coord
_loiterCoordinate = _loiterTangentCoordinate.atDistanceAndAzimuth(radius, heading - 180 - 90);
_loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
_ignoreRecalcSignals = true;
emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
emit loiterCoordinateChanged(_loiterCoordinate);
emit coordinateChanged(_loiterCoordinate);
_calcGlideSlope();
_ignoreRecalcSignals = false;
}
}
void FixedWingLandingComplexItem::_recalcFromCoordinateChange(void)
{
// Fixed:
// land
// loiter
// radius
// Adjusted:
// loiter tangent
// heading
// distance
// glide slope
if (!_ignoreRecalcSignals && _landingCoordSet) {
// These are our known values
double radius = _loiterRadiusFact.rawValue().toDouble();
double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate);
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);
}
double heading = _loiterTangentCoordinate.azimuthTo(_landingCoordinate);
_ignoreRecalcSignals = true;
_landingHeadingFact.setRawValue(heading);
_landingDistanceFact.setRawValue(landToTangentDistance);
emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
_calcGlideSlope();
_ignoreRecalcSignals = false;
}
}
void FixedWingLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact(void)
{
_loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
......
src/MissionManager/FixedWingLandingComplexItem.h
View file @ 828bd7ae
......@@ -104,9 +104,6 @@ signals:
void valueSetIsDistanceChanged (bool valueSetIsDistance);
private slots:
void _recalcFromHeadingAndDistanceChange (void);
void _recalcFromCoordinateChange (void);
void _recalcFromRadiusChange (void);
void _updateLoiterCoodinateAltitudeFromFact (void);
void _updateLandingCoodinateAltitudeFromFact (void);
double _mathematicAngleToHeading (double angle);
......@@ -116,7 +113,7 @@ private slots:
void _signalLastSequenceNumberChanged (void);
private:
void _calcGlideSlope (void);
void _calcGlideSlope(void) final;
QMap<QString, FactMetaData*> _metaDataMap;
......
src/MissionManager/LandingComplexItem.cc
View file @ 828bd7ae
......@@ -25,11 +25,23 @@ LandingComplexItem::LandingComplexItem(PlanMasterController* masterController, b
{
_isIncomplete = false;
// The follow is used to compress multiple recalc calls in a row to into a single call.
// The following is used to compress multiple recalc calls in a row to into a single call.
connect(this, &LandingComplexItem::_updateFlightPathSegmentsSignal, this, &LandingComplexItem::_updateFlightPathSegmentsDontCallDirectly, Qt::QueuedConnection);
qgcApp()->addCompressedSignal(QMetaMethod::fromSignal(&LandingComplexItem::_updateFlightPathSegmentsSignal));
}
void LandingComplexItem::_init(void)
{
connect(landingDistance(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromHeadingAndDistanceChange);
connect(landingHeading(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromHeadingAndDistanceChange);
connect(loiterRadius(), &Fact::valueChanged, this, &LandingComplexItem::_recalcFromRadiusChange);
connect(this, &LandingComplexItem::loiterClockwiseChanged, this, &LandingComplexItem::_recalcFromRadiusChange);
connect(this, &LandingComplexItem::loiterCoordinateChanged, this, &LandingComplexItem::_recalcFromCoordinateChange);
connect(this, &LandingComplexItem::landingCoordinateChanged, this, &LandingComplexItem::_recalcFromCoordinateChange);
}
void LandingComplexItem::setLandingHeadingToTakeoffHeading()
{
TakeoffMissionItem* takeoffMissionItem = _missionController->takeoffMissionItem();
......@@ -104,3 +116,124 @@ QPointF LandingComplexItem::_rotatePoint(const QPointF& point, const QPointF& or
return rotated;
}
void LandingComplexItem::_recalcFromHeadingAndDistanceChange(void)
{
// Fixed:
// land
// heading
// distance
// radius
// Adjusted:
// loiter
// loiter tangent
// glide slope
if (!_ignoreRecalcSignals && _landingCoordSet) {
// These are our known values
double radius = loiterRadius()->rawValue().toDouble();
double landToTangentDistance = landingDistance()->rawValue().toDouble();
double heading = landingHeading()->rawValue().toDouble();
// Heading is from loiter to land, hence +180
_loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, heading + 180);
// Loiter coord is 90 degrees counter clockwise from tangent coord
_loiterCoordinate = _loiterTangentCoordinate.atDistanceAndAzimuth(radius, heading - 180 - 90);
_loiterCoordinate.setAltitude(loiterAltitude()->rawValue().toDouble());
_ignoreRecalcSignals = true;
emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
emit loiterCoordinateChanged(_loiterCoordinate);
emit coordinateChanged(_loiterCoordinate);
_calcGlideSlope();
_ignoreRecalcSignals = false;
}
}
void LandingComplexItem::_recalcFromRadiusChange(void)
{
// Fixed:
// land
// loiter tangent
// distance
// radius
// heading
// Adjusted:
// loiter
if (!_ignoreRecalcSignals) {
// These are our known values
double radius = loiterRadius()->rawValue().toDouble();
double landToTangentDistance = landingDistance()->rawValue().toDouble();
double heading = landingHeading()->rawValue().toDouble();
double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
if (landToLoiterDistance < radius) {
// Degnenerate case: Move tangent to loiter point
_loiterTangentCoordinate = _loiterCoordinate;
double heading = _landingCoordinate.azimuthTo(_loiterTangentCoordinate);
_ignoreRecalcSignals = true;
landingHeading()->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);
_loiterCoordinate.setAltitude(loiterAltitude()->rawValue().toDouble());
_ignoreRecalcSignals = true;
emit loiterCoordinateChanged(_loiterCoordinate);
emit coordinateChanged(_loiterCoordinate);
_ignoreRecalcSignals = false;
}
}
}
void LandingComplexItem::_recalcFromCoordinateChange(void)
{
// Fixed:
// land
// loiter
// radius
// Adjusted:
// loiter tangent
// heading
// distance
// glide slope
if (!_ignoreRecalcSignals && _landingCoordSet) {
// These are our known values
double radius = loiterRadius()->rawValue().toDouble();
double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate);
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);
}
double heading = _loiterTangentCoordinate.azimuthTo(_landingCoordinate);
_ignoreRecalcSignals = true;
landingHeading()->setRawValue(heading);
landingDistance()->setRawValue(landToTangentDistance);
emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
_calcGlideSlope();
_ignoreRecalcSignals = false;
}
}
src/MissionManager/LandingComplexItem.h
View file @ 828bd7ae
......@@ -42,6 +42,7 @@ public:
Q_INVOKABLE void setLandingHeadingToTakeoffHeading();
// These are virtual so the Facts can be created with their own specific FactGroup metadata
virtual Fact* loiterAltitude (void) = 0;
virtual Fact* loiterRadius (void) = 0;
virtual Fact* landingAltitude (void) = 0;
......@@ -70,10 +71,14 @@ signals:
void _updateFlightPathSegmentsSignal(void);
protected slots:
virtual void _recalcFromHeadingAndDistanceChange(void) = 0;
void _updateFlightPathSegmentsDontCallDirectly(void);
void _updateFlightPathSegmentsDontCallDirectly (void);
void _recalcFromHeadingAndDistanceChange (void);
void _recalcFromCoordinateChange (void);
protected:
virtual void _calcGlideSlope(void) = 0;
void _init (void);
QPointF _rotatePoint(const QPointF& point, const QPointF& origin, double angle);
int _sequenceNumber = 0;
......@@ -85,4 +90,7 @@ protected:
bool _ignoreRecalcSignals = false;
bool _loiterClockwise = true;
bool _altitudesAreRelative = true;
private slots:
void _recalcFromRadiusChange(void);
};
src/MissionManager/VTOLLandingComplexItem.cc
View file @ 828bd7ae
......@@ -55,6 +55,8 @@ VTOLLandingComplexItem::VTOLLandingComplexItem(PlanMasterController* masterContr
_editorQml = "qrc:/qml/VTOLLandingPatternEditor.qml";
_isIncomplete = false;
_init();
// We adjust landing distance meta data to Plan View settings unless there was a custom build override
if (QGC::fuzzyCompare(_landingDistanceFact.rawValue().toDouble(), _landingDistanceFact.rawDefaultValue().toDouble())) {
Fact* vtolTransitionDistanceFact = qgcApp()->toolbox()->settingsManager()->planViewSettings()->vtolTransitionDistance();
......@@ -67,15 +69,6 @@ VTOLLandingComplexItem::VTOLLandingComplexItem(PlanMasterController* masterContr
connect(&_loiterAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact);
connect(&_landingAltitudeFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_updateLandingCoodinateAltitudeFromFact);
connect(&_landingDistanceFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_recalcFromHeadingAndDistanceChange);
connect(&_landingHeadingFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_recalcFromHeadingAndDistanceChange);
connect(&_loiterRadiusFact, &Fact::valueChanged, this, &VTOLLandingComplexItem::_recalcFromRadiusChange);
connect(this, &VTOLLandingComplexItem::loiterClockwiseChanged, this, &VTOLLandingComplexItem::_recalcFromRadiusChange);
connect(this, &VTOLLandingComplexItem::loiterCoordinateChanged, this, &VTOLLandingComplexItem::_recalcFromCoordinateChange);
connect(this, &VTOLLandingComplexItem::landingCoordinateChanged, this, &VTOLLandingComplexItem::_recalcFromCoordinateChange);
connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::_amslEntryAltChanged);
connect(this, &VTOLLandingComplexItem::altitudesAreRelativeChanged, this, &VTOLLandingComplexItem::_amslExitAltChanged);
connect(_missionController, &MissionController::plannedHomePositionChanged, this, &VTOLLandingComplexItem::_amslEntryAltChanged);
......@@ -483,123 +476,6 @@ double VTOLLandingComplexItem::_headingToMathematicAngle(double heading)
return heading - 90 * -1;
}
void VTOLLandingComplexItem::_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 = _landingCoordinate.distanceTo(_loiterCoordinate);
if (landToLoiterDistance < radius) {
// Degnenerate case: Move tangent to loiter point
_loiterTangentCoordinate = _loiterCoordinate;
double heading = _landingCoordinate.azimuthTo(_loiterTangentCoordinate);
_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);
_loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
_ignoreRecalcSignals = true;
emit loiterCoordinateChanged(_loiterCoordinate);
emit coordinateChanged(_loiterCoordinate);
_ignoreRecalcSignals = false;
}
}
}
void VTOLLandingComplexItem::_recalcFromHeadingAndDistanceChange(void)
{
// Fixed:
// land
// heading
// distance
// radius
// Adjusted:
// loiter
// loiter tangent
// glide slope
if (!_ignoreRecalcSignals && _landingCoordSet) {
// These are our known values
double radius = _loiterRadiusFact.rawValue().toDouble();
double landToTangentDistance = _landingDistanceFact.rawValue().toDouble();
double heading = _landingHeadingFact.rawValue().toDouble();
// Heading is from loiter to land, hence +180
_loiterTangentCoordinate = _landingCoordinate.atDistanceAndAzimuth(landToTangentDistance, heading + 180);
// Loiter coord is 90 degrees counter clockwise from tangent coord
_loiterCoordinate = _loiterTangentCoordinate.atDistanceAndAzimuth(radius, heading - 180 - 90);
_loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
_ignoreRecalcSignals = true;
emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
emit loiterCoordinateChanged(_loiterCoordinate);
emit coordinateChanged(_loiterCoordinate);
_ignoreRecalcSignals = false;
}
}
void VTOLLandingComplexItem::_recalcFromCoordinateChange(void)
{
// Fixed:
// land
// loiter
// radius
// Adjusted:
// loiter tangent
// heading
// distance
// glide slope
if (!_ignoreRecalcSignals && _landingCoordSet) {
// These are our known values
double radius = _loiterRadiusFact.rawValue().toDouble();
double landToLoiterDistance = _landingCoordinate.distanceTo(_loiterCoordinate);
double landToLoiterHeading = _landingCoordinate.azimuthTo(_loiterCoordinate);
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);
}
double heading = _loiterTangentCoordinate.azimuthTo(_landingCoordinate);
_ignoreRecalcSignals = true;
_landingHeadingFact.setRawValue(heading);
_landingDistanceFact.setRawValue(landToTangentDistance);
emit loiterTangentCoordinateChanged(_loiterTangentCoordinate);
_ignoreRecalcSignals = false;
}
}
void VTOLLandingComplexItem::_updateLoiterCoodinateAltitudeFromFact(void)
{
_loiterCoordinate.setAltitude(_loiterAltitudeFact.rawValue().toDouble());
......@@ -644,3 +520,8 @@ double VTOLLandingComplexItem::amslExitAlt(void) const
return _landingAltitudeFact.rawValue().toDouble() + (_altitudesAreRelative ? _missionController->plannedHomePosition().altitude() : 0);
}
void VTOLLandingComplexItem::_calcGlideSlope(void)
{
// No glide slope calc for VTOL
}
src/MissionManager/VTOLLandingComplexItem.h
View file @ 828bd7ae
......@@ -91,9 +91,6 @@ public:
static const char* stopTakingVideoName;
private slots:
void _recalcFromHeadingAndDistanceChange (void) final;
void _recalcFromCoordinateChange (void);
void _recalcFromRadiusChange (void);
void _updateLoiterCoodinateAltitudeFromFact (void);
void _updateLandingCoodinateAltitudeFromFact (void);
double _mathematicAngleToHeading (double angle);
......@@ -102,6 +99,7 @@ private slots:
void _signalLastSequenceNumberChanged (void);
private:
void _calcGlideSlope(void) final;
QMap<QString, FactMetaData*> _metaDataMap;
......
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