Commit bfd329e4 authored by Valentin Platzgummer's avatar Valentin Platzgummer

start mission issue (shown in inproper situations) fixed

parent df489c3e
......@@ -121,7 +121,7 @@ QGCView {
wimaController.dataContainer = Qt.binding(function() { return dataContainerPointer })
}
onUploadAndExecuteConfirmRequired: {
onReturnUserRequestConfirmRequired: {
_guidedController.confirmAction(_guidedController.actionUploadAndExecute)
}
......@@ -615,12 +615,6 @@ QGCView {
wimaController: _wimaController
planMasterController: _planMasterController
missionController: _missionController
onInitSmartRTL: {
if (wimaController.checkSmartRTLPreCondition()) {
_guidedController.confirmAction(_guidedController.actionInitSmartRTL)
}
}
}
//-------------------------------------------------------------------------
......
......@@ -35,7 +35,6 @@ Item {
property real _controllerProgressPct: _controllerValid ? planMasterController.missionController.progressPct : 0
property real _margins: ScreenTools.defaultFontPixelWidth / 2
signal initSmartRTL();
DeadMouseArea {
anchors.fill: parent
......@@ -316,7 +315,7 @@ Item {
QGCButton {
id: buttonSmartRTL
text: qsTr("Smart RTL")
onClicked: initSmartRTL()
onClicked: wimaController.initSmartRTL();
Layout.columnSpan: 2
Layout.fillWidth: true
}
......
......@@ -54,7 +54,6 @@ Item {
readonly property string gotoTitle: qsTr("Goto Location")
readonly property string vtolTransitionTitle: qsTr("VTOL Transition")
readonly property string overrideUploadTitle: qsTr("Override Lock")
readonly property string initSmartRTLTitle: qsTr("Smart RTL")
readonly property string uploadAndExecuteTitle: qsTr("Upload and Execute")
readonly property string returnBatteryLowTitle: qsTr("Battery Low")
......@@ -77,7 +76,6 @@ Item {
readonly property string vtolTransitionFwdMessage: qsTr("Transition VTOL to fixed wing flight.")
readonly property string vtolTransitionMRMessage: qsTr("Transition VTOL to multi-rotor flight.")
readonly property string overrideUploadMessage: qsTr("Vehicle is not inside service area. Upload nevertheless?")
readonly property string initSmartRTLMessage: qsTr("Pause the vehicle at it's current position and calculate a return path?")
readonly property string uploadAndExecuteMessage: qsTr("Upload and execute the displayed return path?")
readonly property string returnBatteryLowMessage: qsTr("Upload and execute the displayed return path?")
......@@ -103,8 +101,7 @@ Item {
readonly property int actionMVStartMission: 19
readonly property int actionVtolTransitionToFwdFlight: 20
readonly property int actionVtolTransitionToMRFlight: 21
readonly property int actionOverrideUpload: 22
readonly property int actionInitSmartRTL: 23
readonly property int actionOverrideUpload: 22
readonly property int actionUploadAndExecute: 24
readonly property int actionReturnBatteryLow: 25 // very similar to actionUploadAndExecute, is triggered on low battery
......@@ -132,7 +129,7 @@ Item {
property bool _guidedActionsEnabled: (!ScreenTools.isDebug && QGroundControl.corePlugin.options.guidedActionsRequireRCRSSI && _activeVehicle) ? _rcRSSIAvailable : _activeVehicle
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
property string _flightMode: _activeVehicle ? _activeVehicle.flightMode : ""
property bool _missionAvailable: wimaEnabled ? wimaMenu.missionReadyForStart : missionController.containsItems
property bool _missionAvailable: missionController.containsItems && missionController.progressPct === 1
property bool _missionActive: _activeVehicle ? _vehicleArmed && (_vehicleInLandMode || _vehicleInRTLMode || _vehicleInMissionMode) : false
property bool _vehicleArmed: _activeVehicle ? _activeVehicle.armed : false
property bool _vehicleFlying: _activeVehicle ? _activeVehicle.flying : false
......@@ -352,11 +349,6 @@ Item {
confirmDialog.message = overrideUploadMessage
confirmDialog.hideTrigger = true
break
case actionInitSmartRTL:
confirmDialog.title = initSmartRTLTitle
confirmDialog.message = initSmartRTLMessage
confirmDialog.hideTrigger = true
break
case actionUploadAndExecute:
confirmDialog.title = uploadAndExecuteTitle
confirmDialog.message = uploadAndExecuteMessage
......@@ -445,9 +437,6 @@ Item {
case actionOverrideUpload:
wimaController.forceUploadToVehicle()
break
case actionInitSmartRTL:
wimaController.initSmartRTL()
break
case actionUploadAndExecute:
case actionReturnBatteryLow:
wimaController.executeSmartRTL()
......
......@@ -88,7 +88,6 @@ MissionController::MissionController(PlanMasterController* masterController, QOb
_remainingDistanceTimeTimer.setInterval(REMAINING_DIST_TIME_UPDATE_INTERVAL);
connect(&_remainingDistanceTimeTimer, &QTimer::timeout, this, &MissionController::_updateRemainingDistanceTime);
}
MissionController::~MissionController()
......@@ -243,6 +242,24 @@ void MissionController::_warnIfTerrainFrameUsed(void)
}
}
void MissionController::_setRemainingDistance(double dist)
{
if (qFuzzyCompare(dist, _remainingDistance) == false) {
_remainingDistance = dist;
emit remainingDistanceChanged();
}
}
void MissionController::_setRemainingTime(double time)
{
if (qFuzzyCompare(time, _remainingTime) == false) {
_remainingTime = time;
emit remainingTimeChanged();
}
}
void MissionController::sendToVehicle(void)
{
if (_masterController->offline()) {
......@@ -1680,11 +1697,8 @@ void MissionController::_enableDisableRemainingDistTimeCalculation(bool flying)
_remainingDistanceTimeTimer.start();
} else {
_remainingDistanceTimeTimer.stop();
_remainingTime = -1;
_remainingDistance = -1;
emit remainingTimeChanged();
emit remainingDistanceChanged();
_setRemainingTime(-1);
_setRemainingDistance(-1);
}
}
......@@ -1694,11 +1708,11 @@ void MissionController::_updateRemainingDistanceTime()
double time = 0;
bool success = distanceTimeToMissionEnd(dist, time, _missionManager->currentIndex() /*missionIndex >= 1*/, true /* useVehiclePostion */);
if (success) {
_remainingTime = time;
_remainingDistance = dist;
emit remainingTimeChanged();
emit remainingDistanceChanged();
_setRemainingTime(time);
_setRemainingDistance(dist);
} else {
_setRemainingTime(-1);
_setRemainingDistance(-1);
}
}
......
......@@ -318,6 +318,8 @@ private:
void _addTimeDistance(bool vtolInHover, double hoverTime, double cruiseTime, double extraTime, double distance, int seqNum);
int _insertComplexMissionItemWorker(ComplexMissionItem* complexItem, int i);
void _warnIfTerrainFrameUsed(void);
void _setRemainingDistance(double dist);
void _setRemainingTime(double time);
private:
MissionManager* _missionManager;
......
......@@ -4,7 +4,7 @@
"shortDescription": "The battery threshold in percents, for which low battery handling measures get triggered.",
"type": "double",
"units": "%",
"defaultValue": 40
"defaultValue": 35
},
{
"name": "enableLowBatteryHandling",
......@@ -17,6 +17,6 @@
"shortDescription": "If the remaining estimated mission time is lower than this value, low battery handling will not be triggered.",
"type": "uint64",
"units": "s",
"defaultValue": 60
"defaultValue": 15
}
]
......@@ -37,16 +37,16 @@ WimaController::WimaController(QObject *parent)
, _endWaypointIndex (0)
, _startWaypointIndex (0)
, _uploadOverrideRequired (false)
, _measurementPathLength (-1)
, _measurementPathLength (-1)
, _arrivalPathLength (-1)
, _returnPathLength (-1)
, _phaseDistance (-1)
, _phaseDuration (-1)
, _phaseDistanceBuffer (-1)
, _phaseDurationBuffer (-1)
, _vehicleHasLowBattery (false)
, _lowBatteryHandlingTriggered(false)
, _executingSmartRTL (false)
, _vehicleHasLowBattery (false)
, _lowBatteryHandlingTriggered (false)
, _executingSmartRTL (false)
{
_showAllMissionItems.setRawValue(true);
......@@ -61,7 +61,9 @@ WimaController::WimaController(QObject *parent)
// setup low battery handling
connect(&_checkBatteryTimer, &QTimer::timeout, this, &WimaController::checkBatteryLevel);
_checkBatteryTimer.setInterval(500);
_checkBatteryTimer.setInterval(CHECK_BATTERY_INTERVAL);
Fact *enableLowBatteryHandling = qgcApp()->toolbox()->settingsManager()->wimaSettings()->enableLowBatteryHandling();
connect(enableLowBatteryHandling, &Fact::rawValueChanged, this, &WimaController::enableDisableLowBatteryHandling);
enableDisableLowBatteryHandling(enableLowBatteryHandling->rawValue());
......@@ -343,26 +345,15 @@ bool WimaController::calcReturnPath()
return true;
}
bool WimaController::executeSmartRTL()
void WimaController::executeSmartRTL()
{
QString errorString;
bool retValue = _executeSmartRTL(errorString);
if (!retValue) {
qgcApp()->showMessage(errorString);
}
return retValue;
_executeSmartRTL();
}
bool WimaController::initSmartRTL()
void WimaController::initSmartRTL()
{
masterController()->managerVehicle()->pauseVehicle();
QString errorString;
bool retValue = calcReturnPath();
if (!retValue)
return false;
emit uploadAndExecuteConfirmRequired();
return true;
_srtlReason = UserRequest;
_initSmartRTL();
}
void WimaController::removeVehicleTrajectoryHistory()
......@@ -928,8 +919,6 @@ void WimaController::checkBatteryLevel()
bool enabled = _enableWimaController.rawValue().toBool();
unsigned int minTime = wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt();
static short attemptCounter = 0;
if (managerVehicle != nullptr && enabled == true) {
Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
......@@ -944,26 +933,9 @@ void WimaController::checkBatteryLevel()
_lowBatteryHandlingTriggered = true;
}
else {
QString errorString;
attemptCounter++;
if (_checkSmartRTLPreCondition(errorString) == true) {
managerVehicle->pauseVehicle();
if (_calcReturnPath(errorString)) {
_lowBatteryHandlingTriggered = true;
attemptCounter = 0;
emit returnBatteryLowConfirmRequired();
} else {
if (attemptCounter > 3) {
qgcApp()->showMessage(tr("Battery level is low. Smart RTL not possible."));
qgcApp()->showMessage(errorString);
}
}
}
if (attemptCounter > 3) { // try 3 times, somtimes vehicle is outside joined area
_lowBatteryHandlingTriggered = true;
attemptCounter = 0;
}
_lowBatteryHandlingTriggered = true;
_srtlReason = BatteryLow;
_initSmartRTL();
}
}
}
......@@ -1035,10 +1007,15 @@ bool WimaController::_checkSmartRTLPreCondition(QString &errorString)
return false;
}
Vehicle *managerVehicle = masterController()->managerVehicle();
if (!managerVehicle->flying()) {
errorString.append(tr("Vehicle is not flying. Smart RTL not available."));
return false;
}
if (!managerVehicle->flying()) {
errorString.append(tr("Vehicle is not flying. Smart RTL not available."));
return false;
}
if (!_joinedArea.containsCoordinate(managerVehicle->coordinate())) {
errorString.append(tr("Vehicle not inside save area. Smart RTL not available."));
return false;
}
return true;
}
......@@ -1136,8 +1113,12 @@ bool WimaController::_calcReturnPath(QString &errorSring)
}
//qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
_setPhaseDistance(-1);
_setPhaseDuration(-1);
double dist = 0;
double time = 0;
if (!_missionController->distanceTimeToMissionEnd(dist, time, 1, false))
qWarning("WimaController::calcNextPhase: distanceTimeToMissionEnd returned false!");
_setPhaseDistance(dist);
_setPhaseDuration(time);
_missionController->removeAll(); // remove items from _missionController, will be added on upload
updateAltitude();
......@@ -1161,15 +1142,46 @@ void WimaController::_setVehicleHasLowBattery(bool batteryLow)
}
}
bool WimaController::_executeSmartRTL(QString &errorSring)
void WimaController::_initSmartRTL()
{
QString errorString;
static int attemptCounter = 0;
attemptCounter++;
if (_checkSmartRTLPreCondition(errorString) == true) {
_masterController->managerVehicle()->pauseVehicle();
connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::smartRTLCleanUp);
if (_calcReturnPath(errorString)) {
_executingSmartRTL = true;
attemptCounter = 0;
switch(_srtlReason) {
case BatteryLow:
emit returnBatteryLowConfirmRequired();
break;
case UserRequest:
emit returnUserRequestConfirmRequired();
break;
default:
qWarning("\nWimaController::_initSmartRTL: default case reached!");
}
return;
}
}
if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) { // try 3 times, somtimes vehicle is outside joined area
errorString.append(tr("Smart RTL: No success after maximum number of attempts."));
qgcApp()->showMessage(errorString);
attemptCounter = 0;
} else {
_smartRTLAttemptTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this, &WimaController::_initSmartRTL);
}
}
void WimaController::_executeSmartRTL()
{
Q_UNUSED(errorSring)
_executingSmartRTL = true;
connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this, &WimaController::smartRTLCleanUp);
forceUploadToVehicle();
masterController()->managerVehicle()->startMission();
return true;
}
void WimaController::_loadCurrentMissionItemsFromBuffer()
......
......@@ -24,6 +24,10 @@
#include "WimaSettings.h"
#include "SettingsManager.h"
#define CHECK_BATTERY_INTERVAL 1000
#define SMART_RTL_MAX_ATTEMPTS 3
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
class WimaController : public QObject
{
......@@ -109,8 +113,8 @@ public:
Q_INVOKABLE void removeFromVehicle();
Q_INVOKABLE bool checkSmartRTLPreCondition(); // wrapper for _checkSmartRTLPreCondition(QString &errorString)
Q_INVOKABLE bool calcReturnPath(); // wrapper for _calcReturnPath(QString &errorSring)#
Q_INVOKABLE bool executeSmartRTL(); // wrapper for _executeSmartRTL(QString &errorSring)
Q_INVOKABLE bool initSmartRTL();
Q_INVOKABLE void executeSmartRTL(); // wrapper for _executeSmartRTL(QString &errorSring)
Q_INVOKABLE void initSmartRTL();
Q_INVOKABLE void removeVehicleTrajectoryHistory();
......@@ -151,23 +155,24 @@ public:
bool extractCoordinateList(QmlObjectListModel &missionItems, QVariantList &coordinateList, int startIndex, int endIndex);
signals:
void masterControllerChanged (void);
void missionControllerChanged (void);
void visualItemsChanged (void);
void currentFileChanged ();
void dataContainerChanged ();
void readyForSaveSendChanged (bool ready);
void missionItemsChanged (void);
void currentMissionItemsChanged (void);
void waypointPathChanged (void);
void currentWaypointPathChanged (void);
void uploadOverrideRequiredChanged (void);
void phaseDistanceChanged (void);
void phaseDurationChanged (void);
void uploadAndExecuteConfirmRequired(void);
void vehicleHasLowBatteryChanged (void);
void returnBatteryLowConfirmRequired(void);
void masterControllerChanged (void);
void missionControllerChanged (void);
void visualItemsChanged (void);
void currentFileChanged ();
void dataContainerChanged ();
void readyForSaveSendChanged (bool ready);
void missionItemsChanged (void);
void currentMissionItemsChanged (void);
void waypointPathChanged (void);
void currentWaypointPathChanged (void);
void uploadOverrideRequiredChanged (void);
void phaseDistanceChanged (void);
void phaseDurationChanged (void);
void vehicleHasLowBatteryChanged (void);
void returnBatteryLowConfirmRequired (void);
void returnUserRequestConfirmRequired (void);
private:
enum SRTL_Reason {BatteryLow, UserRequest};
private slots:
bool fetchContainerData();
bool calcNextPhase(void);
......@@ -183,6 +188,9 @@ private slots:
void smartRTLCleanUp (bool flying); // cleans up after successfull smart RTL
void enableDisableLowBatteryHandling (QVariant enable);
void reverseChangedHandler ();
void _initSmartRTL ();
void _executeSmartRTL ();
private:
void _setPhaseDistance(double distance);
......@@ -190,7 +198,6 @@ private:
bool _checkSmartRTLPreCondition(QString &errorString); // should be called from gui, befor calcReturnPath()
bool _calcReturnPath(QString &errorSring); // Calculates return path (destination: service area center) for a flying vehicle
void _setVehicleHasLowBattery(bool batteryLow);
bool _executeSmartRTL(QString &errorSring);
void _loadCurrentMissionItemsFromBuffer();
void _saveCurrentMissionItemsToBuffer();
......@@ -242,6 +249,9 @@ private:
double _phaseDurationBuffer; // buffer for storing _phaseDuration when doing smart RTL
QTimer _checkBatteryTimer;
QTimer _smartRTLAttemptTimer;
SRTL_Reason _srtlReason;
bool _vehicleHasLowBattery;
bool _lowBatteryHandlingTriggered;
bool _executingSmartRTL;
......
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