From b0a54951b58673283a90816770efd9a368dabc1e Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Thu, 23 Mar 2017 10:17:09 -0700 Subject: [PATCH] Only show gimbal yaw if vehicle points to next waypoint --- src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc | 10 ++++++++++ src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h | 1 + src/FirmwarePlugin/FirmwarePlugin.cc | 5 +++++ src/FirmwarePlugin/FirmwarePlugin.h | 3 +++ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 9 +++++++++ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 1 + src/MissionEditor/MissionEditor.qml | 1 - src/MissionManager/MissionController.cc | 9 ++++++--- src/Vehicle/Vehicle.cc | 4 ++++ src/Vehicle/Vehicle.h | 3 +++ 10 files changed, 42 insertions(+), 4 deletions(-) diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 3c3ef9e5e..5bdb2b627 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -224,3 +224,13 @@ QString ArduCopterFirmwarePlugin::takeControlFlightMode(void) { return QStringLiteral("Stabilize"); } + + +bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const +{ + if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR"))) { + Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("WP_YAW_BEHAVIOR")); + return yawMode && yawMode->rawValue().toInt() != 0; + } + return false; +} diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index 9eb8e0d73..40c5f7623 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -69,6 +69,7 @@ public: QString geoFenceRadiusParam(Vehicle* vehicle) final; QString offlineEditingParamFile(Vehicle* vehicle) final { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Copter.OfflineEditing.params"); } QString takeControlFlightMode(void) final; + bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const final; private: static bool _remapParamNameIntialized; diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 18c2d31a4..498be2571 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -431,3 +431,8 @@ const QVariantList& FirmwarePlugin::cameraList(const Vehicle* vehicle) return _cameraList; } + +bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const +{ + return vehicle->multiRotor() ? false : true; +} diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index a6bfa5c30..2a9f51da2 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -267,6 +267,9 @@ public: /// Returns a list of CameraMetaData objects for available cameras on the vehicle. virtual const QVariantList& cameraList(const Vehicle* vehicle); + /// @true: When flying a mission the vehicle is always facing towards the next waypoint + virtual bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const; + // FIXME: Hack workaround for non pluginize FollowMe support static const char* px4FollowMeFlightMode; diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index c399aab72..cd2314cbc 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -498,3 +498,12 @@ QString PX4FirmwarePlugin::takeControlFlightMode(void) { return QString(_manualFlightMode); } + +bool PX4FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const +{ + if ( vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE"))) { + Fact* yawMode = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("MIS_YAWMODE")); + return yawMode && yawMode->rawValue().toInt() == 1; + } + return false; +} diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index ef0eed447..7394abb02 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -63,6 +63,7 @@ public: QString missionFlightMode (void) override; QString rtlFlightMode (void) override; QString takeControlFlightMode (void) override; + bool vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) const override; protected: typedef struct { diff --git a/src/MissionEditor/MissionEditor.qml b/src/MissionEditor/MissionEditor.qml index 61f599d41..b3ecbf9f7 100644 --- a/src/MissionEditor/MissionEditor.qml +++ b/src/MissionEditor/MissionEditor.qml @@ -236,7 +236,6 @@ QGCView { /// Sets a new current mission item /// @param sequenceNumber - index for new item, -1 to clear current item function setCurrentItem(sequenceNumber) { - console.log("setCurrentItem", sequenceNumber, _currentMissionIndex) if (sequenceNumber !== _currentMissionIndex) { _currentMissionItem = undefined _currentMissionIndex = -1 diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index e7c26d259..293f71621 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -939,9 +939,12 @@ void MissionController::_recalcMissionFlightStatus() } // Look for gimbal change - double gimbalYaw = item->specifiedGimbalYaw(); - if (!qIsNaN(gimbalYaw)) { - _missionFlightStatus.gimbalYaw = gimbalYaw; + if (_activeVehicle->vehicleYawsToNextWaypointInMission()) { + // We current only support gimbal display in this mode + double gimbalYaw = item->specifiedGimbalYaw(); + if (!qIsNaN(gimbalYaw)) { + _missionFlightStatus.gimbalYaw = gimbalYaw; + } } if (i == 0) { diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 70b88a0e2..bd6359fd0 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -2436,6 +2436,10 @@ const QVariantList& Vehicle::cameraList(void) const return emptyList; } +bool Vehicle::vehicleYawsToNextWaypointInMission(void) const +{ + return _firmwarePlugin->vehicleYawsToNextWaypointInMission(this); +} //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index c2c9a71a3..4e5353768 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -660,6 +660,9 @@ public: bool supportsMissionItemInt(void) const { return _supportsMissionItemInt; } + /// @true: When flying a mission the vehicle is always facing towards the next waypoint + bool vehicleYawsToNextWaypointInMission(void) const; + public slots: /// Sets the firmware plugin instance data associated with this Vehicle. This object will be parented to the Vehicle /// and destroyed when the vehicle goes away. -- 2.22.0