Skip to content

Q
qgroundcontrol
Commit 3e5ea4de authored by DonLakeFlyer's avatar DonLakeFlyer
Browse files
Options

PX4 PID tuning support

parent 858af395
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 716 additions and 67 deletions
qgroundcontrol.qrc
View file @ 3e5ea4de
......@@ -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 @ 3e5ea4de
......@@ -10,6 +10,7 @@
import QtQuick 2.3
import QtQuick.Controls 1.2
import QtQuick.Dialogs 1.2
import QtQuick.Layouts 1.2
import QGroundControl 1.0
import QGroundControl.FactSystem 1.0
......@@ -30,6 +31,8 @@ QGCView {
property string pageDescription: vehicleComponent ? vehicleComponent.description : ""
property real availableWidth: width - pageLoader.x
property real availableHeight: height - pageLoader.y
property bool showAdvanced: false
property alias advanced: advancedCheckBox.checked
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
property bool _vehicleArmed: _activeVehicle ? _activeVehicle.armed : false
......@@ -53,31 +56,45 @@ QGCView {
contentHeight: pageLoader.y + pageLoader.item.height
clip: true
Column {
id: headingColumn
width: setupPanel.width
RowLayout {
id: headingRow
anchors.left: parent.left
anchors.right: parent.right
spacing: _margins
layoutDirection: Qt.RightToLeft
QGCLabel {
font.pointSize: ScreenTools.largeFontPointSize
text: !setupView.enabled ? _pageTitle + "<font color=\"red\">" + qsTr(" (Disabled while the vehicle is %1)").arg(_disableReason) + "</font>" : _pageTitle
visible: !ScreenTools.isShortScreen
QGCCheckBox {
id: advancedCheckBox
text: qsTr("Advanced")
visible: showAdvanced
}
QGCLabel {
anchors.left: parent.left
anchors.right: parent.right
wrapMode: Text.WordWrap
text: pageDescription
visible: !ScreenTools.isShortScreen
Column {
spacing: _margins
Layout.fillWidth: true
QGCLabel {
font.pointSize: ScreenTools.largeFontPointSize
text: !setupView.enabled ? _pageTitle + "<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 {
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 @ 3e5ea4de
......@@ -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/Vehicle/SetpointFact.json 0 → 100644
View file @ 3e5ea4de
[
{
"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 @ 3e5ea4de
......@@ -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 @ 3e5ea4de
......@@ -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 @ 3e5ea4de
......@@ -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