Skip to content

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

bugs solved

parent 6cda7cf4
Hide whitespace changes
Inline Side-by-side
Showing with 104 additions and 8 deletions
src/Wima/WimaController.cc
View file @ e1cd6a87
...@@ -440,7 +440,8 @@ bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVa ...@@ -440,7 +440,8 @@ bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVa
for (int i = 0; i < geoCoordintateList.size(); i++) { for (int i = 0; i < geoCoordintateList.size(); i++) {
QGeoCoordinate vertex = geoCoordintateList[i]; QGeoCoordinate vertex = geoCoordintateList[i];
if (qFuzzyIsNull(vertex.latitude()) && qFuzzyIsNull(vertex.longitude())) if ( (qFuzzyIsNull(vertex.latitude()) && qFuzzyIsNull(vertex.longitude()))
|| !vertex.isValid())
geoCoordintateList.removeAt(i); geoCoordintateList.removeAt(i);
} }
...@@ -691,6 +692,16 @@ bool WimaController::calcNextPhase() ...@@ -691,6 +692,16 @@ bool WimaController::calcNextPhase()
} }
settingsItem->setCoordinate(_takeoffLandPostion); settingsItem->setCoordinate(_takeoffLandPostion);
// qDebug("homeposition");
// for (int i = 0; i < missionControllerVisuals->count(); ++i) {
// VisualMissionItem *item = missionControllerVisuals->value<VisualMissionItem*>(i);
// if (item != nullptr) {
// qDebug() << item->coordinate();
// } else {
// qDebug("Error");
// }
// }
// set takeoff position for first mission item (bug) // set takeoff position for first mission item (bug)
missionController()->insertSimpleMissionItem(_takeoffLandPostion, 1); missionController()->insertSimpleMissionItem(_takeoffLandPostion, 1);
SimpleMissionItem *takeoffItem = missionControllerVisuals->value<SimpleMissionItem*>(1); SimpleMissionItem *takeoffItem = missionControllerVisuals->value<SimpleMissionItem*>(1);
...@@ -700,6 +711,16 @@ bool WimaController::calcNextPhase() ...@@ -700,6 +711,16 @@ bool WimaController::calcNextPhase()
} }
takeoffItem->setCoordinate(_takeoffLandPostion); takeoffItem->setCoordinate(_takeoffLandPostion);
// qDebug("takeoff");
// for (int i = 0; i < missionControllerVisuals->count(); ++i) {
// VisualMissionItem *item = missionControllerVisuals->value<VisualMissionItem*>(i);
// if (item != nullptr) {
// qDebug() << item->coordinate();
// } else {
// qDebug("Error");
// }
// }
// create change speed item, after take off // create change speed item, after take off
_missionController->insertSimpleMissionItem(_takeoffLandPostion, 2); _missionController->insertSimpleMissionItem(_takeoffLandPostion, 2);
SimpleMissionItem *speedItem = missionControllerVisuals->value<SimpleMissionItem*>(2); SimpleMissionItem *speedItem = missionControllerVisuals->value<SimpleMissionItem*>(2);
...@@ -707,14 +728,34 @@ bool WimaController::calcNextPhase() ...@@ -707,14 +728,34 @@ bool WimaController::calcNextPhase()
qWarning("WimaController::calcNextPhase(): nullptr"); qWarning("WimaController::calcNextPhase(): nullptr");
return false; return false;
} }
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);// set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCoordinate(_takeoffLandPostion); speedItem->setCoordinate(_takeoffLandPostion);
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble()); speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());
// qDebug("speed");
// for (int i = 0; i < missionControllerVisuals->count(); ++i) {
// VisualMissionItem *item = missionControllerVisuals->value<VisualMissionItem*>(i);
// if (item != nullptr) {
// qDebug() << item->coordinate();
// } else {
// qDebug("Error");
// }
// }
// insert arrival path // insert arrival path
for (auto coordinate : arrivalPath) for (auto coordinate : arrivalPath)
_missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count()); _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
// qDebug("arrival");
// for (int i = 0; i < missionControllerVisuals->count(); ++i) {
// VisualMissionItem *item = missionControllerVisuals->value<VisualMissionItem*>(i);
// if (item != nullptr) {
// qDebug() << item->coordinate();
// } else {
// qDebug("Error");
// }
// }
// create change speed item, after arrival path // create change speed item, after arrival path
int index = missionControllerVisuals->count(); int index = missionControllerVisuals->count();
_missionController->insertSimpleMissionItem(CSWpList.first(), index); _missionController->insertSimpleMissionItem(CSWpList.first(), index);
...@@ -723,14 +764,34 @@ bool WimaController::calcNextPhase() ...@@ -723,14 +764,34 @@ bool WimaController::calcNextPhase()
qWarning("WimaController::calcNextPhase(): nullptr"); qWarning("WimaController::calcNextPhase(): nullptr");
return false; return false;
} }
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCoordinate(CSWpList.first()); speedItem->setCoordinate(CSWpList.first());
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
speedItem->missionItem().setParam2(_flightSpeed.rawValue().toDouble()); speedItem->missionItem().setParam2(_flightSpeed.rawValue().toDouble());
// qDebug("speed");
// for (int i = 0; i < missionControllerVisuals->count(); ++i) {
// VisualMissionItem *item = missionControllerVisuals->value<VisualMissionItem*>(i);
// if (item != nullptr) {
// qDebug() << item->coordinate();
// } else {
// qDebug("Error");
// }
// }
// insert Circular Survey coordinates // insert Circular Survey coordinates
for (auto coordinate : CSWpList) for (auto coordinate : CSWpList)
_missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count()); _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
// qDebug("survey");
// for (int i = 0; i < missionControllerVisuals->count(); ++i) {
// VisualMissionItem *item = missionControllerVisuals->value<VisualMissionItem*>(i);
// if (item != nullptr) {
// qDebug() << item->coordinate();
// } else {
// qDebug("Error");
// }
// }
// create change speed item, after circular survey // create change speed item, after circular survey
index = missionControllerVisuals->count(); index = missionControllerVisuals->count();
_missionController->insertSimpleMissionItem(CSWpList.last(), index); _missionController->insertSimpleMissionItem(CSWpList.last(), index);
...@@ -739,14 +800,34 @@ bool WimaController::calcNextPhase() ...@@ -739,14 +800,34 @@ bool WimaController::calcNextPhase()
qWarning("WimaController::calcNextPhase(): nullptr"); qWarning("WimaController::calcNextPhase(): nullptr");
return false; return false;
} }
speedItem->setCoordinate(_takeoffLandPostion); speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); speedItem->setCoordinate(CSWpList.last());
speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble()); speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());
// qDebug("speed");
// for (int i = 0; i < missionControllerVisuals->count(); ++i) {
// VisualMissionItem *item = missionControllerVisuals->value<VisualMissionItem*>(i);
// if (item != nullptr) {
// qDebug() << item->coordinate();
// } else {
// qDebug("Error");
// }
// }
// insert return path coordinates // insert return path coordinates
for (auto coordinate : returnPath) for (auto coordinate : returnPath)
_missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count()); _missionController->insertSimpleMissionItem(coordinate, missionControllerVisuals->count());
// qDebug("return path");
// for (int i = 0; i < missionControllerVisuals->count(); ++i) {
// VisualMissionItem *item = missionControllerVisuals->value<VisualMissionItem*>(i);
// if (item != nullptr) {
// qDebug() << item->coordinate();
// } else {
// qDebug("Error");
// }
// }
// set land command for last mission item // set land command for last mission item
index = missionControllerVisuals->count(); index = missionControllerVisuals->count();
_missionController->insertSimpleMissionItem(_takeoffLandPostion, index); _missionController->insertSimpleMissionItem(_takeoffLandPostion, index);
...@@ -757,6 +838,16 @@ bool WimaController::calcNextPhase() ...@@ -757,6 +838,16 @@ bool WimaController::calcNextPhase()
} }
_missionController->setLandCommand(*landItem); _missionController->setLandCommand(*landItem);
// qDebug("land");
// for (int i = 0; i < missionControllerVisuals->count(); ++i) {
// VisualMissionItem *item = missionControllerVisuals->value<VisualMissionItem*>(i);
// if (item != nullptr) {
// qDebug() << item->coordinate();
// } else {
// qDebug("Error");
// }
// }
// copy to _currentMissionItems // copy to _currentMissionItems
for ( int i = 1; i < missionControllerVisuals->count(); i++) { for ( int i = 1; i < missionControllerVisuals->count(); i++) {
SimpleMissionItem *visualItem = missionControllerVisuals->value<SimpleMissionItem*>(i); SimpleMissionItem *visualItem = missionControllerVisuals->value<SimpleMissionItem*>(i);
...@@ -880,6 +971,8 @@ void WimaController::checkBatteryLevel() ...@@ -880,6 +971,8 @@ void WimaController::checkBatteryLevel()
WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings(); WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings();
int batteryThreshold = wimaSettings->lowBatteryThreshold()->rawValue().toInt(); int batteryThreshold = wimaSettings->lowBatteryThreshold()->rawValue().toInt();
static long attemptCounter = 0;
if (managerVehicle != nullptr) { if (managerVehicle != nullptr) {
Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName); Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName); Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
...@@ -890,10 +983,14 @@ void WimaController::checkBatteryLevel() ...@@ -890,10 +983,14 @@ void WimaController::checkBatteryLevel()
_setVehicleHasLowBattery(true); _setVehicleHasLowBattery(true);
if (!_lowBatteryHandlingTriggered) { if (!_lowBatteryHandlingTriggered) {
QString errorString; QString errorString;
attemptCounter++;
if (attemptCounter > 3) {
_lowBatteryHandlingTriggered = true;
attemptCounter = 0;
}
if (_checkSmartRTLPreCondition(errorString) == true) { if (_checkSmartRTLPreCondition(errorString) == true) {
managerVehicle->pauseVehicle(); managerVehicle->pauseVehicle();
if (_calcReturnPath(errorString)) { if (_calcReturnPath(errorString)) {
_lowBatteryHandlingTriggered = true;
emit returnBatteryLowConfirmRequired(); emit returnBatteryLowConfirmRequired();
} else { } else {
qgcApp()->showMessage(tr("Battery level is low. Smart RTL not possible.")); qgcApp()->showMessage(tr("Battery level is low. Smart RTL not possible."));
......
src/Wima/WimaController.h
View file @ e1cd6a87
...@@ -240,7 +240,6 @@ private: ...@@ -240,7 +240,6 @@ private:
bool _lowBatteryHandlingTriggered; bool _lowBatteryHandlingTriggered;
bool _executingSmartRTL; 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