diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 9a62d9debb942bdb050b8138dd61be60e790bc45..4d3a1259efac58805282ce2db21b6ed412519a0f 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -71,7 +71,8 @@ float MockLink::_vehicleLatitude = 47.633033f; float MockLink::_vehicleLongitude = -122.08794f; float MockLink::_vehicleAltitude = 2.5f; -const char* MockConfiguration::_firmwareTypeKey = "FirmwareType"; +const char* MockConfiguration::_firmwareTypeKey = "FirmwareType"; +const char* MockConfiguration::_sendStatusTextKey = "SendStatusText"; MockLink::MockLink(MockConfiguration* config) : _missionItemHandler(this) @@ -85,10 +86,12 @@ MockLink::MockLink(MockConfiguration* config) , _mavState(MAV_STATE_STANDBY) , _autopilotType(MAV_AUTOPILOT_PX4) , _fileServer(NULL) + , _sendStatusText(false) { _config = config; if (_config) { _autopilotType = config->firmwareType(); + _sendStatusText = config->sendStatusText(); } union px4_custom_mode px4_cm; @@ -168,6 +171,10 @@ void MockLink::_run1HzTasks(void) if (_mavlinkStarted && _connected) { _sendHeartBeat(); _sendHomePosition(); + if (_sendStatusText) { + _sendStatusText = false; + _sendStatusTextMessages(); + } } } @@ -747,9 +754,42 @@ void MockLink::_sendGpsRawInt(void) respondWithMavlinkMessage(msg); } +void MockLink::_sendStatusTextMessages(void) +{ + struct StatusMessage { + MAV_SEVERITY severity; + const char* msg; + }; + + static const struct StatusMessage rgMessages[] = { + { MAV_SEVERITY_INFO, "#Testing audio output" }, + { MAV_SEVERITY_EMERGENCY, "Status text emergency" }, + { MAV_SEVERITY_ALERT, "Status text alert" }, + { MAV_SEVERITY_CRITICAL, "Status text critical" }, + { MAV_SEVERITY_ERROR, "Status text error" }, + { MAV_SEVERITY_WARNING, "Status text warning" }, + { MAV_SEVERITY_NOTICE, "Status text notice" }, + { MAV_SEVERITY_INFO, "Status text info" }, + { MAV_SEVERITY_DEBUG, "Status text debug" }, + }; + + for (size_t i=0; iseverity, + status->msg); + respondWithMavlinkMessage(msg); + } +} + MockConfiguration::MockConfiguration(const QString& name) : LinkConfiguration(name) , _firmwareType(MAV_AUTOPILOT_PX4) + , _sendStatusText(false) { } @@ -757,21 +797,29 @@ MockConfiguration::MockConfiguration(const QString& name) MockConfiguration::MockConfiguration(MockConfiguration* source) : LinkConfiguration(source) { - _firmwareType = source->_firmwareType; + _firmwareType = source->_firmwareType; + _sendStatusText = source->_sendStatusText; } void MockConfiguration::copyFrom(LinkConfiguration *source) { LinkConfiguration::copyFrom(source); MockConfiguration* usource = dynamic_cast(source); - Q_ASSERT(usource != NULL); - _firmwareType = usource->_firmwareType; + + if (!usource) { + qWarning() << "dynamic_cast failed" << source << usource; + return; + } + + _firmwareType = usource->_firmwareType; + _sendStatusText = usource->_sendStatusText; } void MockConfiguration::saveSettings(QSettings& settings, const QString& root) { settings.beginGroup(root); settings.setValue(_firmwareTypeKey, (int)_firmwareType); + settings.setValue(_sendStatusTextKey, _sendStatusText); settings.sync(); settings.endGroup(); } @@ -780,6 +828,7 @@ void MockConfiguration::loadSettings(QSettings& settings, const QString& root) { settings.beginGroup(root); _firmwareType = (MAV_AUTOPILOT)settings.value(_firmwareTypeKey, (int)MAV_AUTOPILOT_PX4).toInt(); + _sendStatusText = settings.value(_sendStatusTextKey, false).toBool(); settings.endGroup(); } @@ -789,7 +838,7 @@ void MockConfiguration::updateSettings() MockLink* ulink = dynamic_cast(_link); if (ulink) { // Restart connect not supported - Q_ASSERT(false); + qWarning() << "updateSettings not supported"; //ulink->_restartConnection(); } } diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 9da4fe7b4c7bf28f887a0e4c67f922d34d81f152..1ee7b95887ecf27f7da1a5693c2415e7e232faa9 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -44,6 +44,10 @@ public: MAV_AUTOPILOT firmwareType(void) { return _firmwareType; } void setFirmwareType(MAV_AUTOPILOT firmwareType) { _firmwareType = firmwareType; } + /// @param sendStatusText true: mavlink status text messages will be sent for each severity, as well as voice output info message + void setSendStatusText(bool sendStatusText) { _sendStatusText = sendStatusText; } + bool sendStatusText(void) { return _sendStatusText; } + // Overrides from LinkConfiguration int type(void) { return LinkConfiguration::TypeMock; } void copyFrom(LinkConfiguration* source); @@ -52,9 +56,11 @@ public: void updateSettings(void); private: - MAV_AUTOPILOT _firmwareType; + MAV_AUTOPILOT _firmwareType; + bool _sendStatusText; static const char* _firmwareTypeKey; + static const char* _sendStatusTextKey; }; class MockLink : public LinkInterface @@ -70,6 +76,7 @@ public: int vehicleId(void) { return _vehicleSystemId; } MAV_AUTOPILOT getAutopilotType(void) { return _autopilotType; } void setAutopilotType(MAV_AUTOPILOT autopilot) { _autopilotType = autopilot; } + void setSendStatusText(bool sendStatusText) { _sendStatusText = sendStatusText; } void emitRemoteControlChannelRawChanged(int channel, uint16_t raw); /// Sends the specified mavlink message to QGC @@ -149,6 +156,7 @@ private: void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat); void _sendHomePosition(void); void _sendGpsRawInt(void); + void _sendStatusTextMessages(void); MockLinkMissionItemHandler _missionItemHandler; @@ -173,6 +181,8 @@ private: MockLinkFileServer* _fileServer; + bool _sendStatusText; + static float _vehicleLatitude; static float _vehicleLongitude; static float _vehicleAltitude; diff --git a/src/ui/MockLinkConfiguration.cc b/src/ui/MockLinkConfiguration.cc index c6b3e477ceb91620d9f87445f1758c85e0614a57..3c8e05d2642e03ce26acbce43ca9b47082225fe6 100644 --- a/src/ui/MockLinkConfiguration.cc +++ b/src/ui/MockLinkConfiguration.cc @@ -43,9 +43,12 @@ MockLinkConfiguration::MockLinkConfiguration(MockConfiguration *config, QWidget break; } - connect(_ui->px4Radio, &QRadioButton::clicked, this, &MockLinkConfiguration::_px4RadioClicked); - connect(_ui->apmRadio, &QRadioButton::clicked, this, &MockLinkConfiguration::_apmRadioClicked); - connect(_ui->genericRadio, &QRadioButton::clicked, this, &MockLinkConfiguration::_genericRadioClicked); + _ui->sendStatusTextCheckBox->setChecked(config->sendStatusText()); + + connect(_ui->px4Radio, &QRadioButton::clicked, this, &MockLinkConfiguration::_px4RadioClicked); + connect(_ui->apmRadio, &QRadioButton::clicked, this, &MockLinkConfiguration::_apmRadioClicked); + connect(_ui->genericRadio, &QRadioButton::clicked, this, &MockLinkConfiguration::_genericRadioClicked); + connect(_ui->sendStatusTextCheckBox, &QCheckBox::clicked, this, &MockLinkConfiguration::_sendStatusTextClicked); } MockLinkConfiguration::~MockLinkConfiguration() @@ -73,3 +76,8 @@ void MockLinkConfiguration::_genericRadioClicked(bool checked) _config->setFirmwareType(MAV_AUTOPILOT_GENERIC); } } + +void MockLinkConfiguration::_sendStatusTextClicked(bool checked) +{ + _config->setSendStatusText(checked); +} diff --git a/src/ui/MockLinkConfiguration.h b/src/ui/MockLinkConfiguration.h index 34feb999f39f8493d14f5c6cf5578e004b7f3948..61e55db10dcee1a6d264861200cc6ed0eb0a86dc 100644 --- a/src/ui/MockLinkConfiguration.h +++ b/src/ui/MockLinkConfiguration.h @@ -44,6 +44,7 @@ private slots: void _px4RadioClicked(bool checked); void _apmRadioClicked(bool checked); void _genericRadioClicked(bool checked); + void _sendStatusTextClicked(bool checked); private: Ui::MockLinkConfiguration* _ui; diff --git a/src/ui/MockLinkConfiguration.ui b/src/ui/MockLinkConfiguration.ui index d54fbb091abbf227b45934cb6427718f0f3629eb..1519985366115e7d0c045bb3757888494da11d36 100644 --- a/src/ui/MockLinkConfiguration.ui +++ b/src/ui/MockLinkConfiguration.ui @@ -35,6 +35,13 @@ + + + + Send status text + voice + + +