diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index f4cbcc688b64fb6380a36f134a737e26338b1a55..f34e5c351bec9686d34431097fe7a7ad1688648d 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -50,6 +50,11 @@ FirmwarePluginFactoryRegister* FirmwarePluginFactoryRegister::instance(void) return _instance; } +FirmwarePlugin::FirmwarePlugin(MAV_TYPE vehicleType) +{ + _vehicleType = vehicleType; +} + AutoPilotPlugin* FirmwarePlugin::autopilotPlugin(Vehicle* vehicle) { return new GenericAutoPilotPlugin(vehicle, vehicle); diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 55798a43ab94c7ad53addbfe85435235a0f76342..26fc3a86a32f4062e3a653a56084f023763d214b 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -49,6 +49,8 @@ public: TakeoffVehicleCapability = 1 << 4, ///< Vehicle supports guided takeoff } FirmwareCapabilities; + FirmwarePlugin(MAV_TYPE vehicleType = MAV_TYPE_GENERIC); + /// Maps from on parameter name to another /// key: parameter name to translate from /// value: mapped parameter name @@ -344,6 +346,9 @@ protected: // Returns regex QString to extract version information from text virtual QString _versionRegex() { return QString(); } +protected: + MAV_TYPE _vehicleType = MAV_TYPE_GENERIC; + private: QVariantList _toolBarIndicatorList; static QVariantList _cameraList; ///< Standard QGC camera list diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 63b7054fff104f56ed5a8077e74866f3f9cf52f2..a151439f252f3f3e4db27cc8c8bad3c066fdc94c 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -35,8 +35,9 @@ PX4FirmwarePluginInstanceData::PX4FirmwarePluginInstanceData(QObject* parent) } -PX4FirmwarePlugin::PX4FirmwarePlugin(void) - : _manualFlightMode (tr("Manual")) +PX4FirmwarePlugin::PX4FirmwarePlugin(MAV_TYPE vehicleType) + : FirmwarePlugin(vehicleType) + , _manualFlightMode (tr("Manual")) , _acroFlightMode (tr("Acro")) , _stabilizedFlightMode (tr("Stabilized")) , _rattitudeFlightMode (tr("Rattitude")) @@ -257,7 +258,7 @@ FactMetaData* PX4FirmwarePlugin::getMetaDataForFact(QObject* parameterMetaData, qWarning() << "Internal error: pointer passed to PX4FirmwarePlugin::getMetaDataForFact not PX4ParameterMetaData"; } - return NULL; + return nullptr; } void PX4FirmwarePlugin::addMetaDataToFact(QObject* parameterMetaData, Fact* fact, MAV_TYPE vehicleType) @@ -306,22 +307,16 @@ QString PX4FirmwarePlugin::missionCommandOverrides(MAV_TYPE vehicleType) const switch (vehicleType) { case MAV_TYPE_GENERIC: return QStringLiteral(":/json/PX4/MavCmdInfoCommon.json"); - break; case MAV_TYPE_FIXED_WING: return QStringLiteral(":/json/PX4/MavCmdInfoFixedWing.json"); - break; case MAV_TYPE_QUADROTOR: return QStringLiteral(":/json/PX4/MavCmdInfoMultiRotor.json"); - break; case MAV_TYPE_VTOL_QUADROTOR: return QStringLiteral(":/json/PX4/MavCmdInfoVTOL.json"); - break; case MAV_TYPE_SUBMARINE: return QStringLiteral(":/json/PX4/MavCmdInfoSub.json"); - break; case MAV_TYPE_GROUND_ROVER: return QStringLiteral(":/json/PX4/MavCmdInfoRover.json"); - break; default: qWarning() << "PX4FirmwarePlugin::missionCommandOverrides called with bad MAV_TYPE:" << vehicleType; return QString(); @@ -410,13 +405,14 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel qDebug() << takeoffAltRel << takeoffAltRelFromVehicle << takeoffAltAMSL << vehicleAltitudeAMSL; connect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult); - vehicle->sendMavCommand(vehicle->defaultComponentId(), - MAV_CMD_NAV_TAKEOFF, - true, // show error is fails - -1, // No pitch requested - 0, 0, // param 2-4 unused - NAN, NAN, NAN, // No yaw, lat, lon - takeoffAltAMSL); // AMSL altitude + vehicle->sendMavCommand( + vehicle->defaultComponentId(), + MAV_CMD_NAV_TAKEOFF, + true, // show error is fails + -1, // No pitch requested + 0, 0, // param 2-4 unused + NAN, NAN, NAN, // No yaw, lat, lon + static_cast(takeoffAltAMSL)); // AMSL altitude } void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) @@ -446,8 +442,8 @@ void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord MAV_DO_REPOSITION_FLAGS_CHANGE_MODE, 0.0f, NAN, - gotoCoord.latitude(), - gotoCoord.longitude(), + static_cast(gotoCoord.latitude()), + static_cast(gotoCoord.longitude()), vehicle->altitudeAMSL()->rawValue().toFloat()); } } @@ -475,7 +471,7 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu NAN, NAN, NAN, - vehicle->homePosition().altitude() + newAltRel); + static_cast(vehicle->homePosition().altitude() + newAltRel)); } void PX4FirmwarePlugin::startMission(Vehicle* vehicle) @@ -594,3 +590,8 @@ QString PX4FirmwarePlugin::_getLatestVersionFileUrl(Vehicle* vehicle){ QString PX4FirmwarePlugin::_versionRegex() { return QStringLiteral("v([0-9,\\.]*) Stable"); } + +bool PX4FirmwarePlugin::supportsNegativeThrust(void) +{ + return _vehicleType == MAV_TYPE_GROUND_ROVER; +} diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 3e7f794ce770d707fc138e1d2e9ab01c8b83f443..072933c67df8cda0c1141606b5e040b0b43b1719 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -23,8 +23,8 @@ class PX4FirmwarePlugin : public FirmwarePlugin Q_OBJECT public: - PX4FirmwarePlugin(void); - ~PX4FirmwarePlugin(); + PX4FirmwarePlugin (MAV_TYPE vehicleType); + ~PX4FirmwarePlugin () override; // Overrides from FirmwarePlugin @@ -69,6 +69,7 @@ public: QString autoDisarmParameter (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("COM_DISARM_LAND"); } uint32_t highLatencyCustomModeTo32Bits (uint16_t hlCustomMode) override; bool supportsTerrainFrame (void) const override { return false; } + bool supportsNegativeThrust (void) override; protected: typedef struct { @@ -122,7 +123,7 @@ class PX4FirmwarePluginInstanceData : public QObject Q_OBJECT public: - PX4FirmwarePluginInstanceData(QObject* parent = NULL); + PX4FirmwarePluginInstanceData(QObject* parent = nullptr); bool versionNotified; ///< true: user notified over version issue }; diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePluginFactory.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePluginFactory.cc index 66708e2193cebfaf95633ea650e52759b6334920..3e714cee32ae736413bafe894d584d22e3f05117 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePluginFactory.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePluginFactory.cc @@ -13,6 +13,7 @@ #include "PX4FirmwarePluginFactory.h" #include "PX4/PX4FirmwarePlugin.h" + PX4FirmwarePluginFactory PX4FirmwarePluginFactory; PX4FirmwarePluginFactory::PX4FirmwarePluginFactory(void) @@ -24,21 +25,17 @@ PX4FirmwarePluginFactory::PX4FirmwarePluginFactory(void) QList PX4FirmwarePluginFactory::supportedFirmwareTypes(void) const { QList list; - list.append(MAV_AUTOPILOT_PX4); return list; } FirmwarePlugin* PX4FirmwarePluginFactory::firmwarePluginForAutopilot(MAV_AUTOPILOT autopilotType, MAV_TYPE vehicleType) { - Q_UNUSED(vehicleType); - if (autopilotType == MAV_AUTOPILOT_PX4) { if (!_pluginInstance) { - _pluginInstance = new PX4FirmwarePlugin; + _pluginInstance = new PX4FirmwarePlugin(vehicleType); } return _pluginInstance; } - return nullptr; }