Commit 774fad4e authored by DonLakeFlyer's avatar DonLakeFlyer

Major cleanup to Guided Bar

parent b28e00a0
......@@ -806,13 +806,3 @@ QString APMFirmwarePlugin::internalParameterMetaDataFile(Vehicle* vehicle)
return QString();
}
}
QString APMFirmwarePlugin::missionFlightMode(void)
{
return QStringLiteral("Auto");
}
QString APMFirmwarePlugin::rtlFlightMode(void)
{
return QStringLiteral("RTL");
}
......@@ -97,8 +97,6 @@ public:
RallyPointManager* newRallyPointManager (Vehicle* vehicle) { return new APMRallyPointManager(vehicle); }
QString brandImageIndoor (const Vehicle* vehicle) const { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
QString brandImageOutdoor (const Vehicle* vehicle) const { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); }
QString missionFlightMode (void) final;
QString rtlFlightMode (void) final;
protected:
/// All access to singleton is through stack specific implementation
......
......@@ -162,13 +162,22 @@ void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QG
vehicle->missionManager()->writeArduPilotGuidedMissionItem(coordWithAltitude, false /* altChangeOnly */);
}
void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel)
void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
{
if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) {
qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known."));
return;
}
// Don't allow altitude to fall below 3 meters above home
double currentAltRel = vehicle->altitudeRelative()->rawValue().toDouble();
if (altitudeChange <= 0 && currentAltRel <= 3) {
return;
}
if (currentAltRel + altitudeChange < 3) {
altitudeChange = 3 - currentAltRel;
}
mavlink_message_t msg;
mavlink_set_position_target_local_ned_t cmd;
......@@ -180,7 +189,7 @@ void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double
cmd.type_mask = 0xFFF8; // Only x/y/z valid
cmd.x = 0.0f;
cmd.y = 0.0f;
cmd.z = -(altitudeRel - vehicle->altitudeRelative()->rawValue().toDouble());
cmd.z = -(altitudeChange);
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
mavlink_msg_set_position_target_local_ned_encode_chan(mavlink->getSystemId(),
......@@ -192,11 +201,6 @@ void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double
vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
}
bool ArduCopterFirmwarePlugin::isPaused(const Vehicle* vehicle) const
{
return vehicle->flightMode() == "Brake";
}
void ArduCopterFirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
vehicle->setFlightMode("Brake");
......@@ -228,12 +232,6 @@ QString ArduCopterFirmwarePlugin::geoFenceRadiusParam(Vehicle* vehicle)
return QStringLiteral("FENCE_RADIUS");
}
QString ArduCopterFirmwarePlugin::takeControlFlightMode(void)
{
return QStringLiteral("Stabilize");
}
bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
{
if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"))) {
......
......@@ -54,7 +54,6 @@ public:
// Overrides from FirmwarePlugin
bool isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
bool isPaused(const Vehicle* vehicle) const final;
void setGuidedMode(Vehicle* vehicle, bool guidedMode) final;
void pauseVehicle(Vehicle* vehicle) final;
void guidedModeRTL(Vehicle* vehicle) final;
......@@ -64,14 +63,18 @@ public:
void guidedModeTakeoff(Vehicle* vehicle) final;
#endif
void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final;
void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final;
void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange) final;
const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
bool multiRotorCoaxialMotors(Vehicle* vehicle) final;
bool multiRotorXConfig(Vehicle* vehicle) final;
QString geoFenceRadiusParam(Vehicle* vehicle) final;
QString offlineEditingParamFile(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); }
QString takeControlFlightMode(void) final;
QString pauseFlightMode(void) const override { return QString("Brake"); }
QString missionFlightMode(void) const override { return QString("Auto"); }
QString rtlFlightMode(void) const override { return QString("RTL"); }
QString landFlightMode(void) const override { return QString("Land"); }
QString takeControlFlightMode(void) const override { return QString("Stablize"); }
bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const final;
private:
......
......@@ -86,8 +86,3 @@ int ArduPlaneFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVer
// Remapping supports up to 3.8
return majorVersionNumber == 3 ? 8 : Vehicle::versionNotSetValue;
}
QString ArduPlaneFirmwarePlugin::takeControlFlightMode(void)
{
return QStringLiteral("Manual");
}
......@@ -57,7 +57,6 @@ public:
// Overrides from FirmwarePlugin
QString offlineEditingParamFile(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Plane.OfflineEditing.params"); }
QString takeControlFlightMode(void) final;
const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;
......
......@@ -237,13 +237,6 @@ void FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
}
bool FirmwarePlugin::isPaused(const Vehicle* vehicle) const
{
// Not supported by generic vehicle
Q_UNUSED(vehicle);
return false;
}
void FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
{
// Not supported by generic vehicle
......@@ -314,21 +307,6 @@ int FirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVersionNumbe
return 0;
}
QString FirmwarePlugin::missionFlightMode(void)
{
return QString();
}
QString FirmwarePlugin::rtlFlightMode(void)
{
return QString();
}
QString FirmwarePlugin::takeControlFlightMode(void)
{
return QString();
}
QString FirmwarePlugin::vehicleImageOpaque(const Vehicle* vehicle) const
{
Q_UNUSED(vehicle);
......
......@@ -93,15 +93,27 @@ public:
/// @param[out] custom_mode Custom mode for SET_MODE mavlink message
virtual bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode);
/// Returns The flight mode which indicates the vehicle is paused
virtual QString pauseFlightMode(void) const { return QString(); }
/// Returns the flight mode for running missions
virtual QString missionFlightMode(void) const { return QString(); }
/// Returns the flight mode for RTL
virtual QString rtlFlightMode(void) const { return QString(); }
/// Returns the flight mode for Land
virtual QString landFlightMode(void) const { return QString(); }
/// Returns the flight mode to use when the operator wants to take back control from autonomouse flight.
virtual QString takeControlFlightMode(void) const { return QString(); }
/// Returns whether the vehicle is in guided mode or not.
virtual bool isGuidedMode(const Vehicle* vehicle) const;
/// Set guided flight mode
virtual void setGuidedMode(Vehicle* vehicle, bool guidedMode);
/// Returns whether the vehicle is paused or not.
virtual bool isPaused(const Vehicle* vehicle) const;
/// Causes the vehicle to stop at current position. If guide mode is supported, vehicle will be let in guide mode.
/// If not, vehicle will be left in Loiter.
virtual void pauseVehicle(Vehicle* vehicle);
......@@ -125,20 +137,9 @@ public:
/// Command vehicle to move to specified location (altitude is included and relative)
virtual void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord);
/// Command vehicle to change to the specified relatice altitude
virtual void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel);
/// Returns the flight mode for running missions
virtual QString missionFlightMode(void);
/// Returns the flight mode for RTL
virtual QString rtlFlightMode(void);
/// Returns the flight mode to use when the operator wants to take back control from autonomouse flight.
virtual QString takeControlFlightMode(void);
/// Command vehicle to change altitude
/// @param altitudeChange If > 0, go up by amount specified, if < 0, go down by amount specified
virtual void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange);
/// FIXME: This isn't quite correct being here. All code for Joystick suvehicleTypepport is currently firmware specific
/// not just this. I'm going to try to change that. If not, this will need to be removed.
......
......@@ -399,7 +399,7 @@ void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord
vehicle->altitudeAMSL()->rawValue().toFloat());
}
void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel)
void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeChange)
{
if (!vehicle->homePositionAvailable()) {
qgcApp()->showMessage(tr("Unable to change altitude, home position unknown."));
......@@ -410,6 +410,17 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
return;
}
// Don't allow altitude to fall below 3 meters above home
double currentAltRel = vehicle->altitudeRelative()->rawValue().toDouble();
double newAltRel = currentAltRel;
if (altitudeChange <= 0 && currentAltRel <= 3) {
return;
}
if (currentAltRel + altitudeChange < 3) {
altitudeChange = 3 - currentAltRel;
}
newAltRel = currentAltRel + altitudeChange;
vehicle->sendMavCommand(vehicle->defaultComponentId(),
MAV_CMD_DO_REPOSITION,
true, // show error is fails
......@@ -419,7 +430,7 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
NAN,
NAN,
NAN,
vehicle->homePosition().altitude() + altitudeRel);
vehicle->homePosition().altitude() + newAltRel);
}
void PX4FirmwarePlugin::startMission(Vehicle* vehicle)
......@@ -504,21 +515,6 @@ void PX4FirmwarePlugin::_handleAutopilotVersion(Vehicle* vehicle, mavlink_messag
}
}
QString PX4FirmwarePlugin::missionFlightMode(void)
{
return QString(_missionFlightMode);
}
QString PX4FirmwarePlugin::rtlFlightMode(void)
{
return QString(_rtlFlightMode);
}
QString PX4FirmwarePlugin::takeControlFlightMode(void)
{
return QString(_manualFlightMode);
}
bool PX4FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const
{
if ( vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"))) {
......
......@@ -37,6 +37,11 @@ public:
QString flightMode (uint8_t base_mode, uint32_t custom_mode) const override;
bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) override;
void setGuidedMode (Vehicle* vehicle, bool guidedMode) override;
QString pauseFlightMode (void) const override { return _holdFlightMode; }
QString missionFlightMode (void) const override { return _missionFlightMode; }
QString rtlFlightMode (void) const override { return _rtlFlightMode; }
QString landFlightMode (void) const override { return _landingFlightMode; }
QString takeControlFlightMode (void) const override { return _manualFlightMode; }
void pauseVehicle (Vehicle* vehicle) override;
void guidedModeRTL (Vehicle* vehicle) override;
void guidedModeLand (Vehicle* vehicle) override;
......@@ -61,9 +66,6 @@ public:
QString offlineEditingParamFile(Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/PX4/PX4.OfflineEditing.params"); }
QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); }
QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/PX4/BrandImage"); }
QString missionFlightMode (void) override;
QString rtlFlightMode (void) override;
QString takeControlFlightMode (void) override;
bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const override;
protected:
......
......@@ -54,6 +54,7 @@ Map {
function _possiblyCenterToVehiclePosition() {
if (!firstVehiclePositionReceived && allowVehicleLocationCenter && activeVehicleCoordinate.isValid) {
console.log("Moving to initial vehicle position", QGroundControl.flightMapInitialZoom)
firstVehiclePositionReceived = true
center = activeVehicleCoordinate
zoomLevel = QGroundControl.flightMapInitialZoom
......
......@@ -17,7 +17,6 @@ QGCAttitudeHUD 1.0 QGCAttitudeHUD.qml
QGCAttitudeWidget 1.0 QGCAttitudeWidget.qml
QGCCompassWidget 1.0 QGCCompassWidget.qml
QGCPitchIndicator 1.0 QGCPitchIndicator.qml
QGCSlider 1.0 QGCSlider.qml
ValuesWidget 1.0 ValuesWidget.qml
VehicleHealthWidget 1.0 VehicleHealthWidget.qml
VibrationWidget 1.0 VibrationWidget.qml
......
......@@ -498,7 +498,21 @@ bool SimpleMissionItem::friendlyEditAllowed(void) const
}
if (specifiesCoordinate() || specifiesAltitudeOnly()) {
return _missionItem.frame() == MAV_FRAME_GLOBAL || _missionItem.frame() == MAV_FRAME_GLOBAL_RELATIVE_ALT;
MAV_FRAME frame = _missionItem.frame();
switch (frame) {
case MAV_FRAME_GLOBAL:
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
#if 0
Coming soon
case MAV_FRAME_GLOBAL_INT:
case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
#endif
return true;
break;
default:
return false;
}
}
return true;
......
......@@ -10,47 +10,63 @@
import QtQuick 2.3
import QtQuick.Controls 1.2
import QtQuick.Controls.Styles 1.4
import QtQuick.Controls.Private 1.0
import QGroundControl.Palette 1.0
import QGroundControl.ScreenTools 1.0
import QtQuick.Controls.Private 1.0
Slider {
property var _qgcPal: QGCPalette { colorGroupEnabled: enabled }
id: _root
implicitHeight: ScreenTools.implicitSliderHeight
// Value indicator starts display from center instead of min value
property bool indicatorCentered: false
QGCPalette { id: qgcPal; colorGroupEnabled: enabled }
style: SliderStyle {
groove: Item {
property color fillColor: "#49d"
anchors.verticalCenter: parent.verticalCenter
implicitWidth: Math.round(TextSingleton.implicitHeight * 4.5)
implicitHeight: Math.max(6, Math.round(TextSingleton.implicitHeight * 0.3))
implicitWidth: Math.round(ScreenTools.defaultFontPixelHeight * 4.5)
implicitHeight: Math.round(ScreenTools.defaultFontPixelHeight * 0.3)
Rectangle {
radius: height/2
anchors.fill: parent
border.width: 1
border.color: "#888"
gradient: Gradient {
GradientStop { color: "#bbb" ; position: 0 }
GradientStop { color: "#ccc" ; position: 0.6 }
GradientStop { color: "#ccc" ; position: 1 }
}
radius: height / 2
anchors.fill: parent
color: qgcPal.button
border.width: 1
border.color: qgcPal.buttonText
}
Item {
clip: true
width: styleData.handlePosition
clip: true
x: _root.indicatorCentered ? indicatorCenteredIndicatorStart : 0
width: _root.indicatorCentered ? centerIndicatorWidth : styleData.handlePosition
height: parent.height
property real indicatorCenteredIndicatorStart: Math.min(styleData.handlePosition, parent.width / 2)
property real indicatorCenteredIndicatorStop: Math.max(styleData.handlePosition, parent.width / 2)
property real centerIndicatorWidth: indicatorCenteredIndicatorStop - indicatorCenteredIndicatorStart
Rectangle {
anchors.fill: parent
border.color: Qt.darker(fillColor, 1.2)
radius: height/2
gradient: Gradient {
GradientStop {color: Qt.lighter(fillColor, 1.3) ; position: 0}
GradientStop {color: fillColor ; position: 1.4}
}
anchors.fill: parent
color: qgcPal.colorBlue
border.color: Qt.darker(color, 1.2)
radius: height/2
}
}
}
handle: Rectangle {
anchors.centerIn: parent
color: qgcPal.button
border.color: qgcPal.buttonText
border.width: 1
implicitWidth: _radius * 2
implicitHeight: _radius * 2
radius: _radius
property real _radius: Math.round(ScreenTools.defaultFontPixelHeight * 0.75)
}
}
}
......@@ -70,6 +70,7 @@ Item {
property real implicitTextFieldHeight: Math.round(defaultFontPixelHeight * (isMobile ? 2.0 : 1.6))
property real implicitComboBoxHeight: Math.round(defaultFontPixelHeight * (isMobile ? 2.0 : 1.6))
property real implicitComboBoxWidth: Math.round(defaultFontPixelWidth * (isMobile ? 7.0 : 5.0))
property real implicitSliderHeight: isMobile ? Math.Max(defaultFontPixelHeight, minTouchPixels) : defaultFontPixelHeight
readonly property string normalFontFamily: "opensans"
readonly property string demiboldFontFamily: "opensans-demibold"
......
......@@ -7,11 +7,11 @@ import QGroundControl.Palette 1.0
/// The SliderSwitch control implements a sliding switch control similar to the power off
/// control on an iPhone.
Rectangle {
id: _root
width: label.contentWidth + (_diameter * 2.5) + (_border * 4)
height: Math.max(ScreenTools.isMobile ? ScreenTools.minTouchPixels : 0, label.height * 2.5)
radius: height /2
color: qgcPal.window
id: _root
implicitWidth: label.contentWidth + (_diameter * 2.5) + (_border * 4)
implicitHeight: Math.max(ScreenTools.isMobile ? ScreenTools.minTouchPixels : 0, label.height * 2.5)
radius: height /2
color: qgcPal.window
signal accept ///< Action confirmed
signal reject ///< Action rejected
......@@ -28,7 +28,7 @@ Rectangle {
id: label
anchors.horizontalCenter: parent.horizontalCenter
anchors.verticalCenter: parent.verticalCenter
text: qsTr("Slide to %1").arg(confirmText)
text: confirmText
}
Rectangle {
......
......@@ -1985,13 +1985,13 @@ void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
_firmwarePlugin->guidedModeGotoLocation(this, gotoCoord);
}
void Vehicle::guidedModeChangeAltitude(double altitudeRel)
void Vehicle::guidedModeChangeAltitude(double altitudeChange)
{
if (!guidedModeSupported()) {
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
return;
}
_firmwarePlugin->guidedModeChangeAltitude(this, altitudeRel);
_firmwarePlugin->guidedModeChangeAltitude(this, altitudeChange);
}
void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
......@@ -2397,11 +2397,21 @@ QString Vehicle::missionFlightMode(void) const
return _firmwarePlugin->missionFlightMode();
}
QString Vehicle::pauseFlightMode(void) const
{
return _firmwarePlugin->pauseFlightMode();
}
QString Vehicle::rtlFlightMode(void) const
{
return _firmwarePlugin->rtlFlightMode();
}
QString Vehicle::landFlightMode(void) const
{
return _firmwarePlugin->landFlightMode();
}
QString Vehicle::takeControlFlightMode(void) const
{
return _firmwarePlugin->takeControlFlightMode();
......
......@@ -294,7 +294,9 @@ public:
Q_PROPERTY(QString brandImageOutdoor READ brandImageOutdoor NOTIFY firmwareTypeChanged)
Q_PROPERTY(QStringList unhealthySensors READ unhealthySensors NOTIFY unhealthySensorsChanged)
Q_PROPERTY(QString missionFlightMode READ missionFlightMode CONSTANT)
Q_PROPERTY(QString pauseFlightMode READ pauseFlightMode CONSTANT)
Q_PROPERTY(QString rtlFlightMode READ rtlFlightMode CONSTANT)
Q_PROPERTY(QString landFlightMode READ landFlightMode CONSTANT)
Q_PROPERTY(QString takeControlFlightMode READ takeControlFlightMode CONSTANT)
Q_PROPERTY(QString firmwareTypeString READ firmwareTypeString NOTIFY firmwareTypeChanged)
Q_PROPERTY(QString vehicleTypeString READ vehicleTypeString NOTIFY vehicleTypeChanged)
......@@ -384,8 +386,9 @@ public:
/// Command vehicle to move to specified location (altitude is included and relative)
Q_INVOKABLE void guidedModeGotoLocation(const QGeoCoordinate& gotoCoord);
/// Command vehicle to change to the specified relatice altitude
Q_INVOKABLE void guidedModeChangeAltitude(double altitudeRel);
/// Command vehicle to change altitude
/// @param altitudeChange If > 0, go up by amount specified, if < 0, go down by amount specified
Q_INVOKABLE void guidedModeChangeAltitude(double altitudeChange);
/// Command vehicle to orbit given center point
/// @param centerCoord Center Coordinates
......@@ -575,7 +578,9 @@ public:
QString brandImageOutdoor () const;
QStringList unhealthySensors () const;
QString missionFlightMode () const;
QString pauseFlightMode () const;
QString rtlFlightMode () const;
QString landFlightMode () const;
QString takeControlFlightMode () const;
double defaultCruiseSpeed () const { return _defaultCruiseSpeed; }
double defaultHoverSpeed () const { return _defaultHoverSpeed; }
......
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