Commit 8563f08b authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #3863 from birchera/compute_hover_cruise_distance

[Mission stats] Compute distance & time
parents ea8fa1b2 c10431fe
...@@ -694,6 +694,10 @@ QGCView { ...@@ -694,6 +694,10 @@ QGCView {
currentMissionItem: _currentMissionItem currentMissionItem: _currentMissionItem
missionItems: controller.visualItems missionItems: controller.visualItems
expandedWidth: missionItemEditor.x - (ScreenTools.defaultFontPixelWidth * 2) expandedWidth: missionItemEditor.x - (ScreenTools.defaultFontPixelWidth * 2)
missionDistance: controller.missionDistance
missionMaxTelemetry: controller.missionMaxTelemetry
cruiseDistance: controller.cruiseDistance
hoverDistance: controller.hoverDistance
visible: !ScreenTools.isShortScreen visible: !ScreenTools.isShortScreen
} }
} // FlightMap } // FlightMap
......
...@@ -15,13 +15,23 @@ import QGroundControl.ScreenTools 1.0 ...@@ -15,13 +15,23 @@ import QGroundControl.ScreenTools 1.0
import QGroundControl.Controls 1.0 import QGroundControl.Controls 1.0
import QGroundControl.Palette 1.0 import QGroundControl.Palette 1.0
import QGroundControl 1.0 import QGroundControl 1.0
import QGroundControl.FactSystem 1.0
import QGroundControl.FactControls 1.0
Rectangle { Rectangle {
readonly property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle readonly property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
property Fact _offlineEditingVehicleType: QGroundControl.offlineEditingVehicleType
property Fact _offlineEditingCruiseSpeed: QGroundControl.offlineEditingCruiseSpeed
property Fact _offlineEditingHoverSpeed: QGroundControl.offlineEditingHoverSpeed
property var currentMissionItem ///< Mission item to display status for property var currentMissionItem ///< Mission item to display status for
property var missionItems ///< List of all available mission items property var missionItems ///< List of all available mission items
property real expandedWidth ///< Width of control when expanded property real expandedWidth ///< Width of control when expanded
property real missionDistance
property real missionMaxTelemetry
property real cruiseDistance
property real hoverDistance
width: _expanded ? expandedWidth : _collapsedWidth width: _expanded ? expandedWidth : _collapsedWidth
height: Math.max(valueGrid.height, valueMissionGrid.height) + (_margins * 2) height: Math.max(valueGrid.height, valueMissionGrid.height) + (_margins * 2)
...@@ -37,17 +47,25 @@ Rectangle { ...@@ -37,17 +47,25 @@ Rectangle {
property real _distance: _statusValid ? _currentMissionItem.distance : 0 property real _distance: _statusValid ? _currentMissionItem.distance : 0
property real _altDifference: _statusValid ? _currentMissionItem.altDifference : 0 property real _altDifference: _statusValid ? _currentMissionItem.altDifference : 0
property real _gradient: _statusValid || _currentMissionItem.distance == 0 ? Math.atan(_currentMissionItem.altDifference / _currentMissionItem.distance) : 0 property real _gradient: _statusValid && _currentMissionItem.distance > 0 ? Math.atan(_currentMissionItem.altDifference / _currentMissionItem.distance) : 0
property real _gradientPercent: isNaN(_gradient) ? 0 : _gradient * 100 property real _gradientPercent: isNaN(_gradient) ? 0 : _gradient * 100
property real _azimuth: _statusValid ? _currentMissionItem.azimuth : -1 property real _azimuth: _statusValid ? _currentMissionItem.azimuth : -1
property int _numberShots: _currentSurvey ? _currentMissionItem.cameraShots : 0 property int _numberShots: _currentSurvey ? _currentMissionItem.cameraShots : 0
property real _coveredArea: _currentSurvey ? _currentMissionItem.coveredArea : 0 property real _coveredArea: _currentSurvey ? _currentMissionItem.coveredArea : 0
property real _missionDistance: _missionValid ? missionDistance : 0
property real _missionMaxTelemetry: _missionValid ? missionMaxTelemetry : 0
property real _missionTime: _missionValid && _missionSpeed > 0 ? (_isVTOL ? _hoverTime + _cruiseTime : _missionDistance / _missionSpeed) : 0
property real _hoverDistance: _missionValid ? hoverDistance : 0
property real _cruiseDistance: _missionValid ? cruiseDistance : 0
property real _hoverTime: _missionValid && _offlineEditingHoverSpeed.value > 0 ? _hoverDistance / _offlineEditingHoverSpeed.value : 0
property real _cruiseTime: _missionValid && _offlineEditingCruiseSpeed.value > 0 ? _cruiseDistance / _offlineEditingCruiseSpeed.value : 0
property bool _statusValid: currentMissionItem != undefined property bool _statusValid: currentMissionItem != undefined
property bool _vehicleValid: _activeVehicle != undefined property bool _vehicleValid: _activeVehicle != undefined
property bool _missionValid: missionItems != undefined property bool _missionValid: missionItems != undefined
property bool _currentSurvey: _statusValid ? currentMissionItem.commandName == "Survey" : false property bool _currentSurvey: _statusValid ? _currentMissionItem.commandName == "Survey" : false
property bool _isVTOL: _vehicleValid ? _activeVehicle.vtol : false property bool _isVTOL: _vehicleValid ? _activeVehicle.vtol : _offlineEditingVehicleType.enumStringValue == "VTOL" //hardcoded
property real _missionSpeed: _offlineEditingVehicleType.enumStringValue == "Fixedwing" ? _offlineEditingCruiseSpeed.value : _offlineEditingHoverSpeed.value
property string _distanceText: _statusValid ? QGroundControl.metersToAppSettingsDistanceUnits(_distance).toFixed(2) + " " + QGroundControl.appSettingsDistanceUnitsString : " " property string _distanceText: _statusValid ? QGroundControl.metersToAppSettingsDistanceUnits(_distance).toFixed(2) + " " + QGroundControl.appSettingsDistanceUnitsString : " "
property string _altText: _statusValid ? QGroundControl.metersToAppSettingsDistanceUnits(_altDifference).toFixed(2) + " " + QGroundControl.appSettingsDistanceUnitsString : " " property string _altText: _statusValid ? QGroundControl.metersToAppSettingsDistanceUnits(_altDifference).toFixed(2) + " " + QGroundControl.appSettingsDistanceUnitsString : " "
...@@ -55,13 +73,13 @@ Rectangle { ...@@ -55,13 +73,13 @@ Rectangle {
property string _azimuthText: _statusValid ? Math.round(_azimuth) : " " property string _azimuthText: _statusValid ? Math.round(_azimuth) : " "
property string _numberShotsText: _currentSurvey ? _numberShots.toFixed(0) : " " property string _numberShotsText: _currentSurvey ? _numberShots.toFixed(0) : " "
property string _coveredAreaText: _currentSurvey ? QGroundControl.squareMetersToAppSettingsAreaUnits(_coveredArea).toFixed(2) + " " + QGroundControl.appSettingsAreaUnitsString : " " property string _coveredAreaText: _currentSurvey ? QGroundControl.squareMetersToAppSettingsAreaUnits(_coveredArea).toFixed(2) + " " + QGroundControl.appSettingsAreaUnitsString : " "
property string _totalDistanceText: _missionValid ? "30.91" + " " + QGroundControl.appSettingsDistanceUnitsString : " " property string _missionDistanceText: _missionValid ? QGroundControl.metersToAppSettingsDistanceUnits(_missionDistance).toFixed(2) + " " + QGroundControl.appSettingsDistanceUnitsString : " "
property string _totalTimeText: _missionValid ? "34min 23s" : " " property string _missionTimeText: _missionValid ? _missionTime.toFixed(0) + "s" : " "
property string _maxTelemDistText: _missionValid ? "5.23" + " " + QGroundControl.appSettingsDistanceUnitsString : " " property string _missionMaxTelemetryText: _missionValid ? QGroundControl.metersToAppSettingsDistanceUnits(_missionMaxTelemetry).toFixed(2) + " " + QGroundControl.appSettingsDistanceUnitsString : " "
property string _hoverDistanceText: _missionValid ? "0.47" + " " + QGroundControl.appSettingsDistanceUnitsString : " " property string _hoverDistanceText: _missionValid ? QGroundControl.metersToAppSettingsDistanceUnits(_hoverDistance).toFixed(2) + " " + QGroundControl.appSettingsDistanceUnitsString : " "
property string _cruiseDistanceText: _missionValid ? "30.44" + " " + QGroundControl.appSettingsDistanceUnitsString : " " property string _cruiseDistanceText: _missionValid ? QGroundControl.metersToAppSettingsDistanceUnits(_cruiseDistance).toFixed(2) + " " + QGroundControl.appSettingsDistanceUnitsString : " "
property string _hoverTimeText: _missionValid ? "4min 02s" : " " property string _hoverTimeText: _missionValid ? _hoverTime.toFixed(0) + "s" : " "
property string _cruiseTimeText: _missionValid ? "34min 21s" : " " property string _cruiseTimeText: _missionValid ? _cruiseTime.toFixed(0) + "s" : " "
readonly property real _margins: ScreenTools.defaultFontPixelWidth readonly property real _margins: ScreenTools.defaultFontPixelWidth
...@@ -158,17 +176,17 @@ Rectangle { ...@@ -158,17 +176,17 @@ Rectangle {
columnSpacing: _margins columnSpacing: _margins
anchors.verticalCenter: parent.verticalCenter anchors.verticalCenter: parent.verticalCenter
QGCLabel { text: qsTr("Total mission [WIP]") } QGCLabel { text: qsTr("Total mission") }
QGCLabel { text: qsTr(" ") } QGCLabel { text: qsTr(" ") }
QGCLabel { text: qsTr("Distance:") } QGCLabel { text: qsTr("Distance:") }
QGCLabel { text: _totalDistanceText } QGCLabel { text: _missionDistanceText }
QGCLabel { text: qsTr("Time:") } QGCLabel { text: qsTr("Time:") }
QGCLabel { text: _totalTimeText } QGCLabel { text: _missionTimeText }
QGCLabel { text: qsTr("Max telem dist:") } QGCLabel { text: qsTr("Max telem dist:") }
QGCLabel { text: _maxTelemDistText } QGCLabel { text: _missionMaxTelemetryText }
QGCLabel { QGCLabel {
text: qsTr("Hover distance:") text: qsTr("Hover distance:")
......
...@@ -36,6 +36,7 @@ ComplexMissionItem::ComplexMissionItem(Vehicle* vehicle, QObject* parent) ...@@ -36,6 +36,7 @@ ComplexMissionItem::ComplexMissionItem(Vehicle* vehicle, QObject* parent)
, _dirty(false) , _dirty(false)
, _cameraTrigger(false) , _cameraTrigger(false)
, _gridAltitudeRelative(true) , _gridAltitudeRelative(true)
, _surveyDistance(0.0)
, _cameraShots(0) , _cameraShots(0)
, _coveredArea(0.0) , _coveredArea(0.0)
, _gridAltitudeFact (0, "Altitude:", FactMetaData::valueTypeDouble) , _gridAltitudeFact (0, "Altitude:", FactMetaData::valueTypeDouble)
...@@ -64,12 +65,21 @@ const ComplexMissionItem& ComplexMissionItem::operator=(const ComplexMissionItem ...@@ -64,12 +65,21 @@ const ComplexMissionItem& ComplexMissionItem::operator=(const ComplexMissionItem
setAltPercent(other._altPercent); setAltPercent(other._altPercent);
setAzimuth(other._azimuth); setAzimuth(other._azimuth);
setDistance(other._distance); setDistance(other._distance);
setSurveyDistance(other._surveyDistance);
setCameraShots(other._cameraShots); setCameraShots(other._cameraShots);
setCoveredArea(other._coveredArea); setCoveredArea(other._coveredArea);
return *this; return *this;
} }
void ComplexMissionItem::setSurveyDistance(double surveyDistance)
{
if (!qFuzzyCompare(_surveyDistance, surveyDistance)) {
_surveyDistance = surveyDistance;
emit surveyDistanceChanged(_surveyDistance);
}
}
void ComplexMissionItem::setCameraShots(int cameraShots) void ComplexMissionItem::setCameraShots(int cameraShots)
{ {
if (_cameraShots != cameraShots) { if (_cameraShots != cameraShots) {
...@@ -271,6 +281,19 @@ bool ComplexMissionItem::load(const QJsonObject& complexObject, QString& errorSt ...@@ -271,6 +281,19 @@ bool ComplexMissionItem::load(const QJsonObject& complexObject, QString& errorSt
return true; return true;
} }
double ComplexMissionItem::greatestDistanceTo(const QGeoCoordinate &other) const
{
double greatestDistance = 0.0;
for (int i=0; i<_gridPoints.count(); i++) {
QGeoCoordinate currentCoord = _gridPoints[i].value<QGeoCoordinate>();
double distance = currentCoord.distanceTo(other);
if (distance > greatestDistance) {
greatestDistance = distance;
}
}
return greatestDistance;
}
void ComplexMissionItem::_setExitCoordinate(const QGeoCoordinate& coordinate) void ComplexMissionItem::_setExitCoordinate(const QGeoCoordinate& coordinate)
{ {
if (_exitCoordinate != coordinate) { if (_exitCoordinate != coordinate) {
...@@ -342,6 +365,7 @@ void ComplexMissionItem::_generateGrid(void) ...@@ -342,6 +365,7 @@ void ComplexMissionItem::_generateGrid(void)
convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &geoCoord); convertNedToGeo(-point.y(), point.x(), 0, tangentOrigin, &geoCoord);
_gridPoints += QVariant::fromValue(geoCoord); _gridPoints += QVariant::fromValue(geoCoord);
} }
setSurveyDistance(surveyDistance);
setCameraShots((int)floor(surveyDistance / _cameraTriggerDistanceFact.rawValue().toDouble())); setCameraShots((int)floor(surveyDistance / _cameraTriggerDistanceFact.rawValue().toDouble()));
emit gridPointsChanged(); emit gridPointsChanged();
......
...@@ -37,6 +37,7 @@ public: ...@@ -37,6 +37,7 @@ public:
Q_PROPERTY(int lastSequenceNumber READ lastSequenceNumber NOTIFY lastSequenceNumberChanged) Q_PROPERTY(int lastSequenceNumber READ lastSequenceNumber NOTIFY lastSequenceNumberChanged)
Q_PROPERTY(QVariantList gridPoints READ gridPoints NOTIFY gridPointsChanged) Q_PROPERTY(QVariantList gridPoints READ gridPoints NOTIFY gridPointsChanged)
Q_PROPERTY(double surveyDistance READ surveyDistance NOTIFY surveyDistanceChanged)
Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged) Q_PROPERTY(int cameraShots READ cameraShots NOTIFY cameraShotsChanged)
Q_PROPERTY(double coveredArea READ coveredArea NOTIFY coveredAreaChanged) Q_PROPERTY(double coveredArea READ coveredArea NOTIFY coveredAreaChanged)
...@@ -52,9 +53,11 @@ public: ...@@ -52,9 +53,11 @@ public:
Fact* gridSpacing(void) { return &_gridSpacingFact; } Fact* gridSpacing(void) { return &_gridSpacingFact; }
Fact* cameraTriggerDistance(void) { return &_cameraTriggerDistanceFact; } Fact* cameraTriggerDistance(void) { return &_cameraTriggerDistanceFact; }
double surveyDistance (void) const { return _surveyDistance; }
int cameraShots (void) const { return _cameraShots; } int cameraShots (void) const { return _cameraShots; }
double coveredArea (void) const { return _coveredArea; } double coveredArea (void) const { return _coveredArea; }
void setSurveyDistance (double surveyDistance);
void setCameraShots (int cameraShots); void setCameraShots (int cameraShots);
void setCoveredArea (double coveredArea); void setCoveredArea (double coveredArea);
...@@ -71,6 +74,11 @@ public: ...@@ -71,6 +74,11 @@ public:
/// @return true: load success, false: load failed, errorString set /// @return true: load success, false: load failed, errorString set
bool load(const QJsonObject& complexObject, QString& errorString); bool load(const QJsonObject& complexObject, QString& errorString);
/// Get the point of complex mission item furthest away from a coordinate
/// @param other QGeoCoordinate to which distance is calculated
/// @return the greatest distance from any point of the complex item to some coordinate
double greatestDistanceTo(const QGeoCoordinate &other) const;
// Overrides from VisualMissionItem // Overrides from VisualMissionItem
bool dirty (void) const final { return _dirty; } bool dirty (void) const final { return _dirty; }
...@@ -102,6 +110,7 @@ signals: ...@@ -102,6 +110,7 @@ signals:
void cameraTriggerChanged (bool cameraTrigger); void cameraTriggerChanged (bool cameraTrigger);
void gridAltitudeRelativeChanged (bool gridAltitudeRelative); void gridAltitudeRelativeChanged (bool gridAltitudeRelative);
void surveyDistanceChanged (double surveyDistance);
void cameraShotsChanged (int cameraShots); void cameraShotsChanged (int cameraShots);
void coveredAreaChanged (double coveredArea); void coveredAreaChanged (double coveredArea);
...@@ -130,6 +139,7 @@ private: ...@@ -130,6 +139,7 @@ private:
bool _cameraTrigger; bool _cameraTrigger;
bool _gridAltitudeRelative; bool _gridAltitudeRelative;
double _surveyDistance;
int _cameraShots; int _cameraShots;
double _coveredArea; double _coveredArea;
......
...@@ -18,6 +18,7 @@ ...@@ -18,6 +18,7 @@
#include "ComplexMissionItem.h" #include "ComplexMissionItem.h"
#include "JsonHelper.h" #include "JsonHelper.h"
#include "ParameterLoader.h" #include "ParameterLoader.h"
#include "QGroundControlQmlGlobal.h"
#ifndef __mobile__ #ifndef __mobile__
#include "QGCFileDialog.h" #include "QGCFileDialog.h"
...@@ -44,6 +45,10 @@ MissionController::MissionController(QObject *parent) ...@@ -44,6 +45,10 @@ MissionController::MissionController(QObject *parent)
, _firstItemsFromVehicle(false) , _firstItemsFromVehicle(false)
, _missionItemsRequested(false) , _missionItemsRequested(false)
, _queuedSend(false) , _queuedSend(false)
, _missionDistance(0.0)
, _missionMaxTelemetry(0.0)
, _cruiseDistance(0.0)
, _hoverDistance(0.0)
{ {
} }
...@@ -602,6 +607,23 @@ void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionIte ...@@ -602,6 +607,23 @@ void MissionController::_calcPrevWaypointValues(double homeAlt, VisualMissionIte
} }
} }
void MissionController::_calcHomeDist(VisualMissionItem* currentItem, VisualMissionItem* homeItem, double* distance)
{
QGeoCoordinate currentCoord = currentItem->coordinate();
QGeoCoordinate homeCoord = homeItem->exitCoordinate();
bool distanceOk = false;
distanceOk = true;
qCDebug(MissionControllerLog) << "distanceOk" << distanceOk;
if (distanceOk) {
*distance = homeCoord.distanceTo(currentCoord);
} else {
*distance = 0.0;
}
}
void MissionController::_recalcWaypointLines(void) void MissionController::_recalcWaypointLines(void)
{ {
bool firstCoordinateItem = true; bool firstCoordinateItem = true;
...@@ -692,7 +714,6 @@ void MissionController::_recalcAltitudeRangeBearing() ...@@ -692,7 +714,6 @@ void MissionController::_recalcAltitudeRangeBearing()
bool firstCoordinateItem = true; bool firstCoordinateItem = true;
VisualMissionItem* lastCoordinateItem = qobject_cast<VisualMissionItem*>(_visualItems->get(0)); VisualMissionItem* lastCoordinateItem = qobject_cast<VisualMissionItem*>(_visualItems->get(0));
SimpleMissionItem* homeItem = qobject_cast<SimpleMissionItem*>(lastCoordinateItem); SimpleMissionItem* homeItem = qobject_cast<SimpleMissionItem*>(lastCoordinateItem);
if (!homeItem) { if (!homeItem) {
...@@ -717,19 +738,50 @@ void MissionController::_recalcAltitudeRangeBearing() ...@@ -717,19 +738,50 @@ void MissionController::_recalcAltitudeRangeBearing()
const double homePositionAltitude = homeItem->coordinate().altitude(); const double homePositionAltitude = homeItem->coordinate().altitude();
minAltSeen = maxAltSeen = homeItem->coordinate().altitude(); minAltSeen = maxAltSeen = homeItem->coordinate().altitude();
double missionDistance = 0.0;
double missionMaxTelemetry = 0.0;
bool vtolCalc = (QGroundControlQmlGlobal::offlineEditingVehicleType()->enumStringValue() == "VTOL" || (_activeVehicle && _activeVehicle->vtol())) ? true : false ;
double cruiseDistance = 0.0;
double hoverDistance = 0.0;
bool hoverDistanceCalc = false;
bool hoverTransition = false;
bool cruiseTransition = false;
bool hoverDistanceReset = false;
bool linkBackToHome = false; bool linkBackToHome = false;
for (int i=1; i<_visualItems->count(); i++) { for (int i=1; i<_visualItems->count(); i++) {
VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i)); VisualMissionItem* item = qobject_cast<VisualMissionItem*>(_visualItems->get(i));
// Assume the worst // Assume the worst
item->setAzimuth(0.0); item->setAzimuth(0.0);
item->setDistance(0.0); item->setDistance(0.0);
// If we still haven't found the first coordinate item and we hit a a takeoff command link back to home // If we still haven't found the first coordinate item and we hit a takeoff command link back to home
if (firstCoordinateItem && if (firstCoordinateItem &&
item->isSimpleItem() && item->isSimpleItem() &&
qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) { qobject_cast<SimpleMissionItem*>(item)->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF) {
linkBackToHome = true; linkBackToHome = true;
hoverDistanceCalc = true;
}
if (item->isSimpleItem() && vtolCalc) {
SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(item);
if (simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_DO_VTOL_TRANSITION){ //transition waypoint value
if (simpleItem->missionItem().param1() == 3){ //hover mode value
hoverDistanceCalc = true;
hoverTransition = true;
}
else if (simpleItem->missionItem().param1() == 4){
hoverDistanceCalc = false;
cruiseTransition = true;
}
}
if(!hoverTransition && cruiseTransition && !hoverDistanceReset && !linkBackToHome){
hoverDistance = missionDistance;
hoverDistanceReset = true;
}
} }
if (item->specifiesCoordinate()) { if (item->specifiesCoordinate()) {
...@@ -754,7 +806,7 @@ void MissionController::_recalcAltitudeRangeBearing() ...@@ -754,7 +806,7 @@ void MissionController::_recalcAltitudeRangeBearing()
if (!item->isStandaloneCoordinate()) { if (!item->isStandaloneCoordinate()) {
firstCoordinateItem = false; firstCoordinateItem = false;
if (lastCoordinateItem != homeItem || (showHomePosition && linkBackToHome)) { if (lastCoordinateItem != homeItem || (showHomePosition && linkBackToHome)) {
double azimuth, distance, altDifference; double azimuth, distance, altDifference, telemetryDistance;
// Subsequent coordinate items link to last coordinate item. If the last coordinate item // Subsequent coordinate items link to last coordinate item. If the last coordinate item
// is an invalid home position we skip the line // is an invalid home position we skip the line
...@@ -762,12 +814,46 @@ void MissionController::_recalcAltitudeRangeBearing() ...@@ -762,12 +814,46 @@ void MissionController::_recalcAltitudeRangeBearing()
item->setAltDifference(altDifference); item->setAltDifference(altDifference);
item->setAzimuth(azimuth); item->setAzimuth(azimuth);
item->setDistance(distance); item->setDistance(distance);
missionDistance += distance;
if (item->isSimpleItem()) {
_calcHomeDist(item, homeItem, &telemetryDistance);
if (vtolCalc) {
SimpleMissionItem* simpleItem = qobject_cast<SimpleMissionItem*>(item);
if (simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_NAV_TAKEOFF || hoverDistanceCalc){
hoverDistance += distance;
}
cruiseDistance = missionDistance - hoverDistance;
if(simpleItem->command() == MavlinkQmlSingleton::MAV_CMD_NAV_LAND && !linkBackToHome && !cruiseTransition && !hoverTransition){
hoverDistance = cruiseDistance;
cruiseDistance = missionDistance - hoverDistance;
}
}
} else {
missionDistance += qobject_cast<ComplexMissionItem*>(item)->surveyDistance();
telemetryDistance = qobject_cast<ComplexMissionItem*>(item)->greatestDistanceTo(homeItem->exitCoordinate());
if (vtolCalc){
cruiseDistance += qobject_cast<ComplexMissionItem*>(item)->surveyDistance(); //assume all survey missions undertaken in cruise
}
}
if (telemetryDistance > missionMaxTelemetry) {
missionMaxTelemetry = telemetryDistance;
}
} }
lastCoordinateItem = item; lastCoordinateItem = item;
} }
} }
} }
setMissionDistance(missionDistance);
setMissionMaxTelemetry(missionMaxTelemetry);
setCruiseDistance(cruiseDistance);
setHoverDistance(hoverDistance);
// Walk the list again calculating altitude percentages // Walk the list again calculating altitude percentages
double altRange = maxAltSeen - minAltSeen; double altRange = maxAltSeen - minAltSeen;
for (int i=0; i<_visualItems->count(); i++) { for (int i=0; i<_visualItems->count(); i++) {
...@@ -921,6 +1007,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem) ...@@ -921,6 +1007,7 @@ void MissionController::_initVisualItem(VisualMissionItem* visualItem)
// We need to track changes of lastSequenceNumber so we can recalc sequence numbers for subsequence items // We need to track changes of lastSequenceNumber so we can recalc sequence numbers for subsequence items
ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(visualItem); ComplexMissionItem* complexItem = qobject_cast<ComplexMissionItem*>(visualItem);
connect(complexItem, &ComplexMissionItem::lastSequenceNumberChanged, this, &MissionController::_recalcSequence); connect(complexItem, &ComplexMissionItem::lastSequenceNumberChanged, this, &MissionController::_recalcSequence);
connect(complexItem, &ComplexMissionItem::surveyDistanceChanged, this, &MissionController::_recalcAltitudeRangeBearing);
} }
} }
...@@ -1017,6 +1104,38 @@ void MissionController::setAutoSync(bool autoSync) ...@@ -1017,6 +1104,38 @@ void MissionController::setAutoSync(bool autoSync)
#endif #endif
} }
void MissionController::setMissionDistance(double missionDistance)
{
if (!qFuzzyCompare(_missionDistance, missionDistance)) {
_missionDistance = missionDistance;
emit missionDistanceChanged(_missionDistance);
}
}
void MissionController::setMissionMaxTelemetry(double missionMaxTelemetry)
{
if (!qFuzzyCompare(_missionMaxTelemetry, missionMaxTelemetry)) {
_missionMaxTelemetry = missionMaxTelemetry;
emit missionMaxTelemetryChanged(_missionMaxTelemetry);
}
}
void MissionController::setCruiseDistance(double cruiseDistance)
{
if (!qFuzzyCompare(_cruiseDistance, cruiseDistance)) {
_cruiseDistance = cruiseDistance;
emit cruiseDistanceChanged(_cruiseDistance);
}
}
void MissionController::setHoverDistance(double hoverDistance)
{
if (!qFuzzyCompare(_hoverDistance, hoverDistance)) {
_hoverDistance = hoverDistance;
emit hoverDistanceChanged(_hoverDistance);
}
}
void MissionController::_dirtyChanged(bool dirty) void MissionController::_dirtyChanged(bool dirty)
{ {
if (dirty && _autoSync) { if (dirty && _autoSync) {
...@@ -1051,7 +1170,7 @@ void MissionController::_inProgressChanged(bool inProgress) ...@@ -1051,7 +1170,7 @@ void MissionController::_inProgressChanged(bool inProgress)
bool MissionController::_findLastAltitude(double* lastAltitude, MAV_FRAME* frame) bool MissionController::_findLastAltitude(double* lastAltitude, MAV_FRAME* frame)
{ {
bool found = false; bool found = false;
double foundAltitude; double foundAltitude;
MAV_FRAME foundFrame; MAV_FRAME foundFrame;
......
...@@ -40,6 +40,12 @@ public: ...@@ -40,6 +40,12 @@ public:
Q_PROPERTY(bool autoSync READ autoSync WRITE setAutoSync NOTIFY autoSyncChanged) Q_PROPERTY(bool autoSync READ autoSync WRITE setAutoSync NOTIFY autoSyncChanged)
Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged) Q_PROPERTY(bool syncInProgress READ syncInProgress NOTIFY syncInProgressChanged)
Q_PROPERTY(double missionDistance READ missionDistance NOTIFY missionDistanceChanged)
Q_PROPERTY(double missionMaxTelemetry READ missionMaxTelemetry NOTIFY missionMaxTelemetryChanged)
Q_PROPERTY(double cruiseDistance READ cruiseDistance NOTIFY cruiseDistanceChanged)
Q_PROPERTY(double hoverDistance READ hoverDistance NOTIFY hoverDistanceChanged)
Q_INVOKABLE void start(bool editMode); Q_INVOKABLE void start(bool editMode);
Q_INVOKABLE void getMissionItems(void); Q_INVOKABLE void getMissionItems(void);
Q_INVOKABLE void sendMissionItems(void); Q_INVOKABLE void sendMissionItems(void);
...@@ -70,6 +76,17 @@ public: ...@@ -70,6 +76,17 @@ public:
void setAutoSync(bool autoSync); void setAutoSync(bool autoSync);
bool syncInProgress(void); bool syncInProgress(void);
double missionDistance (void) const { return _missionDistance; }
double missionMaxTelemetry (void) const { return _missionMaxTelemetry; }
double cruiseDistance (void) const { return _cruiseDistance; }
double hoverDistance (void) const { return _hoverDistance; }
void setMissionDistance (double missionDistance );
void setMissionMaxTelemetry (double missionMaxTelemetry);
void setCruiseDistance (double cruiseDistance );
void setHoverDistance (double hoverDistance );
static const char* jsonSimpleItemsKey; ///< Key for simple items in a json file static const char* jsonSimpleItemsKey; ///< Key for simple items in a json file
signals: signals:
...@@ -79,6 +96,10 @@ signals: ...@@ -79,6 +96,10 @@ signals:
void autoSyncChanged(bool autoSync); void autoSyncChanged(bool autoSync);
void newItemsFromVehicle(void); void newItemsFromVehicle(void);
void syncInProgressChanged(bool syncInProgress); void syncInProgressChanged(bool syncInProgress);
void missionDistanceChanged(double missionDistance);
void missionMaxTelemetryChanged(double missionMaxTelemetry);
void cruiseDistanceChanged(double cruiseDistance);
void hoverDistanceChanged(double hoverDistance);
private slots: private slots:
void _newMissionItemsAvailableFromVehicle(); void _newMissionItemsAvailableFromVehicle();
...@@ -103,6 +124,7 @@ private: ...@@ -103,6 +124,7 @@ private:
void _autoSyncSend(void); void _autoSyncSend(void);
void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle); void _setupActiveVehicle(Vehicle* activeVehicle, bool forceLoadFromVehicle);
static void _calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference); static void _calcPrevWaypointValues(double homeAlt, VisualMissionItem* currentItem, VisualMissionItem* prevItem, double* azimuth, double* distance, double* altDifference);
static void _calcHomeDist(VisualMissionItem* currentItem, VisualMissionItem* homeItem, double* distance);
bool _findLastAltitude(double* lastAltitude, MAV_FRAME* frame); bool _findLastAltitude(double* lastAltitude, MAV_FRAME* frame);
bool _findLastAcceptanceRadius(double* lastAcceptanceRadius); bool _findLastAcceptanceRadius(double* lastAcceptanceRadius);
void _addPlannedHomePosition(QmlObjectListModel* visualItems, bool addToCenter); void _addPlannedHomePosition(QmlObjectListModel* visualItems, bool addToCenter);
...@@ -123,6 +145,10 @@ private: ...@@ -123,6 +145,10 @@ private:
bool _firstItemsFromVehicle; bool _firstItemsFromVehicle;
bool _missionItemsRequested; bool _missionItemsRequested;
bool _queuedSend; bool _queuedSend;
double _missionDistance;
double _missionMaxTelemetry;
double _cruiseDistance;
double _hoverDistance;
static const char* _settingsGroup; static const char* _settingsGroup;
static const char* _jsonVersionKey; static const char* _jsonVersionKey;
......
...@@ -31,7 +31,7 @@ ...@@ -31,7 +31,7 @@
class VisualMissionItem : public QObject class VisualMissionItem : public QObject
{ {
Q_OBJECT Q_OBJECT
public: public:
VisualMissionItem(Vehicle* vehicle, QObject* parent = NULL); VisualMissionItem(Vehicle* vehicle, QObject* parent = NULL);
VisualMissionItem(const VisualMissionItem& other, QObject* parent = NULL); VisualMissionItem(const VisualMissionItem& other, QObject* parent = NULL);
...@@ -73,7 +73,7 @@ public: ...@@ -73,7 +73,7 @@ public:
Q_PROPERTY(bool isCurrentItem READ isCurrentItem WRITE setIsCurrentItem NOTIFY isCurrentItemChanged) Q_PROPERTY(bool isCurrentItem READ isCurrentItem WRITE setIsCurrentItem NOTIFY isCurrentItemChanged)
Q_PROPERTY(int sequenceNumber READ sequenceNumber WRITE setSequenceNumber NOTIFY sequenceNumberChanged) Q_PROPERTY(int sequenceNumber READ sequenceNumber WRITE setSequenceNumber NOTIFY sequenceNumberChanged)
Q_PROPERTY(bool specifiesCoordinate READ specifiesCoordinate NOTIFY specifiesCoordinateChanged) ///< Item is associated with a coordinate position Q_PROPERTY(bool specifiesCoordinate READ specifiesCoordinate NOTIFY specifiesCoordinateChanged) ///< Item is associated with a coordinate position
Q_PROPERTY(bool isStandaloneCoordinate READ isStandaloneCoordinate NOTIFY isStandaloneCoordinateChanged) ///< Wayoint line does not go through item Q_PROPERTY(bool isStandaloneCoordinate READ isStandaloneCoordinate NOTIFY isStandaloneCoordinateChanged) ///< Waypoint line does not go through item
Q_PROPERTY(bool isSimpleItem READ isSimpleItem NOTIFY isSimpleItemChanged) ///< Simple or Complex MissionItem Q_PROPERTY(bool isSimpleItem READ isSimpleItem NOTIFY isSimpleItemChanged) ///< Simple or Complex MissionItem
/// List of child mission items. Child mission item are subsequent mision items which do not specify a coordinate. They /// List of child mission items. Child mission item are subsequent mision items which do not specify a coordinate. They
...@@ -81,7 +81,7 @@ public: ...@@ -81,7 +81,7 @@ public:
Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT) Q_PROPERTY(QmlObjectListModel* childItems READ childItems CONSTANT)
// Property accesors // Property accesors
double altDifference (void) const { return _altDifference; } double altDifference (void) const { return _altDifference; }
double altPercent (void) const { return _altPercent; } double altPercent (void) const { return _altPercent; }
double azimuth (void) const { return _azimuth; } double azimuth (void) const { return _azimuth; }
......
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