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

PX4 MC PID tuning page: minimal working version 

This brings the PID tuning for MC into a minimally working state
QGC4.4
Beat Küng 4 years ago committed by Lorenz Meier
parent
a5fb759816
commit
a51f13971f
6 changed files with 291 additions and 203 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 17
      src/AutoPilotPlugins/PX4/PX4TuningComponent.cc
  2. 90
      src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml
  3. 66
      src/QmlControls/FactSliderPanel.qml
  4. 239
      src/QmlControls/PIDTuning.qml
  5. 76
      src/Vehicle/Vehicle.cc
  6. 6
      src/Vehicle/Vehicle.h

17
src/AutoPilotPlugins/PX4/PX4TuningComponent.cc
Unescape View File

@ -12,9 +12,24 @@ @@ -12,9 +12,24 @@
#include "PX4AutoPilotPlugin.h"
#include "AirframeComponent.h"
static bool isCopter(MAV_TYPE type) {
switch (type) {
case MAV_TYPE_QUADROTOR:
case MAV_TYPE_COAXIAL:
case MAV_TYPE_HELICOPTER:
case MAV_TYPE_HEXAROTOR:
case MAV_TYPE_OCTOROTOR:
case MAV_TYPE_TRICOPTER:
return true;
default:
break;
}
return false;
}
PX4TuningComponent::PX4TuningComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent)
: VehicleComponent(vehicle, autopilot, parent)
, _name(tr("Tuning"))
, _name(isCopter(vehicle->vehicleType()) ? tr("PID Tuning") : tr("Tuning"))
{
}

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

@ -72,26 +72,86 @@ SetupPage { @@ -72,26 +72,86 @@ SetupPage {
Component {
id: advancePageComponent
PIDTuning {
property var rollList: ListModel {
ListElement {
title: qsTr("Overall Multiplier (MC_ROLLRATE_K)")
description: qsTr("Multiplier for P, I and D gains: increase for more responsiveness, reduce if the rates overshoot (and increasing D does not help).")
param: "MC_ROLLRATE_K"
min: 0.3
max: 3
step: 0.05
}
ListElement {
title: qsTr("Differential Gain (MC_ROLLRATE_D)")
description: qsTr("Damping: increase to reduce overshoots and oscillations, but not higher than really needed.")
param: "MC_ROLLRATE_D"
min: 0.0004
max: 0.01
step: 0.0002
}
ListElement {
title: qsTr("Integral Gain (MC_ROLLRATE_I)")
description: qsTr("Generally does not need much adjustment, reduce this when seeing slow oscillations.")
param: "MC_ROLLRATE_I"
min: 0.1
max: 0.5
step: 0.025
}
}
property var pitchList: ListModel {
ListElement {
title: qsTr("Overall Multiplier (MC_PITCHRATE_K)")
description: qsTr("Multiplier for P, I and D gains: increase for more responsiveness, reduce if the rates overshoot (and increasing D does not help).")
param: "MC_PITCHRATE_K"
min: 0.3
max: 3
step: 0.05
}
ListElement {
title: qsTr("Differential Gain (MC_PITCHRATE_D)")
description: qsTr("Damping: increase to reduce overshoots and oscillations, but not higher than really needed.")
param: "MC_PITCHRATE_D"
min: 0.0004
max: 0.01
step: 0.0002
}
ListElement {
title: qsTr("Integral Gain (MC_PITCHRATE_I)")
description: qsTr("Generally does not need much adjustment, reduce this when seeing slow oscillations.")
param: "MC_PITCHRATE_I"
min: 0.1
max: 0.5
step: 0.025
}
}
property var yawList: ListModel {
ListElement {
title: qsTr("Overall Multiplier (MC_YAWRATE_K)")
description: qsTr("Multiplier for P, I and D gains: increase for more responsiveness, reduce if the rates overshoot (and increasing D does not help).")
param: "MC_YAWRATE_K"
min: 0.3
max: 3
step: 0.05
}
ListElement {
title: qsTr("Integral Gain (MC_YAWRATE_I)")
description: qsTr("Generally does not need much adjustment, reduce this when seeing slow oscillations.")
param: "MC_YAWRATE_I"
min: 0.04
max: 0.4
step: 0.02
}
}
anchors.left: parent.left
anchors.right: parent.right
tuneList: [ qsTr("Roll"), qsTr("Pitch"), qsTr("Yaw") ]
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_YAWRATE_FF") ] ]
rollList,
pitchList,
yawList
]
}
} // Component - Advanced Page
} // Column

