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

smart RTL AaR speed issue and Stat sec. duration AaR issue solved

parent 8f2e7aff
Hide whitespace changes
Inline Side-by-side
src/Wima/WimaController.cc
View file @ 3b25253b
......@@ -37,6 +37,9 @@ WimaController::WimaController(QObject *parent)
, _endWaypointIndex (0)
, _startWaypointIndex (0)
, _uploadOverrideRequired (false)
, _measurementPathLength (-1)
, _arrivalPathLength (-1)
, _returnPathLength (-1)
, _phaseDistance (-1)
, _phaseDuration (-1)
, _vehicleHasLowBattery (false)
......@@ -44,13 +47,13 @@ WimaController::WimaController(QObject *parent)
, _executingSmartRTL (false)
{
//_nextPhaseStartWaypointIndex.setRawValue(int(1));
_showAllMissionItems.setRawValue(true);
_showCurrentMissionItems.setRawValue(true);
connect(&_overlapWaypoints, &Fact::rawValueChanged, this, &WimaController::updateNextWaypoint);
connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this, &WimaController::recalcCurrentPhase);
connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase);
connect(&_flightSpeed, &Fact::rawValueChanged, this, &WimaController::updateflightSpeed);
connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this, &WimaController::updateArrivalReturnSpeed);
connect(&_altitude, &Fact::rawValueChanged, this, &WimaController::updateAltitude);
connect(&_reverse, &Fact::rawValueChanged, this, &WimaController::reverseChangedHandler);
......@@ -288,6 +291,25 @@ bool WimaController::forceUploadToVehicle()
for (int i = 0; i < _currentMissionItems.count(); i++){
SimpleMissionItem *item = _currentMissionItems.value<SimpleMissionItem *>(i);
_missionController->insertSimpleMissionItem(*item, visuals->count());
if (item->command() == MAV_CMD_DO_CHANGE_SPEED) {
qWarning() << item->missionItem().param2();
}
}
qWarning("blah");
for (int i = 0; i < _missionController->visualItems()->count(); i++){
SimpleMissionItem *item = _missionController->visualItems()->value<SimpleMissionItem*>(i);
if (item == nullptr)
continue;
if (item->command() == MAV_CMD_DO_CHANGE_SPEED) {
qWarning() << item->missionItem().param2();
}
}
for (int i = 0; i < _missionController->visualItems()->count(); i++){
SimpleMissionItem *item = _missionController->visualItems()->value<SimpleMissionItem*>(i);
if (item == nullptr)
continue;
qWarning() << i << ":" << item->command();
}
_masterController->sendToVehicle();
......@@ -640,6 +662,12 @@ bool WimaController::calcNextPhase()
}
// calculate phase length
_measurementPathLength = 0;
for (int i = 0; i < CSWpList.size()-1; ++i)
_measurementPathLength += CSWpList[i].distanceTo(CSWpList[i+1]);
// set start waypoint index for next phase
if (!lastMissionPhaseReached) {
disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this, &WimaController::calcNextPhase);
......@@ -666,20 +694,32 @@ bool WimaController::calcNextPhase()
qWarning("WimaController::calcNextPhase(): Not able to calc path from home to first waypoint.");
return false;
}
// calculate arrival path length
_arrivalPathLength = 0;
for (int i = 0; i < arrivalPath.size()-1; ++i)
_arrivalPathLength += arrivalPath[i].distanceTo(arrivalPath[i+1]);
arrivalPath.removeFirst();
arrivalPath.removeLast();
//arrivalPath.removeLast();
// calculate path from last waypoint to home
QVector<QGeoCoordinate> returnPath;
if ( !calcShortestPath(CSWpList.last(), _takeoffLandPostion, returnPath) ) {
qWarning("WimaController::calcNextPhase(): Not able to calc path from home to first waypoint.");
return false;
}
int returnPathLength = returnPath.size()-2;
}
// calculate arrival path length
_returnPathLength = 0;
for (int i = 0; i < returnPath.size()-1; ++i)
_returnPathLength += returnPath[i].distanceTo(returnPath[i+1]);
returnPath.removeFirst();
returnPath.removeLast();
// create Mission Items
_missionController->removeAll();
QmlObjectListModel* missionControllerVisuals = _missionController->visualItems();
......@@ -692,16 +732,6 @@ 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);
......@@ -711,16 +741,6 @@ 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);
......@@ -732,30 +752,10 @@ bool WimaController::calcNextPhase()
speedItem->setCoordinate(_takeoffLandPostion);
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);
......@@ -768,30 +768,10 @@ bool WimaController::calcNextPhase()
speedItem->setCoordinate(CSWpList.first());
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);
......@@ -804,30 +784,10 @@ bool WimaController::calcNextPhase()
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);
......@@ -838,16 +798,6 @@ 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);
......@@ -861,8 +811,10 @@ bool WimaController::calcNextPhase()
_currentMissionItems.append(visualItemCopy);
}
_setPhaseDistance(_missionController->missionDistance());
_setPhaseDuration(_missionController->missionDistance()/_flightSpeed.rawValue().toDouble());
_setPhaseDistance(_measurementPathLength + _arrivalPathLength + _returnPathLength);
_setPhaseDuration(_measurementPathLength/_flightSpeed.rawValue().toDouble()
+ (_arrivalPathLength + _returnPathLength)
/ _arrivalReturnSpeed.rawValue().toDouble());
_missionController->removeAll(); // remove items from _missionController, will be added on upload
updateAltitude();
......@@ -922,12 +874,14 @@ void WimaController::updateflightSpeed()
speedItemCounter++;
if (speedItemCounter == 2) {
item->missionItem().setParam2(_flightSpeed.rawValue().toDouble());
_setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble());
break;
}
}
}
_setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble()
+ (_arrivalPathLength + _returnPathLength)
/ _arrivalReturnSpeed.rawValue().toDouble());
if (speedItemCounter != 3)
qWarning("WimaController::updateflightSpeed(): internal error.");
}
......@@ -942,12 +896,13 @@ void WimaController::updateArrivalReturnSpeed()
if (speedItemCounter != 2) {
item->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());
}
if (speedItemCounter == 3)
break;
}
}
_setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble()
+ (_arrivalPathLength + _returnPathLength)
/ _arrivalReturnSpeed.rawValue().toDouble());
if (speedItemCounter != 3)
qWarning("WimaController::updateArrivalReturnSpeed(): internal error.");
......@@ -1125,9 +1080,9 @@ bool WimaController::_calcReturnPath(QString &errorSring)
qWarning("WimaController: nullptr");
return false;
}
speedItem->setCoordinate(speedItemCoordinate);
speedItem->setCommand(MAV_CMD_DO_CHANGE_SPEED);
speedItem->missionItem().setParam2(_flightSpeed.rawValue().toDouble());
speedItem->setCoordinate(speedItemCoordinate);
speedItem->missionItem().setParam2(_arrivalReturnSpeed.rawValue().toDouble());
// set second item command to ordinary waypoint (is takeoff)
SimpleMissionItem *secondItem = missionControllerVisuals->value<SimpleMissionItem*>(2);
......@@ -1162,8 +1117,10 @@ bool WimaController::_calcReturnPath(QString &errorSring)
}
//qWarning() << "_currentMissionItems.count()" << _currentMissionItems.count();
_setPhaseDistance(_missionController->missionDistance());
_setPhaseDuration(_missionController->missionDistance()/_flightSpeed.rawValue().toDouble());
_setPhaseDistance(_phaseDistance + _arrivalPathLength + _returnPathLength);
_setPhaseDuration(_phaseDistance/_flightSpeed.rawValue().toDouble()
+ (_arrivalPathLength + _returnPathLength)
/ _arrivalReturnSpeed.rawValue().toDouble());
_missionController->removeAll(); // remove items from _missionController, will be added on upload
updateAltitude();
......
src/Wima/WimaController.h
View file @ 3b25253b
......@@ -232,6 +232,9 @@ private:
// (which is not part of the arrival path) of _currentMissionItem
bool _uploadOverrideRequired; // Is set to true if uploadToVehicle() did not suceed because the vehicle is not inside the service area.
// The user can override the upload lock with a slider, this will reset this variable to false.
double _measurementPathLength; // the lenght of the phase in meters
double _arrivalPathLength; // the length of the arrival and return path in meters
double _returnPathLength; // the length of the arrival and return path in meters
double _phaseDistance; // the lenth in meters of the current phase
double _phaseDuration; // the phase duration in seconds
......
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