zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #6295 from DonLakeFlyer/PX4PIDTuning 

PX4 PID tuning support
QGC4.4
Don Gagne 7 years ago committed by GitHub
parent
2d706e6c1f 3e5ea4de06
commit
655964c6b0
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 716 additions and 67 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      qgroundcontrol.qrc
  2. 45
      src/AutoPilotPlugins/Common/SetupPage.qml
  3. 1
      src/AutoPilotPlugins/PX4/PX4TuningComponent.h
  4. 522
      src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml
  5. 44
      src/Vehicle/SetpointFact.json
  6. 93
      src/Vehicle/Vehicle.cc
  7. 56
      src/Vehicle/Vehicle.h
  8. 21
      src/Vehicle/VehicleFact.json

1
qgroundcontrol.qrc
Unescape View File

@ -224,6 +224,7 @@
<file alias="Vehicle/ClockFact.json">src/Vehicle/ClockFact.json</file> <file alias="Vehicle/ClockFact.json">src/Vehicle/ClockFact.json</file>
<file alias="Vehicle/GPSFact.json">src/Vehicle/GPSFact.json</file> <file alias="Vehicle/GPSFact.json">src/Vehicle/GPSFact.json</file>
<file alias="Vehicle/GPSRTKFact.json">src/Vehicle/GPSRTKFact.json</file> <file alias="Vehicle/GPSRTKFact.json">src/Vehicle/GPSRTKFact.json</file>
<file alias="Vehicle/SetpointFact.json">src/Vehicle/SetpointFact.json</file>
<file alias="Vehicle/SubmarineFact.json">src/Vehicle/SubmarineFact.json</file> <file alias="Vehicle/SubmarineFact.json">src/Vehicle/SubmarineFact.json</file>
<file alias="Vehicle/TemperatureFact.json">src/Vehicle/TemperatureFact.json</file> <file alias="Vehicle/TemperatureFact.json">src/Vehicle/TemperatureFact.json</file>
<file alias="Vehicle/VehicleFact.json">src/Vehicle/VehicleFact.json</file> <file alias="Vehicle/VehicleFact.json">src/Vehicle/VehicleFact.json</file>

45
src/AutoPilotPlugins/Common/SetupPage.qml
Unescape View File

