From dab6ec1f30e5a0e35db8f7b1cb72b8adf2cf40b9 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 2 May 2019 16:46:36 -0700 Subject: [PATCH] Save/Restore stream rates. Auto flight mode support --- src/QmlControls/PIDTuning.qml | 88 +++++++++++++++++++++-------------- src/Vehicle/Vehicle.cc | 78 +++++++++++++++++++++++++------ src/Vehicle/Vehicle.h | 8 ++++ 3 files changed, 124 insertions(+), 50 deletions(-) diff --git a/src/QmlControls/PIDTuning.qml b/src/QmlControls/PIDTuning.qml index 781c3e076..d563b7894 100644 --- a/src/QmlControls/PIDTuning.qml +++ b/src/QmlControls/PIDTuning.qml @@ -146,7 +146,7 @@ RowLayout { saveTuningParamValues() } - Component.onDestruction: _activeVehicle.setPIDTuningTelemetryMode(true) + Component.onDestruction: _activeVehicle.setPIDTuningTelemetryMode(false) on_CurrentTuneTypeChanged: { saveTuningParamValues() @@ -197,14 +197,6 @@ RowLayout { running: false repeat: true - function startOrStop() { - if (dataTimer.running) { - dataTimer.stop() - } else { - dataTimer.start() - } - } - onTriggered: { _valueXAxis.max = _msecs _valueRateXAxis.max = _msecs @@ -238,25 +230,25 @@ RowLayout { spacing: _margins Layout.alignment: Qt.AlignTop - QGCLabel { text: qsTr("Tuning Axis:") } + Column { + QGCLabel { text: qsTr("Tuning Axis:") } - RowLayout { - spacing: _margins + RowLayout { + spacing: _margins - Repeater { - model: tuneList - QGCRadioButton { - text: modelData - checked: _currentTuneType === modelData - exclusiveGroup: tuneTypeRadios + Repeater { + model: tuneList + QGCRadioButton { + text: modelData + checked: _currentTuneType === modelData + exclusiveGroup: tuneTypeRadios - onClicked: _currentTuneType = modelData + onClicked: _currentTuneType = modelData + } } } } - Item { width: 1; height: 1 } - QGCLabel { text: qsTr("Tuning Values:") } GridLayout { @@ -328,26 +320,27 @@ RowLayout { } } } - Item { width: 1; height: 1 } - QGCLabel { text: qsTr("Clipboard Values:") } + Column { + QGCLabel { text: qsTr("Clipboard Values:") } - GridLayout { - rows: savedRepeater.model.length - flow: GridLayout.TopToBottom - rowSpacing: _margins - columnSpacing: _margins + GridLayout { + rows: savedRepeater.model.length + flow: GridLayout.TopToBottom + rowSpacing: 0 + columnSpacing: _margins - Repeater { - model: params[tuneList.indexOf(_currentTuneType)] + Repeater { + model: params[tuneList.indexOf(_currentTuneType)] - QGCLabel { text: modelData.name } - } + QGCLabel { text: modelData.name } + } - Repeater { - id: savedRepeater + Repeater { + id: savedRepeater - QGCLabel { text: modelData } + QGCLabel { text: modelData } + } } } @@ -379,7 +372,30 @@ RowLayout { QGCButton { text: dataTimer.running ? qsTr("Stop") : qsTr("Start") - onClicked: dataTimer.startOrStop() + onClicked: { + dataTimer.running = !dataTimer.running + if (autoModeChange.checked) { + _activeVehicle.flightMode = dataTimer.running ? "Stabilized" : _activeVehicle.pauseFlightMode + } + } + } + } + + QGCCheckBox { + id: autoModeChange + text: qsTr("Automatic Flight Mode Switching") + } + + Column { + visible: autoModeChange.checked + QGCLabel { + text: qsTr("Switches to 'Stabilized' when you click Start.") + font.pointSize: ScreenTools.smallFontPointSize + } + + QGCLabel { + text: qsTr("Switches to '%1' when you click Stop.").arg(_activeVehicle.pauseFlightMode) + font.pointSize: ScreenTools.smallFontPointSize } } } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 2126b1c48..4b025de62 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -185,6 +185,8 @@ Vehicle::Vehicle(LinkInterface* link, , _lastAnnouncedLowBatteryPercent(100) , _priorityLinkCommanded(false) , _orbitActive(false) + , _pidTuningTelemetryMode(false) + , _pidTuningWaitingForRates(false) , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble) @@ -386,6 +388,8 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _uid(0) , _lastAnnouncedLowBatteryPercent(100) , _orbitActive(false) + , _pidTuningTelemetryMode(false) + , _pidTuningWaitingForRates(false) , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble) @@ -519,6 +523,8 @@ void Vehicle::_commonInit(void) } } #endif + + _pidTuningMessages << MAVLINK_MSG_ID_ATTITUDE << MAVLINK_MSG_ID_ATTITUDE_TARGET; } Vehicle::~Vehicle() @@ -794,7 +800,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS: _handleOrbitExecutionStatus(message); break; - + case MAVLINK_MSG_ID_MESSAGE_INTERVAL: + _handleMessageInterval(message); + break; case MAVLINK_MSG_ID_PING: _handlePing(link, message); break; @@ -3859,29 +3867,71 @@ int Vehicle::versionCompare(int major, int minor, int patch) return _firmwarePlugin->versionCompare(this, major, minor, patch); } -void Vehicle::setPIDTuningTelemetryMode(bool pidTuning) +void Vehicle::_handleMessageInterval(const mavlink_message_t& message) { - qDebug() << "setPIDTuningTelemetryMode" << pidTuning; - QList rgTelemetry; + if (_pidTuningWaitingForRates) { + mavlink_message_interval_t messageInterval; + + mavlink_msg_message_interval_decode(&message, &messageInterval); + + int msgId = messageInterval.message_id; + if (_pidTuningMessages.contains(msgId)) { + _pidTuningMessageRatesUsecs[msgId] = messageInterval.interval_us; + } + if (_pidTuningMessageRatesUsecs.count() == _pidTuningMessages.count()) { + // We have back all the rates we requested + _pidTuningWaitingForRates = false; + _pidTuningAdjustRates(true); + } + } +} + +void Vehicle::setPIDTuningTelemetryMode(bool pidTuning) +{ if (pidTuning) { - rgTelemetry << MAVLINK_MSG_ID_ATTITUDE << MAVLINK_MSG_ID_ATTITUDE_TARGET; - for (int telemetry: rgTelemetry) { + if (!_pidTuningTelemetryMode) { + // First step is to get the current message rates before we adjust them + _pidTuningTelemetryMode = true; + _pidTuningWaitingForRates = true; + _pidTuningMessageRatesUsecs.clear(); + for (int telemetry: _pidTuningMessages) { + sendMavCommand(defaultComponentId(), + MAV_CMD_GET_MESSAGE_INTERVAL, + true, // show error + telemetry); + } + } + } else { + if (_pidTuningTelemetryMode) { + _pidTuningTelemetryMode = false; + if (_pidTuningWaitingForRates) { + // We never finished waiting for previous rates + _pidTuningWaitingForRates = false; + } else { + _pidTuningAdjustRates(false); + } + } + } +} + +void Vehicle::_pidTuningAdjustRates(bool setRatesForTuning) +{ + int requestedRate = (int)(1000000.0 / 30.0); // 30 Hz in usecs + + for (int telemetry: _pidTuningMessages) { + + if (requestedRate < _pidTuningMessageRatesUsecs[telemetry]) { sendMavCommand(defaultComponentId(), MAV_CMD_SET_MESSAGE_INTERVAL, true, // show error telemetry, - pidTuning ? (1000000.0 / 30.0) /* Hz */ : -1 /* default Hz */); + setRatesForTuning ? requestedRate : _pidTuningMessageRatesUsecs[telemetry]); } } - setLiveUpdates(pidTuning); - _setpointFactGroup.setLiveUpdates(pidTuning); - - sendMavCommand(defaultComponentId(), - MAV_CMD_GET_MESSAGE_INTERVAL, - true, // show error - MAVLINK_MSG_ID_ATTITUDE); + setLiveUpdates(setRatesForTuning); + _setpointFactGroup.setLiveUpdates(setRatesForTuning); } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 71190b5c6..bc194260f 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -1267,6 +1267,7 @@ private: void _handleEstimatorStatus(mavlink_message_t& message); void _handleStatusText(mavlink_message_t& message, bool longVersion); void _handleOrbitExecutionStatus(const mavlink_message_t& message); + void _handleMessageInterval(const mavlink_message_t& message); // ArduPilot dialect messages #if !defined(NO_ARDUPILOT_DIALECT) void _handleCameraFeedback(const mavlink_message_t& message); @@ -1293,6 +1294,7 @@ private: void _setCapabilities(uint64_t capabilityBits); void _updateArmed(bool armed); bool _apmArmingNotRequired(void); + void _pidTuningAdjustRates(bool setRatesForTuning); int _id; ///< Mavlink system id int _defaultComponentId; @@ -1475,6 +1477,12 @@ private: QTimer _orbitTelemetryTimer; static const int _orbitTelemetryTimeoutMsecs = 3000; // No telemetry for this amount and orbit will go inactive + // PID Tuning telemetry mode + bool _pidTuningTelemetryMode; + bool _pidTuningWaitingForRates; + QList _pidTuningMessages; + QMap _pidTuningMessageRatesUsecs; + // FactGroup facts Fact _rollFact; -- 2.22.0