diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 1e93b86..f5e3b47 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -630,6 +630,15 @@ QString APMFirmwarePlugin::getHobbsMeter(Vehicle* vehicle) return timeStr; } +bool APMFirmwarePlugin::hasGripper(const Vehicle* vehicle) const +{ + if(vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, "GRIP_ENABLE")) { + bool _hasGripper = (vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("GRIP_ENABLE"))->rawValue().toInt()) == 1 ? true : false; + return _hasGripper; + } + return false; +} + bool APMFirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const { return vehicle->flightMode() == "Guided"; diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 5afabd8..bbef05f 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -77,6 +77,7 @@ public: QString brandImageIndoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); } QString brandImageOutdoor (const Vehicle* vehicle) const override { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImage"); } QString getHobbsMeter (Vehicle* vehicle) override; + bool hasGripper (const Vehicle* vehicle) const override; protected: /// All access to singleton is through stack specific implementation diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index cc94042..a0bb21e 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -159,6 +159,9 @@ public: /// @return The minimum equivalent airspeed setpoint virtual double minimumEquivalentAirspeed(Vehicle* /*vehicle*/) { return NAN; } + /// @return Return true if the GCS has enabled Grip_enable option + virtual bool hasGripper(const Vehicle* /*vehicle*/) const { return false; } + /// @return Return true if we have received the ground speed limits for the mulirotor. virtual bool mulirotorSpeedLimitsAvailable(Vehicle* /*vehicle*/) { return false; } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 5549747..1d75579 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -728,3 +728,12 @@ QString PX4FirmwarePlugin::getHobbsMeter(Vehicle* vehicle) qCDebug(VehicleLog) << "Hobbs Meter string:" << timeStr; return timeStr; } + +bool PX4FirmwarePlugin::hasGripper(const Vehicle* vehicle) const +{ + if(vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("PD_GRIPPER_EN"))) { + bool _hasGripper = (vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("PD_GRIPPER_EN"))->rawValue().toInt()) != 0 ? true : false; + return _hasGripper; + } + return false; +} \ No newline at end of file diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 3fb7185..ff8e9eb 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -72,6 +72,7 @@ public: uint32_t highLatencyCustomModeTo32Bits (uint16_t hlCustomMode) override; bool supportsNegativeThrust (Vehicle* vehicle) override; QString getHobbsMeter (Vehicle* vehicle) override; + bool hasGripper (const Vehicle* vehicle) const override; protected: typedef struct { diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index de37158..fb96cae 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2709,6 +2709,11 @@ double Vehicle::minimumEquivalentAirspeed() return _firmwarePlugin->minimumEquivalentAirspeed(this); } +bool Vehicle::hasGripper() const +{ + return _firmwarePlugin->hasGripper(this); +} + void Vehicle::startMission() { _firmwarePlugin->startMission(this); @@ -4270,3 +4275,20 @@ void Vehicle::setGripperAction(GRIPPER_ACTIONS gripperAction) gripperAction, // Param2: Gripper Action 0, 0, 0, 0, 0); // Param 3 ~ 7 : unused } + +void Vehicle::sendGripperAction(GRIPPER_OPTIONS gripperOption) +{ + switch(gripperOption) { + case Gripper_release: + setGripperAction(GRIPPER_ACTION_RELEASE); + break; + case Gripper_grab: + setGripperAction(GRIPPER_ACTION_GRAB); + break; + case Invalid_option: + qDebug("unknown function"); + break; + default: + break; + } +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 05c11fd..0902e6c 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -249,6 +249,7 @@ public: Q_PROPERTY(qreal gimbalPitch READ gimbalPitch NOTIFY gimbalPitchChanged) Q_PROPERTY(qreal gimbalYaw READ gimbalYaw NOTIFY gimbalYawChanged) Q_PROPERTY(bool gimbalData READ gimbalData NOTIFY gimbalDataChanged) + Q_PROPERTY(bool hasGripper READ hasGripper CONSTANT) Q_PROPERTY(bool isROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged) Q_PROPERTY(CheckList checkListState READ checkListState WRITE setCheckListState NOTIFY checkListStateChanged) Q_PROPERTY(bool readyToFlyAvailable READ readyToFlyAvailable NOTIFY readyToFlyAvailableChanged) ///< true: readyToFly signalling is available on this vehicle @@ -470,7 +471,7 @@ public: bool roiModeSupported () const; bool takeoffVehicleSupported () const; QString gotoFlightMode () const; - + bool hasGripper () const; bool haveMRSpeedLimits() const { return _multirotor_speed_limits_available; } bool haveFWSpeedLimits() const { return _fixed_wing_airspeed_limits_available; } @@ -529,8 +530,18 @@ public: * @brief Send MAV_CMD_DO_GRIPPER command to trigger specified action in the vehicle * * @param gripperAction Gripper action to trigger - */ + */ + + enum GRIPPER_OPTIONS + { + Gripper_release = GRIPPER_ACTION_RELEASE, + Gripper_grab = GRIPPER_ACTION_GRAB, + Invalid_option = GRIPPER_ACTIONS_ENUM_END, + }; + Q_ENUM(GRIPPER_OPTIONS) + void setGripperAction(GRIPPER_ACTIONS gripperAction); + Q_INVOKABLE void sendGripperAction(GRIPPER_OPTIONS gripperOption); bool fixedWing() const; bool multiRotor() const;