@ -10,6 +10,7 @@
import QtQuick 2.3 import QtQuick 2.3
import QtQuick.Controls 1.2 import QtQuick.Controls 1.2
import QtQuick.Dialogs 1.2 import QtQuick.Dialogs 1.2
import QtQuick.Layouts 1.2
import QGroundControl 1.0 import QGroundControl 1.0
import QGroundControl.FactSystem 1.0 import QGroundControl.FactSystem 1.0
@ -30,6 +31,8 @@ QGCView {
property string pageDescription: vehicleComponent ? vehicleComponent.description : "" property string pageDescription: vehicleComponent ? vehicleComponent.description : ""
property real availableWidth: width - pageLoader.x property real availableWidth: width - pageLoader.x
property real availableHeight: height - pageLoader.y property real availableHeight: height - pageLoader.y
property bool showAdvanced: false
property alias advanced: advancedCheckBox.checked
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
property bool _vehicleArmed: _activeVehicle ? _activeVehicle.armed : false property bool _vehicleArmed: _activeVehicle ? _activeVehicle.armed : false
@ -53,31 +56,45 @@ QGCView {
contentHeight: pageLoader.y + pageLoader.item.height contentHeight: pageLoader.y + pageLoader.item.height
clip: true clip: true
Column { RowLayout {
id: headingColumn id: headingRow
width: setupPanel.width anchors.left: parent.left
anchors.right: parent.right
spacing: _margins spacing: _margins
layoutDirection: Qt.RightToLeft
QGCLabel { QGCCheckBox {
font.pointSize: ScreenTools.largeFontPointSize id: advancedCheckBox
text: !setupView.enabled ? _pageTitle + "<font color=\"red\">" + qsTr(" (Disabled while the vehicle is %1)").arg(_disableReason) + "</font>" : _pageTitle text: qsTr("Advanced")
visible: !ScreenTools.isShortScreen visible: showAdvanced
} }
QGCLabel { Column {
anchors.left: parent.left spacing: _margins
anchors.right: parent.right Layout.fillWidth: true
wrapMode: Text.WordWrap
text: pageDescription QGCLabel {
visible: !ScreenTools.isShortScreen font.pointSize: ScreenTools.largeFontPointSize
text: !setupView.enabled ? _pageTitle + "<font color=\"red\">" + qsTr(" (Disabled while the vehicle is %1)").arg(_disableReason) + "</font>" : _pageTitle
visible: !ScreenTools.isShortScreen
}
QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
wrapMode: Text.WordWrap
text: pageDescription
visible: !ScreenTools.isShortScreen
}
} }
} }
Loader { Loader {
id: pageLoader id: pageLoader
anchors.topMargin: _margins anchors.topMargin: _margins
anchors.top: headingColumn.bottom anchors.top: headingRow.bottom
} }
// Overlay to display when vehicle is armed and this setup page needs // Overlay to display when vehicle is armed and this setup page needs
// to be disabled // to be disabled
Rectangle { Rectangle {

1
src/AutoPilotPlugins/PX4/PX4TuningComponent.h
Unescape View File

@ -32,6 +32,7 @@ public:
QUrl setupSource(void) const final; QUrl setupSource(void) const final;
QUrl summaryQmlSource(void) const final; QUrl summaryQmlSource(void) const final;
bool allowSetupWhileArmed(void) const final { return true; } bool allowSetupWhileArmed(void) const final { return true; }
bool allowSetupWhileFlying(void) const final { return true; }
private: private:
const QString _name; const QString _name;

522
src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml
Unescape View File

@ -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 QGroundControl 1.0
import QtQuick.Controls 1.2 import QGroundControl.Controls 1.0
import QGroundControl.FactSystem 1.0
import QGroundControl.Controls 1.0 import QGroundControl.FactControls 1.0
import QGroundControl.ScreenTools 1.0
SetupPage { SetupPage {
id: tuningPage id: tuningPage
@ -20,47 +25,486 @@ SetupPage {
Component { Component {
id: pageComponent id: pageComponent
FactSliderPanel { Column {
width: availableWidth width: availableWidth
qgcViewPanel: tuningPage.viewPanel
property real _chartHeight: ScreenTools.defaultFontPixelHeight * 20
sliderModel: ListModel { property real _margins: ScreenTools.defaultFontPixelHeight / 2
ListElement { property string _currentTuneType: _tuneList[0]
title: qsTr("Hover Throttle") property real _roll: _activeVehicle.roll.value
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.") property real _rollSetpoint: _activeVehicle.setpoint.roll.value
param: "MPC_THR_HOVER" property real _rollRate: _activeVehicle.rollRate.value
min: 20 property real _rollRateSetpoint: _activeVehicle.setpoint.rollRate.value
max: 80 property real _pitch: _activeVehicle.pitch.value
step: 1 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 { _savedTuningParamValues = [ ]
title: qsTr("Manual minimum throttle") var currentTuneParams = _params[tuneTypeIndex]
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.") for (var i=0; i<currentTuneParams.length; i++) {
param: "MPC_MANTHR_MIN" _savedTuningParamValues.push(currentTuneParams[i].valueString)
min: 0
max: 15
step: 1
} }
savedRepeater.model = _savedTuningParamValues
}
function resetToSavedTuningParamValues() {
var tuneTypeIndex = currentTuneTypeIndex()
ListElement { for (var i=0; i<_savedTuningParamValues.length; i++) {
title: qsTr("Roll sensitivity") _params[tuneTypeIndex][i].value = _savedTuningParamValues[i]
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
} }
}
Component.onCompleted: {
showAdvanced = true
saveTuningParamValues()
}
on_CurrentTuneTypeChanged: {
saveTuningParamValues()
resetGraphs()
}
FactPanelController {
id: controller
factPanel: tuningPage.viewPanel
}
ExclusiveGroup {
id: tuneTypeRadios
}
ValueAxis {
id: valueXAxis
min: 0
max: 0
labelFormat: "%d"
titleText: "sec"
}
ValueAxis {
id: valueRateXAxis
min: 0
max: 0
labelFormat: "%d"
titleText: "sec"
}
ValueAxis {
id: valueYAxis
min: 0
max: 10
titleText: "deg"
tickCount: ((max - min) / _tickSeparation) + 1
}
ValueAxis {
id: valueRateYAxis
min: 0
max: 10
titleText: "deg/s"
tickCount: ((max - min) / _tickSeparation) + 1
}
Timer {
id: dataTimer
interval: 50
running: false
repeat: true
onTriggered: {
var seconds = _msecs / 1000
_valueXAxis.max = seconds
_valueRateXAxis.max = seconds
getValues()
valueSeries.append(seconds, _value)
adjustYAxisMin(_valueYAxis, _value)
adjustYAxisMax(_valueYAxis, _value)
valueSetpointSeries.append(seconds, _valueSetpoint)
adjustYAxisMin(_valueYAxis, _valueSetpoint)
adjustYAxisMax(_valueYAxis, _valueSetpoint)
valueRateSeries.append(seconds, _valueRate)
adjustYAxisMin(_valueRateYAxis, _valueRate)
adjustYAxisMax(_valueRateYAxis, _valueRate)
valueRateSetpointSeries.append(seconds, _valueRateSetpoint)
adjustYAxisMin(_valueRateYAxis, _valueRateSetpoint)
adjustYAxisMax(_valueRateYAxis, _valueRateSetpoint)
_msecs += interval
if (valueSeries.count > _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 { ListElement {
title: qsTr("Pitch sensitivity") title: qsTr("Manual minimum throttle")
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.") 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: "MC_PITCH_TC" param: "MPC_MANTHR_MIN"
min: 0.15 min: 0
max: 0.25 max: 15
step: 0.01 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 } // SetupPage

44
src/Vehicle/SetpointFact.json
Unescape View File

@ -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"
}
]

93
src/Vehicle/Vehicle.cc
Unescape View File

@ -10,6 +10,7 @@
#include <QTime> #include <QTime>
#include <QDateTime> #include <QDateTime>
#include <QLocale> #include <QLocale>
#include <QQuaternion>
#include "Vehicle.h" #include "Vehicle.h"
#include "MAVLinkProtocol.h" #include "MAVLinkProtocol.h"
@ -54,6 +55,9 @@ const char* Vehicle::_joystickEnabledSettingsKey = "JoystickEnabled";
const char* Vehicle::_rollFactName = "roll"; const char* Vehicle::_rollFactName = "roll";
const char* Vehicle::_pitchFactName = "pitch"; const char* Vehicle::_pitchFactName = "pitch";
const char* Vehicle::_headingFactName = "heading"; 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::_airSpeedFactName = "airSpeed";
const char* Vehicle::_groundSpeedFactName = "groundSpeed"; const char* Vehicle::_groundSpeedFactName = "groundSpeed";
const char* Vehicle::_climbRateFactName = "climbRate"; const char* Vehicle::_climbRateFactName = "climbRate";
@ -166,6 +170,9 @@ Vehicle::Vehicle(LinkInterface* link,
, _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble)
, _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble)
, _headingFact (0, _headingFactName, 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) , _groundSpeedFact (0, _groundSpeedFactName, FactMetaData::valueTypeDouble)
, _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble) , _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble)
, _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble) , _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble)
@ -353,6 +360,9 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble)
, _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble)
, _headingFact (0, _headingFactName, 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) , _groundSpeedFact (0, _groundSpeedFactName, FactMetaData::valueTypeDouble)
, _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble) , _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble)
, _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble) , _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble)
@ -413,6 +423,9 @@ void Vehicle::_commonInit(void)
_addFact(&_rollFact, _rollFactName); _addFact(&_rollFact, _rollFactName);
_addFact(&_pitchFact, _pitchFactName); _addFact(&_pitchFact, _pitchFactName);
_addFact(&_headingFact, _headingFactName); _addFact(&_headingFact, _headingFactName);
_addFact(&_rollRateFact, _rollRateFactName);
_addFact(&_pitchRateFact, _pitchRateFactName);
_addFact(&_yawRateFact, _yawRateFactName);
_addFact(&_groundSpeedFact, _groundSpeedFactName); _addFact(&_groundSpeedFact, _groundSpeedFactName);
_addFact(&_airSpeedFact, _airSpeedFactName); _addFact(&_airSpeedFact, _airSpeedFactName);
_addFact(&_climbRateFact, _climbRateFactName); _addFact(&_climbRateFact, _climbRateFactName);
@ -696,6 +709,12 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_HIGH_LATENCY2: case MAVLINK_MSG_ID_HIGH_LATENCY2:
_handleHighLatency2(message); _handleHighLatency2(message);
break; break;
case MAVLINK_MSG_ID_ATTITUDE:
_handleAttitude(message);
break;
case MAVLINK_MSG_ID_ATTITUDE_TARGET:
_handleAttitudeTarget(message);
break;
case MAVLINK_MSG_ID_SERIAL_CONTROL: 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); _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) void Vehicle::_handleGpsRawInt(mavlink_message_t& message)
{ {
mavlink_gps_raw_int_t gpsRawInt; 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_ACCEL, MAV_SYS_STATUS_SENSOR_3D_ACCEL },
{ HL_FAILURE_FLAG_3D_GYRO, MAV_SYS_STATUS_SENSOR_3D_GYRO }, { HL_FAILURE_FLAG_3D_GYRO, MAV_SYS_STATUS_SENSOR_3D_GYRO },
{ HL_FAILURE_FLAG_3D_MAG, MAV_SYS_STATUS_SENSOR_3D_MAG }, { 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 // Map from MAV_FAILURE bits to standard SYS_STATUS message handling
@ -3217,7 +3253,6 @@ VehicleVibrationFactGroup::VehicleVibrationFactGroup(QObject* parent)
_zAxisFact.setRawValue(std::numeric_limits<float>::quiet_NaN()); _zAxisFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
} }
const char* VehicleTemperatureFactGroup::_temperature1FactName = "temperature1"; const char* VehicleTemperatureFactGroup::_temperature1FactName = "temperature1";
const char* VehicleTemperatureFactGroup::_temperature2FactName = "temperature2"; const char* VehicleTemperatureFactGroup::_temperature2FactName = "temperature2";
const char* VehicleTemperatureFactGroup::_temperature3FactName = "temperature3"; const char* VehicleTemperatureFactGroup::_temperature3FactName = "temperature3";
@ -3261,3 +3296,35 @@ void VehicleClockFactGroup::_updateAllValues(void)
FactGroup::_updateAllValues(); 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<float>::quiet_NaN());
_pitchFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_yawFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_rollRateFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_pitchRateFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_yawRateFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
}

