Skip to content

Q
qgroundcontrol
Unverified Commit 655964c6 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #6295 from DonLakeFlyer/PX4PIDTuning 

PX4 PID tuning support
parents 2d706e6c 3e5ea4de
Show whitespace changes
Inline Side-by-side
Showing with 716 additions and 67 deletions
qgroundcontrol.qrc
View file @ 655964c6
......@@ -224,6 +224,7 @@
<file alias="Vehicle/ClockFact.json">src/Vehicle/ClockFact.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/SetpointFact.json">src/Vehicle/SetpointFact.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/VehicleFact.json">src/Vehicle/VehicleFact.json</file>
......
src/AutoPilotPlugins/Common/SetupPage.qml
View file @ 655964c6
......@@ -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,10 +56,22 @@ QGCView {
contentHeight: pageLoader.y + pageLoader.item.height
clip: true
RowLayout {
id: headingRow
anchors.left: parent.left
anchors.right: parent.right
spacing: _margins
layoutDirection: Qt.RightToLeft
QGCCheckBox {
id: advancedCheckBox
text: qsTr("Advanced")
visible: showAdvanced
}
Column {
id: headingColumn
width: setupPanel.width
spacing: _margins
Layout.fillWidth: true
QGCLabel {
font.pointSize: ScreenTools.largeFontPointSize
......@@ -72,12 +87,14 @@ QGCView {
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 {
......
src/AutoPilotPlugins/PX4/PX4TuningComponent.h
View file @ 655964c6
......@@ -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;
......
src/AutoPilotPlugins/PX4/PX4TuningComponentCopter.qml
View file @ 655964c6
......@@ -7,11 +7,16 @@
*
****************************************************************************/
import QtQuick 2.3
import QtQuick.Controls 1.2
import QtCharts 2.2
import QtQuick.Layouts 1.2
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,9 +25,243 @@ SetupPage {
Component {
id: pageComponent
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()
_savedTuningParamValues = [ ]
var currentTuneParams = _params[tuneTypeIndex]
for (var i=0; i<currentTuneParams.length; i++) {
_savedTuningParamValues.push(currentTuneParams[i].valueString)
}
savedRepeater.model = _savedTuningParamValues
}
function resetToSavedTuningParamValues() {
var tuneTypeIndex = currentTuneTypeIndex()
for (var i=0; i<_savedTuningParamValues.length; i++) {
_params[tuneTypeIndex][i].value = _savedTuningParamValues[i]
}
}
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 {
......@@ -42,7 +281,8 @@ SetupPage {
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.")
......@@ -60,7 +300,211 @@ SetupPage {
max: 0.25
step: 0.01
}
*/
}
}
// 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
}
}
}
} // Component
} // RowLayout - Advanced Page
} // Column
} // Component - pageComponent
} // SetupPage
src/Vehicle/SetpointFact.json 0 → 100644
View file @ 655964c6
[
{
"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"
}
]
src/Vehicle/Vehicle.cc
View file @ 655964c6
......@@ -10,6 +10,7 @@
#include <QTime>
#include <QDateTime>
#include <QLocale>
#include <QQuaternion>
#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<float>::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<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());
}
src/Vehicle/Vehicle.h
View file @ 655964c6
......@@ -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;
......
src/Vehicle/VehicleFact.json
View file @ 655964c6
......@@ -20,6 +20,27 @@
"decimalPlaces": 0,
"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",
......
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