From 78ee54ee91bfe2cb49c077e47435933ed86eceb0 Mon Sep 17 00:00:00 2001 From: davidsastresas Date: Sat, 9 Mar 2024 13:39:34 +0100 Subject: [PATCH] Joystick: fix gimbal control and add some new button actions for gimbal control --- src/Joystick/Joystick.cc | 67 ++++++++++++++++++++---------------------------- src/Joystick/Joystick.h | 9 +++---- 2 files changed, 31 insertions(+), 45 deletions(-) diff --git a/src/Joystick/Joystick.cc b/src/Joystick/Joystick.cc index 970f6b9..13682c0 100644 --- a/src/Joystick/Joystick.cc +++ b/src/Joystick/Joystick.cc @@ -16,6 +16,7 @@ #include "VideoManager.h" #include "QGCCameraManager.h" #include "QGCCameraControl.h" +#include "GimbalController.h" #include @@ -64,6 +65,8 @@ const char* Joystick::_buttonActionGimbalUp = QT_TR_NOOP("Gimbal Up"); const char* Joystick::_buttonActionGimbalLeft = QT_TR_NOOP("Gimbal Left"); const char* Joystick::_buttonActionGimbalRight = QT_TR_NOOP("Gimbal Right"); const char* Joystick::_buttonActionGimbalCenter = QT_TR_NOOP("Gimbal Center"); +const char* Joystick::_buttonActionGimbalYawLock = QT_TR_NOOP("Gimbal Yaw Lock"); +const char* Joystick::_buttonActionGimbalYawFollow = QT_TR_NOOP("Gimbal Yaw Follow"); const char* Joystick::_buttonActionEmergencyStop = QT_TR_NOOP("Emergency Stop"); const char* Joystick::_buttonActionGripperGrab = QT_TR_NOOP("Gripper Close"); const char* Joystick::_buttonActionGripperRelease = QT_TR_NOOP("Gripper Open"); @@ -640,6 +643,7 @@ void Joystick::_handleAxis() axis = _rgFunctionAxis[throttleFunction]; float throttle = _adjustRange(_rgAxisValues[axis],_rgCalibration[axis], _throttleMode==ThrottleModeDownZero?false:_deadband); + // These are only used for printing JoystickValuesLog float gimbalPitch = 0.0f; float gimbalYaw = 0.0f; @@ -717,10 +721,10 @@ void Joystick::startPolling(Vehicle* vehicle) disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); - disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep); - disconnect(this, &Joystick::gimbalYawStep, _activeVehicle, &Vehicle::gimbalYawStep); - disconnect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); - disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue); + disconnect(this, &Joystick::gimbalYawLock, _activeVehicle->gimbalController(), &GimbalController::gimbalYawLock); + disconnect(this, &Joystick::centerGimbal, _activeVehicle->gimbalController(), &GimbalController::centerGimbal); + disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStep); + disconnect(this, &Joystick::gimbalYawStep, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStep); disconnect(this, &Joystick::emergencyStop, _activeVehicle, &Vehicle::emergencyStop); disconnect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); disconnect(this, &Joystick::landingGearDeploy, _activeVehicle, &Vehicle::landingGearDeploy); @@ -743,10 +747,10 @@ void Joystick::startPolling(Vehicle* vehicle) connect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); connect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); connect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); - connect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep); - connect(this, &Joystick::gimbalYawStep, _activeVehicle, &Vehicle::gimbalYawStep); - connect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); - connect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue); + connect(this, &Joystick::gimbalYawLock, _activeVehicle->gimbalController(), &GimbalController::gimbalYawLock); + connect(this, &Joystick::centerGimbal, _activeVehicle->gimbalController(), &GimbalController::centerGimbal); + connect(this, &Joystick::gimbalPitchStep, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStep); + connect(this, &Joystick::gimbalYawStep, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStep); connect(this, &Joystick::emergencyStop, _activeVehicle, &Vehicle::emergencyStop); connect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); connect(this, &Joystick::landingGearDeploy, _activeVehicle, &Vehicle::landingGearDeploy); @@ -767,10 +771,10 @@ void Joystick::stopPolling(void) disconnect(this, &Joystick::setArmed, _activeVehicle, &Vehicle::setArmedShowError); disconnect(this, &Joystick::setVtolInFwdFlight, _activeVehicle, &Vehicle::setVtolInFwdFlight); disconnect(this, &Joystick::setFlightMode, _activeVehicle, &Vehicle::setFlightMode); - disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle, &Vehicle::gimbalPitchStep); - disconnect(this, &Joystick::gimbalYawStep, _activeVehicle, &Vehicle::gimbalYawStep); - disconnect(this, &Joystick::centerGimbal, _activeVehicle, &Vehicle::centerGimbal); - disconnect(this, &Joystick::gimbalControlValue, _activeVehicle, &Vehicle::gimbalControlValue); + disconnect(this, &Joystick::gimbalYawLock, _activeVehicle->gimbalController(), &GimbalController::gimbalYawLock); + disconnect(this, &Joystick::centerGimbal, _activeVehicle->gimbalController(), &GimbalController::centerGimbal); + disconnect(this, &Joystick::gimbalPitchStep, _activeVehicle->gimbalController(), &GimbalController::gimbalPitchStep); + disconnect(this, &Joystick::gimbalYawStep, _activeVehicle->gimbalController(), &GimbalController::gimbalYawStep); disconnect(this, &Joystick::gripperAction, _activeVehicle, &Vehicle::setGripperAction); disconnect(this, &Joystick::landingGearDeploy, _activeVehicle, &Vehicle::landingGearDeploy); disconnect(this, &Joystick::landingGearRetract, _activeVehicle, &Vehicle::landingGearRetract); @@ -1050,21 +1054,21 @@ void Joystick::_executeButtonAction(const QString& action, bool buttonDown) } else if(action == _buttonActionToggleVideoRecord) { if (buttonDown) emit toggleVideoRecord(); } else if(action == _buttonActionGimbalUp) { - if (buttonDown) _pitchStep(1); + if (buttonDown) emit gimbalPitchStep(1); } else if(action == _buttonActionGimbalDown) { - if (buttonDown) _pitchStep(-1); + if (buttonDown) emit gimbalPitchStep(-1); } else if(action == _buttonActionGimbalLeft) { - if (buttonDown) _yawStep(-1); + if (buttonDown) emit gimbalYawStep(-1); } else if(action == _buttonActionGimbalRight) { - if (buttonDown) _yawStep(1); + if (buttonDown) emit gimbalYawStep(1); } else if(action == _buttonActionGimbalCenter) { - if (buttonDown) { - _localPitch = 0.0; - _localYaw = 0.0; - emit gimbalControlValue(0.0, 0.0); - } + if (buttonDown) emit centerGimbal(); + } else if(action == _buttonActionGimbalYawLock) { + if (buttonDown) emit gimbalYawLock(true); + } else if(action == _buttonActionGimbalYawFollow) { + if (buttonDown) emit gimbalYawLock(false); } else if(action == _buttonActionEmergencyStop) { - if(buttonDown) emit emergencyStop(); + if (buttonDown) emit emergencyStop(); } else if(action == _buttonActionGripperGrab) { if(buttonDown) { emit gripperAction(GRIPPER_ACTION_GRAB); @@ -1089,23 +1093,6 @@ void Joystick::_executeButtonAction(const QString& action, bool buttonDown) } } -void Joystick::_pitchStep(int direction) -{ - _localPitch += static_cast(direction); - //-- Arbitrary range - if(_localPitch < -90.0) _localPitch = -90.0; - if(_localPitch > 35.0) _localPitch = 35.0; - emit gimbalControlValue(_localPitch, _localYaw); -} - -void Joystick::_yawStep(int direction) -{ - _localYaw += static_cast(direction); - if(_localYaw < -180.0) _localYaw = -180.0; - if(_localYaw > 180.0) _localYaw = 180.0; - emit gimbalControlValue(_localPitch, _localYaw); -} - bool Joystick::_validAxis(int axis) const { if(axis >= 0 && axis < _axisCount) { @@ -1168,6 +1155,8 @@ void Joystick::_buildActionList(Vehicle* activeVehicle) _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalLeft, true)); _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalRight, true)); _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalCenter)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalYawLock)); + _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGimbalYawFollow)); _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionEmergencyStop)); _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperGrab)); _assignableButtonActions.append(new AssignableButtonAction(this, _buttonActionGripperRelease)); diff --git a/src/Joystick/Joystick.h b/src/Joystick/Joystick.h index 1f8e496..e969db5 100644 --- a/src/Joystick/Joystick.h +++ b/src/Joystick/Joystick.h @@ -213,7 +213,7 @@ signals: void gimbalPitchStep (int direction); void gimbalYawStep (int direction); void centerGimbal (); - void gimbalControlValue (double pitch, double yaw); + void gimbalYawLock (bool lock); void setArmed (bool arm); void setVtolInFwdFlight (bool set); void setFlightMode (const QString& flightMode); @@ -236,11 +236,6 @@ protected: void _handleButtons (); void _buildActionList (Vehicle* activeVehicle); - void _pitchStep (int direction); - void _yawStep (int direction); - double _localYaw = 0.0; - double _localPitch = 0.0; - private: virtual bool _open () = 0; virtual void _close () = 0; @@ -355,6 +350,8 @@ private: static const char* _buttonActionGimbalLeft; static const char* _buttonActionGimbalRight; static const char* _buttonActionGimbalCenter; + static const char* _buttonActionGimbalYawLock; + static const char* _buttonActionGimbalYawFollow; static const char* _buttonActionEmergencyStop; static const char* _buttonActionGripperGrab; static const char* _buttonActionGripperRelease;