Skip to content
GitLab
Commit e1cd6a87 authored by Valentin Platzgummer's avatar Valentin Platzgummer
Browse files

bugs solved

parent 6cda7cf4
Hide whitespace changes
Inline Side-by-side
src/Wima/WimaController.cc
View file @ e1cd6a87
......@@ -440,7 +440,8 @@ bool WimaController::extractCoordinateList(QmlObjectListModel &missionItems, QVa
for (int i = 0; i < geoCoordintateList.size(); i++) {
QGeoCoordinate vertex = geoCoordintateList[i];
if (qFuzzyIsNull(vertex.latitude()) && qFuzzyIsNull(vertex.longitude()))
if ( (qFuzzyIsNull(vertex.latitude()) && qFuzzyIsNull(vertex.longitude()))
|| !vertex.isValid())
geoCoordintateList.removeAt(i);
}
......@@ -691,6 +692,16 @@ bool WimaController::calcNextPhase()
}
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)
missionController()->insertSimpleMissionItem(_takeoffLandPostion, 1);
SimpleMissionItem *takeoffItem = missionControllerVisuals->value<SimpleMissionItem*>(1);
......@@ -700,6 +711,16 @@ bool WimaController::calcNextPhase()
}
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
_missionController->insertSimpleMissionItem(_takeoffLandPostion, 2);
SimpleMissionItem *speedItem = missionControllerVisuals->value<SimpleMissionItem*>(2);
......@@ -707,14 +728,34 @@ bool WimaController::calcNextPhase()
qWarning("WimaController::calcNextPhase(): nullptr");
return false;
}
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);// set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCoordinate(_takeoffLandPostion);
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
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
for (auto coordinate : arrivalPath)
_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
int index = missionControllerVisuals->count();
_missionController->insertSimpleMissionItem(CSWpList.first(), index);
......@@ -723,14 +764,34 @@ bool WimaController::calcNextPhase()
qWarning("WimaController::calcNextPhase(): nullptr");
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->setCommand(MAV_CMD_DO_CHANGE_SPEED);
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
for (auto coordinate : CSWpList)
_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
index = missionControllerVisuals->count();
_missionController->insertSimpleMissionItem(CSWpList.last(), index);
......@@ -739,14 +800,34 @@ bool WimaController::calcNextPhase()
qWarning("WimaController::calcNextPhase(): nullptr");
return false;
}
speedItem->setCoordinate(_takeoffLandPostion);
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED); // set coordinate must be after setCommand (setCommand sets coordinate to zero)
speedItem->setCoordinate(CSWpList.last());
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
for (auto coordinate : returnPath)
_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
index = missionControllerVisuals->count();
_missionController->insertSimpleMissionItem(_takeoffLandPostion, index);
......@@ -757,6 +838,16 @@ bool WimaController::calcNextPhase()
}
_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
for ( int i = 1; i < missionControllerVisuals->count(); i++) {
SimpleMissionItem *visualItem = missionControllerVisuals->value<SimpleMissionItem*>(i);
......@@ -880,6 +971,8 @@ void WimaController::checkBatteryLevel()
WimaSettings* wimaSettings = qgcApp()->toolbox()->settingsManager()->wimaSettings();
int batteryThreshold = wimaSettings->lowBatteryThreshold()->rawValue().toInt();
static long attemptCounter = 0;
if (managerVehicle != nullptr) {
Fact *battery1percentRemaining = managerVehicle->battery1FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
Fact *battery2percentRemaining = managerVehicle->battery2FactGroup()->getFact(VehicleBatteryFactGroup::_percentRemainingFactName);
......@@ -890,10 +983,14 @@ void WimaController::checkBatteryLevel()
_setVehicleHasLowBattery(true);
if (!_lowBatteryHandlingTriggered) {
QString errorString;
attemptCounter++;
if (attemptCounter > 3) {
_lowBatteryHandlingTriggered = true;
attemptCounter = 0;
}
if (_checkSmartRTLPreCondition(errorString) == true) {
managerVehicle->pauseVehicle();
if (_calcReturnPath(errorString)) {
_lowBatteryHandlingTriggered = true;
if (_calcReturnPath(errorString)) {
emit returnBatteryLowConfirmRequired();
} else {
qgcApp()->showMessage(tr("Battery level is low. Smart RTL not possible."));
......
src/Wima/WimaController.h
View file @ e1cd6a87
......@@ -240,7 +240,6 @@ private:
bool _lowBatteryHandlingTriggered;
bool _executingSmartRTL;
};
/*
......
Supports Markdown
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