Skip to content

Q
qgroundcontrol
Commit 246bfba0 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #4903 from DonLakeFlyer/BatteryCalc 

Plan: Support battery calculations
parents 00f02f07 9bac4fbc
Hide whitespace changes
Inline Side-by-side
Showing with 140 additions and 135 deletions
src/FirmwarePlugin/FirmwarePlugin.cc
View file @ 246bfba0
......@@ -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;
}
src/FirmwarePlugin/FirmwarePlugin.h
View file @ 246bfba0
......@@ -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;
......
src/MissionManager/MissionController.cc
View file @ 246bfba0
......@@ -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<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)
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<double>::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);
......
src/MissionManager/MissionController.h
View file @ 246bfba0
......@@ -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; //</ Either cruise or hover speed based on vehicle type and vtol state
double gimbalYaw; ///< NaN signals yaw was never changed
double maxTelemetryDistance;
double totalDistance;
double totalTime;
double hoverDistance;
double hoverTime;
double cruiseDistance;
double cruiseTime;
double cruiseSpeed;
double hoverSpeed;
double vehicleSpeed; ///< Either cruise or hover speed based on vehicle type and vtol state
double gimbalYaw; ///< NaN signals yaw was never changed
int mAhBattery; ///< 0 for not available
int hoverAmps; ///< Amp consumption during hover
int cruiseAmps; ///< Amp consumption during cruise
double ampMinutesAvailable; ///< Amp minutes available from single battery
double hoverAmpsTotal; ///< Total hover amps used
double cruiseAmpsTotal; ///< Total cruise amps used
int batteryChangePoint; ///< -1 for not supported, 0 for not needed
int batteriesRequired; ///< -1 for not supported
} MissionFlightStatus_t;
Q_PROPERTY(QmlObjectListModel* visualItems READ visualItems NOTIFY visualItemsChanged)
......@@ -65,6 +73,9 @@ public:
Q_PROPERTY(double missionCruiseTime READ missionCruiseTime NOTIFY missionCruiseTimeChanged)
Q_PROPERTY(double missionMaxTelemetry READ missionMaxTelemetry NOTIFY missionMaxTelemetryChanged)
Q_PROPERTY(int batteryChangePoint READ batteryChangePoint NOTIFY batteryChangePointChanged)
Q_PROPERTY(int batteriesRequired READ batteriesRequired NOTIFY batteriesRequiredChanged)
Q_INVOKABLE void removeMissionItem(int index);
/// Add a new simple mission item to the list
......@@ -124,6 +135,9 @@ public:
double missionCruiseTime (void) const { return _missionFlightStatus.cruiseTime; }
double missionMaxTelemetry (void) const { return _missionFlightStatus.maxTelemetryDistance; }
int batteryChangePoint (void) const { return _missionFlightStatus.batteryChangePoint; } ///< -1 for not supported, 0 for not needed
int batteriesRequired (void) const { return _missionFlightStatus.batteriesRequired; } ///< -1 for not supported
signals:
void visualItemsChanged(void);
void waypointLinesChanged(void);
......@@ -138,6 +152,8 @@ signals:
void complexMissionItemNamesChanged(void);
void resumeMissionItemChanged(void);
void resumeMissionReady(void);
void batteryChangePointChanged(int batteryChangePoint);
void batteriesRequiredChanged(int batteriesRequired);
private slots:
void _newMissionItemsAvailableFromVehicle(bool removeAllRequested);
......@@ -171,16 +187,13 @@ private:
static bool _loadJsonMissionFileV2(Vehicle* vehicle, const QJsonObject& json, QmlObjectListModel* visualItems, QString& errorString);
static bool _loadTextMissionFile(Vehicle* vehicle, QTextStream& stream, QmlObjectListModel* visualItems, QString& errorString);
int _nextSequenceNumber(void);
void _setMissionDistance(double missionDistance);
void _setMissionTime(double missionTime);
void _setMissionHoverDistance(double missionHoverDistance);
void _setMissionHoverTime(double missionHoverTime);
void _setMissionCruiseDistance(double missionCruiseDistance);
void _setMissionCruiseTime(double missionCruiseTime);
void _setMissionMaxTelemetry(double missionMaxTelemetry);
static void _scanForAdditionalSettings(QmlObjectListModel* visualItems, Vehicle* vehicle);
static bool _convertToMissionItems(QmlObjectListModel* visualMissionItems, QList<MissionItem*>& 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;
......
src/PlanView/PlanToolBar.qml
View file @ 246bfba0
......@@ -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 }
}
}
......
Markdown is supported
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