56
src/Vehicle/Vehicle.h
Unescape View File

@ -40,6 +40,43 @@ Q_DECLARE_LOGGING_CATEGORY(VehicleLog)
class Vehicle; 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 class VehicleVibrationFactGroup : public FactGroup
{ {
Q_OBJECT Q_OBJECT
@ -370,6 +407,9 @@ public:
Q_PROPERTY(Fact* roll READ roll CONSTANT) Q_PROPERTY(Fact* roll READ roll CONSTANT)
Q_PROPERTY(Fact* pitch READ pitch CONSTANT) Q_PROPERTY(Fact* pitch READ pitch CONSTANT)
Q_PROPERTY(Fact* heading READ heading 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* groundSpeed READ groundSpeed CONSTANT)
Q_PROPERTY(Fact* airSpeed READ airSpeed CONSTANT) Q_PROPERTY(Fact* airSpeed READ airSpeed CONSTANT)
Q_PROPERTY(Fact* climbRate READ climbRate CONSTANT) Q_PROPERTY(Fact* climbRate READ climbRate CONSTANT)
@ -385,6 +425,7 @@ public:
Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT) Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT)
Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT) Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT)
Q_PROPERTY(FactGroup* clock READ clockFactGroup 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 firmwareMajorVersion READ firmwareMajorVersion NOTIFY firmwareVersionChanged)
Q_PROPERTY(int firmwareMinorVersion READ firmwareMinorVersion NOTIFY firmwareVersionChanged) Q_PROPERTY(int firmwareMinorVersion READ firmwareMinorVersion NOTIFY firmwareVersionChanged)
@ -645,8 +686,11 @@ public:
unsigned maxProtoVersion () const { return _maxProtoVersion; } unsigned maxProtoVersion () const { return _maxProtoVersion; }
Fact* roll (void) { return &_rollFact; } Fact* roll (void) { return &_rollFact; }
Fact* heading (void) { return &_headingFact; }
Fact* pitch (void) { return &_pitchFact; } 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* airSpeed (void) { return &_airSpeedFact; }
Fact* groundSpeed (void) { return &_groundSpeedFact; } Fact* groundSpeed (void) { return &_groundSpeedFact; }
Fact* climbRate (void) { return &_climbRateFact; } Fact* climbRate (void) { return &_climbRateFact; }
@ -662,6 +706,7 @@ public:
FactGroup* vibrationFactGroup (void) { return &_vibrationFactGroup; } FactGroup* vibrationFactGroup (void) { return &_vibrationFactGroup; }
FactGroup* temperatureFactGroup (void) { return &_temperatureFactGroup; } FactGroup* temperatureFactGroup (void) { return &_temperatureFactGroup; }
FactGroup* clockFactGroup (void) { return &_clockFactGroup; } FactGroup* clockFactGroup (void) { return &_clockFactGroup; }
FactGroup* setpointFactGroup (void) { return &_setpointFactGroup; }
void setConnectionLostEnabled(bool connectionLostEnabled); void setConnectionLostEnabled(bool connectionLostEnabled);
@ -916,6 +961,8 @@ private:
void _handleScaledPressure2(mavlink_message_t& message); void _handleScaledPressure2(mavlink_message_t& message);
void _handleScaledPressure3(mavlink_message_t& message); void _handleScaledPressure3(mavlink_message_t& message);
void _handleHighLatency2(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 // ArduPilot dialect messages
#if !defined(NO_ARDUPILOT_DIALECT) #if !defined(NO_ARDUPILOT_DIALECT)
void _handleCameraFeedback(const mavlink_message_t& message); void _handleCameraFeedback(const mavlink_message_t& message);
@ -1106,6 +1153,9 @@ private:
Fact _rollFact; Fact _rollFact;
Fact _pitchFact; Fact _pitchFact;
Fact _headingFact; Fact _headingFact;
Fact _rollRateFact;
Fact _pitchRateFact;
Fact _yawRateFact;
Fact _groundSpeedFact; Fact _groundSpeedFact;
Fact _airSpeedFact; Fact _airSpeedFact;
Fact _climbRateFact; Fact _climbRateFact;
@ -1122,10 +1172,14 @@ private:
VehicleVibrationFactGroup _vibrationFactGroup; VehicleVibrationFactGroup _vibrationFactGroup;
VehicleTemperatureFactGroup _temperatureFactGroup; VehicleTemperatureFactGroup _temperatureFactGroup;
VehicleClockFactGroup _clockFactGroup; VehicleClockFactGroup _clockFactGroup;
VehicleSetpointFactGroup _setpointFactGroup;
static const char* _rollFactName; static const char* _rollFactName;
static const char* _pitchFactName; static const char* _pitchFactName;
static const char* _headingFactName; static const char* _headingFactName;
static const char* _rollRateFactName;
static const char* _pitchRateFactName;
static const char* _yawRateFactName;
static const char* _groundSpeedFactName; static const char* _groundSpeedFactName;
static const char* _airSpeedFactName; static const char* _airSpeedFactName;
static const char* _climbRateFactName; static const char* _climbRateFactName;

21
src/Vehicle/VehicleFact.json
Unescape View File

@ -21,6 +21,27 @@
"units": "deg" "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", "name": "groundSpeed",
"shortDescription": "Ground Speed", "shortDescription": "Ground Speed",
"type": "double", "type": "double",

Write Preview
Loading…
Cancel
Save