From deb7f9230443ae39d6a1a83af6b49483c47f56f2 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 2 Mar 2016 14:40:12 -0800 Subject: [PATCH 1/6] Remove unused code --- qgroundcontrol.pro | 4 ---- src/GAudioOutput.cc | 7 +++---- src/GAudioOutput.h | 6 +++--- src/audio/QGCAudioWorker.cpp | 32 +------------------------------- src/audio/QGCAudioWorker.h | 13 ++----------- 5 files changed, 9 insertions(+), 53 deletions(-) diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index c0c7b95..c4bc239 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -92,10 +92,6 @@ QT += \ bluetooth \ } -contains(DEFINES, QGC_NOTIFY_TUNES_ENABLED) { - QT += multimedia -} - # testlib is needed even in release flavor for QSignalSpy support QT += testlib diff --git a/src/GAudioOutput.cc b/src/GAudioOutput.cc index e8804f9..673172b 100644 --- a/src/GAudioOutput.cc +++ b/src/GAudioOutput.cc @@ -88,12 +88,11 @@ bool GAudioOutput::isMuted() return muted; } -bool GAudioOutput::say(const QString& inText, int severity) +bool GAudioOutput::say(const QString& inText) { - if (!muted) { + if (!muted && !qgcApp()->runningUnitTests()) { #if defined __android__ #if defined QGC_SPEECH_ENABLED - Q_UNUSED(severity); static const char V_jniClassName[] {"org/qgroundcontrol/qgchelper/UsbDeviceJNI"}; QAndroidJniEnvironment env; if (env->ExceptionCheck()) { @@ -105,7 +104,7 @@ bool GAudioOutput::say(const QString& inText, int severity) QAndroidJniObject::callStaticMethod(V_jniClassName, "say", "(Ljava/lang/String;)V", javaMessage.object()); #endif #else - emit textToSpeak(inText, severity); + emit textToSpeak(inText); #endif } return true; diff --git a/src/GAudioOutput.h b/src/GAudioOutput.h index c121c68..50bb468 100644 --- a/src/GAudioOutput.h +++ b/src/GAudioOutput.h @@ -79,14 +79,14 @@ public: bool isMuted(); public slots: - /** @brief Say this text if current output priority matches */ - bool say(const QString& text, int severity = 6); + /** @brief Say this text */ + bool say(const QString& text); /** @brief Mute/unmute sound */ void mute(bool mute); signals: void mutedChanged(bool); - bool textToSpeak(QString text, int severity = 1); + bool textToSpeak(QString text); void beepOnce(); protected: diff --git a/src/audio/QGCAudioWorker.cpp b/src/audio/QGCAudioWorker.cpp index 10e2e33..d64101b 100644 --- a/src/audio/QGCAudioWorker.cpp +++ b/src/audio/QGCAudioWorker.cpp @@ -76,9 +76,6 @@ QGCAudioWorker::QGCAudioWorker(QObject *parent) : #if defined _MSC_VER && defined QGC_SPEECH_ENABLED pVoice(NULL), #endif - #ifdef QGC_NOTIFY_TUNES_ENABLED - sound(NULL), - #endif emergency(false), muted(false) { @@ -89,10 +86,6 @@ QGCAudioWorker::QGCAudioWorker(QObject *parent) : void QGCAudioWorker::init() { -#ifdef QGC_NOTIFY_TUNES_ENABLED - sound = new QSound(":/res/Alert"); -#endif - #if defined Q_OS_LINUX && !defined __android__ && defined QGC_SPEECH_ENABLED espeak_Initialize(AUDIO_OUTPUT_SYNCH_PLAYBACK, 500, NULL, 0); // initialize for playback with 500ms buffer and no options (see speak_lib.h) espeak_VOICE *espeak_voice = espeak_GetCurrentVoice(); @@ -135,11 +128,10 @@ QGCAudioWorker::~QGCAudioWorker() #endif } -void QGCAudioWorker::say(QString inText, int severity) +void QGCAudioWorker::say(QString inText) { #ifdef __android__ Q_UNUSED(inText); - Q_UNUSED(severity); #else static bool threadInit = false; if (!threadInit) { @@ -150,17 +142,6 @@ void QGCAudioWorker::say(QString inText, int severity) if (!muted) { QString text = fixTextMessageForAudio(inText); - // Prepend high priority text with alert beep - if (severity < GAudioOutput::AUDIO_SEVERITY_CRITICAL) { - beep(); - } - -#ifdef QGC_NOTIFY_TUNES_ENABLED - // Wait for the last sound to finish - while (!sound->isFinished()) { - QGC::SLEEP::msleep(100); - } -#endif #if defined _MSC_VER && defined QGC_SPEECH_ENABLED HRESULT hr = pVoice->Speak(text.toStdWString().c_str(), SPF_DEFAULT, NULL); @@ -195,17 +176,6 @@ void QGCAudioWorker::mute(bool mute) } } -void QGCAudioWorker::beep() -{ - - if (!muted) - { -#ifdef QGC_NOTIFY_TUNES_ENABLED - sound->play(":/res/Alert"); -#endif - } -} - bool QGCAudioWorker::isMuted() { return this->muted; diff --git a/src/audio/QGCAudioWorker.h b/src/audio/QGCAudioWorker.h index cf93ff4..a48d404 100644 --- a/src/audio/QGCAudioWorker.h +++ b/src/audio/QGCAudioWorker.h @@ -3,9 +3,6 @@ #include #include -#ifdef QGC_NOTIFY_TUNES_ENABLED -#include -#endif #if defined _MSC_VER && defined QGC_SPEECH_ENABLED // Documentation: http://msdn.microsoft.com/en-us/library/ee125082%28v=VS.85%29.aspx @@ -29,20 +26,14 @@ public: signals: public slots: - /** @brief Say this text if current output priority matches */ - void say(QString text, int severity = 1); - - /** @brief Sound a single beep */ - void beep(); + /** @brief Say this text */ + void say(QString text); protected: int voiceIndex; ///< The index of the flite voice to use (awb, slt, rms) #if defined _MSC_VER && defined QGC_SPEECH_ENABLED ISpVoice *pVoice; #endif -#ifdef QGC_NOTIFY_TUNES_ENABLED - QSound *sound; -#endif bool emergency; ///< Emergency status flag QTimer *emergencyTimer; bool muted; From 95bbc54f398b54180388f0992e0122ebe8c5f485 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 2 Mar 2016 14:40:25 -0800 Subject: [PATCH 2/6] Fix for missing qgcView --- src/FactSystem/FactControls/FactTextField.qml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/FactSystem/FactControls/FactTextField.qml b/src/FactSystem/FactControls/FactTextField.qml index edd594f..525bc35 100644 --- a/src/FactSystem/FactControls/FactTextField.qml +++ b/src/FactSystem/FactControls/FactTextField.qml @@ -23,7 +23,7 @@ QGCTextField { inputMethodHints: Qt.ImhFormattedNumbersOnly onEditingFinished: { - if (qgcView) { + if (typeof qgcView !== 'undefined' && qgcView) { var errorString = fact.validate(text, false /* convertOnly */) if (errorString == "") { fact.value = text From 903459f74e4a7f388b66ff105a46dce518a12b63 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 2 Mar 2016 14:40:40 -0800 Subject: [PATCH 3/6] Min/Max/DefaultValue support --- src/FactSystem/FactGroup.cc | 17 +++++++++++++++-- src/FactSystem/FactGroup.h | 3 +++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/FactSystem/FactGroup.cc b/src/FactSystem/FactGroup.cc index bea0959..01c4a50 100644 --- a/src/FactSystem/FactGroup.cc +++ b/src/FactSystem/FactGroup.cc @@ -40,6 +40,9 @@ const char* FactGroup::_versionJsonKey = "version"; const char* FactGroup::_typeJsonKey = "type"; const char* FactGroup::_shortDescriptionJsonKey = "shortDescription"; const char* FactGroup::_unitsJsonKey = "units"; +const char* FactGroup::_defaultValueJsonKey = "defaultValue"; +const char* FactGroup::_minJsonKey = "min"; +const char* FactGroup::_maxJsonKey = "max"; FactGroup::FactGroup(int updateRateMsecs, const QString& metaDataFile, QObject* parent) : QObject(parent) @@ -187,8 +190,8 @@ void FactGroup::_loadMetaData(const QString& jsonFilename) QStringList keys; QList types; - keys << _nameJsonKey << _decimalPlacesJsonKey; - types << QJsonValue::String << QJsonValue::Double; + keys << _nameJsonKey << _decimalPlacesJsonKey << _typeJsonKey << _shortDescriptionJsonKey << _unitsJsonKey << _defaultValueJsonKey << _minJsonKey << _maxJsonKey; + types << QJsonValue::String << QJsonValue::Double << QJsonValue::String << QJsonValue::String << QJsonValue::String << QJsonValue::Double << QJsonValue::Double << QJsonValue::Double; if (!JsonHelper::validateKeyTypes(jsonObject, keys, types, errorString)) { qWarning() << errorString; return; @@ -219,6 +222,16 @@ void FactGroup::_loadMetaData(const QString& jsonFilename) metaData->setShortDescription(jsonObject.value(_shortDescriptionJsonKey).toString()); metaData->setRawUnits(jsonObject.value(_unitsJsonKey).toString()); + if (jsonObject.contains(_defaultValueJsonKey)) { + metaData->setRawDefaultValue(jsonObject.value(_defaultValueJsonKey).toDouble()); + } + if (jsonObject.contains(_minJsonKey)) { + metaData->setRawMin(jsonObject.value(_minJsonKey).toDouble()); + } + if (jsonObject.contains(_maxJsonKey)) { + metaData->setRawMax(jsonObject.value(_maxJsonKey).toDouble()); + } + for (int i=0; i Date: Wed, 2 Mar 2016 14:41:55 -0800 Subject: [PATCH 4/6] Disconnect Vehicle support --- src/FlightMap/Widgets/QGCInstrumentWidget.qml | 3 +-- src/FlightMap/Widgets/ValuesWidget.qml | 3 +-- src/FlightMap/Widgets/VibrationWidget.qml | 3 +-- src/QmlControls/QGroundControlQmlGlobal.h | 2 +- src/Vehicle/MultiVehicleManager.cc | 3 +++ src/Vehicle/MultiVehicleManager.h | 4 ++++ 6 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/FlightMap/Widgets/QGCInstrumentWidget.qml b/src/FlightMap/Widgets/QGCInstrumentWidget.qml index b90f1b0..0573beb 100644 --- a/src/FlightMap/Widgets/QGCInstrumentWidget.qml +++ b/src/FlightMap/Widgets/QGCInstrumentWidget.qml @@ -131,7 +131,7 @@ Item { Rectangle { anchors.fill: _valuesWidget color: _backgroundColor - visible: !_showCompass && _activeVehicle + visible: !_showCompass radius: _spacing } @@ -145,7 +145,6 @@ Item { } } - Rectangle { id: _spacer2 height: 1 diff --git a/src/FlightMap/Widgets/ValuesWidget.qml b/src/FlightMap/Widgets/ValuesWidget.qml index 596e2fd..0e275e5 100644 --- a/src/FlightMap/Widgets/ValuesWidget.qml +++ b/src/FlightMap/Widgets/ValuesWidget.qml @@ -33,7 +33,6 @@ import QGroundControl 1.0 QGCFlickable { id: _root - visible: _activeVehicle height: Math.min(maxHeight, _smallFlow.y + _smallFlow.height) contentHeight: _smallFlow.y + _smallFlow.height flickableDirection: Flickable.VerticalFlick @@ -43,7 +42,7 @@ QGCFlickable { property color textColor property var maxHeight - property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.disconnectedVehicle property real _margins: ScreenTools.defaultFontPixelWidth / 2 QGCPalette { id:qgcPal; colorGroupEnabled: true } diff --git a/src/FlightMap/Widgets/VibrationWidget.qml b/src/FlightMap/Widgets/VibrationWidget.qml index 83f838d..cc973fe 100644 --- a/src/FlightMap/Widgets/VibrationWidget.qml +++ b/src/FlightMap/Widgets/VibrationWidget.qml @@ -33,7 +33,6 @@ import QGroundControl 1.0 QGCFlickable { id: _root - visible: _activeVehicle height: Math.min(maxHeight, innerItem.height) contentHeight: innerItem.height flickableDirection: Flickable.VerticalFlick @@ -43,7 +42,7 @@ QGCFlickable { property color backgroundColor property var maxHeight - property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle ? QGroundControl.multiVehicleManager.activeVehicle : QGroundControl.multiVehicleManager.disconnectedVehicle property real _margins: ScreenTools.defaultFontPixelWidth / 2 property real _barWidth: Math.round(ScreenTools.defaultFontPixelWidth * 3) diff --git a/src/QmlControls/QGroundControlQmlGlobal.h b/src/QmlControls/QGroundControlQmlGlobal.h index beae942..de35f49 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.h +++ b/src/QmlControls/QGroundControlQmlGlobal.h @@ -55,7 +55,7 @@ public: Q_PROPERTY(LinkManager* linkManager READ linkManager CONSTANT) Q_PROPERTY(MissionCommands* missionCommands READ missionCommands CONSTANT) Q_PROPERTY(MultiVehicleManager* multiVehicleManager READ multiVehicleManager CONSTANT) - Q_PROPERTY(QGCMapEngineManager* mapEngineManager READ mapEngineManager CONSTANT) + Q_PROPERTY(QGCMapEngineManager* mapEngineManager READ mapEngineManager CONSTANT) Q_PROPERTY(qreal zOrderTopMost READ zOrderTopMost CONSTANT) ///< z order for top most items, toolbar, main window sub view Q_PROPERTY(qreal zOrderWidgets READ zOrderWidgets CONSTANT) ///< z order value to widgets, for example: zoom controls, hud widgetss diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 7646e92..019694d 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -50,6 +50,7 @@ MultiVehicleManager::MultiVehicleManager(QGCApplication* app) , _activeVehicleAvailable(false) , _parameterReadyVehicleAvailable(false) , _activeVehicle(NULL) + , _disconnectedVehicle(NULL) , _firmwarePluginManager(NULL) , _autopilotPluginManager(NULL) , _joystickManager(NULL) @@ -66,6 +67,8 @@ MultiVehicleManager::MultiVehicleManager(QGCApplication* app) if (_gcsHeartbeatEnabled) { _gcsHeartbeatTimer.start(); } + + _disconnectedVehicle = new Vehicle(this); } void MultiVehicleManager::setToolbox(QGCToolbox *toolbox) diff --git a/src/Vehicle/MultiVehicleManager.h b/src/Vehicle/MultiVehicleManager.h index c1bd659..31fb92f 100644 --- a/src/Vehicle/MultiVehicleManager.h +++ b/src/Vehicle/MultiVehicleManager.h @@ -57,6 +57,9 @@ public: Q_PROPERTY(QmlObjectListModel* vehicles READ vehicles CONSTANT) Q_PROPERTY(bool gcsHeartBeatEnabled READ gcsHeartbeatEnabled WRITE setGcsHeartbeatEnabled NOTIFY gcsHeartBeatEnabledChanged) + /// A disconnected vehicle is used to access FactGroup information for the Vehicle object when no active vehicle is available + Q_PROPERTY(Vehicle* disconnectedVehicle MEMBER _disconnectedVehicle CONSTANT) + // Methods Q_INVOKABLE Vehicle* getVehicleById(int vehicleId); @@ -104,6 +107,7 @@ private: bool _activeVehicleAvailable; ///< true: An active vehicle is available bool _parameterReadyVehicleAvailable; ///< true: An active vehicle with ready parameters is available Vehicle* _activeVehicle; ///< Currently active vehicle from a ui perspective + Vehicle* _disconnectedVehicle; ///< Disconnected vechicle for FactGroup access QList _vehiclesBeingDeleted; ///< List of Vehicles being deleted in queued phases Vehicle* _vehicleBeingSetActive; ///< Vehicle being set active in queued phases From 33edc72574274dbbc3badac6bc6d6e6929358cdd Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 2 Mar 2016 14:42:08 -0800 Subject: [PATCH 5/6] Move flight mode and battery speech to Vehicle --- src/uas/UAS.cc | 134 +++------------------------------------------------------ src/uas/UAS.h | 22 ---------- 2 files changed, 5 insertions(+), 151 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 4d5c1c2..3b5529f 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -41,8 +41,6 @@ QGC_LOGGING_CATEGORY(UASLog, "UASLog") -#define UAS_DEFAULT_BATTERY_WARNLEVEL 20 - /** * Gets the settings from the previous UAS (name, airframe, autopilot, battery specs) * by calling readSettings. This means the new UAS will have the same settings @@ -63,17 +61,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi custom_mode(0), status(-1), - startVoltage(-1.0f), - tickVoltage(10.5f), - lastTickVoltageValue(13.0f), - tickLowpassVoltage(12.0f), - warnLevelPercent(UAS_DEFAULT_BATTERY_WARNLEVEL), - currentVoltage(12.6f), - lpVoltage(-1.0f), - currentCurrent(0.4f), - chargeLevel(-1), - lowBattAlarm(false), - startTime(QGC::groundTimeMilliseconds()), onboardTimeOffset(0), @@ -294,12 +281,8 @@ void UAS::receiveMessage(mavlink_message_t message) QString audiostring = QString("System %1").arg(uasId); QString stateAudio = ""; - QString modeAudio = ""; QString navModeAudio = ""; bool statechanged = false; - bool modechanged = false; - - QString audiomodeText = _firmwarePluginManager->firmwarePluginForAutopilot((MAV_AUTOPILOT)state.autopilot, (MAV_TYPE)state.type)->flightMode(state.base_mode, state.custom_mode); if ((state.system_status != this->status) && state.system_status != MAV_STATE_UNINIT) { @@ -318,27 +301,14 @@ void UAS::receiveMessage(mavlink_message_t message) stateAudio = uasState; } - if (this->base_mode != state.base_mode || this->custom_mode != state.custom_mode) - { - modechanged = true; - this->base_mode = state.base_mode; - this->custom_mode = state.custom_mode; - modeAudio = " is now in " + audiomodeText + " flight mode"; - } - // We got the mode receivedMode = true; // AUDIO - if (modechanged && statechanged) - { - // Output both messages - audiostring += modeAudio + " and " + stateAudio; - } - else if (modechanged || statechanged) + if (statechanged) { // Output the one message - audiostring += modeAudio + stateAudio; + audiostring += stateAudio; } if (statechanged && ((int)state.system_status == (int)MAV_STATE_CRITICAL || state.system_status == (int)MAV_STATE_EMERGENCY)) @@ -346,7 +316,7 @@ void UAS::receiveMessage(mavlink_message_t message) _say(QString("Emergency for system %1").arg(this->getUASID()), GAudioOutput::AUDIO_SEVERITY_EMERGENCY); QTimer::singleShot(3000, qgcApp()->toolbox()->audioOutput(), SLOT(startEmergency())); } - else if (modechanged || statechanged) + else if (statechanged) { _say(audiostring.toLower()); } @@ -378,62 +348,6 @@ void UAS::receiveMessage(mavlink_message_t message) emit loadChanged(this,state.load/10.0f); emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time); - currentVoltage = state.voltage_battery/1000.0f; - - if (state.voltage_battery > 0.0f && state.voltage_battery != UINT16_MAX) { - // Battery charge/time remaining/voltage calculations - currentVoltage = state.voltage_battery/1000.0f; - filterVoltage(currentVoltage); - tickLowpassVoltage = tickLowpassVoltage * 0.8f + 0.2f * currentVoltage; - - // We don't want to tick above the threshold - if (tickLowpassVoltage > tickVoltage) - { - lastTickVoltageValue = tickLowpassVoltage; - } - - if ((startVoltage > 0.0f) && (tickLowpassVoltage < tickVoltage) && (fabs(lastTickVoltageValue - tickLowpassVoltage) > 0.1f) - /* warn if lower than treshold */ - && (lpVoltage < tickVoltage) - /* warn only if we have at least the voltage of an empty LiPo cell, else we're sampling something wrong */ - && (currentVoltage > 3.3f) - /* warn only if current voltage is really still lower by a reasonable amount */ - && ((currentVoltage - 0.2f) < tickVoltage) - /* warn only every 20 seconds */ - && (QGC::groundTimeUsecs() - lastVoltageWarning) > 20000000) - { - _say(QString("Low battery system %1: %2 volts").arg(getUASID()).arg(lpVoltage, 0, 'f', 1, QChar(' '))); - lastVoltageWarning = QGC::groundTimeUsecs(); - lastTickVoltageValue = tickLowpassVoltage; - } - - if (startVoltage == -1.0f && currentVoltage > 0.1f) startVoltage = currentVoltage; - chargeLevel = state.battery_remaining; - - emit batteryChanged(this, lpVoltage, currentCurrent, getChargeLevel(), 0); - } - - emit valueChanged(uasId, name.arg("battery_remaining"), "%", getChargeLevel(), time); - emit valueChanged(uasId, name.arg("battery_voltage"), "V", currentVoltage, time); - - // And if the battery current draw is measured, log that also. - if (state.current_battery != -1) - { - currentCurrent = ((double)state.current_battery)/100.0f; - emit valueChanged(uasId, name.arg("battery_current"), "A", currentCurrent, time); - } - - // LOW BATTERY ALARM - if (chargeLevel >= 0 && (getChargeLevel() < warnLevelPercent)) - { - // An audio alarm. Does not generate any signals. - startLowBattAlarm(); - } - else - { - stopLowBattAlarm(); - } - // control_sensors_enabled: // relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control emit attitudeControlEnabled(state.onboard_control_sensors_enabled & (1 << 11)); @@ -1179,19 +1093,6 @@ quint64 UAS::getUnixTime(quint64 time) } /** - * @param value battery voltage - */ -float UAS::filterVoltage(float value) -{ - if (lpVoltage < 0.0f) { - lpVoltage = value; - } - - lpVoltage = lpVoltage * 0.6f + value * 0.4f; - return lpVoltage; -} - -/** * Get the status of the code and a description of the status. * Status can be unitialized, booting up, calibrating sensors, active * standby, cirtical, emergency, shutdown or unknown. @@ -2077,31 +1978,6 @@ QMap UAS::getComponents() return components; } -/** - * @return charge level in percent - 0 - 100 - */ -float UAS::getChargeLevel() -{ - return chargeLevel; -} - -void UAS::startLowBattAlarm() -{ - if (!lowBattAlarm) - { - _say(tr("System %1 has low battery").arg(getUASID())); - lowBattAlarm = true; - } -} - -void UAS::stopLowBattAlarm() -{ - if (lowBattAlarm) - { - lowBattAlarm = false; - } -} - void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax) { if (!_vehicle) { @@ -2164,8 +2040,8 @@ void UAS::unsetRCToParameterMap() void UAS::_say(const QString& text, int severity) { - if (!qgcApp()->runningUnitTests()) - qgcApp()->toolbox()->audioOutput()->say(text, severity); + Q_UNUSED(severity); + qgcApp()->toolbox()->audioOutput()->say(text); } void UAS::shutdownVehicle(void) diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 4917bcb..1a859f6 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -77,8 +77,6 @@ public: /** @brief The time interval the robot is switched on */ quint64 getUptime() const; - /** @brief Add one measurement and get low-passed voltage */ - float filterVoltage(float value); Q_PROPERTY(double latitude READ getLatitude WRITE setLatitude NOTIFY latitudeChanged) Q_PROPERTY(double longitude READ getLongitude WRITE setLongitude NOTIFY longitudeChanged) @@ -376,21 +374,6 @@ protected: //COMMENTS FOR TEST UNIT uint32_t custom_mode; ///< The current mode of the MAV int status; ///< The current status of the MAV - // dongfang: This looks like a candidate for being moved off to a separate class. - /// BATTERY / ENERGY - float startVoltage; ///< Voltage at system start - float tickVoltage; ///< Voltage where 0.1 V ticks are told - float lastTickVoltageValue; ///< The last voltage where a tick was announced - float tickLowpassVoltage; ///< Lowpass-filtered voltage for the tick announcement - float warnLevelPercent; ///< Warning level, in percent - double currentVoltage; ///< Voltage currently measured - float lpVoltage; ///< Low-pass filtered voltage - double currentCurrent; ///< Battery current currently measured - bool batteryRemainingEstimateEnabled; ///< If the estimate is enabled, QGC will try to estimate the remaining battery life - float chargeLevel; ///< Charge level of battery, in percent - bool lowBattAlarm; ///< Switch if battery is low - - /// TIMEKEEPING quint64 startTime; ///< The time the UAS was switched on quint64 onboardTimeOffset; @@ -486,8 +469,6 @@ protected: //COMMENTS FOR TEST UNIT #endif public: - /** @brief Get the current charge level */ - float getChargeLevel(); /** @brief Get the human-readable status message for this code */ void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription); @@ -559,9 +540,6 @@ public slots: void stopHil(); #endif - void startLowBattAlarm(); - void stopLowBattAlarm(); - /** @brief Set the values for the manual control of the vehicle */ void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode); From 7285dfb588ccf5de3d2d4a1f688055e867f13c31 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 2 Mar 2016 14:42:18 -0800 Subject: [PATCH 6/6] Talking battery and flight modes --- src/Vehicle/BatteryFact.json | 10 ++ src/Vehicle/Vehicle.cc | 225 ++++++++++++++++++++++++++------- src/Vehicle/Vehicle.h | 82 +++++++----- src/ui/preferences/GeneralSettings.qml | 28 ++++ 4 files changed, 267 insertions(+), 78 deletions(-) diff --git a/src/Vehicle/BatteryFact.json b/src/Vehicle/BatteryFact.json index 3c3b861..6f6e133 100644 --- a/src/Vehicle/BatteryFact.json +++ b/src/Vehicle/BatteryFact.json @@ -17,6 +17,16 @@ "units": "%" }, { + "name": "percentRemainingAnnounce", + "shortDescription": "Percent", + "type": "int32", + "decimalPlaces": 0, + "units": "%", + "defaultValue": 30, + "min": 0, + "max": 100 + }, + { "name": "mahConsumed", "shortDescription": "Consumed", "type": "int32", diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index c6f3f82..14db6c2 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -60,36 +60,7 @@ const char* Vehicle::_batteryFactGroupName = "battery"; const char* Vehicle::_windFactGroupName = "wind"; const char* Vehicle::_vibrationFactGroupName = "vibration"; -const char* VehicleGPSFactGroup::_hdopFactName = "hdop"; -const char* VehicleGPSFactGroup::_vdopFactName = "vdop"; -const char* VehicleGPSFactGroup::_courseOverGroundFactName = "courseOverGround"; -const char* VehicleGPSFactGroup::_countFactName = "count"; -const char* VehicleGPSFactGroup::_lockFactName = "lock"; - -const char* VehicleBatteryFactGroup::_voltageFactName = "voltage"; -const char* VehicleBatteryFactGroup::_percentRemainingFactName = "percentRemaining"; -const char* VehicleBatteryFactGroup::_mahConsumedFactName = "mahConsumed"; -const char* VehicleBatteryFactGroup::_currentFactName = "current"; -const char* VehicleBatteryFactGroup::_temperatureFactName = "temperature"; -const char* VehicleBatteryFactGroup::_cellCountFactName = "cellCount"; - -const double VehicleBatteryFactGroup::_voltageUnavailable = -1.0; -const int VehicleBatteryFactGroup::_percentRemainingUnavailable = -1; -const int VehicleBatteryFactGroup::_mahConsumedUnavailable = -1; -const int VehicleBatteryFactGroup::_currentUnavailable = -1; -const double VehicleBatteryFactGroup::_temperatureUnavailable = -1.0; -const int VehicleBatteryFactGroup::_cellCountUnavailable = -1.0; - -const char* VehicleWindFactGroup::_directionFactName = "direction"; -const char* VehicleWindFactGroup::_speedFactName = "speed"; -const char* VehicleWindFactGroup::_verticalSpeedFactName = "verticalSpeed"; - -const char* VehicleVibrationFactGroup::_xAxisFactName = "xAxis"; -const char* VehicleVibrationFactGroup::_yAxisFactName = "yAxis"; -const char* VehicleVibrationFactGroup::_zAxisFactName = "zAxis"; -const char* VehicleVibrationFactGroup::_clipCount1FactName = "clipCount1"; -const char* VehicleVibrationFactGroup::_clipCount2FactName = "clipCount2"; -const char* VehicleVibrationFactGroup::_clipCount3FactName = "clipCount3"; +const int Vehicle::_lowBatteryAnnounceRepeatMSecs = 30 * 1000; Vehicle::Vehicle(LinkInterface* link, int vehicleId, @@ -101,6 +72,7 @@ Vehicle::Vehicle(LinkInterface* link, : FactGroup(_vehicleUIUpdateRateMSecs, ":/json/Vehicle/VehicleFact.json") , _id(vehicleId) , _active(false) + , _disconnectedVehicle(false) , _firmwareType(firmwareType) , _vehicleType(vehicleType) , _firmwarePlugin(NULL) @@ -165,8 +137,10 @@ Vehicle::Vehicle(LinkInterface* link, _mavlink = qgcApp()->toolbox()->mavlinkProtocol(); connect(_mavlink, &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived); - connect(this, &Vehicle::_sendMessageOnThread, this, &Vehicle::_sendMessage, Qt::QueuedConnection); + + connect(this, &Vehicle::_sendMessageOnThread, this, &Vehicle::_sendMessage, Qt::QueuedConnection); connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection); + connect(this, &Vehicle::flightModeChanged, this, &Vehicle::_announceflightModeChanged); _uas = new UAS(_mavlink, this, _firmwarePluginManager); @@ -227,6 +201,9 @@ Vehicle::Vehicle(LinkInterface* link, _mapTrajectoryTimer.setInterval(_mapTrajectoryMsecsBetweenPoints); connect(&_mapTrajectoryTimer, &QTimer::timeout, this, &Vehicle::_addNewMapTrajectoryPoint); + // Invalidate the timer to signal first announce + _lowBatteryAnnounceTimer.invalidate(); + // Build FactGroup object model _addFact(&_rollFact, _rollFactName); @@ -249,6 +226,93 @@ Vehicle::Vehicle(LinkInterface* link, _vibrationFactGroup.setVehicle(this); } +// Disconnected Vehicle +Vehicle::Vehicle(QObject* parent) + : FactGroup(_vehicleUIUpdateRateMSecs, ":/json/Vehicle/VehicleFact.json", parent) + , _id(0) + , _active(false) + , _disconnectedVehicle(false) + , _firmwareType(MAV_AUTOPILOT_PX4) + , _vehicleType(MAV_TYPE_QUADROTOR) + , _firmwarePlugin(NULL) + , _autopilotPlugin(NULL) + , _joystickMode(JoystickModeRC) + , _joystickEnabled(false) + , _uas(NULL) + , _coordinate(37.803784, -122.462276) + , _coordinateValid(false) + , _homePositionAvailable(false) + , _mav(NULL) + , _currentMessageCount(0) + , _messageCount(0) + , _currentErrorCount(0) + , _currentWarningCount(0) + , _currentNormalCount(0) + , _currentMessageType(MessageNone) + , _navigationAltitudeError(0.0f) + , _navigationSpeedError(0.0f) + , _navigationCrosstrackError(0.0f) + , _navigationTargetBearing(0.0f) + , _refreshTimer(new QTimer(this)) + , _updateCount(0) + , _rcRSSI(0) + , _rcRSSIstore(100.0) + , _autoDisconnect(false) + , _connectionLost(false) + , _connectionLostEnabled(true) + , _missionManager(NULL) + , _missionManagerInitialRequestComplete(false) + , _parameterLoader(NULL) + , _armed(false) + , _base_mode(0) + , _custom_mode(0) + , _nextSendMessageMultipleIndex(0) + , _firmwarePluginManager(NULL) + , _autopilotPluginManager(NULL) + , _joystickManager(NULL) + , _flowImageIndex(0) + , _allLinksInactiveSent(false) + , _messagesReceived(0) + , _messagesSent(0) + , _messagesLost(0) + , _messageSeq(0) + , _compID(0) + , _heardFrom(false) + , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) + , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) + , _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble) + , _groundSpeedFact (0, _groundSpeedFactName, FactMetaData::valueTypeDouble) + , _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble) + , _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble) + , _altitudeRelativeFact (0, _altitudeRelativeFactName, FactMetaData::valueTypeDouble) + , _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble) + , _gpsFactGroup(this) + , _batteryFactGroup(this) + , _windFactGroup(this) + , _vibrationFactGroup(this) +{ + // Build FactGroup object model + + _addFact(&_rollFact, _rollFactName); + _addFact(&_pitchFact, _pitchFactName); + _addFact(&_headingFact, _headingFactName); + _addFact(&_groundSpeedFact, _groundSpeedFactName); + _addFact(&_airSpeedFact, _airSpeedFactName); + _addFact(&_climbRateFact, _climbRateFactName); + _addFact(&_altitudeRelativeFact, _altitudeRelativeFactName); + _addFact(&_altitudeAMSLFact, _altitudeAMSLFactName); + + _addFactGroup(&_gpsFactGroup, _gpsFactGroupName); + _addFactGroup(&_batteryFactGroup, _batteryFactGroupName); + _addFactGroup(&_windFactGroup, _windFactGroupName); + _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName); + + _gpsFactGroup.setVehicle(NULL); + _batteryFactGroup.setVehicle(NULL); + _windFactGroup.setVehicle(NULL); + _vibrationFactGroup.setVehicle(NULL); +} + Vehicle::~Vehicle() { qCDebug(VehicleLog) << "~Vehicle" << this; @@ -399,6 +463,13 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message) _batteryFactGroup.voltage()->setRawValue((double)sysStatus.voltage_battery / 1000.0); } _batteryFactGroup.percentRemaining()->setRawValue(sysStatus.battery_remaining); + + if (sysStatus.battery_remaining > 0 && sysStatus.battery_remaining < _batteryFactGroup.percentRemainingAnnounce()->rawValue().toInt()) { + if (!_lowBatteryAnnounceTimer.isValid() || _lowBatteryAnnounceTimer.elapsed() > _lowBatteryAnnounceRepeatMSecs) { + _lowBatteryAnnounceTimer.restart(); + _say(QString("Low battery on %1: %2 percent remaining").arg(_vehicleIdSpeech()).arg(sysStatus.battery_remaining)); + } + } } void Vehicle::_handleBatteryStatus(mavlink_message_t& message) @@ -1217,7 +1288,7 @@ void Vehicle::_connectionLostTimeout(void) _connectionLost = true; _heardFrom = false; emit connectionLostChanged(true); - _say(QString("connection lost to vehicle %1").arg(id()), GAudioOutput::AUDIO_SEVERITY_NOTICE); + _say(QString("communication lost to %1").arg(_vehicleIdSpeech())); if (_autoDisconnect) { disconnectInactiveVehicle(); } @@ -1230,14 +1301,13 @@ void Vehicle::_connectionActive(void) if (_connectionLost) { _connectionLost = false; emit connectionLostChanged(false); - _say(QString("connection regained to vehicle %1").arg(id()), GAudioOutput::AUDIO_SEVERITY_NOTICE); + _say(QString("communication regained to %1").arg(_vehicleIdSpeech())); } } -void Vehicle::_say(const QString& text, int severity) +void Vehicle::_say(const QString& text) { - if (!qgcApp()->runningUnitTests()) - qgcApp()->toolbox()->audioOutput()->say(text.toLower(), severity); + qgcApp()->toolbox()->audioOutput()->say(text.toLower()); } bool Vehicle::fixedWing(void) const @@ -1268,6 +1338,26 @@ void Vehicle::_setCoordinateValid(bool coordinateValid) } } +/// Returns the string to speak to identify the vehicle +QString Vehicle::_vehicleIdSpeech(void) +{ + if (qgcApp()->toolbox()->multiVehicleManager()->vehicles()->count() > 1) { + return QString("vehicle %1").arg(id()); + } else { + return QStringLiteral("vehicle"); + } +} + +void Vehicle::_announceflightModeChanged(const QString& flightMode) +{ + _say(QString("%1 is now in %2 flight mode").arg(_vehicleIdSpeech()).arg(flightMode)); +} + +const char* VehicleGPSFactGroup::_hdopFactName = "hdop"; +const char* VehicleGPSFactGroup::_vdopFactName = "vdop"; +const char* VehicleGPSFactGroup::_courseOverGroundFactName = "courseOverGround"; +const char* VehicleGPSFactGroup::_countFactName = "count"; +const char* VehicleGPSFactGroup::_lockFactName = "lock"; VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent) : FactGroup(1000, ":/json/Vehicle/GPSFact.json", parent) @@ -1293,6 +1383,11 @@ void VehicleGPSFactGroup::setVehicle(Vehicle* vehicle) { _vehicle = vehicle; + if (!vehicle) { + // Disconnected Vehicle + return; + } + connect(_vehicle->uas(), &UASInterface::localizationChanged, this, &VehicleGPSFactGroup::_setSatLoc); UAS* pUas = dynamic_cast(_vehicle->uas()); @@ -1336,22 +1431,43 @@ void VehicleGPSFactGroup::_setSatLoc(UASInterface*, int fix) } } +const char* VehicleBatteryFactGroup::_voltageFactName = "voltage"; +const char* VehicleBatteryFactGroup::_percentRemainingFactName = "percentRemaining"; +const char* VehicleBatteryFactGroup::_percentRemainingAnnounceFactName = "percentRemainingAnnounce"; +const char* VehicleBatteryFactGroup::_mahConsumedFactName = "mahConsumed"; +const char* VehicleBatteryFactGroup::_currentFactName = "current"; +const char* VehicleBatteryFactGroup::_temperatureFactName = "temperature"; +const char* VehicleBatteryFactGroup::_cellCountFactName = "cellCount"; + +const char* VehicleBatteryFactGroup::_settingsGroup = "Vehicle.battery"; +const int VehicleBatteryFactGroup::_percentRemainingAnnounceDefault = 30; + +const double VehicleBatteryFactGroup::_voltageUnavailable = -1.0; +const int VehicleBatteryFactGroup::_percentRemainingUnavailable = -1; +const int VehicleBatteryFactGroup::_mahConsumedUnavailable = -1; +const int VehicleBatteryFactGroup::_currentUnavailable = -1; +const double VehicleBatteryFactGroup::_temperatureUnavailable = -1.0; +const int VehicleBatteryFactGroup::_cellCountUnavailable = -1.0; + +SettingsFact VehicleBatteryFactGroup::_percentRemainingAnnounceFact (_settingsGroup, _percentRemainingAnnounceFactName, FactMetaData::valueTypeInt32, _percentRemainingAnnounceDefault); + VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent) : FactGroup(1000, ":/json/Vehicle/BatteryFact.json", parent) , _vehicle(NULL) - , _voltageFact (0, _voltageFactName, FactMetaData::valueTypeDouble) - , _percentRemainingFact (0, _percentRemainingFactName, FactMetaData::valueTypeInt32) - , _mahConsumedFact (0, _mahConsumedFactName, FactMetaData::valueTypeInt32) - , _currentFact (0, _currentFactName, FactMetaData::valueTypeInt32) - , _temperatureFact (0, _temperatureFactName, FactMetaData::valueTypeDouble) - , _cellCountFact (0, _cellCountFactName, FactMetaData::valueTypeInt32) -{ - _addFact(&_voltageFact, _voltageFactName); - _addFact(&_percentRemainingFact, _percentRemainingFactName); - _addFact(&_mahConsumedFact, _mahConsumedFactName); - _addFact(&_currentFact, _currentFactName); - _addFact(&_temperatureFact, _temperatureFactName); - _addFact(&_cellCountFact, _cellCountFactName); + , _voltageFact (0, _voltageFactName, FactMetaData::valueTypeDouble) + , _percentRemainingFact (0, _percentRemainingFactName, FactMetaData::valueTypeInt32) + , _mahConsumedFact (0, _mahConsumedFactName, FactMetaData::valueTypeInt32) + , _currentFact (0, _currentFactName, FactMetaData::valueTypeInt32) + , _temperatureFact (0, _temperatureFactName, FactMetaData::valueTypeDouble) + , _cellCountFact (0, _cellCountFactName, FactMetaData::valueTypeInt32) +{ + _addFact(&_voltageFact, _voltageFactName); + _addFact(&_percentRemainingFact, _percentRemainingFactName); + _addFact(&_percentRemainingAnnounceFact, _percentRemainingAnnounceFactName); + _addFact(&_mahConsumedFact, _mahConsumedFactName); + _addFact(&_currentFact, _currentFactName); + _addFact(&_temperatureFact, _temperatureFactName); + _addFact(&_cellCountFact, _cellCountFactName); // Start out as not available _voltageFact.setRawValue (_voltageUnavailable); @@ -1367,6 +1483,10 @@ void VehicleBatteryFactGroup::setVehicle(Vehicle* vehicle) _vehicle = vehicle; } +const char* VehicleWindFactGroup::_directionFactName = "direction"; +const char* VehicleWindFactGroup::_speedFactName = "speed"; +const char* VehicleWindFactGroup::_verticalSpeedFactName = "verticalSpeed"; + VehicleWindFactGroup::VehicleWindFactGroup(QObject* parent) : FactGroup(1000, ":/json/Vehicle/WindFact.json", parent) , _vehicle(NULL) @@ -1389,6 +1509,13 @@ void VehicleWindFactGroup::setVehicle(Vehicle* vehicle) _vehicle = vehicle; } +const char* VehicleVibrationFactGroup::_xAxisFactName = "xAxis"; +const char* VehicleVibrationFactGroup::_yAxisFactName = "yAxis"; +const char* VehicleVibrationFactGroup::_zAxisFactName = "zAxis"; +const char* VehicleVibrationFactGroup::_clipCount1FactName = "clipCount1"; +const char* VehicleVibrationFactGroup::_clipCount2FactName = "clipCount2"; +const char* VehicleVibrationFactGroup::_clipCount3FactName = "clipCount3"; + VehicleVibrationFactGroup::VehicleVibrationFactGroup(QObject* parent) : FactGroup(1000, ":/json/Vehicle/VibrationFact.json", parent) , _vehicle(NULL) diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 84522cd..96d43c3 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -29,6 +29,7 @@ #include #include +#include #include "FactGroup.h" #include "LinkInterface.h" @@ -36,6 +37,7 @@ #include "QmlObjectListModel.h" #include "MAVLinkProtocol.h" #include "UASMessageHandler.h" +#include "SettingsFact.h" class UAS; class UASInterface; @@ -66,12 +68,12 @@ public: Q_PROPERTY(Fact* clipCount2 READ clipCount2 CONSTANT) Q_PROPERTY(Fact* clipCount3 READ clipCount3 CONSTANT) - Fact* xAxis(void) { return &_xAxisFact; } - Fact* yAxis(void) { return &_yAxisFact; } - Fact* zAxis(void) { return &_zAxisFact; } - Fact* clipCount1(void) { return &_clipCount1Fact; } - Fact* clipCount2(void) { return &_clipCount2Fact; } - Fact* clipCount3(void) { return &_clipCount3Fact; } + Fact* xAxis (void) { return &_xAxisFact; } + Fact* yAxis (void) { return &_yAxisFact; } + Fact* zAxis (void) { return &_zAxisFact; } + Fact* clipCount1 (void) { return &_clipCount1Fact; } + Fact* clipCount2 (void) { return &_clipCount2Fact; } + Fact* clipCount3 (void) { return &_clipCount3Fact; } void setVehicle(Vehicle* vehicle); @@ -103,9 +105,9 @@ public: Q_PROPERTY(Fact* speed READ speed CONSTANT) Q_PROPERTY(Fact* verticalSpeed READ verticalSpeed CONSTANT) - Fact* direction(void) { return &_directionFact; } - Fact* speed(void) { return &_speedFact; } - Fact* verticalSpeed(void) { return &_verticalSpeedFact; } + Fact* direction (void) { return &_directionFact; } + Fact* speed (void) { return &_speedFact; } + Fact* verticalSpeed (void) { return &_verticalSpeedFact; } void setVehicle(Vehicle* vehicle); @@ -133,11 +135,11 @@ public: Q_PROPERTY(Fact* count READ count CONSTANT) Q_PROPERTY(Fact* lock READ lock CONSTANT) - Fact* hdop(void) { return &_hdopFact; } - Fact* vdop(void) { return &_vdopFact; } - Fact* courseOverGround(void) { return &_courseOverGroundFact; } - Fact* count(void) { return &_countFact; } - Fact* lock(void) { return &_lockFact; } + Fact* hdop (void) { return &_hdopFact; } + Fact* vdop (void) { return &_vdopFact; } + Fact* courseOverGround (void) { return &_courseOverGroundFact; } + Fact* count (void) { return &_countFact; } + Fact* lock (void) { return &_lockFact; } void setVehicle(Vehicle* vehicle); @@ -175,24 +177,33 @@ public: Q_PROPERTY(Fact* mahConsumed READ mahConsumed CONSTANT) Q_PROPERTY(Fact* current READ current CONSTANT) Q_PROPERTY(Fact* temperature READ temperature CONSTANT) - Q_PROPERTY(Fact* cellCount READ cellCount CONSTANT) + Q_PROPERTY(Fact* cellCount READ cellCount CONSTANT) + + /// If percentRemaining falls below this value, warning will be output through speech + Q_PROPERTY(Fact* percentRemainingAnnounce READ percentRemainingAnnounce CONSTANT) + + Fact* voltage (void) { return &_voltageFact; } + Fact* percentRemaining (void) { return &_percentRemainingFact; } + Fact* percentRemainingAnnounce (void) { return &_percentRemainingAnnounceFact; } + Fact* mahConsumed (void) { return &_mahConsumedFact; } + Fact* current (void) { return &_currentFact; } + Fact* temperature (void) { return &_temperatureFact; } + Fact* cellCount (void) { return &_cellCountFact; } - Fact* voltage(void) { return &_voltageFact; } - Fact* percentRemaining(void) { return &_percentRemainingFact; } - Fact* mahConsumed(void) { return &_mahConsumedFact; } - Fact* current(void) { return &_currentFact; } - Fact* temperature(void) { return &_temperatureFact; } - Fact* cellCount(void) { return &_cellCountFact; } void setVehicle(Vehicle* vehicle); static const char* _voltageFactName; static const char* _percentRemainingFactName; + static const char* _percentRemainingAnnounceFactName; static const char* _mahConsumedFactName; static const char* _currentFactName; static const char* _temperatureFactName; static const char* _cellCountFactName; + static const char* _settingsGroup; + static const int _percentRemainingAnnounceDefault; + static const double _voltageUnavailable; static const int _percentRemainingUnavailable; static const int _mahConsumedUnavailable; @@ -201,13 +212,15 @@ public: static const int _cellCountUnavailable; private: - Vehicle* _vehicle; - Fact _voltageFact; - Fact _percentRemainingFact; - Fact _mahConsumedFact; - Fact _currentFact; - Fact _temperatureFact; - Fact _cellCountFact; + Vehicle* _vehicle; + Fact _voltageFact; + Fact _percentRemainingFact; + Fact _mahConsumedFact; + Fact _currentFact; + Fact _temperatureFact; + Fact _cellCountFact; + + static SettingsFact _percentRemainingAnnounceFact; }; class Vehicle : public FactGroup @@ -222,6 +235,11 @@ public: FirmwarePluginManager* firmwarePluginManager, AutoPilotPluginManager* autopilotPluginManager, JoystickManager* joystickManager); + + // The following is used to create a disconnected Vehicle. A disconnected vehicle is primarily used to access FactGroup information + // without needing a real connection. + Vehicle(QObject* parent = NULL); + ~Vehicle(); Q_PROPERTY(int id READ id CONSTANT) @@ -505,6 +523,7 @@ private slots: void _addNewMapTrajectoryPoint(void); void _parametersReady(bool parametersReady); void _remoteControlRSSIChanged(uint8_t rssi); + void _announceflightModeChanged(const QString& flightMode); void _handleTextMessage (int newCount); void _handletextMessageReceived (UASMessage* message); @@ -540,11 +559,13 @@ private: void _mapTrajectoryStart(void); void _mapTrajectoryStop(void); void _connectionActive(void); - void _say(const QString& text, int severity); + void _say(const QString& text); + QString _vehicleIdSpeech(void); private: int _id; ///< Mavlink system id bool _active; + bool _disconnectedVehicle; ///< This Vehicle is a "disconnected" vehicle for ui use when no active vehicle is available MAV_AUTOPILOT _firmwareType; MAV_TYPE _vehicleType; @@ -635,6 +656,9 @@ private: uint8_t _compID; bool _heardFrom; + static const int _lowBatteryAnnounceRepeatMSecs; // Amount of time in between each low battery announcement + QElapsedTimer _lowBatteryAnnounceTimer; + // FactGroup facts Fact _rollFact; diff --git a/src/ui/preferences/GeneralSettings.qml b/src/ui/preferences/GeneralSettings.qml index a4c8e27..04b361b 100644 --- a/src/ui/preferences/GeneralSettings.qml +++ b/src/ui/preferences/GeneralSettings.qml @@ -40,6 +40,8 @@ Rectangle { anchors.fill: parent anchors.margins: ScreenTools.defaultFontPixelWidth + property Fact _percentRemainingAnnounce: QGroundControl.multiVehicleManager.disconnectedVehicle.battery.percentRemainingAnnounce + QGCPalette { id: qgcPal colorGroupEnabled: enabled @@ -127,6 +129,32 @@ Rectangle { } } } + //----------------------------------------------------------------- + //-- Battery talker + Row { + spacing: ScreenTools.defaultFontPixelWidth + + QGCCheckBox { + id: announcePercentCheckbox + anchors.baseline: announcePercent.baseline + text: "Announce battery percent lower than:" + checked: _percentRemainingAnnounce.value != 0 + + onClicked: { + if (checked) { + _percentRemainingAnnounce.value = _percentRemainingAnnounce.defaultValueString + } else { + _percentRemainingAnnounce.value = 0 + } + } + } + + FactTextField { + id: announcePercent + fact: _percentRemainingAnnounce + enabled: announcePercentCheckbox.checked + } + } Item { height: ScreenTools.defaultFontPixelHeight / 2