Skip to content

Q
qgroundcontrol
Commit a1d6f37c authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files
Options

smart rtl and smart rtl low battery added

parent f8bfa0a1
Hide whitespace changes
Inline Side-by-side
Showing with 317 additions and 26 deletions
src/FlightDisplay/FlightDisplayView.qml
View file @ a1d6f37c
......@@ -119,6 +119,14 @@ QGCView {
Component.onCompleted: {
wimaController.dataContainer = Qt.binding(function() { return dataContainerPointer })
}
onUploadAndExecuteConfirmRequired: {
_guidedController.confirmAction(_guidedController.actionUploadAndExecute)
}
onReturnBatteryLowConfirmRequired: {
_guidedController.confirmAction(_guidedController.actionReturnBatteryLow)
}
}
BuiltInPreFlightCheckModel {
......@@ -549,6 +557,12 @@ QGCView {
wimaController: wimaController
planMasterController: masterController
onInitSmartRTL: {
if (wimaController.checkSmartRTLPreCondition()) {
_guidedController.confirmAction(_guidedController.actionInitSmartRTL)
}
}
}
//-------------------------------------------------------------------------
......
src/FlightDisplay/FlightDisplayWimaMenu.qml
View file @ a1d6f37c
......@@ -28,6 +28,8 @@ Item {
property bool _controllerValid: planMasterController !== undefined
property real _controllerProgressPct: _controllerValid ? planMasterController.missionController.progressPct : 0
signal initSmartRTL();
// Progress bar visibility
on_ControllerProgressPctChanged: {
if (_controllerProgressPct === 1) {
......@@ -213,6 +215,16 @@ Item {
Layout.fillWidth: true
}
QGCButton {
id: buttonSmartRTL
text: qsTr("Smart RTL")
onClicked: initSmartRTL()
Layout.fillWidth: true
}
// placeholder, remove if you add stuff
QGCLabel { text: "" }
// progess bar
Rectangle {
id: progressBar
......
src/FlightDisplay/GuidedActionsController.qml
View file @ a1d6f37c
......@@ -53,6 +53,9 @@ 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")
readonly property string armMessage: qsTr("Arm the vehicle.")
readonly property string disarmMessage: qsTr("Disarm the vehicle")
......@@ -72,7 +75,11 @@ Item {
readonly property string mvPauseMessage: qsTr("Pause all vehicles at their current position.")
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 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?")
readonly property int actionRTL: 1
readonly property int actionLand: 2
......@@ -95,7 +102,10 @@ Item {
readonly property int actionMVStartMission: 19
readonly property int actionVtolTransitionToFwdFlight: 20
readonly property int actionVtolTransitionToMRFlight: 21
readonly property int actionOverrideUpload: 22
readonly property int actionOverrideUpload: 22
readonly property int actionInitSmartRTL: 23
readonly property int actionUploadAndExecute: 24
readonly property int actionReturnBatteryLow: 25 // very similar to actionUploadAndExecute, is triggered on low battery
property bool showEmergenyStop: _guidedActionsEnabled && !_hideEmergenyStop && _vehicleArmed && _vehicleFlying
property bool showArm: _guidedActionsEnabled && !_vehicleArmed
......@@ -340,6 +350,21 @@ 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
confirmDialog.hideTrigger = true
break
case actionReturnBatteryLow:
confirmDialog.title = returnBatteryLowTitle
confirmDialog.message = returnBatteryLowMessage
confirmDialog.hideTrigger = true
break
default:
console.warn("Unknown actionCode", actionCode)
return
......@@ -418,6 +443,18 @@ Item {
case actionOverrideUpload:
wimaController.forceUploadToVehicle()
break
case actionInitSmartRTL:
_activeVehicle.pauseVehicle()
wimaController.calcReturnPath()
break
case actionUploadAndExecute:
wimaController.forceUploadToVehicle()
_activeVehicle.startMission()
break
case actionReturnBatteryLow:
wimaController.forceUploadToVehicle()
_activeVehicle.startMission()
break
default:
console.warn(qsTr("Internal error: unknown actionCode"), actionCode)
break
......
src/Wima/WimaController.cc
View file @ a1d6f37c
......@@ -35,6 +35,8 @@ WimaController::WimaController(QObject *parent)
, _uploadOverrideRequired (false)
, _phaseDistance(-1)
, _phaseDuration(-1)
, _vehicleHasLowBattery(false)
, _lowBatteryHandlingTriggered(false)
{
_nextPhaseStartWaypointIndex.setRawValue(int(1));
......@@ -45,6 +47,12 @@ WimaController::WimaController(QObject *parent)
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase);
connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::updateSpeed);
connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::updateAltitude);
// battery timer
connect(&_checkBatteryTimer, &QTimer::timeout, this, &WimaController::checkBatteryLevel);
_checkBatteryTimer.setInterval(500);
_checkBatteryTimer.start();
}
QmlObjectListModel* WimaController::visualItems()
......@@ -144,6 +152,11 @@ double WimaController::phaseDuration() const
return _phaseDuration;
}
bool WimaController::vehicleHasLowBattery() const
{
return _vehicleHasLowBattery;
}
Fact *WimaController::startWaypointIndex()
{
return &_nextPhaseStartWaypointIndex;
......@@ -252,6 +265,29 @@ bool WimaController::forceUploadToVehicle()
void WimaController::removeFromVehicle()
{
_masterController->removeAllFromVehicle();
_missionController->removeAll();
}
bool WimaController::checkSmartRTLPreCondition()
{
QString errorString;
bool retValue = _checkSmartRTLPreCondition(errorString);
if (retValue == false) {
qgcApp()->showMessage(errorString);
return false;
}
return true;
}
bool WimaController::calcReturnPath()
{
QString errorString;
bool retValue = _calcReturnPath(errorString);
if (retValue == false) {
qgcApp()->showMessage(errorString);
return false;
}
return true;
}
void WimaController::saveToCurrent()
......@@ -345,15 +381,15 @@ bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVa
bool retValue = extractCoordinateList(missionItems, geoCoordintateList, startIndex, endIndex);
if (!retValue)
return false;
for (int i = 0; i < geoCoordintateList.size(); i++) {
QGeoCoordinate vertex = geoCoordintateList[i];
if (qFuzzyIsNull(vertex.latitude()) && qFuzzyIsNull(vertex.longitude()))
geoCoordintateList.removeAt(i);
}
if (!retValue)
return false;
for (auto coordinate : geoCoordintateList)
coordinateList.append(QVariant::fromValue(coordinate));
......@@ -373,8 +409,8 @@ bool WimaController::fetchContainerData()
// reset visual items
_visualItems.clear();
_missionItems.clear();
_currentMissionItems.clear();
_missionItems.clearAndDeleteContents();
_currentMissionItems.clearAndDeleteContents();
_waypointPath.clear();
_currentWaypointPath.clear();
......@@ -438,7 +474,7 @@ bool WimaController::fetchContainerData()
}
// extract mission items
QList<const MissionItem*> tempMissionItems = planData.missionItems();
QList<MissionItem> tempMissionItems = planData.missionItems();
if (tempMissionItems.size() < 1)
return false;
......@@ -448,8 +484,7 @@ bool WimaController::fetchContainerData()
// create SimpleMissionItem by using _missionController
for ( int i = 0; i < tempMissionItems.size(); i++) {
const MissionItem *missionItem = tempMissionItems[i];
_missionController->insertSimpleMissionItem(*missionItem, missionControllerVisualItems->count());
_missionController->insertSimpleMissionItem(tempMissionItems[i], missionControllerVisualItems->count());
}
// copy mission items from _missionController to _missionItems
for ( int i = 1; i < missionControllerVisualItems->count(); i++) {
......@@ -492,7 +527,7 @@ bool WimaController::calcNextPhase()
if (_missionItems.count() < 1 || _lastMissionPhaseReached)
return false;
_currentMissionItems.clear();
_currentMissionItems.clearAndDeleteContents();
_currentWaypointPath.clear();
emit currentMissionItemsChanged();
emit currentWaypointPathChanged();
......@@ -683,6 +718,40 @@ void WimaController::updateAltitude()
}
}
void WimaController::checkBatteryLevel()
{
Vehicle *managerVehicle = masterController()->managerVehicle();
int batteryThreshold = 94; // percent
if (managerVehicle != nullptr) {
Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold
&& battery2percentRemaining->rawValue().toDouble() < batteryThreshold) {
_setVehicleHasLowBattery(true);
if (!_lowBatteryHandlingTriggered) {
QString errorString;
if (_checkSmartRTLPreCondition(errorString) == true) {
managerVehicle->pauseVehicle();
if (_calcReturnPath(errorString)) {
emit returnBatteryLowConfirmRequired();
} else {
qgcApp()->showMessage(tr("Battery level is low. Smart RTL not possible."));
qgcApp()->showMessage(errorString);
}
}
_lowBatteryHandlingTriggered = true;
}
}
else {
_setVehicleHasLowBattery(false);
_lowBatteryHandlingTriggered = false;
}
}
}
void WimaController::_setPhaseDistance(double distance)
{
if (!qFuzzyCompare(distance, _phaseDistance)) {
......@@ -701,5 +770,143 @@ void WimaController::_setPhaseDuration(double duration)
}
}
bool WimaController::_checkSmartRTLPreCondition(QString &errorString)
{
if (!_localPlanDataValid) {
errorString.append(tr("No WiMA data available. Please define at least a measurement and a service area."));
return false;
}
Vehicle *managerVehicle = masterController()->managerVehicle();
if (!managerVehicle->flying()) {
errorString.append(tr("Vehicle is not flying. Smart RTL not available."));
return false;
}
}
bool WimaController::_calcReturnPath(QString &errorSring)
{
// it is assumed that checkSmartRTLPreCondition() was called first and true was returned
Vehicle *managerVehicle = masterController()->managerVehicle();
QGeoCoordinate currentVehiclePosition = managerVehicle->coordinate();
// check if vehicle inside _joinedArea, this statement is not inside checkSmartRTLPreCondition() because during checkSmartRTLPreCondition() vehicle is not paused yet
if (!_joinedArea.containsCoordinate(currentVehiclePosition)) {
errorSring.append(tr("Vehicle not inside joined area. Action not supported."));
return false;
}
// calculate return path
QList<QGeoCoordinate> returnPath;
calcShortestPath(currentVehiclePosition, _takeoffLandPostion, returnPath);
// successful?
if (returnPath.isEmpty()) {
errorSring.append(tr("Not able to calculate return path."));
return false;
}
// qWarning() << "returnPath.size()" << returnPath.size();
// buffer _currentMissionItems
//qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
//qWarning() << "_missionItemsBuffer.count()" << _missionItemsBuffer.count();
_missionItemsBuffer.clear();
int numCurrentMissionItems = _currentMissionItems.count();
for (int i = 0; i < numCurrentMissionItems; i++)
_missionItemsBuffer.append(_currentMissionItems.removeAt(0));
//qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
//qWarning() << "_missionItemsBuffer.count()" << _missionItemsBuffer.count();
// create Mission Items
removeFromVehicle();
QmlObjectListModel* missionControllerVisuals = _missionController->visualItems();
// set homeposition of settingsItem
MissionSettingsItem* settingsItem = missionControllerVisuals->value<MissionSettingsItem *>(0);
if (settingsItem == nullptr) {
qWarning("WimaController: nullptr");
return false;
}
settingsItem->setCoordinate(_takeoffLandPostion);
// copy from returnPath to _missionController
QGeoCoordinate speedItemCoordinate = returnPath.first();
for (auto coordinate : returnPath) {
_missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
}
//qWarning() << "missionControllerVisuals->count()" << missionControllerVisuals->count();
// create speed item
int speedItemIndex = 1;
_missionController->insertSimpleMissionItem(speedItemCoordinate, speedItemIndex);
SimpleMissionItem *speedItem = missionControllerVisuals->value<SimpleMissionItem*>(speedItemIndex);
if (speedItem == nullptr) {
qWarning("WimaController: nullptr");
return false;
}
speedItem->setCoordinate(speedItemCoordinate);
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
speedItem->missionItem().setParam2(_flightSpeed.rawValue().toDouble());
// set second item command to ordinary waypoint (is takeoff)
SimpleMissionItem *secondItem = missionControllerVisuals->value<SimpleMissionItem*>(2);
if (secondItem == nullptr) {
qWarning("WimaController: nullptr");
return false;
}
secondItem->setCoordinate(speedItemCoordinate);
secondItem->setCommand(MAV_CMD_NAV_WAYPOINT);
// set land command for last mission item
SimpleMissionItem *landItem = missionControllerVisuals->value<SimpleMissionItem*>(missionControllerVisuals->count()-1);
if (landItem == nullptr) {
qWarning("WimaController: nullptr");
return false;
}
_missionController->setLandCommand(*landItem);
// copy to _currentMissionItems
//qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
for ( int i = 1; i < missionControllerVisuals->count(); i++) {
SimpleMissionItem *visualItem = missionControllerVisuals->value<SimpleMissionItem*>(i);
if (visualItem == nullptr) {
qWarning("WimaController: Nullptr at SimpleMissionItem!");
_currentMissionItems.clear();
return false;
}
SimpleMissionItem *visualItemCopy = new SimpleMissionItem(*visualItem, true, this);
_currentMissionItems.append(visualItemCopy);
}
//qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
_setPhaseDistance(_missionController->missionDistance());
_setPhaseDuration(_missionController->missionDistance()/_flightSpeed.rawValue().toDouble());
_missionController->removeAll(); // remove items from _missionController, will be added on upload
updateAltitude();
updateCurrentPath();
emit currentMissionItemsChanged();
//qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
_showAllMissionItems.setRawValue(false);
managerVehicle->trajectoryPoints()->clear();
emit uploadAndExecuteConfirmRequired();
return true;
}
void WimaController::_setVehicleHasLowBattery(bool batteryLow)
{
if (_vehicleHasLowBattery != batteryLow) {
_vehicleHasLowBattery = batteryLow;
emit vehicleHasLowBatteryChanged();
qWarning() << "WimaController::_setVehicleHasLowBattery(bool batteryLow)" << _vehicleHasLowBattery;
}
}
src/Wima/WimaController.h
View file @ a1d6f37c
......@@ -56,6 +56,7 @@ public:
Q_PROPERTY(bool uploadOverrideRequired READ uploadOverrideRequired WRITE setUploadOverrideRequired NOTIFY uploadOverrideRequiredChanged)
Q_PROPERTY(double phaseDistance READ phaseDistance NOTIFY phaseDistanceChanged)
Q_PROPERTY(double phaseDuration READ phaseDuration NOTIFY phaseDurationChanged)
Q_PROPERTY(bool vehicleHasLowBattery READ vehicleHasLowBattery NOTIFY vehicleHasLowBatteryChanged)
......@@ -84,6 +85,7 @@ public:
bool uploadOverrideRequired (void) const;
double phaseDistance (void) const;
double phaseDuration (void) const;
bool vehicleHasLowBattery (void) const;
// Property setters
......@@ -98,7 +100,10 @@ public:
Q_INVOKABLE void resetPhase();
Q_INVOKABLE bool uploadToVehicle();
Q_INVOKABLE bool forceUploadToVehicle();
Q_INVOKABLE void removeFromVehicle();
Q_INVOKABLE void removeFromVehicle();
Q_INVOKABLE bool checkSmartRTLPreCondition(); // wrapper for _checkSmartRTLPreCondition(QString &errorString)
Q_INVOKABLE bool calcReturnPath(); // wrapper for _calcReturnPath(QString &errorSring)
Q_INVOKABLE void saveToCurrent ();
Q_INVOKABLE void saveToFile (const QString& filename);
......@@ -148,6 +153,9 @@ signals:
void uploadOverrideRequiredChanged (void);
void phaseDistanceChanged (void);
void phaseDurationChanged (void);
void uploadAndExecuteConfirmRequired(void);
void vehicleHasLowBatteryChanged (void);
void returnBatteryLowConfirmRequired(void);
private slots:
bool fetchContainerData();
......@@ -159,10 +167,17 @@ private slots:
bool setTakeoffLandPosition (void);
void updateSpeed (void);
void updateAltitude (void);
void checkBatteryLevel (void);
private:
void _setPhaseDistance(double distance);
void _setPhaseDuration(double duration);
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);
private:
......@@ -179,6 +194,7 @@ private:
QmlObjectListModel _missionItems; // all mission itmes (Mission Items) generaded by wimaPlaner, displayed in flightView
QmlObjectListModel _currentMissionItems; // contains the current mission items, which are a sub set of _missionItems,
// _currentMissionItems contains a number of mission items which can be worked off with a single battery chrage
QmlObjectListModel _missionItemsBuffer; // Buffer to store mission items, e.g. for storing _currentMissionItems when smartRTL() is invoked
QVariantList _waypointPath; // path connecting the items in _missionItems
QVariantList _currentWaypointPath; // path connecting the items in _currentMissionItems
QGeoCoordinate _takeoffLandPostion;
......@@ -204,6 +220,10 @@ private:
// The user can override the upload lock with a slider, this will reset this variable to false.
double _phaseDistance; // the lenth in meters of the current phase
double _phaseDuration; // the phase duration in seconds
QTimer _checkBatteryTimer;
bool _vehicleHasLowBattery;
bool _lowBatteryHandlingTriggered;
};
src/Wima/WimaPlanData.cc
View file @ a1d6f37c
......@@ -100,9 +100,12 @@ void WimaPlanData::append(const WimaMeasurementAreaData &areaData)
}
}
void WimaPlanData::append(const QList<const MissionItem *> &missionItems)
void WimaPlanData::append(const QList<MissionItem *> &missionItems)
{
_missionItems.append(missionItems);
for (MissionItem *item : missionItems) {
MissionItem copy = MissionItem(*item, this);
_missionItems.append(copy);
}
}
/*!
......@@ -121,7 +124,7 @@ QList<const WimaAreaData *> WimaPlanData::areaList() const
return _areaList;
}
QList<const MissionItem *> WimaPlanData::missionItems() const
QList<MissionItem> WimaPlanData::missionItems() const
{
return _missionItems;
}
......
src/Wima/WimaPlanData.h
View file @ a1d6f37c
......@@ -22,17 +22,20 @@ public:
void append(const WimaServiceAreaData &areaData);
void append(const WimaCorridorData &areaData);
void append(const WimaMeasurementAreaData &areaData);
void append(const QList<const MissionItem*> &missionItems);
void append(const QList<MissionItem *> &missionItems);
void clear();
QList<const WimaAreaData *> areaList() const;
QList<const MissionItem *> missionItems() const;
QList<MissionItem> missionItems() const;
signals:
void areaListChanged();
private:
void _clearAndDeleteMissionItems();
private:
WimaJoinedAreaData _joinedArea;
WimaServiceAreaData _serviceArea;
......@@ -40,5 +43,5 @@ private:
WimaMeasurementAreaData _measurementArea;
QList<const WimaAreaData*> _areaList;
QList<const MissionItem*> _missionItems;
QList<MissionItem> _missionItems;
};
src/Wima/WimaPlaner.cc
View file @ a1d6f37c
......@@ -701,17 +701,12 @@ WimaPlanData WimaPlaner::toPlanData()
planData.append(WimaJoinedAreaData(_joinedArea));
// convert mission items to mavlink commands
QObject deleteObject; // used to automatically delete content of rgMissionItems after this function call
QList<MissionItem*> rgMissionItems;
MissionController::convertToMissionItems(_missionController->visualItems(), rgMissionItems, this);
// add const qualifier...
QList<const MissionItem*> rgMissionItemsConst;
for (int i = _arrivalPathLength + 1; i < rgMissionItems.size() - _returnPathLength; i++) { // i = _arrivalPathLength + 1: + 1 = MissionSettingsItem ...
rgMissionItemsConst.append(rgMissionItems.value(i));
}
MissionController::convertToMissionItems(_missionController->visualItems(), rgMissionItems, &deleteObject);
// store mavlink commands
planData.append(rgMissionItemsConst);
planData.append(rgMissionItems);
return planData;
}
......
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