|
|
|
@ -57,16 +57,7 @@ MissionController::MissionController(QObject *parent)
@@ -57,16 +57,7 @@ MissionController::MissionController(QObject *parent)
|
|
|
|
|
, _fwLandingMissionItemName(tr("Fixed Wing Landing")) |
|
|
|
|
, _appSettings(qgcApp()->toolbox()->settingsManager()->appSettings()) |
|
|
|
|
{ |
|
|
|
|
_missionFlightStatus.maxTelemetryDistance = 0; |
|
|
|
|
_missionFlightStatus.totalDistance = 0; |
|
|
|
|
_missionFlightStatus.totalTime = 0; |
|
|
|
|
_missionFlightStatus.hoverDistance = 0; |
|
|
|
|
_missionFlightStatus.hoverTime = 0; |
|
|
|
|
_missionFlightStatus.cruiseDistance = 0; |
|
|
|
|
_missionFlightStatus.cruiseTime = 0; |
|
|
|
|
_missionFlightStatus.cruiseSpeed = 0; |
|
|
|
|
_missionFlightStatus.hoverSpeed = 0; |
|
|
|
|
_missionFlightStatus.gimbalYaw = 0; |
|
|
|
|
_resetMissionFlightStatus(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
MissionController::~MissionController() |
|
|
|
@ -74,6 +65,40 @@ MissionController::~MissionController()
@@ -74,6 +65,40 @@ MissionController::~MissionController()
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_resetMissionFlightStatus(void) |
|
|
|
|
{ |
|
|
|
|
_missionFlightStatus.totalDistance = 0.0; |
|
|
|
|
_missionFlightStatus.maxTelemetryDistance = 0.0; |
|
|
|
|
_missionFlightStatus.totalTime = 0.0; |
|
|
|
|
_missionFlightStatus.hoverTime = 0.0; |
|
|
|
|
_missionFlightStatus.cruiseTime = 0.0; |
|
|
|
|
_missionFlightStatus.hoverDistance = 0.0; |
|
|
|
|
_missionFlightStatus.cruiseDistance = 0.0; |
|
|
|
|
_missionFlightStatus.cruiseSpeed = _activeVehicle ? _activeVehicle->defaultCruiseSpeed() : std::numeric_limits<double>::quiet_NaN(); |
|
|
|
|
_missionFlightStatus.hoverSpeed = _activeVehicle ? _activeVehicle->defaultHoverSpeed() : std::numeric_limits<double>::quiet_NaN(); |
|
|
|
|
_missionFlightStatus.vehicleSpeed = _activeVehicle ? (_activeVehicle->multiRotor() || _activeVehicle->vtol() ? _missionFlightStatus.hoverSpeed : _missionFlightStatus.cruiseSpeed) : std::numeric_limits<double>::quiet_NaN(); |
|
|
|
|
_missionFlightStatus.gimbalYaw = std::numeric_limits<double>::quiet_NaN(); |
|
|
|
|
|
|
|
|
|
// Battery information
|
|
|
|
|
|
|
|
|
|
_missionFlightStatus.mAhBattery = 0; |
|
|
|
|
_missionFlightStatus.hoverAmps = 0; |
|
|
|
|
_missionFlightStatus.cruiseAmps = 0; |
|
|
|
|
_missionFlightStatus.ampMinutesAvailable = 0; |
|
|
|
|
_missionFlightStatus.hoverAmpsTotal = 0; |
|
|
|
|
_missionFlightStatus.cruiseAmpsTotal = 0; |
|
|
|
|
_missionFlightStatus.batteryChangePoint = -1; |
|
|
|
|
_missionFlightStatus.batteriesRequired = -1; |
|
|
|
|
|
|
|
|
|
if (_activeVehicle) { |
|
|
|
|
_activeVehicle->firmwarePlugin()->batteryConsumptionData(_activeVehicle, _missionFlightStatus.mAhBattery, _missionFlightStatus.hoverAmps, _missionFlightStatus.cruiseAmps); |
|
|
|
|
if (_missionFlightStatus.mAhBattery != 0) { |
|
|
|
|
double batteryPercentRemainingAnnounce = qgcApp()->toolbox()->settingsManager()->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toDouble(); |
|
|
|
|
_missionFlightStatus.ampMinutesAvailable = (double)_missionFlightStatus.mAhBattery / 1000.0 * 60.0 * ((100.0 - batteryPercentRemainingAnnounce) / 100.0); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::start(bool editMode) |
|
|
|
|
{ |
|
|
|
|
qCDebug(MissionControllerLog) << "start editMode" << editMode; |
|
|
|
@ -871,6 +896,36 @@ void MissionController::_recalcWaypointLines(void)
@@ -871,6 +896,36 @@ void MissionController::_recalcWaypointLines(void)
|
|
|
|
|
emit waypointLinesChanged(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_updateBatteryInfo(int waypointIndex) |
|
|
|
|
{ |
|
|
|
|
if (_missionFlightStatus.mAhBattery != 0) { |
|
|
|
|
_missionFlightStatus.hoverAmpsTotal = (_missionFlightStatus.hoverTime / 60.0) * _missionFlightStatus.hoverAmps; |
|
|
|
|
_missionFlightStatus.cruiseAmpsTotal = (_missionFlightStatus.cruiseTime / 60.0) * _missionFlightStatus.cruiseAmps; |
|
|
|
|
_missionFlightStatus.batteriesRequired = ceil((_missionFlightStatus.hoverAmpsTotal + _missionFlightStatus.cruiseAmpsTotal) / _missionFlightStatus.ampMinutesAvailable); |
|
|
|
|
if (_missionFlightStatus.batteriesRequired == 2 && _missionFlightStatus.batteryChangePoint == -1) { |
|
|
|
|
_missionFlightStatus.batteryChangePoint = waypointIndex - 1; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_addHoverTime(double hoverTime, double hoverDistance, int waypointIndex) |
|
|
|
|
{ |
|
|
|
|
_missionFlightStatus.totalTime += hoverTime; |
|
|
|
|
_missionFlightStatus.hoverTime += hoverTime; |
|
|
|
|
_missionFlightStatus.hoverDistance += hoverDistance; |
|
|
|
|
_missionFlightStatus.totalDistance += hoverDistance; |
|
|
|
|
_updateBatteryInfo(waypointIndex); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_addCruiseTime(double cruiseTime, double cruiseDistance, int waypointIndex) |
|
|
|
|
{ |
|
|
|
|
_missionFlightStatus.totalTime += cruiseTime; |
|
|
|
|
_missionFlightStatus.cruiseTime += cruiseTime; |
|
|
|
|
_missionFlightStatus.cruiseDistance += cruiseDistance; |
|
|
|
|
_missionFlightStatus.totalDistance += cruiseDistance; |
|
|
|
|
_updateBatteryInfo(waypointIndex); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_recalcMissionFlightStatus() |
|
|
|
|
{ |
|
|
|
|
if (!_visualItems->count()) { |
|
|
|
@ -900,17 +955,7 @@ void MissionController::_recalcMissionFlightStatus()
@@ -900,17 +955,7 @@ void MissionController::_recalcMissionFlightStatus()
|
|
|
|
|
|
|
|
|
|
double lastVehicleYaw = 0; |
|
|
|
|
|
|
|
|
|
_missionFlightStatus.totalDistance = 0.0; |
|
|
|
|
_missionFlightStatus.maxTelemetryDistance = 0.0; |
|
|
|
|
_missionFlightStatus.totalTime = 0.0; |
|
|
|
|
_missionFlightStatus.hoverTime = 0.0; |
|
|
|
|
_missionFlightStatus.cruiseTime = 0.0; |
|
|
|
|
_missionFlightStatus.hoverDistance = 0.0; |
|
|
|
|
_missionFlightStatus.cruiseDistance = 0.0; |
|
|
|
|
_missionFlightStatus.cruiseSpeed = _activeVehicle->defaultCruiseSpeed(); |
|
|
|
|
_missionFlightStatus.hoverSpeed = _activeVehicle->defaultHoverSpeed(); |
|
|
|
|
_missionFlightStatus.vehicleSpeed = _activeVehicle->multiRotor() || _activeVehicle->vtol() ? _missionFlightStatus.hoverSpeed : _missionFlightStatus.cruiseSpeed; |
|
|
|
|
_missionFlightStatus.gimbalYaw = std::numeric_limits<double>::quiet_NaN(); |
|
|
|
|
_resetMissionFlightStatus(); |
|
|
|
|
|
|
|
|
|
bool vtolInHover = true; |
|
|
|
|
bool linkBackToHome = false; |
|
|
|
@ -1022,7 +1067,6 @@ void MissionController::_recalcMissionFlightStatus()
@@ -1022,7 +1067,6 @@ void MissionController::_recalcMissionFlightStatus()
|
|
|
|
|
item->setAzimuth(azimuth); |
|
|
|
|
item->setDistance(distance); |
|
|
|
|
|
|
|
|
|
_missionFlightStatus.totalDistance += distance; |
|
|
|
|
_missionFlightStatus.maxTelemetryDistance = qMax(_missionFlightStatus.maxTelemetryDistance, _calcDistanceToHome(item, _settingsItem)); |
|
|
|
|
|
|
|
|
|
// Calculate time/distance
|
|
|
|
@ -1030,24 +1074,15 @@ void MissionController::_recalcMissionFlightStatus()
@@ -1030,24 +1074,15 @@ void MissionController::_recalcMissionFlightStatus()
|
|
|
|
|
double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; |
|
|
|
|
if (_activeVehicle->vtol()) { |
|
|
|
|
if (vtolInHover) { |
|
|
|
|
_missionFlightStatus.totalTime += hoverTime; |
|
|
|
|
_missionFlightStatus.hoverTime += hoverTime; |
|
|
|
|
_missionFlightStatus.hoverDistance += distance; |
|
|
|
|
_addHoverTime(hoverTime, distance, item->sequenceNumber()); |
|
|
|
|
} else { |
|
|
|
|
_missionFlightStatus.totalTime += cruiseTime; |
|
|
|
|
_missionFlightStatus.cruiseTime += cruiseTime; |
|
|
|
|
_missionFlightStatus.cruiseDistance += distance; |
|
|
|
|
_addCruiseTime(cruiseTime, distance, item->sequenceNumber()); |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
if (_activeVehicle->multiRotor()) { |
|
|
|
|
_missionFlightStatus.totalTime += hoverTime; |
|
|
|
|
_missionFlightStatus.hoverTime += hoverTime; |
|
|
|
|
_missionFlightStatus.hoverDistance += distance; |
|
|
|
|
_addHoverTime(hoverTime, distance, item->sequenceNumber()); |
|
|
|
|
} else { |
|
|
|
|
_missionFlightStatus.totalTime += cruiseTime; |
|
|
|
|
_missionFlightStatus.cruiseTime += cruiseTime; |
|
|
|
|
_missionFlightStatus.cruiseDistance += distance; |
|
|
|
|
|
|
|
|
|
_addCruiseTime(cruiseTime, distance, item->sequenceNumber()); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -1055,31 +1090,21 @@ void MissionController::_recalcMissionFlightStatus()
@@ -1055,31 +1090,21 @@ void MissionController::_recalcMissionFlightStatus()
|
|
|
|
|
if (complexItem) { |
|
|
|
|
// Add in distance/time inside complex items as well
|
|
|
|
|
double distance = complexItem->complexDistance(); |
|
|
|
|
_missionFlightStatus.totalDistance += distance; |
|
|
|
|
_missionFlightStatus.maxTelemetryDistance = qMax(_missionFlightStatus.maxTelemetryDistance, complexItem->greatestDistanceTo(complexItem->exitCoordinate())); |
|
|
|
|
|
|
|
|
|
double hoverTime = _missionFlightStatus.totalDistance / _missionFlightStatus.hoverSpeed; |
|
|
|
|
double cruiseTime = _missionFlightStatus.totalDistance / _missionFlightStatus.cruiseSpeed; |
|
|
|
|
double hoverTime = distance / _missionFlightStatus.hoverSpeed; |
|
|
|
|
double cruiseTime = distance / _missionFlightStatus.cruiseSpeed; |
|
|
|
|
if (_activeVehicle->vtol()) { |
|
|
|
|
if (vtolInHover) { |
|
|
|
|
_missionFlightStatus.totalTime += hoverTime; |
|
|
|
|
_missionFlightStatus.hoverTime += hoverTime; |
|
|
|
|
_missionFlightStatus.hoverDistance += distance; |
|
|
|
|
_addHoverTime(hoverTime, distance, item->sequenceNumber()); |
|
|
|
|
} else { |
|
|
|
|
_missionFlightStatus.totalTime += cruiseTime; |
|
|
|
|
_missionFlightStatus.cruiseTime += cruiseTime; |
|
|
|
|
_missionFlightStatus.cruiseDistance += distance; |
|
|
|
|
_addCruiseTime(cruiseTime, distance, item->sequenceNumber()); |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
if (_activeVehicle->multiRotor()) { |
|
|
|
|
_missionFlightStatus.totalTime += hoverTime; |
|
|
|
|
_missionFlightStatus.hoverTime += hoverTime; |
|
|
|
|
_missionFlightStatus.hoverDistance += distance; |
|
|
|
|
_addHoverTime(hoverTime, distance, item->sequenceNumber()); |
|
|
|
|
} else { |
|
|
|
|
_missionFlightStatus.totalTime += cruiseTime; |
|
|
|
|
_missionFlightStatus.cruiseTime += cruiseTime; |
|
|
|
|
_missionFlightStatus.cruiseDistance += distance; |
|
|
|
|
|
|
|
|
|
_addCruiseTime(cruiseTime, distance, item->sequenceNumber()); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -1092,6 +1117,9 @@ void MissionController::_recalcMissionFlightStatus()
@@ -1092,6 +1117,9 @@ void MissionController::_recalcMissionFlightStatus()
|
|
|
|
|
} |
|
|
|
|
lastCoordinateItem->setMissionVehicleYaw(lastVehicleYaw); |
|
|
|
|
|
|
|
|
|
if (_missionFlightStatus.mAhBattery != 0 && _missionFlightStatus.batteryChangePoint == -1) { |
|
|
|
|
_missionFlightStatus.batteryChangePoint = 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
emit missionMaxTelemetryChanged(_missionFlightStatus.maxTelemetryDistance); |
|
|
|
|
emit missionDistanceChanged(_missionFlightStatus.totalDistance); |
|
|
|
@ -1100,6 +1128,8 @@ void MissionController::_recalcMissionFlightStatus()
@@ -1100,6 +1128,8 @@ void MissionController::_recalcMissionFlightStatus()
|
|
|
|
|
emit missionTimeChanged(); |
|
|
|
|
emit missionHoverTimeChanged(); |
|
|
|
|
emit missionCruiseTimeChanged(); |
|
|
|
|
emit batteryChangePointChanged(_missionFlightStatus.batteryChangePoint); |
|
|
|
|
emit batteriesRequiredChanged(_missionFlightStatus.batteriesRequired); |
|
|
|
|
|
|
|
|
|
// Walk the list again calculating altitude percentages
|
|
|
|
|
double altRange = maxAltSeen - minAltSeen; |
|
|
|
@ -1326,62 +1356,6 @@ void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate&
@@ -1326,62 +1356,6 @@ void MissionController::_activeVehicleHomePositionChanged(const QGeoCoordinate&
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_setMissionMaxTelemetry(double missionMaxTelemetry) |
|
|
|
|
{ |
|
|
|
|
if (!qFuzzyCompare(_missionFlightStatus.maxTelemetryDistance, missionMaxTelemetry)) { |
|
|
|
|
_missionFlightStatus.maxTelemetryDistance = missionMaxTelemetry; |
|
|
|
|
emit missionMaxTelemetryChanged(missionMaxTelemetry); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_setMissionDistance(double missionDistance) |
|
|
|
|
{ |
|
|
|
|
if (!qFuzzyCompare(_missionFlightStatus.totalDistance, missionDistance)) { |
|
|
|
|
_missionFlightStatus.totalDistance = missionDistance; |
|
|
|
|
emit missionDistanceChanged(missionDistance); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_setMissionTime(double missionTime) |
|
|
|
|
{ |
|
|
|
|
if (!qFuzzyCompare(_missionFlightStatus.totalTime, missionTime)) { |
|
|
|
|
_missionFlightStatus.totalTime = missionTime; |
|
|
|
|
emit missionTimeChanged(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_setMissionHoverTime(double missionHoverTime) |
|
|
|
|
{ |
|
|
|
|
if (!qFuzzyCompare(_missionFlightStatus.hoverTime, missionHoverTime)) { |
|
|
|
|
_missionFlightStatus.hoverTime = missionHoverTime; |
|
|
|
|
emit missionHoverTimeChanged(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_setMissionHoverDistance(double missionHoverDistance) |
|
|
|
|
{ |
|
|
|
|
if (!qFuzzyCompare(_missionFlightStatus.hoverDistance, missionHoverDistance)) { |
|
|
|
|
_missionFlightStatus.hoverDistance = missionHoverDistance; |
|
|
|
|
emit missionHoverDistanceChanged(missionHoverDistance); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_setMissionCruiseTime(double missionCruiseTime) |
|
|
|
|
{ |
|
|
|
|
if (!qFuzzyCompare(_missionFlightStatus.cruiseTime, missionCruiseTime)) { |
|
|
|
|
_missionFlightStatus.cruiseTime = missionCruiseTime; |
|
|
|
|
emit missionCruiseTimeChanged(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_setMissionCruiseDistance(double missionCruiseDistance) |
|
|
|
|
{ |
|
|
|
|
if (!qFuzzyCompare(_missionFlightStatus.cruiseDistance, missionCruiseDistance)) { |
|
|
|
|
_missionFlightStatus.cruiseDistance = missionCruiseDistance; |
|
|
|
|
emit missionCruiseDistanceChanged(missionCruiseDistance); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void MissionController::_inProgressChanged(bool inProgress) |
|
|
|
|
{ |
|
|
|
|
emit syncInProgressChanged(inProgress); |
|
|
|
|