diff --git a/src/Wima/WimaController.cc b/src/Wima/WimaController.cc index 2789098c0b7d69ee999fa654048121a971230486..decad5bd779d4d354ae12f27cae7ea8ae9eb0f37 100644 --- a/src/Wima/WimaController.cc +++ b/src/Wima/WimaController.cc @@ -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(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(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(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 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(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(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(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(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(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(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(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(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(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(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(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(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(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(); diff --git a/src/Wima/WimaController.h b/src/Wima/WimaController.h index 180b3d3fc8c364e450e90231564f51fd47b5e706..5c958e44e3618f6cf1b3e25241eb69598a95627e 100644 --- a/src/Wima/WimaController.h +++ b/src/Wima/WimaController.h @@ -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