66
src/QmlControls/FactSliderPanel.qml
Unescape View File

@ -10,6 +10,8 @@ @@ -10,6 +10,8 @@
import QtQuick 2.3
import QtQuick.Controls 1.2
import QtQuick.Controls.Styles 1.4
import QtQuick.Layouts 1.2
import QGroundControl.FactSystem 1.0
import QGroundControl.FactControls 1.0
@ -63,12 +65,15 @@ Column { @@ -63,12 +65,15 @@ Column {
anchors.left: parent.left
anchors.right: parent.right
anchors.top: sliderRect.top
spacing: _margins
QGCLabel {
text: title
font.family: ScreenTools.demiboldFontFamily
}
Item {
width: 1
height: _margins
}
Slider {
anchors.left: parent.left
@ -78,6 +83,8 @@ Column { @@ -78,6 +83,8 @@ Column {
stepSize: step
tickmarksEnabled: true
value: _fact.value
id: slider
property int handleWidth: 0
property Fact _fact: controller.getParameterFact(-1, param)
@ -86,6 +93,63 @@ Column { @@ -86,6 +93,63 @@ Column {
_fact.value = value
}
}
style: SliderStyle {
tickmarks: Repeater {
id: repeater
model: control.stepSize > 0 ? 1 + (control.maximumValue - control.minimumValue) / control.stepSize : 0
property int unused: get()
function get() {
slider.handleWidth = styleData.handleWidth
return 0
}
Rectangle {
color: Qt.hsla(palette.text.hslHue, palette.text.hslSaturation, palette.text.hslLightness, 0.5)
width: 2
height: 4
y: repeater.height
x: styleData.handleWidth / 2 + index * ((repeater.width - styleData.handleWidth) / (repeater.count-1))
}
}
}
}
Item { // spacing
width: 1
height: 4
}
RowLayout {
anchors.left: parent.left
anchors.right: parent.right
QGCLabel {
id: leftValueLabel
text: slider.minimumValue
horizontalAlignment: Text.AlignLeft
}
Item {
QGCLabel {
visible: slider.value != slider.minimumValue && slider.value != slider.maximumValue
text: Math.round(slider._fact.value*100000)/100000
x: getX()
function getX() {
var span = slider.maximumValue - slider.minimumValue
var x = slider.handleWidth / 2 + (slider.value-slider.minimumValue)/span * (slider.width-slider.handleWidth) - width / 2
// avoid overlapping text
var minX = leftValueLabel.x + leftValueLabel.width + _margins/2
if (x < minX) x = minX
var maxX = rightValueLabel.x - _margins/2 - width
if (x > maxX) x = maxX
return x
}
}
}
QGCLabel {
id: rightValueLabel
text: slider.maximumValue
Layout.alignment: Qt.AlignRight
}
}
QGCLabel {

239
src/QmlControls/PIDTuning.qml
Unescape View File

@ -27,28 +27,19 @@ RowLayout { @@ -27,28 +27,19 @@ RowLayout {
property real _chartHeight: ScreenTools.defaultFontPixelHeight * 20
property real _margins: ScreenTools.defaultFontPixelHeight / 2
property string _currentTuneType: tuneList[0]
property real _roll: globals.activeVehicle.roll.value
property real _rollSetpoint: globals.activeVehicle.setpoint.roll.value
property real _rollRate: globals.activeVehicle.rollRate.value
property real _rollRateSetpoint: globals.activeVehicle.setpoint.rollRate.value
property real _pitch: globals.activeVehicle.pitch.value
property real _pitchSetpoint: globals.activeVehicle.setpoint.pitch.value
property real _pitchRate: globals.activeVehicle.pitchRate.value
property real _pitchRateSetpoint: globals.activeVehicle.setpoint.pitchRate.value
property real _yaw: globals.activeVehicle.heading.value
property real _yawSetpoint: globals.activeVehicle.setpoint.yaw.value
property real _yawRate: globals.activeVehicle.yawRate.value
property real _yawRateSetpoint: globals.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 double _last_t: 0
property var _savedTuningParamValues: [ ]
// The following are set when getValues is called
property real _value
property real _valueSetpoint
property real _valueRate
property real _valueRateSetpoint
@ -57,6 +48,7 @@ RowLayout { @@ -57,6 +48,7 @@ RowLayout {
readonly property int _tuneListRollIndex: 0
readonly property int _tuneListPitchIndex: 1
readonly property int _tuneListYawIndex: 2
readonly property int _chartDisplaySec: 3 // number of seconds to display
function adjustYAxisMin(yAxis, newValue) {
var newMin = Math.min(yAxis.min, newValue)
@ -78,37 +70,26 @@ RowLayout { @@ -78,37 +70,26 @@ RowLayout {
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
_last_t = 0
}
function currentTuneTypeIndex() {
@ -126,9 +107,10 @@ RowLayout { @@ -126,9 +107,10 @@ RowLayout {
var tuneTypeIndex = currentTuneTypeIndex()
_savedTuningParamValues = [ ]
var currentTuneParams = params[tuneTypeIndex]
for (var i=0; i<currentTuneParams.length; i++) {
_savedTuningParamValues.push(currentTuneParams[i].valueString)
for (var i=0; i<params[tuneTypeIndex].count; i++) {
var currentTuneParam = controller.getParameterFact(-1,
params[tuneTypeIndex].get(i).param)
_savedTuningParamValues.push(currentTuneParam.valueString)
}
savedRepeater.model = _savedTuningParamValues
}
@ -136,8 +118,10 @@ RowLayout { @@ -136,8 +118,10 @@ RowLayout {
function resetToSavedTuningParamValues() {
var tuneTypeIndex = currentTuneTypeIndex()
for (var i=0; i<_savedTuningParamValues.length; i++) {
params[tuneTypeIndex][i].value = _savedTuningParamValues[i]
for (var i=0; i<params[tuneTypeIndex].count; i++) {
var currentTuneParam = controller.getParameterFact(-1,
params[tuneTypeIndex].get(i).param)
currentTuneParam.value = _savedTuningParamValues[i]
}
}
@ -154,32 +138,15 @@ RowLayout { @@ -154,32 +138,15 @@ RowLayout {
}
ValueAxis {
id: valueXAxis
min: 0
max: 0
labelFormat: "%d"
titleText: "msec"
tickCount: 11
}
ValueAxis {
id: valueRateXAxis
min: 0
max: 0
labelFormat: "%d"
titleText: "msec"
labelFormat: "%.2f"
titleText: "sec"
tickCount: 11
}
ValueAxis {
id: valueYAxis
min: 0
max: 10
titleText: "deg"
tickCount: Math.min(((max - min) / _tickSeparation), _maxTickSections) + 1
}
ValueAxis {
id: valueRateYAxis
min: 0
max: 10
@ -190,32 +157,31 @@ RowLayout { @@ -190,32 +157,31 @@ RowLayout {
Timer {
id: dataTimer
interval: 10
running: false
running: true
repeat: true
onTriggered: {
_valueXAxis.max = _msecs
_valueRateXAxis.max = _msecs
_valueRateXAxis.max = _msecs / 1000
_valueRateXAxis.min = _msecs / 1000 - _chartDisplaySec
getValues()
valueSeries.append(_msecs, _value)
adjustYAxisMin(_valueYAxis, _value)
adjustYAxisMax(_valueYAxis, _value)
valueSetpointSeries.append(_msecs, _valueSetpoint)
adjustYAxisMin(_valueYAxis, _valueSetpoint)
adjustYAxisMax(_valueYAxis, _valueSetpoint)
valueRateSeries.append(_msecs, _valueRate)
adjustYAxisMin(_valueRateYAxis, _valueRate)
adjustYAxisMax(_valueRateYAxis, _valueRate)
if (!isNaN(_valueRate)) {
valueRateSeries.append(_msecs/1000, _valueRate)
adjustYAxisMin(_valueRateYAxis, _valueRate)
adjustYAxisMax(_valueRateYAxis, _valueRate)
}
valueRateSetpointSeries.append(_msecs, _valueRateSetpoint)
adjustYAxisMin(_valueRateYAxis, _valueRateSetpoint)
adjustYAxisMax(_valueRateYAxis, _valueRateSetpoint)
if (!isNaN(_valueRateSetpoint)) {
valueRateSetpointSeries.append(_msecs/1000, _valueRateSetpoint)
adjustYAxisMin(_valueRateYAxis, _valueRateSetpoint)
adjustYAxisMax(_valueRateYAxis, _valueRateSetpoint)
}
_msecs += interval
var t = new Date().getTime() // in ms
if (_last_t > 0)
_msecs += t-_last_t
_last_t = t
}
property int _maxPointCount: 10000 / interval
@ -224,6 +190,7 @@ RowLayout { @@ -224,6 +190,7 @@ RowLayout {
Column {
spacing: _margins
Layout.alignment: Qt.AlignTop
width: parent.width * 0.4
Column {
QGCLabel { text: qsTr("Tuning Axis:") }
@ -244,75 +211,24 @@ RowLayout { @@ -244,75 +211,24 @@ RowLayout {
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
var newValue = value - (value * adjustPercentModel.get(adjustPercentCombo.currentIndex).value)
if (newValue >= modelData.min) {
modelData.value = newValue
}
}
}
}
Repeater {
model: parent.factList
FactTextField {
Layout.fillWidth: true
fact: modelData
showUnits: false
}
}
Repeater {
model: parent.factList
QGCButton {
text: "+"
onClicked: {
var value = modelData.value
var newValue = value + (value * adjustPercentModel.get(adjustPercentCombo.currentIndex).value)
if (newValue <= modelData.max) {
modelData.value = newValue
}
}
}
}
// Instantiate all sliders (instead of switching the model), so that
// values are not changed unexpectedly if they do not match with a tick
// value
FactSliderPanel {
width: parent.width
visible: _currentTuneType === tuneList[_tuneListRollIndex]
sliderModel: params[_tuneListRollIndex]
}
RowLayout {
QGCLabel { text: qsTr("Increment/Decrement %") }
QGCComboBox {
id: adjustPercentCombo
textRole: "text"
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 }
}
}
FactSliderPanel {
width: parent.width
visible: _currentTuneType === tuneList[_tuneListPitchIndex]
sliderModel: params[_tuneListPitchIndex]
}
FactSliderPanel {
width: parent.width
visible: _currentTuneType === tuneList[_tuneListYawIndex]
sliderModel: params[_tuneListYawIndex]
}
Column {
@ -327,7 +243,7 @@ RowLayout { @@ -327,7 +243,7 @@ RowLayout {
Repeater {
model: params[tuneList.indexOf(_currentTuneType)]
QGCLabel { text: modelData.name }
QGCLabel { text: param }
}
Repeater {
@ -368,6 +284,7 @@ RowLayout { @@ -368,6 +284,7 @@ RowLayout {
text: dataTimer.running ? qsTr("Stop") : qsTr("Start")
onClicked: {
dataTimer.running = !dataTimer.running
_last_t = 0
if (autoModeChange.checked) {
globals.activeVehicle.flightMode = dataTimer.running ? "Stabilized" : globals.activeVehicle.pauseFlightMode
}
@ -396,37 +313,16 @@ RowLayout { @@ -396,37 +313,16 @@ RowLayout {
Column {
Layout.fillWidth: true
Layout.alignment: Qt.AlignTop
ChartView {
id: ratesChart
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
height: availableHeight * 0.75
title: _currentTuneType + qsTr(" Rate")
antialiasing: true
legend.alignment: Qt.AlignRight
legend.alignment: Qt.AlignBottom
LineSeries {
id: valueRateSeries
@ -437,10 +333,37 @@ RowLayout { @@ -437,10 +333,37 @@ RowLayout {
LineSeries {
id: valueRateSetpointSeries
name: "Command"
name: "Setpoint"
axisY: valueRateYAxis
axisX: valueRateXAxis
}
// enable mouse dragging
MouseArea {
property var _startPoint: undefined
property double _scaling: 0
anchors.fill: parent
onPressed: {
_startPoint = Qt.point(mouse.x, mouse.y)
var start = ratesChart.mapToValue(_startPoint)
var next = ratesChart.mapToValue(Qt.point(mouse.x+1, mouse.y+1))
_scaling = next.x - start.x
}
onPositionChanged: {
if(_startPoint != undefined) {
dataTimer.running = false
var cp = Qt.point(mouse.x, mouse.y)
var dx = (cp.x - _startPoint.x) * _scaling
_startPoint = cp
_valueRateXAxis.max -= dx
_valueRateXAxis.min -= dx
}
}
onReleased: {
_startPoint = undefined
}
}
}
}
} // RowLayout

76
src/Vehicle/Vehicle.cc
Unescape View File

@ -94,11 +94,14 @@ const char* Vehicle::_windFactGroupName = "wind"; @@ -94,11 +94,14 @@ const char* Vehicle::_windFactGroupName = "wind";
const char* Vehicle::_vibrationFactGroupName = "vibration";
const char* Vehicle::_temperatureFactGroupName = "temperature";
const char* Vehicle::_clockFactGroupName = "clock";
const char* Vehicle::_setpointFactGroupName = "setpoint";
const char* Vehicle::_distanceSensorFactGroupName = "distanceSensor";
const char* Vehicle::_escStatusFactGroupName = "escStatus";
const char* Vehicle::_estimatorStatusFactGroupName = "estimatorStatus";
const char* Vehicle::_terrainFactGroupName = "terrain";
const QList<int> Vehicle::_pidTuningMessages = {MAVLINK_MSG_ID_ATTITUDE_QUATERNION, MAVLINK_MSG_ID_ATTITUDE_TARGET};
// Standard connected vehicle
Vehicle::Vehicle(LinkInterface* link,
int vehicleId,
@ -144,6 +147,7 @@ Vehicle::Vehicle(LinkInterface* link, @@ -144,6 +147,7 @@ Vehicle::Vehicle(LinkInterface* link,
, _vibrationFactGroup (this)
, _temperatureFactGroup (this)
, _clockFactGroup (this)
, _setpointFactGroup (this)
, _distanceSensorFactGroup (this)
, _escStatusFactGroup (this)
, _estimatorStatusFactGroup (this)
@ -193,8 +197,6 @@ Vehicle::Vehicle(LinkInterface* link, @@ -193,8 +197,6 @@ Vehicle::Vehicle(LinkInterface* link,
}
#endif
_pidTuningMessages << MAVLINK_MSG_ID_ATTITUDE << MAVLINK_MSG_ID_ATTITUDE_TARGET;
_autopilotPlugin = _firmwarePlugin->autopilotPlugin(this);
_autopilotPlugin->setParent(this);
@ -400,6 +402,7 @@ void Vehicle::_commonInit() @@ -400,6 +402,7 @@ void Vehicle::_commonInit()
_addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName);
_addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName);
_addFactGroup(&_clockFactGroup, _clockFactGroupName);
_addFactGroup(&_setpointFactGroup, _setpointFactGroupName);
_addFactGroup(&_distanceSensorFactGroup, _distanceSensorFactGroupName);
_addFactGroup(&_escStatusFactGroup, _escStatusFactGroupName);
_addFactGroup(&_estimatorStatusFactGroup, _estimatorStatusFactGroupName);
@ -437,8 +440,6 @@ void Vehicle::_commonInit() @@ -437,8 +440,6 @@ void Vehicle::_commonInit()
}
}
#endif
_pidTuningMessages << MAVLINK_MSG_ID_ATTITUDE << MAVLINK_MSG_ID_ATTITUDE_TARGET;
}
Vehicle::~Vehicle()
@ -2818,6 +2819,7 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) @@ -2818,6 +2819,7 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
#endif
int entryIndex = _findMavCommandListEntryIndex(message.compid, static_cast<MAV_CMD>(ack.command));
bool commandInList = false;
if (entryIndex != -1) {
const MavCommandListEntry_t& commandEntry = _mavCommandList[entryIndex];
if (commandEntry.command == ack.command) {
@ -2867,11 +2869,26 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) @@ -2867,11 +2869,26 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message)
}
_mavCommandList.removeAt(entryIndex);
return;
commandInList = true;
}
}
qCDebug(VehicleLog) << "_handleCommandAck Ack not in list" << rawCommandName;
if (!commandInList) {
qCDebug(VehicleLog) << "_handleCommandAck Ack not in list" << rawCommandName;
}
// advance PID tuning setup/teardown
if (ack.command == MAV_CMD_SET_MESSAGE_INTERVAL && _pidTuningNextAdjustIndex >= 0) {
_pidTuningAdjustRates();
}
if (ack.command == MAV_CMD_GET_MESSAGE_INTERVAL && _pidTuningWaitingForRates) {
if (_pidTuningMessageRatesUsecs.count() < _pidTuningMessages.count()) {
sendMavCommand(defaultComponentId(),
MAV_CMD_GET_MESSAGE_INTERVAL,
true, // show error
_pidTuningMessages[_pidTuningMessageRatesUsecs.count()]);
}
}
}
void Vehicle::_waitForMavlinkMessage(WaitForMavlinkMessageResultHandler resultHandler, void* resultHandlerData, int messageId, int timeoutMsecs)
@ -3542,7 +3559,8 @@ void Vehicle::_handleMessageInterval(const mavlink_message_t& message) @@ -3542,7 +3559,8 @@ void Vehicle::_handleMessageInterval(const mavlink_message_t& message)
if (_pidTuningMessageRatesUsecs.count() == _pidTuningMessages.count()) {
// We have back all the rates we requested
_pidTuningWaitingForRates = false;
_pidTuningAdjustRates(true);
_pidTuningNextAdjustIndex = 0;
_pidTuningAdjustRates();
}
}
}
@ -3555,12 +3573,10 @@ void Vehicle::setPIDTuningTelemetryMode(bool pidTuning) @@ -3555,12 +3573,10 @@ void Vehicle::setPIDTuningTelemetryMode(bool pidTuning)
_pidTuningTelemetryMode = true;
_pidTuningWaitingForRates = true;
_pidTuningMessageRatesUsecs.clear();
for (int telemetry: _pidTuningMessages) {
sendMavCommand(defaultComponentId(),
MAV_CMD_GET_MESSAGE_INTERVAL,
true, // show error
telemetry);
}
sendMavCommand(defaultComponentId(),
MAV_CMD_GET_MESSAGE_INTERVAL,
true, // show error
_pidTuningMessages[0]);
}
} else {
if (_pidTuningTelemetryMode) {
@ -3569,29 +3585,37 @@ void Vehicle::setPIDTuningTelemetryMode(bool pidTuning) @@ -3569,29 +3585,37 @@ void Vehicle::setPIDTuningTelemetryMode(bool pidTuning)
// We never finished waiting for previous rates
_pidTuningWaitingForRates = false;
} else {
_pidTuningAdjustRates(false);
_pidTuningNextAdjustIndex = 0;
_pidTuningAdjustRates();
}
}
}
}
void Vehicle::_pidTuningAdjustRates(bool setRatesForTuning)
void Vehicle::_pidTuningAdjustRates()
{
int requestedRate = (int)(1000000.0 / 30.0); // 30 Hz in usecs
int requestedRate = (int)(1000000.0 / 100.0); // 100 Hz in usecs (better set this a bit higher than actually needed,
// to give it more priority in case of exceeing link bandwidth)
for (int telemetry: _pidTuningMessages) {
if (_pidTuningNextAdjustIndex >= _pidTuningMessages.size()) {
_pidTuningNextAdjustIndex = -1;
return;
}
int telemetry = _pidTuningMessages[_pidTuningNextAdjustIndex];
if (requestedRate < _pidTuningMessageRatesUsecs[telemetry]) {
sendMavCommand(defaultComponentId(),
MAV_CMD_SET_MESSAGE_INTERVAL,
true, // show error
telemetry,
setRatesForTuning ? requestedRate : _pidTuningMessageRatesUsecs[telemetry]);
}
if (requestedRate < _pidTuningMessageRatesUsecs[telemetry] || _pidTuningMessageRatesUsecs[telemetry] == 0) {
sendMavCommand(defaultComponentId(),
MAV_CMD_SET_MESSAGE_INTERVAL,
true, // show error
telemetry,
_pidTuningTelemetryMode ? requestedRate : _pidTuningMessageRatesUsecs[telemetry]);
}
setLiveUpdates(setRatesForTuning);
_setpointFactGroup.setLiveUpdates(setRatesForTuning);
if (_pidTuningNextAdjustIndex == 0) {
setLiveUpdates(_pidTuningTelemetryMode);
_setpointFactGroup.setLiveUpdates(_pidTuningTelemetryMode);
}
++_pidTuningNextAdjustIndex;
}
void Vehicle::_initializeCsv()

6
src/Vehicle/Vehicle.h
Unescape View File

@ -939,7 +939,7 @@ private: @@ -939,7 +939,7 @@ private:
void _setCapabilities (uint64_t capabilityBits);
void _updateArmed (bool armed);
bool _apmArmingNotRequired ();
void _pidTuningAdjustRates (bool setRatesForTuning);
void _pidTuningAdjustRates ();
void _initializeCsv ();
void _writeCsvLine ();
void _flightTimerStart ();
@ -1107,8 +1107,9 @@ private: @@ -1107,8 +1107,9 @@ private:
// PID Tuning telemetry mode
bool _pidTuningTelemetryMode = false;
bool _pidTuningWaitingForRates = false;
QList<int> _pidTuningMessages;
int _pidTuningNextAdjustIndex = -1;
QMap<int, int> _pidTuningMessageRatesUsecs;
static const QList<int> _pidTuningMessages;
// Chunked status text support
typedef struct {
@ -1251,6 +1252,7 @@ private: @@ -1251,6 +1252,7 @@ private:
static const char* _vibrationFactGroupName;
static const char* _temperatureFactGroupName;
static const char* _clockFactGroupName;
static const char* _setpointFactGroupName;
static const char* _distanceSensorFactGroupName;
static const char* _escStatusFactGroupName;
static const char* _estimatorStatusFactGroupName;

Write Preview
Loading…
Cancel
Save