Commit ed8e2df7 authored by dogmaphobic's avatar dogmaphobic

Show Orbit button only if supported.

parent 5116e077
......@@ -773,7 +773,7 @@ void ParameterLoader::_saveToEEPROM(void)
{
if (_saveRequired) {
_saveRequired = false;
if (_vehicle->firmwarePlugin()->isCapable(FirmwarePlugin::MavCmdPreflightStorageCapability)) {
if (_vehicle->firmwarePlugin()->isCapable(_vehicle, FirmwarePlugin::MavCmdPreflightStorageCapability)) {
mavlink_message_t msg;
mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _vehicle->id(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
......
......@@ -143,7 +143,7 @@ APMFirmwarePlugin::APMFirmwarePlugin(void)
}
bool APMFirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
bool APMFirmwarePlugin::isCapable(const Vehicle* /*vehicle*/, FirmwareCapabilities capabilities)
{
return (capabilities & (SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
}
......
......@@ -66,14 +66,14 @@ private:
class APMFirmwarePlugin : public FirmwarePlugin
{
Q_OBJECT
public:
// Overrides from FirmwarePlugin
QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle) final;
QList<MAV_CMD> supportedMissionCommands(void) final;
bool isCapable(FirmwareCapabilities capabilities);
bool isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities);
QStringList flightModes(Vehicle* vehicle) final;
QString flightMode(uint8_t base_mode, uint32_t custom_mode) const final;
bool setFlightMode(const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final;
......@@ -103,7 +103,7 @@ protected:
private slots:
void _artooSocketError(QAbstractSocket::SocketError socketError);
private:
void _adjustSeverity(mavlink_message_t* message) const;
void _adjustCalibrationMessageSeverity(mavlink_message_t* message) const;
......
......@@ -98,7 +98,7 @@ int ArduCopterFirmwarePlugin::remapParamNameHigestMinorVersionNumber(int majorVe
return majorVersionNumber == 3 ? 4: Vehicle::versionNotSetValue;
}
bool ArduCopterFirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
bool ArduCopterFirmwarePlugin::isCapable(const Vehicle* /*vehicle*/, FirmwareCapabilities capabilities)
{
return (capabilities & (SetFlightModeCapability | GuidedModeCapability | PauseVehicleCapability)) == capabilities;
}
......
......@@ -48,12 +48,12 @@ public:
class ArduCopterFirmwarePlugin : public APMFirmwarePlugin
{
Q_OBJECT
public:
ArduCopterFirmwarePlugin(void);
// Overrides from FirmwarePlugin
bool isCapable(FirmwareCapabilities capabilities) final;
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;
......
......@@ -15,8 +15,9 @@
const char* guided_mode_not_supported_by_vehicle = "Guided mode not supported by Vehicle.";
bool FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
bool FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities)
{
Q_UNUSED(vehicle);
Q_UNUSED(capabilities);
return false;
}
......
......@@ -40,7 +40,8 @@ public:
SetFlightModeCapability = 1 << 0, ///< FirmwarePlugin::setFlightMode method is supported
MavCmdPreflightStorageCapability = 1 << 1, ///< MAV_CMD_PREFLIGHT_STORAGE is supported
PauseVehicleCapability = 1 << 2, ///< Vehicle supports pausing at current location
GuidedModeCapability = 1 << 3, ///< Vehicle Support guided mode commands
GuidedModeCapability = 1 << 3, ///< Vehicle Supports guided mode commands
OrbitModeCapability = 1 << 4, ///< Vehicle Supports orbit mode
} FirmwareCapabilities;
/// Maps from on parameter name to another
......@@ -62,7 +63,7 @@ public:
virtual void initializeVehicle(Vehicle* vehicle);
/// @return true: Firmware supports all specified capabilites
virtual bool isCapable(FirmwareCapabilities capabilities);
virtual bool isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities);
/// Returns VehicleComponents for specified Vehicle
/// @param vehicle Vehicle to associate with components
......
......@@ -170,8 +170,11 @@ int PX4FirmwarePlugin::manualControlReservedButtonCount(void)
return 0; // 0 buttons reserved for rc switch simulation
}
bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities)
bool PX4FirmwarePlugin::isCapable(const Vehicle *vehicle, FirmwareCapabilities capabilities)
{
if(vehicle->multiRotor()) {
return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability | OrbitModeCapability)) == capabilities;
}
return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability)) == capabilities;
}
......
......@@ -30,7 +30,7 @@ public:
QList<VehicleComponent*> componentsForVehicle(AutoPilotPlugin* vehicle) final;
QList<MAV_CMD> supportedMissionCommands(void) final;
bool isCapable (FirmwareCapabilities capabilities) final;
bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) final;
QStringList flightModes (Vehicle* vehicle) final;
QString flightMode (uint8_t base_mode, uint32_t custom_mode) const final;
bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final;
......@@ -53,7 +53,7 @@ public:
QString internalParameterMetaDataFile (void) final { return QString(":/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml"); }
void getParameterMetaDataVersionInfo (const QString& metaDataFile, int& majorVersion, int& minorVersion) final { PX4ParameterMetaData::getParameterMetaDataVersionInfo(metaDataFile, majorVersion, minorVersion); }
QObject* loadParameterMetaData (const QString& metaDataFile);
bool adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message);
bool adjustIncomingMavlinkMessage (Vehicle* vehicle, mavlink_message_t* message);
// Use these constants to set flight modes using setFlightMode method. Don't use hardcoded string names since the
// names may change.
......
......@@ -501,7 +501,7 @@ Item {
QGCButton {
pointSize: _guidedModeBar._fontPointSize
text: qsTr("Orbit")
visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.guidedModeSupported && _activeVehicle.armed
visible: (_activeVehicle && _activeVehicle.flying) && _activeVehicle.orbitModeSupported && _activeVehicle.armed
onClicked: _guidedModeBar.confirmAction(_guidedModeBar.confirmOrbit)
}
......
......@@ -1223,7 +1223,7 @@ void Vehicle::setArmed(bool armed)
bool Vehicle::flightModeSetAvailable(void)
{
return _firmwarePlugin->isCapable(FirmwarePlugin::SetFlightModeCapability);
return _firmwarePlugin->isCapable(this, FirmwarePlugin::SetFlightModeCapability);
}
QStringList Vehicle::flightModes(void)
......@@ -1583,12 +1583,17 @@ void Vehicle::setFlying(bool flying)
bool Vehicle::guidedModeSupported(void) const
{
return _firmwarePlugin->isCapable(FirmwarePlugin::GuidedModeCapability);
return _firmwarePlugin->isCapable(this, FirmwarePlugin::GuidedModeCapability);
}
bool Vehicle::pauseVehicleSupported(void) const
{
return _firmwarePlugin->isCapable(FirmwarePlugin::PauseVehicleCapability);
return _firmwarePlugin->isCapable(this, FirmwarePlugin::PauseVehicleCapability);
}
bool Vehicle::orbitModeSupported() const
{
return _firmwarePlugin->isCapable(this, FirmwarePlugin::OrbitModeCapability);
}
void Vehicle::guidedModeRTL(void)
......@@ -1639,8 +1644,8 @@ void Vehicle::guidedModeChangeAltitude(double altitudeRel)
void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
{
if (!guidedModeSupported()) {
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
if (!orbitModeSupported()) {
qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle."));
return;
}
_firmwarePlugin->guidedModeOrbit(this, centerCoord, radius, velocity, altitude);
......
......@@ -293,6 +293,9 @@ public:
/// true: pauseVehicle command is supported
Q_PROPERTY(bool pauseVehicleSupported READ pauseVehicleSupported CONSTANT)
/// true: Orbit mode is supported by this vehicle
Q_PROPERTY(bool orbitModeSupported READ orbitModeSupported CONSTANT)
// FactGroup object model properties
Q_PROPERTY(Fact* roll READ roll CONSTANT)
......@@ -380,6 +383,7 @@ public:
bool guidedModeSupported(void) const;
bool pauseVehicleSupported(void) const;
bool orbitModeSupported(void) const;
// Property accessors
......
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