diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 17f583fae9ce91295e8b624914e533bd6d7446ef..a573979a31e162fec5e4f700c9addfd93259d14f 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -437,3 +437,11 @@ bool FirmwarePlugin::_armVehicle(Vehicle* vehicle) } return vehicle->armed(); } + +void FirmwarePlugin::batteryConsumptionData(Vehicle* vehicle, int& mAhBattery, int& hoverAmps, int& cruiseAmps) const +{ + Q_UNUSED(vehicle); + mAhBattery = 0; + hoverAmps = 0; + cruiseAmps = 0; +} diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index fa964cf3369005e8f4c9cb6957afd045d93a9c4b..06cdc935c40c657d9e5ed1a4b57b11c07c0b8a3c 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -276,6 +276,12 @@ public: /// @true: When flying a mission the vehicle is always facing towards the next waypoint virtual bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const; + /// Returns the data needed to do battery consumption calculations + /// @param[out] mAhBattery Battery milliamp-hours rating (0 for no battery data available) + /// @param[out] hoverAmps Current draw in amps during hover + /// @param[out] cruiseAmps Current draw in amps during cruise + virtual void batteryConsumptionData(Vehicle* vehicle, int& mAhBattery, int& hoverAmps, int& cruiseAmps) const; + // FIXME: Hack workaround for non pluginize FollowMe support static const char* px4FollowMeFlightMode; diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 6a704a75152538ef8b9ef3d958120087e16ff730..490416d624e23f8a39b56c99ee6961203f7bf8b6 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -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() } +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::quiet_NaN(); + _missionFlightStatus.hoverSpeed = _activeVehicle ? _activeVehicle->defaultHoverSpeed() : std::numeric_limits::quiet_NaN(); + _missionFlightStatus.vehicleSpeed = _activeVehicle ? (_activeVehicle->multiRotor() || _activeVehicle->vtol() ? _missionFlightStatus.hoverSpeed : _missionFlightStatus.cruiseSpeed) : std::numeric_limits::quiet_NaN(); + _missionFlightStatus.gimbalYaw = std::numeric_limits::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) 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() 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::quiet_NaN(); + _resetMissionFlightStatus(); bool vtolInHover = true; bool linkBackToHome = false; @@ -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() 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() 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() } 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() 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& } } -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); diff --git a/src/MissionManager/MissionController.h b/src/MissionManager/MissionController.h index 1a893ab64926e46f855269c8ad51ccd1a022f2fd..85efdcf3fc50edf8f25dff7eb3250cbb0a691e01 100644 --- a/src/MissionManager/MissionController.h +++ b/src/MissionManager/MissionController.h @@ -38,17 +38,25 @@ public: ~MissionController(); typedef struct { - double maxTelemetryDistance; - double totalDistance; - double totalTime; - double hoverDistance; - double hoverTime; - double cruiseDistance; - double cruiseTime; - double cruiseSpeed; - double hoverSpeed; - double vehicleSpeed; //& rgMissionItems, QObject* missionItemParent); void _setPlannedHomePositionFromFirstCoordinate(void); + void _resetMissionFlightStatus(void); + void _addHoverTime(double hoverTime, double hoverDistance, int waypointIndex); + void _addCruiseTime(double cruiseTime, double cruiseDistance, int wayPointIndex); + void _updateBatteryInfo(int waypointIndex); // Overrides from PlanElementController void _activeVehicleBeingRemoved(void) final; diff --git a/src/PlanView/PlanToolBar.qml b/src/PlanView/PlanToolBar.qml index 550aebb7b0dc923e377b7ff203356226d22a58a7..dc74e8305c501f3ead7777a0f0db6be255cc878b 100644 --- a/src/PlanView/PlanToolBar.qml +++ b/src/PlanView/PlanToolBar.qml @@ -51,14 +51,18 @@ Rectangle { property real _missionDistance: _missionValid ? missionDistance : NaN property real _missionMaxTelemetry: _missionValid ? missionMaxTelemetry : NaN property real _missionTime: _missionValid ? missionTime : NaN - - property string _distanceText: isNaN(_distance) ? "-.-" : QGroundControl.metersToAppSettingsDistanceUnits(_distance).toFixed(1) + " " + QGroundControl.appSettingsDistanceUnitsString - property string _altDifferenceText: isNaN(_altDifference) ? "-.-" : QGroundControl.metersToAppSettingsDistanceUnits(_altDifference).toFixed(1) + " " + QGroundControl.appSettingsDistanceUnitsString - property string _gradientText: isNaN(_gradient) ? "-.-" : _gradientPercent.toFixed(0) + "%" - property string _azimuthText: isNaN(_azimuth) ? "-.-" : Math.round(_azimuth) - property string _missionDistanceText: isNaN(_missionDistance) ? "-.-" : QGroundControl.metersToAppSettingsDistanceUnits(_missionDistance).toFixed(1) + " " + QGroundControl.appSettingsDistanceUnitsString - property string _missionTimeText: isNaN(_missionTime) ? "-.-" : Number(_missionTime / 60).toFixed(1) + " min" - property string _missionMaxTelemetryText: isNaN(_missionMaxTelemetry) ? "-.-" : QGroundControl.metersToAppSettingsDistanceUnits(_missionMaxTelemetry).toFixed(1) + " " + QGroundControl.appSettingsDistanceUnitsString + property int _batteryChangePoint: _controllerValid ? missionController.batteryChangePoint : -1 + property int _batteriesRequired: _controllerValid ? missionController.batteriesRequired : -1 + + property string _distanceText: isNaN(_distance) ? "-.-" : QGroundControl.metersToAppSettingsDistanceUnits(_distance).toFixed(1) + " " + QGroundControl.appSettingsDistanceUnitsString + property string _altDifferenceText: isNaN(_altDifference) ? "-.-" : QGroundControl.metersToAppSettingsDistanceUnits(_altDifference).toFixed(1) + " " + QGroundControl.appSettingsDistanceUnitsString + property string _gradientText: isNaN(_gradient) ? "-.-" : _gradientPercent.toFixed(0) + "%" + property string _azimuthText: isNaN(_azimuth) ? "-.-" : Math.round(_azimuth) + property string _missionDistanceText: isNaN(_missionDistance) ? "-.-" : QGroundControl.metersToAppSettingsDistanceUnits(_missionDistance).toFixed(1) + " " + QGroundControl.appSettingsDistanceUnitsString + property string _missionTimeText: isNaN(_missionTime) ? "-.-" : Number(_missionTime / 60).toFixed(1) + " min" + property string _missionMaxTelemetryText: isNaN(_missionMaxTelemetry) ? "-.-" : QGroundControl.metersToAppSettingsDistanceUnits(_missionMaxTelemetry).toFixed(1) + " " + QGroundControl.appSettingsDistanceUnitsString + property string _batteryChangePointText: _batteryChangePoint < 0 ? "N/A" : _batteryChangePoint + property string _batteriesRequiredText: _batteriesRequired < 0 ? "N/A" : _batteriesRequired readonly property real _margins: ScreenTools.defaultFontPixelWidth @@ -164,12 +168,12 @@ Rectangle { } QGCLabel { text: qsTr("Batteries required:") } - QGCLabel { text: "--.--" } + QGCLabel { text: _batteriesRequiredText } Item { width: 1; height: 1 } QGCLabel { text: qsTr("Swap waypoint:") } - QGCLabel { text: "--" } + QGCLabel { text: _batteryChangePointText } } }