Unverified Commit 63527b4c authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #7395 from DonLakeFlyer/ArduCopterPID

ArduCopter PID tuning support
parents 64eebbef 707b16d8
...@@ -6,6 +6,7 @@ Note: This file only contains high level features or important fixes. ...@@ -6,6 +6,7 @@ Note: This file only contains high level features or important fixes.
### 3.6.0 - Daily Build ### 3.6.0 - Daily Build
* ArduCopter: Add PID Tuning page to Tuning Setup
* ArduPilot: Copter - Advanced Tuning support * ArduPilot: Copter - Advanced Tuning support
* ArduPilot: Rover - Frame setup support * ArduPilot: Rover - Frame setup support
* ArduPilot: Copter - Update support to 3.5+ * ArduPilot: Copter - Update support to 3.5+
......
...@@ -26,16 +26,13 @@ SetupPage { ...@@ -26,16 +26,13 @@ SetupPage {
id: tuningPageComponent id: tuningPageComponent
Column { Column {
width: availableWidth width: availableWidth
spacing: _margins
FactPanelController { id: controller; factPanel: tuningPage.viewPanel } FactPanelController { id: controller; factPanel: tuningPage.viewPanel }
QGCPalette { id: palette; colorGroupEnabled: true } QGCPalette { id: palette; colorGroupEnabled: true }
property bool _rcFeelAvailable: controller.parameterExists(-1, "RC_FEEL")
property bool _atcInputTCAvailable: controller.parameterExists(-1, "ATC_INPUT_TC") property bool _atcInputTCAvailable: controller.parameterExists(-1, "ATC_INPUT_TC")
property Fact _rcFeel: controller.getParameterFact(-1, "RC_FEEL", false)
property Fact _atcInputTC: controller.getParameterFact(-1, "ATC_INPUT_TC", false) property Fact _atcInputTC: controller.getParameterFact(-1, "ATC_INPUT_TC", false)
property Fact _rateRollP: controller.getParameterFact(-1, "ATC_RAT_RLL_P") property Fact _rateRollP: controller.getParameterFact(-1, "ATC_RAT_RLL_P")
property Fact _rateRollI: controller.getParameterFact(-1, "ATC_RAT_RLL_I") property Fact _rateRollI: controller.getParameterFact(-1, "ATC_RAT_RLL_I")
...@@ -69,6 +66,7 @@ SetupPage { ...@@ -69,6 +66,7 @@ SetupPage {
ExclusiveGroup { id: returnAltRadioGroup } ExclusiveGroup { id: returnAltRadioGroup }
Component.onCompleted: { Component.onCompleted: {
showAdvanced = !ScreenTools.isMobile
// Qml Sliders have a strange behavior in which they first set Slider::value to some internal // Qml Sliders have a strange behavior in which they first set Slider::value to some internal
// setting and then set Slider::value to the bound properties value. If you have an onValueChanged // setting and then set Slider::value to the bound properties value. If you have an onValueChanged
// handler which updates your property with the new value, this first value change will trash // handler which updates your property with the new value, this first value change will trash
...@@ -76,9 +74,6 @@ SetupPage { ...@@ -76,9 +74,6 @@ SetupPage {
// after Qml load is done. We also don't track value changes until Qml load completes. // after Qml load is done. We also don't track value changes until Qml load completes.
rollPitch.value = _rateRollP.value rollPitch.value = _rateRollP.value
climb.value = _rateClimbP.value climb.value = _rateClimbP.value
if (_rcFeelAvailable) {
rcFeel.value = _rcFeel.value
}
if (_atcInputTCAvailable) { if (_atcInputTCAvailable) {
atcInputTC.value = _atcInputTC.value atcInputTC.value = _atcInputTC.value
} }
...@@ -124,382 +119,365 @@ SetupPage { ...@@ -124,382 +119,365 @@ SetupPage {
Connections { target: _ch11Opt; onValueChanged: calcAutoTuneChannel() } Connections { target: _ch11Opt; onValueChanged: calcAutoTuneChannel() }
Connections { target: _ch12Opt; onValueChanged: calcAutoTuneChannel() } Connections { target: _ch12Opt; onValueChanged: calcAutoTuneChannel() }
QGCLabel { Column {
text: qsTr("Basic Tuning")
font.family: ScreenTools.demiboldFontFamily
}
Rectangle {
id: basicTuningRect
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
height: basicTuningColumn.y + basicTuningColumn.height + _margins spacing: _margins
color: palette.windowShade visible: !advanced
QGCLabel {
text: qsTr("Basic Tuning")
font.family: ScreenTools.demiboldFontFamily
}
Column { Rectangle {
id: basicTuningColumn id: basicTuningRect
anchors.margins: _margins
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
anchors.top: parent.top height: basicTuningColumn.y + basicTuningColumn.height + _margins
spacing: _margins color: palette.windowShade
Column { Column {
anchors.left: parent.left id: basicTuningColumn
anchors.right: parent.right anchors.margins: _margins
anchors.left: parent.left
QGCLabel { anchors.right: parent.right
text: qsTr("Roll/Pitch Sensitivity") anchors.top: parent.top
font.family: ScreenTools.demiboldFontFamily spacing: _margins
}
QGCLabel { Column {
text: qsTr("Slide to the right if the copter is sluggish or slide to the left if the copter is twitchy")
}
Slider {
id: rollPitch
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
minimumValue: 0.08
maximumValue: 0.4
stepSize: 0.01
tickmarksEnabled: true
onValueChanged: {
if (_loadComplete) {
_rateRollP.value = value
_rateRollI.value = value
_ratePitchP.value = value
_ratePitchI.value = value
}
}
}
}
Column { QGCLabel {
anchors.left: parent.left text: qsTr("Roll/Pitch Sensitivity")
anchors.right: parent.right font.family: ScreenTools.demiboldFontFamily
}
QGCLabel {
text: qsTr("Climb Sensitivity")
font.family: ScreenTools.demiboldFontFamily
}
QGCLabel { QGCLabel {
text: qsTr("Slide to the right to climb more aggressively or slide to the left to climb more gently") text: qsTr("Slide to the right if the copter is sluggish or slide to the left if the copter is twitchy")
} }
Slider { Slider {
id: climb id: rollPitch
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
minimumValue: 0.3 minimumValue: 0.08
maximumValue: 1.0 maximumValue: 0.4
stepSize: 0.02 stepSize: 0.01
tickmarksEnabled: true tickmarksEnabled: true
value: _rateClimbP.value
onValueChanged: {
onValueChanged: { if (_loadComplete) {
if (_loadComplete) { _rateRollP.value = value
_rateClimbP.value = value _rateRollI.value = value
_rateClimbI.value = value * 2 _ratePitchP.value = value
_ratePitchI.value = value
}
} }
} }
} }
}
Column { Column {
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
visible: _rcFeelAvailable
QGCLabel { QGCLabel {
text: qsTr("RC Roll/Pitch Feel") text: qsTr("Climb Sensitivity")
font.family: ScreenTools.demiboldFontFamily font.family: ScreenTools.demiboldFontFamily
} }
QGCLabel { QGCLabel {
text: qsTr("Slide to the left for soft control, slide to the right for crisp control") text: qsTr("Slide to the right to climb more aggressively or slide to the left to climb more gently")
} }
Slider { Slider {
id: rcFeel id: climb
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
minimumValue: 0 minimumValue: 0.3
maximumValue: 100 maximumValue: 1.0
stepSize: 5.0 stepSize: 0.02
tickmarksEnabled: true tickmarksEnabled: true
value: _rateClimbP.value
onValueChanged: {
if (_loadComplete) { onValueChanged: {
_rcFeel.value = value if (_loadComplete) {
_rateClimbP.value = value
_rateClimbI.value = value * 2
}
} }
} }
} }
}
Column { Column {
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
visible: _atcInputTCAvailable visible: _atcInputTCAvailable
QGCLabel { QGCLabel {
text: qsTr("RC Roll/Pitch Feel") text: qsTr("RC Roll/Pitch Feel")
font.family: ScreenTools.demiboldFontFamily font.family: ScreenTools.demiboldFontFamily
} }
QGCLabel { QGCLabel {
text: qsTr("Slide to the left for soft control, slide to the right for crisp control") text: qsTr("Slide to the left for soft control, slide to the right for crisp control")
} }
Slider { Slider {
id: atcInputTC id: atcInputTC
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
minimumValue: _atcInputTC.min minimumValue: _atcInputTC.min
maximumValue: _atcInputTC.max maximumValue: _atcInputTC.max
stepSize: _atcInputTC.increment stepSize: _atcInputTC.increment
tickmarksEnabled: true tickmarksEnabled: true
onValueChanged: { onValueChanged: {
if (_loadComplete) { if (_loadComplete) {
_atcInputTC.value = value _atcInputTC.value = value
}
} }
} }
} }
}
}
} // Rectangle - Basic tuning
QGCLabel {
text: qsTr("Advanced Tuning")
font.family: ScreenTools.demiboldFontFamily
}
Rectangle { Column {
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
height: advColumnLayout.y + advColumnLayout.height + _margins
color: palette.windowShade
ColumnLayout {
id: advColumnLayout
anchors.margins: _margins
anchors.left: parent.left
anchors.right: parent.right
anchors.top: parent.top
spacing: _margins
Column {
Layout.fillWidth: true
QGCLabel { QGCLabel {
text: qsTr("Spin While Armed") text: qsTr("Spin While Armed")
font.family: ScreenTools.demiboldFontFamily font.family: ScreenTools.demiboldFontFamily
} }
QGCLabel { QGCLabel {
text: qsTr("Adjust the amount the motors spin to indicate armed") text: qsTr("Adjust the amount the motors spin to indicate armed")
} }
Slider { Slider {
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
minimumValue: 0 minimumValue: 0
maximumValue: Math.max(0.3, _motSpinArm.rawValue) maximumValue: Math.max(0.3, _motSpinArm.rawValue)
stepSize: 0.01 stepSize: 0.01
tickmarksEnabled: true tickmarksEnabled: true
value: _motSpinArm.rawValue value: _motSpinArm.rawValue
onValueChanged: { onValueChanged: {
if (_loadComplete) { if (_loadComplete) {
_motSpinArm.rawValue = value _motSpinArm.rawValue = value
}
} }
} }
} }
}
Column { Column {
id: lastAdvTuningColumn anchors.left: parent.left
Layout.fillWidth: true anchors.right: parent.right
QGCLabel { QGCLabel {
text: qsTr("Minimum Thrust") text: qsTr("Minimum Thrust")
font.family: ScreenTools.demiboldFontFamily font.family: ScreenTools.demiboldFontFamily
} }
QGCLabel { QGCLabel {
text: qsTr("Adjust the minimum amount of thrust for a stable minimum throttle descent") text: qsTr("Adjust the minimum amount of thrust require for the vehicle to move")
} }
QGCLabel { QGCLabel {
text: qsTr("Warning: This setting should be higher than 'Spin While Armed'") text: qsTr("Warning: This setting should be higher than 'Spin While Armed'")
color: palette.warningText color: palette.warningText
visible: _motSpinMin.rawValue < _motSpinArm.rawValue visible: _motSpinMin.rawValue < _motSpinArm.rawValue
} }
Slider { Slider {
anchors.left: parent.left anchors.left: parent.left
anchors.right: parent.right anchors.right: parent.right
minimumValue: 0 minimumValue: 0
maximumValue: Math.max(0.3, _motSpinMin.rawValue) maximumValue: Math.max(0.3, _motSpinMin.rawValue)
stepSize: 0.01 stepSize: 0.01
tickmarksEnabled: true tickmarksEnabled: true
value: _motSpinMin.rawValue value: _motSpinMin.rawValue
onValueChanged: { onValueChanged: {
if (_loadComplete) { if (_loadComplete) {
_motSpinMin.rawValue = value _motSpinMin.rawValue = value
}
} }
} }
} }
} }
} } // Rectangle - Basic tuning
} // Rectangle - Advanced tuning
Flow {
id: flowLayout
anchors.left: parent.left
anchors.right: parent.right
spacing: _margins
Rectangle { Flow {
height: autoTuneLabel.height + autoTuneRect.height id: flowLayout
width: autoTuneRect.width Layout.fillWidth: true
color: palette.window spacing: _margins
QGCLabel {
id: autoTuneLabel
text: qsTr("AutoTune")
font.family: ScreenTools.demiboldFontFamily
}
Rectangle { Rectangle {
id: autoTuneRect height: autoTuneLabel.height + autoTuneRect.height
width: autoTuneColumn.x + autoTuneColumn.width + _margins width: autoTuneRect.width
height: autoTuneColumn.y + autoTuneColumn.height + _margins color: palette.window
anchors.top: autoTuneLabel.bottom
color: palette.windowShade
Column { QGCLabel {
id: autoTuneColumn id: autoTuneLabel
anchors.margins: _margins text: qsTr("AutoTune")
anchors.left: parent.left font.family: ScreenTools.demiboldFontFamily
anchors.top: parent.top }
spacing: _margins
Row {
spacing: _margins
QGCLabel { text: qsTr("Axes to AutoTune:") } Rectangle {
FactBitmask { fact: _autoTuneAxes } id: autoTuneRect
} width: autoTuneColumn.x + autoTuneColumn.width + _margins
height: autoTuneColumn.y + autoTuneColumn.height + _margins
anchors.top: autoTuneLabel.bottom
color: palette.windowShade
Column {
id: autoTuneColumn
anchors.margins: _margins
anchors.left: parent.left
anchors.top: parent.top
spacing: _margins
Row {
spacing: _margins
QGCLabel { text: qsTr("Axes to AutoTune:") }
FactBitmask { fact: _autoTuneAxes }
}
Row { Row {
spacing: _margins spacing: _margins
QGCLabel { QGCLabel {
anchors.baseline: autoTuneChannelCombo.baseline anchors.baseline: autoTuneChannelCombo.baseline
text: qsTr("Channel for AutoTune switch:") text: qsTr("Channel for AutoTune switch:")
} }
QGCComboBox { QGCComboBox {
id: autoTuneChannelCombo id: autoTuneChannelCombo
width: ScreenTools.defaultFontPixelWidth * 14 width: ScreenTools.defaultFontPixelWidth * 14
model: [qsTr("None"), qsTr("Channel 7"), qsTr("Channel 8"), qsTr("Channel 9"), qsTr("Channel 10"), qsTr("Channel 11"), qsTr("Channel 12") ] model: [qsTr("None"), qsTr("Channel 7"), qsTr("Channel 8"), qsTr("Channel 9"), qsTr("Channel 10"), qsTr("Channel 11"), qsTr("Channel 12") ]
currentIndex: _autoTuneSwitchChannelIndex currentIndex: _autoTuneSwitchChannelIndex
onActivated: { onActivated: {
var channel = index var channel = index
if (channel > 0) { if (channel > 0) {
channel += 6 channel += 6
}
setChannelAutoTuneOption(channel)
} }
setChannelAutoTuneOption(channel)
} }
} }
} }
} } // Rectangle - AutoTune
} // Rectangle - AutoTune } // Rectangle - AutoTuneWrap
} // Rectangle - AutoTuneWrap
Rectangle {
height: inFlightTuneLabel.height + channel6TuningOption.height
width: channel6TuningOption.width
color: palette.window
QGCLabel {
id: inFlightTuneLabel
text: qsTr("In Flight Tuning")
font.family: ScreenTools.demiboldFontFamily
}
Rectangle { Rectangle {
id: channel6TuningOption height: inFlightTuneLabel.height + channel6TuningOption.height
width: channel6TuningOptColumn.width + (_margins * 2) width: channel6TuningOption.width
height: channel6TuningOptColumn.height + ScreenTools.defaultFontPixelHeight color: palette.window
anchors.top: inFlightTuneLabel.bottom
color: qgcPal.windowShade
Column { QGCLabel {
id: channel6TuningOptColumn id: inFlightTuneLabel
anchors.margins: ScreenTools.defaultFontPixelWidth text: qsTr("In Flight Tuning")
anchors.left: parent.left font.family: ScreenTools.demiboldFontFamily
anchors.top: parent.top }
spacing: ScreenTools.defaultFontPixelHeight
Row { Rectangle {
spacing: ScreenTools.defaultFontPixelWidth id: channel6TuningOption
property Fact nullFact: Fact { } width: channel6TuningOptColumn.width + (_margins * 2)
height: channel6TuningOptColumn.height + ScreenTools.defaultFontPixelHeight
anchors.top: inFlightTuneLabel.bottom
color: qgcPal.windowShade
Column {
id: channel6TuningOptColumn
anchors.margins: ScreenTools.defaultFontPixelWidth
anchors.left: parent.left
anchors.top: parent.top
spacing: ScreenTools.defaultFontPixelHeight
Row {
spacing: ScreenTools.defaultFontPixelWidth
property Fact nullFact: Fact { }
QGCLabel {
anchors.baseline: optCombo.baseline
text: qsTr("RC Channel 6 Option (Tuning):")
//color: controller.channelOptionEnabled[modelData] ? "yellow" : qgcPal.text
}
QGCLabel { FactComboBox {
anchors.baseline: optCombo.baseline id: optCombo
text: qsTr("RC Channel 6 Option (Tuning):") width: ScreenTools.defaultFontPixelWidth * 15
//color: controller.channelOptionEnabled[modelData] ? "yellow" : qgcPal.text fact: controller.getParameterFact(-1, "TUNE")
indexModel: false
}
} }
FactComboBox { Row {
id: optCombo spacing: ScreenTools.defaultFontPixelWidth
width: ScreenTools.defaultFontPixelWidth * 15 property Fact nullFact: Fact { }
fact: controller.getParameterFact(-1, "TUNE")
indexModel: false
}
}
Row { QGCLabel {
spacing: ScreenTools.defaultFontPixelWidth anchors.baseline: tuneMinField.baseline
property Fact nullFact: Fact { } text: qsTr("Min:")
//color: controller.channelOptionEnabled[modelData] ? "yellow" : qgcPal.text
}
QGCLabel { FactTextField {
anchors.baseline: tuneMinField.baseline id: tuneMinField
text: qsTr("Min:") validator: DoubleValidator {bottom: 0; top: 32767;}
//color: controller.channelOptionEnabled[modelData] ? "yellow" : qgcPal.text fact: controller.getParameterFact(-1, "TUNE_LOW")
} }
FactTextField { QGCLabel {
id: tuneMinField anchors.baseline: tuneMaxField.baseline
validator: DoubleValidator {bottom: 0; top: 32767;} text: qsTr("Max:")
fact: controller.getParameterFact(-1, "TUNE_LOW") //color: controller.channelOptionEnabled[modelData] ? "yellow" : qgcPal.text
} }
QGCLabel { FactTextField {
anchors.baseline: tuneMaxField.baseline id: tuneMaxField
text: qsTr("Max:") validator: DoubleValidator {bottom: 0; top: 32767;}
//color: controller.channelOptionEnabled[modelData] ? "yellow" : qgcPal.text fact: controller.getParameterFact(-1, "TUNE_HIGH")
}
} }
} // Column - Channel 6 Tuning option
} // Rectangle - Channel 6 Tuning options
} // Rectangle - Channel 6 Tuning options wrap
} // Flow - Tune
}
FactTextField { Loader {
id: tuneMaxField anchors.left: parent.left
validator: DoubleValidator {bottom: 0; top: 32767;} anchors.right: parent.right
fact: controller.getParameterFact(-1, "TUNE_HIGH") sourceComponent: advanced ? advancePageComponent : undefined
} }
}
} // Column - Channel 6 Tuning option Component {
} // Rectangle - Channel 6 Tuning options id: advancePageComponent
} // Rectangle - Channel 6 Tuning options wrap
} // Flow - Tune PIDTuning {
anchors.left: parent.left
anchors.right: parent.right
tuneList: [ qsTr("Roll"), qsTr("Pitch"), qsTr("Yaw") ]
params: [
[ controller.getParameterFact(-1, "ATC_ANG_RLL_P"),
controller.getParameterFact(-1, "ATC_RAT_RLL_P"),
controller.getParameterFact(-1, "ATC_RAT_RLL_I"),
controller.getParameterFact(-1, "ATC_RAT_RLL_D") ],
[ controller.getParameterFact(-1, "ATC_ANG_PIT_P"),
controller.getParameterFact(-1, "ATC_RAT_PIT_P"),
controller.getParameterFact(-1, "ATC_RAT_PIT_I"),
controller.getParameterFact(-1, "ATC_RAT_PIT_D") ],
[ controller.getParameterFact(-1, "ATC_ANG_YAW_P"),
controller.getParameterFact(-1, "ATC_RAT_YAW_P"),
controller.getParameterFact(-1, "ATC_RAT_YAW_I") ] ]
}
} // Component - Advanced Page
} // Column } // Column
} // Component } // Component
} // SetupView } // SetupView
...@@ -323,7 +323,7 @@ RowLayout { ...@@ -323,7 +323,7 @@ RowLayout {
} }
Item { width: 1; height: 1 } Item { width: 1; height: 1 }
QGCLabel { text: qsTr("Saved Tuning Values:") } QGCLabel { text: qsTr("Clipboard Values:") }
GridLayout { GridLayout {
rows: savedRepeater.model.length rows: savedRepeater.model.length
...@@ -348,12 +348,12 @@ RowLayout { ...@@ -348,12 +348,12 @@ RowLayout {
spacing: _margins spacing: _margins
QGCButton { QGCButton {
text: qsTr("Save Values") text: qsTr("Save To Clipboard")
onClicked: saveTuningParamValues() onClicked: saveTuningParamValues()
} }
QGCButton { QGCButton {
text: qsTr("Reset To Saved Values") text: qsTr("Restore From Clipboard")
onClicked: resetToSavedTuningParamValues() onClicked: resetToSavedTuningParamValues()
} }
} }
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment