diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index dd06a03..40f0871 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -224,6 +224,7 @@ src/Vehicle/ClockFact.json src/Vehicle/GPSFact.json src/Vehicle/GPSRTKFact.json + src/Vehicle/SetpointFact.json src/Vehicle/SubmarineFact.json src/Vehicle/TemperatureFact.json src/Vehicle/VehicleFact.json diff --git a/src/AutoPilotPlugins/Common/SetupPage.qml b/src/AutoPilotPlugins/Common/SetupPage.qml index 82dc1dd..891b6f1 100644 --- a/src/AutoPilotPlugins/Common/SetupPage.qml +++ b/src/AutoPilotPlugins/Common/SetupPage.qml @@ -10,6 +10,7 @@ import QtQuick 2.3 import QtQuick.Controls 1.2 import QtQuick.Dialogs 1.2 +import QtQuick.Layouts 1.2 import QGroundControl 1.0 import QGroundControl.FactSystem 1.0 @@ -30,6 +31,8 @@ QGCView { property string pageDescription: vehicleComponent ? vehicleComponent.description : "" property real availableWidth: width - pageLoader.x property real availableHeight: height - pageLoader.y + property bool showAdvanced: false + property alias advanced: advancedCheckBox.checked property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle property bool _vehicleArmed: _activeVehicle ? _activeVehicle.armed : false @@ -53,31 +56,45 @@ QGCView { contentHeight: pageLoader.y + pageLoader.item.height clip: true - Column { - id: headingColumn - width: setupPanel.width + RowLayout { + id: headingRow + anchors.left: parent.left + anchors.right: parent.right spacing: _margins + layoutDirection: Qt.RightToLeft - QGCLabel { - font.pointSize: ScreenTools.largeFontPointSize - text: !setupView.enabled ? _pageTitle + "" + qsTr(" (Disabled while the vehicle is %1)").arg(_disableReason) + "" : _pageTitle - visible: !ScreenTools.isShortScreen + QGCCheckBox { + id: advancedCheckBox + text: qsTr("Advanced") + visible: showAdvanced } - QGCLabel { - anchors.left: parent.left - anchors.right: parent.right - wrapMode: Text.WordWrap - text: pageDescription - visible: !ScreenTools.isShortScreen + Column { + spacing: _margins + Layout.fillWidth: true + + QGCLabel { + font.pointSize: ScreenTools.largeFontPointSize + text: !setupView.enabled ? _pageTitle + "" + qsTr(" (Disabled while the vehicle is %1)").arg(_disableReason) + "" : _pageTitle + visible: !ScreenTools.isShortScreen + } + + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + text: pageDescription + visible: !ScreenTools.isShortScreen + } } } Loader { id: pageLoader anchors.topMargin: _margins - anchors.top: headingColumn.bottom + anchors.top: headingRow.bottom } + // Overlay to display when vehicle is armed and this setup page needs // to be disabled Rectangle { diff --git a/src/AutoPilotPlugins/PX4/PX4TuningComponent.h b/src/AutoPilotPlugins/PX4/PX4TuningComponent.h index 3080b96..5fe989d 100644 --- a/src/AutoPilotPlugins/PX4/PX4TuningComponent.h +++ b/src/AutoPilotPlugins/PX4/PX4TuningComponent.h @@ -32,6 +32,7 @@ public: QUrl setupSource(void) const final; QUrl summaryQmlSource(void) const final; bool allowSetupWhileArmed(void) const final { return true; } + bool allowSetupWhileFlying(void) const final { return true; } private: const QString _name; diff --git a/src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml b/src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml index 5698000..999c1a8 100644 --- a/src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml +++ b/src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml @@ -7,11 +7,16 @@ * ****************************************************************************/ +import QtQuick 2.3 +import QtQuick.Controls 1.2 +import QtCharts 2.2 +import QtQuick.Layouts 1.2 -import QtQuick 2.3 -import QtQuick.Controls 1.2 - -import QGroundControl.Controls 1.0 +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 +import QGroundControl.ScreenTools 1.0 SetupPage { id: tuningPage @@ -20,47 +25,486 @@ SetupPage { Component { id: pageComponent - FactSliderPanel { - width: availableWidth - qgcViewPanel: tuningPage.viewPanel - - sliderModel: ListModel { - ListElement { - title: qsTr("Hover Throttle") - description: qsTr("Adjust throttle so hover is at mid-throttle. Slide to the left if hover is lower than throttle center. Slide to the right if hover is higher than throttle center.") - param: "MPC_THR_HOVER" - min: 20 - max: 80 - step: 1 + Column { + width: availableWidth + + property real _chartHeight: ScreenTools.defaultFontPixelHeight * 20 + property real _margins: ScreenTools.defaultFontPixelHeight / 2 + property string _currentTuneType: _tuneList[0] + property real _roll: _activeVehicle.roll.value + property real _rollSetpoint: _activeVehicle.setpoint.roll.value + property real _rollRate: _activeVehicle.rollRate.value + property real _rollRateSetpoint: _activeVehicle.setpoint.rollRate.value + property real _pitch: _activeVehicle.pitch.value + property real _pitchSetpoint: _activeVehicle.setpoint.pitch.value + property real _pitchRate: _activeVehicle.pitchRate.value + property real _pitchRateSetpoint: _activeVehicle.setpoint.pitchRate.value + property real _yaw: _activeVehicle.heading.value + property real _yawSetpoint: _activeVehicle.setpoint.yaw.value + property real _yawRate: _activeVehicle.yawRate.value + property real _yawRateSetpoint: _activeVehicle.setpoint.yawRate.value + property var _valueXAxis: valueXAxis + property var _valueRateXAxis: valueRateXAxis + property var _valueYAxis: valueYAxis + property var _valueRateYAxis: valueRateYAxis + property int _msecs: 0 + property var _savedTuningParamValues: [ ] + + // The following are set when getValues is called + property real _value + property real _valueSetpoint + property real _valueRate + property real _valueRateSetpoint + + readonly property int _tickSeparation: 5 + readonly property int _tuneListRollIndex: 0 + readonly property int _tuneListPitchIndex: 1 + readonly property int _tuneListYawIndex: 2 + readonly property var _tuneList: [ qsTr("Roll"), qsTr("Pitch"), qsTr("Yaw") ] + readonly property var _params: [ + [ controller.getParameterFact(-1, "MC_ROLL_P"), + controller.getParameterFact(-1, "MC_ROLLRATE_P"), + controller.getParameterFact(-1, "MC_ROLLRATE_I"), + controller.getParameterFact(-1, "MC_ROLLRATE_D"), + controller.getParameterFact(-1, "MC_ROLLRATE_FF") ], + [ controller.getParameterFact(-1, "MC_PITCH_P"), + controller.getParameterFact(-1, "MC_PITCHRATE_P"), + controller.getParameterFact(-1, "MC_PITCHRATE_I"), + controller.getParameterFact(-1, "MC_PITCHRATE_D"), + controller.getParameterFact(-1, "MC_PITCHRATE_FF") ], + [ controller.getParameterFact(-1, "MC_YAW_P"), + controller.getParameterFact(-1, "MC_YAWRATE_P"), + controller.getParameterFact(-1, "MC_YAWRATE_I"), + controller.getParameterFact(-1, "MC_YAWRATE_D"), + controller.getParameterFact(-1, "MC_YAW_FF"), + controller.getParameterFact(-1, "MC_YAWRATE_FF") ] ] + + function adjustYAxisMin(yAxis, newValue) { + var newMin = Math.min(yAxis.min, newValue) + if (newMin % 5 != 0) { + newMin -= 5 + newMin = Math.floor(newMin / _tickSeparation) * _tickSeparation + } + yAxis.min = newMin + } + + function adjustYAxisMax(yAxis, newValue) { + var newMax = Math.max(yAxis.max, newValue) + if (newMax % 5 != 0) { + newMax += 5 + newMax = Math.floor(newMax / _tickSeparation) * _tickSeparation + } + yAxis.max = newMax + } + + function getValues() { + if (_currentTuneType === _tuneList[_tuneListRollIndex]) { + _value = _roll + _valueSetpoint = _rollSetpoint + _valueRate = _rollRate + _valueRateSetpoint = _rollRateSetpoint + } else if (_currentTuneType === _tuneList[_tuneListPitchIndex]) { + _value = _pitch + _valueSetpoint = _pitchSetpoint + _valueRate = _pitchRate + _valueRateSetpoint = _pitchRateSetpoint + } else if (_currentTuneType === _tuneList[_tuneListYawIndex]) { + _value = _yaw + _valueSetpoint = _yawSetpoint + _valueRate = _yawRate + _valueRateSetpoint = _yawRateSetpoint + } + } + + function resetGraphs() { + valueSeries.removePoints(0, valueSeries.count) + valueSetpointSeries.removePoints(0, valueSetpointSeries.count) + valueRateSeries.removePoints(0, valueRateSeries.count) + valueRateSetpointSeries.removePoints(0, valueRateSetpointSeries.count) + _valueXAxis.min = 0 + _valueXAxis.max = 0 + _valueRateXAxis.min = 0 + _valueRateXAxis.max = 0 + _valueYAxis.min = 0 + _valueYAxis.max = 10 + _valueRateYAxis.min = 0 + _valueRateYAxis.max = 10 + _msecs = 0 + } + + function currentTuneTypeIndex() { + if (_currentTuneType === _tuneList[_tuneListRollIndex]) { + return _tuneListRollIndex + } else if (_currentTuneType === _tuneList[_tuneListPitchIndex]) { + return _tuneListPitchIndex + } else if (_currentTuneType === _tuneList[_tuneListYawIndex]) { + return _tuneListYawIndex } + } + + // Save the current set of tuning values so we can reset to them + function saveTuningParamValues() { + var tuneTypeIndex = currentTuneTypeIndex() - ListElement { - title: qsTr("Manual minimum throttle") - description: qsTr("Slide to the left to start the motors with less idle power. Slide to the right if descending in manual flight becomes unstable.") - param: "MPC_MANTHR_MIN" - min: 0 - max: 15 - step: 1 + _savedTuningParamValues = [ ] + var currentTuneParams = _params[tuneTypeIndex] + for (var i=0; i _maxPointCount) { + valueSeries.remove(0) + valueSetpointSeries.remove(0) + valueRateSeries.remove(0) + valueRateSetpointSeries.remove(0) + valueXAxis.min = valueSeries.at(0).x + valueRateXAxis.min = valueSeries.at(0).x + } + } + + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property int _maxPointCount: 10000 / interval + } + + // Standard tuning page + FactSliderPanel { + width: availableWidth + qgcViewPanel: tuningPage.viewPanel + visible: !advanced + + sliderModel: ListModel { + ListElement { + title: qsTr("Hover Throttle") + description: qsTr("Adjust throttle so hover is at mid-throttle. Slide to the left if hover is lower than throttle center. Slide to the right if hover is higher than throttle center.") + param: "MPC_THR_HOVER" + min: 20 + max: 80 + step: 1 + } - ListElement { - title: qsTr("Pitch sensitivity") - description: qsTr("Slide to the left to make pitch control faster and more accurate. Slide to the right if pitch oscillates or is too twitchy.") - param: "MC_PITCH_TC" - min: 0.15 - max: 0.25 - step: 0.01 + ListElement { + title: qsTr("Manual minimum throttle") + description: qsTr("Slide to the left to start the motors with less idle power. Slide to the right if descending in manual flight becomes unstable.") + param: "MPC_MANTHR_MIN" + min: 0 + max: 15 + step: 1 + } +/* + These seem to have disappeared from PX4 firmware! + ListElement { + title: qsTr("Roll sensitivity") + description: qsTr("Slide to the left to make roll control faster and more accurate. Slide to the right if roll oscillates or is too twitchy.") + param: "MC_ROLL_TC" + min: 0.15 + max: 0.25 + step: 0.01 + } + + ListElement { + title: qsTr("Pitch sensitivity") + description: qsTr("Slide to the left to make pitch control faster and more accurate. Slide to the right if pitch oscillates or is too twitchy.") + param: "MC_PITCH_TC" + min: 0.15 + max: 0.25 + step: 0.01 + } +*/ } } - } - } // Component + + // Advanced page + RowLayout { + anchors.left: parent.left + anchors.right: parent.right + layoutDirection: Qt.RightToLeft + visible: advanced + + Column { + spacing: _margins + Layout.alignment: Qt.AlignTop + + QGCLabel { text: qsTr("Tuning Axis:") } + + RowLayout { + spacing: _margins + + Repeater { + model: _tuneList + QGCRadioButton { + text: modelData + checked: _currentTuneType === modelData + exclusiveGroup: tuneTypeRadios + + onClicked: _currentTuneType = modelData + } + } + } + + Item { width: 1; height: 1 } + + QGCLabel { text: qsTr("Tuning Values:") } + + GridLayout { + rows: factList.length + flow: GridLayout.TopToBottom + rowSpacing: _margins + columnSpacing: _margins + + property var factList: _params[_tuneList.indexOf(_currentTuneType)] + + Repeater { + model: parent.factList + + QGCLabel { text: modelData.name } + } + + Repeater { + model: parent.factList + + QGCButton { + text: "-" + onClicked: { + var value = modelData.value + modelData.value -= value * adjustPercentModel.get(adjustPercentCombo.currentIndex).value + } + } + } + + Repeater { + model: parent.factList + + FactTextField { + Layout.fillWidth: true + fact: modelData + showUnits: false + } + } + + Repeater { + model: parent.factList + + QGCButton { + text: "+" + onClicked: { + var value = modelData.value + modelData.value += value * adjustPercentModel.get(adjustPercentCombo.currentIndex).value + } + } + } + } + + RowLayout { + QGCLabel { text: qsTr("Increment/Decrement %") } + + QGCComboBox { + id: adjustPercentCombo + model: ListModel { + id: adjustPercentModel + ListElement { text: "5"; value: 0.05 } + ListElement { text: "10"; value: 0.10 } + ListElement { text: "15"; value: 0.15 } + ListElement { text: "20"; value: 0.20 } + } + } + } + Item { width: 1; height: 1 } + + QGCLabel { text: qsTr("Saved Tuning Values:") } + + GridLayout { + rows: savedRepeater.model.length + flow: GridLayout.TopToBottom + rowSpacing: _margins + columnSpacing: _margins + + Repeater { + model: _params[_tuneList.indexOf(_currentTuneType)] + + QGCLabel { text: modelData.name } + } + + Repeater { + id: savedRepeater + + QGCLabel { text: modelData } + } + } + + RowLayout { + spacing: _margins + + QGCButton { + text: qsTr("Save Values") + onClicked: saveTuningParamValues() + } + + QGCButton { + text: qsTr("Reset To Saved Values") + onClicked: resetToSavedTuningParamValues() + } + } + + Item { width: 1; height: 1 } + + QGCLabel { text: qsTr("Chart:") } + + RowLayout { + spacing: _margins + + QGCButton { + text: qsTr("Clear") + onClicked: resetGraphs() + } + + QGCButton { + text: dataTimer.running ? qsTr("Stop") : qsTr("Start") + onClicked: dataTimer.running = !dataTimer.running + } + } + } + + Column { + Layout.fillWidth: true + + ChartView { + anchors.left: parent.left + anchors.right: parent.right + height: availableHeight / 2 + title: _currentTuneType + antialiasing: true + legend.alignment: Qt.AlignRight + + LineSeries { + id: valueSeries + name: "Response" + axisY: valueYAxis + axisX: valueXAxis + } + + LineSeries { + id: valueSetpointSeries + name: "Command" + axisY: valueYAxis + axisX: valueXAxis + } + } + + ChartView { + anchors.left: parent.left + anchors.right: parent.right + height: availableHeight / 2 + title: _currentTuneType + qsTr(" Rate") + antialiasing: true + legend.alignment: Qt.AlignRight + + LineSeries { + id: valueRateSeries + name: "Response" + axisY: valueRateYAxis + axisX: valueRateXAxis + } + + LineSeries { + id: valueRateSetpointSeries + name: "Command" + axisY: valueRateYAxis + axisX: valueRateXAxis + } + } + } + } // RowLayout - Advanced Page + } // Column + } // Component - pageComponent } // SetupPage diff --git a/src/Vehicle/SetpointFact.json b/src/Vehicle/SetpointFact.json new file mode 100644 index 0000000..58fea58 --- /dev/null +++ b/src/Vehicle/SetpointFact.json @@ -0,0 +1,44 @@ +[ +{ + "name": "roll", + "shortDescription": "Roll Setpoint", + "type": "double", + "decimalPlaces": 1, + "units": "deg" +}, +{ + "name": "pitch", + "shortDescription": "Pitch Setpoint", + "type": "double", + "decimalPlaces": 1, + "units": "deg" +}, +{ + "name": "yaw", + "shortDescription": "Yaw Setpoint", + "type": "double", + "decimalPlaces": 1, + "units": "deg" +}, +{ + "name": "rollRate", + "shortDescription": "Roll Rate Setpoint", + "type": "double", + "decimalPlaces": 1, + "units": "deg/s" +}, +{ + "name": "pitchRate", + "shortDescription": "Pitch Rate Setpoint", + "type": "double", + "decimalPlaces": 1, + "units": "deg/s" +}, +{ + "name": "yawRate", + "shortDescription": "Yaw Rate Setpoint", + "type": "double", + "decimalPlaces": 1, + "units": "deg/s" +} +] diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index a5f2d06..fbd986b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -10,6 +10,7 @@ #include #include #include +#include #include "Vehicle.h" #include "MAVLinkProtocol.h" @@ -54,6 +55,9 @@ const char* Vehicle::_joystickEnabledSettingsKey = "JoystickEnabled"; const char* Vehicle::_rollFactName = "roll"; const char* Vehicle::_pitchFactName = "pitch"; const char* Vehicle::_headingFactName = "heading"; +const char* Vehicle::_rollRateFactName = "rollRate"; +const char* Vehicle::_pitchRateFactName = "pitchRate"; +const char* Vehicle::_yawRateFactName = "yawRate"; const char* Vehicle::_airSpeedFactName = "airSpeed"; const char* Vehicle::_groundSpeedFactName = "groundSpeed"; const char* Vehicle::_climbRateFactName = "climbRate"; @@ -166,6 +170,9 @@ Vehicle::Vehicle(LinkInterface* link, , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble) + , _rollRateFact (0, _rollRateFactName, FactMetaData::valueTypeDouble) + , _pitchRateFact (0, _pitchRateFactName, FactMetaData::valueTypeDouble) + , _yawRateFact (0, _yawRateFactName, FactMetaData::valueTypeDouble) , _groundSpeedFact (0, _groundSpeedFactName, FactMetaData::valueTypeDouble) , _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble) , _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble) @@ -353,6 +360,9 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble) + , _rollRateFact (0, _rollRateFactName, FactMetaData::valueTypeDouble) + , _pitchRateFact (0, _pitchRateFactName, FactMetaData::valueTypeDouble) + , _yawRateFact (0, _yawRateFactName, FactMetaData::valueTypeDouble) , _groundSpeedFact (0, _groundSpeedFactName, FactMetaData::valueTypeDouble) , _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble) , _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble) @@ -413,6 +423,9 @@ void Vehicle::_commonInit(void) _addFact(&_rollFact, _rollFactName); _addFact(&_pitchFact, _pitchFactName); _addFact(&_headingFact, _headingFactName); + _addFact(&_rollRateFact, _rollRateFactName); + _addFact(&_pitchRateFact, _pitchRateFactName); + _addFact(&_yawRateFact, _yawRateFactName); _addFact(&_groundSpeedFact, _groundSpeedFactName); _addFact(&_airSpeedFact, _airSpeedFactName); _addFact(&_climbRateFact, _climbRateFactName); @@ -696,6 +709,12 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_HIGH_LATENCY2: _handleHighLatency2(message); break; + case MAVLINK_MSG_ID_ATTITUDE: + _handleAttitude(message); + break; + case MAVLINK_MSG_ID_ATTITUDE_TARGET: + _handleAttitudeTarget(message); + break; case MAVLINK_MSG_ID_SERIAL_CONTROL: { @@ -760,6 +779,35 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message) _climbRateFact.setRawValue(qIsNaN(vfrHud.climb) ? 0 : vfrHud.climb); } +void Vehicle::_handleAttitudeTarget(mavlink_message_t& message) +{ + mavlink_attitude_target_t attitudeTarget; + + mavlink_msg_attitude_target_decode(&message, &attitudeTarget); + + float roll, pitch, yaw; + mavlink_quaternion_to_euler(attitudeTarget.q, &roll, &pitch, &yaw); + + _setpointFactGroup.roll()->setRawValue(roll); + _setpointFactGroup.pitch()->setRawValue(pitch); + _setpointFactGroup.yaw()->setRawValue(yaw); + + _setpointFactGroup.rollRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_roll_rate)); + _setpointFactGroup.pitchRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_pitch_rate)); + _setpointFactGroup.yawRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_yaw_rate)); +} + +void Vehicle::_handleAttitude(mavlink_message_t& message) +{ + mavlink_attitude_t attitude; + + mavlink_msg_attitude_decode(&message, &attitude); + + rollRate()->setRawValue(qRadiansToDegrees(attitude.rollspeed)); + pitchRate()->setRawValue(qRadiansToDegrees(attitude.pitchspeed)); + yawRate()->setRawValue(qRadiansToDegrees(attitude.yawspeed)); +} + void Vehicle::_handleGpsRawInt(mavlink_message_t& message) { mavlink_gps_raw_int_t gpsRawInt; @@ -869,18 +917,6 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message) { HL_FAILURE_FLAG_3D_ACCEL, MAV_SYS_STATUS_SENSOR_3D_ACCEL }, { HL_FAILURE_FLAG_3D_GYRO, MAV_SYS_STATUS_SENSOR_3D_GYRO }, { HL_FAILURE_FLAG_3D_MAG, MAV_SYS_STATUS_SENSOR_3D_MAG }, -#if 0 - // FIXME: These don't currently map to existing sensor health bits. Support needs to be added to show them - // on health page of instrument panel as well. - { HL_FAILURE_FLAG_TERRAIN=64, /* Terrain subsystem failure. | */ - { HL_FAILURE_FLAG_BATTERY=128, /* Battery failure/critical low battery. | */ - { HL_FAILURE_FLAG_RC_RECEIVER=256, /* RC receiver failure/no rc connection. | */ - { HL_FAILURE_FLAG_OFFBOARD_LINK=512, /* Offboard link failure. | */ - { HL_FAILURE_FLAG_ENGINE=1024, /* Engine failure. | */ - { HL_FAILURE_FLAG_GEOFENCE=2048, /* Geofence violation. | */ - { HL_FAILURE_FLAG_ESTIMATOR=4096, /* Estimator failure, for example measurement rejection or large variances. | */ - { HL_FAILURE_FLAG_MISSION=8192, /* Mission failure. | */ -#endif }; // Map from MAV_FAILURE bits to standard SYS_STATUS message handling @@ -3217,7 +3253,6 @@ VehicleVibrationFactGroup::VehicleVibrationFactGroup(QObject* parent) _zAxisFact.setRawValue(std::numeric_limits::quiet_NaN()); } - const char* VehicleTemperatureFactGroup::_temperature1FactName = "temperature1"; const char* VehicleTemperatureFactGroup::_temperature2FactName = "temperature2"; const char* VehicleTemperatureFactGroup::_temperature3FactName = "temperature3"; @@ -3261,3 +3296,35 @@ void VehicleClockFactGroup::_updateAllValues(void) FactGroup::_updateAllValues(); } + +const char* VehicleSetpointFactGroup::_rollFactName = "roll"; +const char* VehicleSetpointFactGroup::_pitchFactName = "pitch"; +const char* VehicleSetpointFactGroup::_yawFactName = "yaw"; +const char* VehicleSetpointFactGroup::_rollRateFactName = "rollRate"; +const char* VehicleSetpointFactGroup::_pitchRateFactName = "pitchRate"; +const char* VehicleSetpointFactGroup::_yawRateFactName = "yawRate"; + +VehicleSetpointFactGroup::VehicleSetpointFactGroup(QObject* parent) + : FactGroup (1000, ":/json/Vehicle/SetpointFact.json", parent) + , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) + , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) + , _yawFact (0, _yawFactName, FactMetaData::valueTypeDouble) + , _rollRateFact (0, _rollRateFactName, FactMetaData::valueTypeDouble) + , _pitchRateFact(0, _pitchRateFactName, FactMetaData::valueTypeDouble) + , _yawRateFact (0, _yawRateFactName, FactMetaData::valueTypeDouble) +{ + _addFact(&_rollFact, _rollFactName); + _addFact(&_pitchFact, _pitchFactName); + _addFact(&_yawFact, _yawFactName); + _addFact(&_rollRateFact, _rollRateFactName); + _addFact(&_pitchRateFact, _pitchRateFactName); + _addFact(&_yawRateFact, _yawRateFactName); + + // Start out as not available "--.--" + _rollFact.setRawValue(std::numeric_limits::quiet_NaN()); + _pitchFact.setRawValue(std::numeric_limits::quiet_NaN()); + _yawFact.setRawValue(std::numeric_limits::quiet_NaN()); + _rollRateFact.setRawValue(std::numeric_limits::quiet_NaN()); + _pitchRateFact.setRawValue(std::numeric_limits::quiet_NaN()); + _yawRateFact.setRawValue(std::numeric_limits::quiet_NaN()); +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index ba86751..65d4b5e 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -40,6 +40,43 @@ Q_DECLARE_LOGGING_CATEGORY(VehicleLog) class Vehicle; +class VehicleSetpointFactGroup : public FactGroup +{ + Q_OBJECT + +public: + VehicleSetpointFactGroup(QObject* parent = NULL); + + Q_PROPERTY(Fact* roll READ roll CONSTANT) + Q_PROPERTY(Fact* pitch READ pitch CONSTANT) + Q_PROPERTY(Fact* yaw READ yaw CONSTANT) + Q_PROPERTY(Fact* rollRate READ rollRate CONSTANT) + Q_PROPERTY(Fact* pitchRate READ pitchRate CONSTANT) + Q_PROPERTY(Fact* yawRate READ yawRate CONSTANT) + + Fact* roll (void) { return &_rollFact; } + Fact* pitch (void) { return &_pitchFact; } + Fact* yaw (void) { return &_yawFact; } + Fact* rollRate (void) { return &_rollRateFact; } + Fact* pitchRate (void) { return &_pitchRateFact; } + Fact* yawRate (void) { return &_yawRateFact; } + + static const char* _rollFactName; + static const char* _pitchFactName; + static const char* _yawFactName; + static const char* _rollRateFactName; + static const char* _pitchRateFactName; + static const char* _yawRateFactName; + +private: + Fact _rollFact; + Fact _pitchFact; + Fact _yawFact; + Fact _rollRateFact; + Fact _pitchRateFact; + Fact _yawRateFact; +}; + class VehicleVibrationFactGroup : public FactGroup { Q_OBJECT @@ -370,6 +407,9 @@ public: Q_PROPERTY(Fact* roll READ roll CONSTANT) Q_PROPERTY(Fact* pitch READ pitch CONSTANT) Q_PROPERTY(Fact* heading READ heading CONSTANT) + Q_PROPERTY(Fact* rollRate READ rollRate CONSTANT) + Q_PROPERTY(Fact* pitchRate READ pitchRate CONSTANT) + Q_PROPERTY(Fact* yawRate READ yawRate CONSTANT) Q_PROPERTY(Fact* groundSpeed READ groundSpeed CONSTANT) Q_PROPERTY(Fact* airSpeed READ airSpeed CONSTANT) Q_PROPERTY(Fact* climbRate READ climbRate CONSTANT) @@ -385,6 +425,7 @@ public: Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT) Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT) Q_PROPERTY(FactGroup* clock READ clockFactGroup CONSTANT) + Q_PROPERTY(FactGroup* setpoint READ setpointFactGroup CONSTANT) Q_PROPERTY(int firmwareMajorVersion READ firmwareMajorVersion NOTIFY firmwareVersionChanged) Q_PROPERTY(int firmwareMinorVersion READ firmwareMinorVersion NOTIFY firmwareVersionChanged) @@ -645,8 +686,11 @@ public: unsigned maxProtoVersion () const { return _maxProtoVersion; } Fact* roll (void) { return &_rollFact; } - Fact* heading (void) { return &_headingFact; } Fact* pitch (void) { return &_pitchFact; } + Fact* heading (void) { return &_headingFact; } + Fact* rollRate (void) { return &_rollRateFact; } + Fact* pitchRate (void) { return &_pitchRateFact; } + Fact* yawRate (void) { return &_yawRateFact; } Fact* airSpeed (void) { return &_airSpeedFact; } Fact* groundSpeed (void) { return &_groundSpeedFact; } Fact* climbRate (void) { return &_climbRateFact; } @@ -662,6 +706,7 @@ public: FactGroup* vibrationFactGroup (void) { return &_vibrationFactGroup; } FactGroup* temperatureFactGroup (void) { return &_temperatureFactGroup; } FactGroup* clockFactGroup (void) { return &_clockFactGroup; } + FactGroup* setpointFactGroup (void) { return &_setpointFactGroup; } void setConnectionLostEnabled(bool connectionLostEnabled); @@ -916,6 +961,8 @@ private: void _handleScaledPressure2(mavlink_message_t& message); void _handleScaledPressure3(mavlink_message_t& message); void _handleHighLatency2(mavlink_message_t& message); + void _handleAttitude(mavlink_message_t& message); + void _handleAttitudeTarget(mavlink_message_t& message); // ArduPilot dialect messages #if !defined(NO_ARDUPILOT_DIALECT) void _handleCameraFeedback(const mavlink_message_t& message); @@ -1106,6 +1153,9 @@ private: Fact _rollFact; Fact _pitchFact; Fact _headingFact; + Fact _rollRateFact; + Fact _pitchRateFact; + Fact _yawRateFact; Fact _groundSpeedFact; Fact _airSpeedFact; Fact _climbRateFact; @@ -1122,10 +1172,14 @@ private: VehicleVibrationFactGroup _vibrationFactGroup; VehicleTemperatureFactGroup _temperatureFactGroup; VehicleClockFactGroup _clockFactGroup; + VehicleSetpointFactGroup _setpointFactGroup; static const char* _rollFactName; static const char* _pitchFactName; static const char* _headingFactName; + static const char* _rollRateFactName; + static const char* _pitchRateFactName; + static const char* _yawRateFactName; static const char* _groundSpeedFactName; static const char* _airSpeedFactName; static const char* _climbRateFactName; diff --git a/src/Vehicle/VehicleFact.json b/src/Vehicle/VehicleFact.json index ae6d59c..b385571 100644 --- a/src/Vehicle/VehicleFact.json +++ b/src/Vehicle/VehicleFact.json @@ -21,6 +21,27 @@ "units": "deg" }, { + "name": "rollRate", + "shortDescription": "Roll Rate", + "type": "double", + "decimalPlaces": 1, + "units": "deg/s" +}, +{ + "name": "pitchRate", + "shortDescription": "Pitch Rate", + "type": "double", + "decimalPlaces": 1, + "units": "deg/s" +}, +{ + "name": "yawRate", + "shortDescription": "Yaw Rate", + "type": "double", + "decimalPlaces": 1, + "units": "deg/s" +}, +{ "name": "groundSpeed", "shortDescription": "Ground Speed", "type": "double",